From dc24df1819765162befba163ccb03ea307913aa2 Mon Sep 17 00:00:00 2001 From: ymd-stella Date: Sat, 24 Feb 2024 19:33:12 +0900 Subject: [PATCH] Fix check_epipolar_constraint --- src/stella_vslam/mapping_module.cc | 5 +++-- src/stella_vslam/mapping_module.h | 4 ++++ src/stella_vslam/match/robust.cc | 24 +++++++++++------------- src/stella_vslam/match/robust.h | 10 +++++++--- 4 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/stella_vslam/mapping_module.cc b/src/stella_vslam/mapping_module.cc index 226bc3245..03b242df7 100644 --- a/src/stella_vslam/mapping_module.cc +++ b/src/stella_vslam/mapping_module.cc @@ -26,7 +26,8 @@ mapping_module::mapping_module(const YAML::Node& yaml_node, data::map_database* num_covisibilities_for_landmark_generation_(yaml_node["num_covisibilities_for_landmark_generation"].as(10)), num_covisibilities_for_landmark_fusion_(yaml_node["num_covisibilities_for_landmark_fusion"].as(10)), erase_temporal_keyframes_(yaml_node["erase_temporal_keyframes"].as(false)), - num_temporal_keyframes_(yaml_node["num_temporal_keyframes"].as(15)) { + num_temporal_keyframes_(yaml_node["num_temporal_keyframes"].as(15)), + residual_rad_thr_(yaml_node["residual_deg_thr"].as(0.2) * M_PI / 180.0) { spdlog::debug("CONSTRUCT: mapping_module"); spdlog::debug("load mapping parameters"); @@ -326,7 +327,7 @@ void mapping_module::create_new_landmarks(std::atomic& abort_create_new_la // vector of matches (idx in the current, idx in the neighbor) std::vector> matches; - robust_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches); + robust_matcher.match_for_triangulation(cur_keyfrm_, ngh_keyfrm, E_ngh_to_cur, matches, residual_rad_thr_); // triangulation triangulate_with_two_keyframes(cur_keyfrm_, ngh_keyfrm, matches); diff --git a/src/stella_vslam/mapping_module.h b/src/stella_vslam/mapping_module.h index b266a9b4d..2df2c3776 100644 --- a/src/stella_vslam/mapping_module.h +++ b/src/stella_vslam/mapping_module.h @@ -269,6 +269,10 @@ class mapping_module { //! Number of temporal keyframes const unsigned int num_temporal_keyframes_ = 15; + + // The default inlier threshold value is 0.2 degree + // (e.g. for the camera with width of 900-pixel and 90-degree FOV, 0.2 degree is equivalent to 2 pixel in the horizontal direction) + float residual_rad_thr_ = 0.2 * M_PI / 180.0; }; } // namespace stella_vslam diff --git a/src/stella_vslam/match/robust.cc b/src/stella_vslam/match/robust.cc index a1830b343..bd6a90c29 100644 --- a/src/stella_vslam/match/robust.cc +++ b/src/stella_vslam/match/robust.cc @@ -11,8 +11,11 @@ namespace stella_vslam { namespace match { -unsigned int robust::match_for_triangulation(const std::shared_ptr& keyfrm_1, const std::shared_ptr& keyfrm_2, const Mat33_t& E_12, - std::vector>& matched_idx_pairs) const { +unsigned int robust::match_for_triangulation(const std::shared_ptr& keyfrm_1, + const std::shared_ptr& keyfrm_2, + const Mat33_t& E_12, + std::vector>& matched_idx_pairs, + const float residual_rad_thr) const { unsigned int num_matches = 0; // Project the center of keyframe 1 to keyframe 2 @@ -112,7 +115,8 @@ unsigned int robust::match_for_triangulation(const std::shared_ptrorb_params_->scale_factors_.at(keypt_1.octave)); + keyfrm_1->orb_params_->scale_factors_.at(keypt_1.octave), + residual_rad_thr); if (is_inlier) { if (hamm_dist < best_hamm_dist) { second_best_hamm_dist = best_hamm_dist; @@ -348,22 +352,16 @@ unsigned int robust::brute_force_match(const data::frame_observation& frm_obs, } bool robust::check_epipolar_constraint(const Vec3_t& bearing_1, const Vec3_t& bearing_2, - const Mat33_t& E_12, const float bearing_1_scale_factor) const { + const Mat33_t& E_12, float residual_rad_thr, + const float bearing_1_scale_factor) const { // Normal vector of the epipolar plane on keyframe 1 const Vec3_t epiplane_in_1 = E_12 * bearing_2; // Acquire the angle formed by the normal vector and the bearing - const auto cos_residual = epiplane_in_1.dot(bearing_1) / epiplane_in_1.norm(); - const auto residual_rad = M_PI / 2.0 - std::abs(std::acos(cos_residual)); - - // The Inlier threshold value is 0.2 degree - // (e.g. for the camera with width of 900-pixel and 90-degree FOV, 0.2 degree is equivalent to 2 pixel in the horizontal direction) - // TODO: should prameterize the threshold - constexpr double residual_deg_thr = 0.2; - constexpr double residual_rad_thr = residual_deg_thr * M_PI / 180.0; + const auto cos_residual = std::min(1.0, std::max(-1.0, epiplane_in_1.dot(bearing_1) / epiplane_in_1.norm())); + const auto residual_rad = std::abs(M_PI / 2.0 - std::acos(cos_residual)); // The larger keypoint scale permits less constraints - // TODO: should consider the threshold weighting return residual_rad < residual_rad_thr * bearing_1_scale_factor; } diff --git a/src/stella_vslam/match/robust.h b/src/stella_vslam/match/robust.h index f069e5ec6..3555e4fd5 100644 --- a/src/stella_vslam/match/robust.h +++ b/src/stella_vslam/match/robust.h @@ -24,8 +24,11 @@ class robust final : public base { ~robust() final = default; - unsigned int match_for_triangulation(const std::shared_ptr& keyfrm_1, const std::shared_ptr& keyfrm_2, const Mat33_t& E_12, - std::vector>& matched_idx_pairs) const; + unsigned int match_for_triangulation(const std::shared_ptr& keyfrm_1, + const std::shared_ptr& keyfrm_2, + const Mat33_t& E_12, + std::vector>& matched_idx_pairs, + const float residual_rad_thr) const; unsigned int match_keyframes(const std::shared_ptr& keyfrm1, const std::shared_ptr& keyfrm2, std::vector>& matched_lms_in_frm, @@ -39,7 +42,8 @@ class robust final : public base { private: bool check_epipolar_constraint(const Vec3_t& bearing_1, const Vec3_t& bearing_2, - const Mat33_t& E_12, const float bearing_1_scale_factor = 1.0) const; + const Mat33_t& E_12, float residual_rad_thr, + const float bearing_1_scale_factor = 1.0) const; }; } // namespace match