raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Take T_cn_cnm1 instead of cam1/T_cam_imu from the calibration file

Browse Source
This commit is contained in:
ke
2018-01-12 13:21:23 -05:00
parent 365c45c46f
commit 050c50defa
3 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
config/camchain-imucam-euroc.yaml
View File

@@ -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

3
src/image_processor.cpp
View File

@@ -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();

16
src/msckf_vio.cpp
View File

@@ -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("===========================================");