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

small changes

Browse Source
This commit is contained in:
Vladyslav Usenko 2019-07-23 17:25:45 +02:00
parent 7a851029b1
commit 5b01a126b7
2 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/vi_estimator/keypoint_vio.cpp
View File

@ -330,7 +330,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
Sophus::SE3d T_0_1 =
calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.cam_id];
if (T_0_1.translation().squaredNorm() < 0.03 * 0.03) continue;
if (T_0_1.translation().squaredNorm() < 0.05 * 0.05) continue;
Eigen::Vector4d p0_triangulated =
triangulate(p0_3d.head<3>(), p1_3d.head<3>(), T_0_1);

8
src/vi_estimator/nfr_mapper.cpp
View File

@ -636,8 +636,12 @@ void NfrMapper::setup_opt() {
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);
Sophus::SE3d T_h_o = T_w_h.inverse() * T_w_o;
if (T_h_o.translation().squaredNorm() < 0.07 * 0.07) continue;
Eigen::Vector4d pos_3d =
triangulate(pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_h_o);
if (!pos_3d.array().isFinite().all() || pos_3d[3] <= 0 || pos_3d[3] > 2.0)
continue;