From 050c50defa5a7fd9a04c1eed5687b405f02919b5 Mon Sep 17 00:00:00 2001 From: ke Date: Fri, 12 Jan 2018 13:21:23 -0500 Subject: [PATCH] Take T_cn_cnm1 instead of cam1/T_cam_imu from the calibration file --- config/camchain-imucam-euroc.yaml | 5 +++++ src/image_processor.cpp | 3 ++- src/msckf_vio.cpp | 16 ++++++---------- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/config/camchain-imucam-euroc.yaml b/config/camchain-imucam-euroc.yaml index dcbd9e5..de4c321 100644 --- a/config/camchain-imucam-euroc.yaml +++ b/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 diff --git a/src/image_processor.cpp b/src/image_processor.cpp index d4ef1f3..adc3c24 100644 --- a/src/image_processor.cpp +++ b/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(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index dc682ce..d9c1b8d 100644 --- a/src/msckf_vio.cpp +++ b/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("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("===========================================");