Take T_cn_cnm1 instead of cam1/T_cam_imu from the calibration file

Author: ke-sun

config/camchain-imucam-euroc.yaml

@@ -19,6 +19,11 @@ cam1:
     -0.999755099723116,   0.013011905181504,   0.017900583825251,  -0.020569771258915,
      0.018223771455443,   0.025158836311552,   0.999517347077547,  -0.008638135126028,
                      0,                   0,                   0,   1.000000000000000]
+  T_cn_cnm1:
+    [0.999997256477881,   0.002312067192424,   0.000376008102415,  -0.110073808127187, 
+    -0.002317135723281,   0.999898048506644,   0.014089835846648,   0.000399121547014,
+    -0.000343393120525,  -0.014090668452714,   0.999900662637729,  -0.000853702503357, 
+                     0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
   distortion_model: radtan

src/image_processor.cpp

@@ -93,7 +93,8 @@ bool ImageProcessor::loadParameters() {
   R_cam0_imu = R_imu_cam0.t();
   t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;
 
-  cv::Mat     T_imu_cam1 = utils::getTransformCV(nh, "cam1/T_cam_imu");
+  cv::Mat T_cam0_cam1 = utils::getTransformCV(nh, "cam1/T_cn_cnm1");
+  cv::Mat T_imu_cam1 = T_cam0_cam1 * T_imu_cam0;
   cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3)));
   cv::Vec3d   t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3));
   R_cam1_imu = R_imu_cam1.t();

src/msckf_vio.cpp

@@ -132,16 +132,12 @@ bool MsckfVio::loadParameters() {
   Isometry3d T_imu_cam0 = utils::getTransformEigen(nh, "cam0/T_cam_imu");
   Isometry3d T_cam0_imu = T_imu_cam0.inverse();
 
-  Isometry3d T_imu_cam1 = utils::getTransformEigen(nh, "cam1/T_cam_imu");
-  Isometry3d T_cam1_imu = T_imu_cam1.inverse();
-
-  Isometry3d T_body_imu = utils::getTransformEigen(nh, "T_imu_body");
-
-  //CAMState::T_imu_cam0 = T_cam0_imu.inverse();
   state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
   state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
-  CAMState::T_cam0_cam1 = T_imu_cam1 * T_cam0_imu;
-  IMUState::T_imu_body = T_body_imu.inverse();
+  CAMState::T_cam0_cam1 =
+    utils::getTransformEigen(nh, "cam1/T_cn_cnm1");
+  IMUState::T_imu_body =
+    utils::getTransformEigen(nh, "T_imu_body").inverse();
 
   // Maximum number of camera states to be stored
   nh.param<int>("max_cam_state_size", max_cam_state_size, 30);
@@ -172,8 +168,8 @@ bool MsckfVio::loadParameters() {
   ROS_INFO("initial extrinsic translation cov: %f",
       extrinsic_translation_cov);
 
-  cout << T_imu_cam1.linear() << endl;
-  cout << T_imu_cam1.translation().transpose() << endl;
+  cout << T_imu_cam0.linear() << endl;
+  cout << T_imu_cam0.translation().transpose() << endl;
 
   ROS_INFO("max camera state #: %d", max_cam_state_size);
   ROS_INFO("===========================================");