ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

small fix

This commit is contained in:
Vladyslav Usenko 2019-07-12 14:05:07 +02:00
parent 8775479353
commit 86f2e4807f
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/vi_estimator/nfr_mapper.cpp
View File

@ -626,8 +626,8 @@ void NfrMapper::setup_opt() {
Sophus::SE3d T_w_h = frame_poses.at(tcid_h.frame_id).getPose() *
calib.T_i_c[tcid_h.cam_id];
Sophus::SE3d T_w_o =
frame_poses.at(tcid_o.cam_id).getPose() * calib.T_i_c[tcid_o.cam_id];
Sophus::SE3d T_w_o = frame_poses.at(tcid_o.frame_id).getPose() *
calib.T_i_c[tcid_o.cam_id];
Eigen::Vector4d pos_3d = triangulate(
pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_w_h.inverse() * T_w_o);