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

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