From 5b01a126b70a3a1f7d81581c91a8a367e104c334 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 23 Jul 2019 17:25:45 +0200 Subject: [PATCH] small changes --- src/vi_estimator/keypoint_vio.cpp | 2 +- src/vi_estimator/nfr_mapper.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 532f233..8dcd369 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -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); diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index fe81b41..8469df2 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -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;