From b3656e05921bdcb69267ec62b6a40c1b24761b22 Mon Sep 17 00:00:00 2001 From: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Date: Sun, 14 Apr 2024 11:53:36 +0900 Subject: [PATCH] Parameterize margins for projection matcher (#583) --- example/euroc/EuRoC_stereo.yaml | 1 + example/kitti/KITTI_stereo_00-02.yaml | 3 +++ example/kitti/KITTI_stereo_03.yaml | 3 +++ example/kitti/KITTI_stereo_04-12.yaml | 3 +++ example/tum_rgbd/TUM_RGBD_rgbd_1.yaml | 3 +++ example/tum_rgbd/TUM_RGBD_rgbd_2.yaml | 3 +++ example/tum_rgbd/TUM_RGBD_rgbd_3.yaml | 3 +++ example/tum_vi/TUM_VI_stereo.yaml | 3 +++ src/stella_vslam/module/frame_tracker.cc | 9 ++++---- src/stella_vslam/module/frame_tracker.h | 8 ++++++- src/stella_vslam/tracking_module.cc | 27 ++++++++++++------------ src/stella_vslam/tracking_module.h | 7 ++++++ 12 files changed, 54 insertions(+), 19 deletions(-) diff --git a/example/euroc/EuRoC_stereo.yaml b/example/euroc/EuRoC_stereo.yaml index 8cc150ed1..f795c807b 100644 --- a/example/euroc/EuRoC_stereo.yaml +++ b/example/euroc/EuRoC_stereo.yaml @@ -86,6 +86,7 @@ Mapping: Tracking: backend: "g2o" enable_temporal_keyframe_only_tracking: false + margin_last_frame_projection: 10.0 KeyframeInserter: wait_for_local_bundle_adjustment: false diff --git a/example/kitti/KITTI_stereo_00-02.yaml b/example/kitti/KITTI_stereo_00-02.yaml index f1104f2f0..4ff59c955 100644 --- a/example/kitti/KITTI_stereo_00-02.yaml +++ b/example/kitti/KITTI_stereo_00-02.yaml @@ -32,6 +32,9 @@ Camera: # Tracking Parameters # #=====================# +Tracking: + margin_last_frame_projection: 10.0 + Preprocessing: min_size: 800 diff --git a/example/kitti/KITTI_stereo_03.yaml b/example/kitti/KITTI_stereo_03.yaml index 542f80e8e..dab8c7899 100644 --- a/example/kitti/KITTI_stereo_03.yaml +++ b/example/kitti/KITTI_stereo_03.yaml @@ -32,6 +32,9 @@ Camera: # Tracking Parameters # #=====================# +Tracking: + margin_last_frame_projection: 10.0 + Preprocessing: min_size: 800 diff --git a/example/kitti/KITTI_stereo_04-12.yaml b/example/kitti/KITTI_stereo_04-12.yaml index 413e86da7..4d8d7ff7b 100644 --- a/example/kitti/KITTI_stereo_04-12.yaml +++ b/example/kitti/KITTI_stereo_04-12.yaml @@ -32,6 +32,9 @@ Camera: # Tracking Parameters # #=====================# +Tracking: + margin_last_frame_projection: 10.0 + Preprocessing: min_size: 800 diff --git a/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml b/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml index f04ead968..a680e365c 100644 --- a/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml +++ b/example/tum_rgbd/TUM_RGBD_rgbd_1.yaml @@ -32,6 +32,9 @@ Camera: # Tracking Parameters # #=====================# +Tracking: + margin_local_map_projection: 10.0 + Preprocessing: min_size: 800 depthmap_factor: 5000.0 # Note: Set it to 1.0 for the rosbag format data set. diff --git a/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml b/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml index 12d3fea83..47ffce0dc 100644 --- a/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml +++ b/example/tum_rgbd/TUM_RGBD_rgbd_2.yaml @@ -32,6 +32,9 @@ Camera: # Tracking Parameters # #=====================# +Tracking: + margin_local_map_projection: 10.0 + Preprocessing: min_size: 800 depthmap_factor: 5000.0 # Note: Set it to 1.0 for the rosbag format data set. diff --git a/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml b/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml index dd3489b60..c5d070c81 100644 --- a/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml +++ b/example/tum_rgbd/TUM_RGBD_rgbd_3.yaml @@ -32,6 +32,9 @@ Camera: # Tracking Parameters # #=====================# +Tracking: + margin_local_map_projection: 10.0 + Preprocessing: min_size: 800 depthmap_factor: 5000.0 # Note: Set it to 1.0 for the rosbag format data set. diff --git a/example/tum_vi/TUM_VI_stereo.yaml b/example/tum_vi/TUM_VI_stereo.yaml index 1e4650a06..ff83a8186 100755 --- a/example/tum_vi/TUM_VI_stereo.yaml +++ b/example/tum_vi/TUM_VI_stereo.yaml @@ -114,6 +114,9 @@ StereoRectifier: # Tracking Parameters # #=====================# +Tracking: + margin_last_frame_projection: 10.0 + Preprocessing: min_size: 800 diff --git a/src/stella_vslam/module/frame_tracker.cc b/src/stella_vslam/module/frame_tracker.cc index 61b215f5d..ec03126a9 100644 --- a/src/stella_vslam/module/frame_tracker.cc +++ b/src/stella_vslam/module/frame_tracker.cc @@ -14,8 +14,8 @@ namespace stella_vslam { namespace module { frame_tracker::frame_tracker(camera::base* camera, const std::shared_ptr& pose_optimizer, - const unsigned int num_matches_thr, bool use_fixed_seed) - : camera_(camera), num_matches_thr_(num_matches_thr), use_fixed_seed_(use_fixed_seed), pose_optimizer_(pose_optimizer) {} + const unsigned int num_matches_thr, bool use_fixed_seed, float margin) + : camera_(camera), num_matches_thr_(num_matches_thr), use_fixed_seed_(use_fixed_seed), margin_(margin), pose_optimizer_(pose_optimizer) {} bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity) const { match::projection projection_matcher(0.9, true); @@ -27,13 +27,12 @@ bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& curr_frm.erase_landmarks(); // Reproject the 3D points observed in the last frame and find 2D-3D matches - const float margin = (camera_->setup_type_ != camera::setup_type_t::Stereo) ? 20 : 10; - auto num_matches = projection_matcher.match_current_and_last_frames(curr_frm, last_frm, margin); + auto num_matches = projection_matcher.match_current_and_last_frames(curr_frm, last_frm, margin_); if (num_matches < num_matches_thr_) { // Increment the margin, and search again curr_frm.erase_landmarks(); - num_matches = projection_matcher.match_current_and_last_frames(curr_frm, last_frm, 2 * margin); + num_matches = projection_matcher.match_current_and_last_frames(curr_frm, last_frm, 2 * margin_); } if (num_matches < num_matches_thr_) { diff --git a/src/stella_vslam/module/frame_tracker.h b/src/stella_vslam/module/frame_tracker.h index c1ac6d048..d255d1df7 100644 --- a/src/stella_vslam/module/frame_tracker.h +++ b/src/stella_vslam/module/frame_tracker.h @@ -22,7 +22,11 @@ namespace module { class frame_tracker { public: - explicit frame_tracker(camera::base* camera, const std::shared_ptr& pose_optimizer, const unsigned int num_matches_thr = 20, bool use_fixed_seed = false); + explicit frame_tracker(camera::base* camera, + const std::shared_ptr& pose_optimizer, + const unsigned int num_matches_thr = 20, + bool use_fixed_seed = false, + float margin = 20.0); bool motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity) const; @@ -37,6 +41,8 @@ class frame_tracker { const unsigned int num_matches_thr_; //! Use fixed random seed for RANSAC if true const bool use_fixed_seed_; + //! margin for projection matcher + const float margin_; std::shared_ptr pose_optimizer_ = nullptr; }; diff --git a/src/stella_vslam/tracking_module.cc b/src/stella_vslam/tracking_module.cc index 81309c998..298033e69 100644 --- a/src/stella_vslam/tracking_module.cc +++ b/src/stella_vslam/tracking_module.cc @@ -22,17 +22,20 @@ namespace stella_vslam { tracking_module::tracking_module(const std::shared_ptr& cfg, camera::base* camera, data::map_database* map_db, data::bow_vocabulary* bow_vocab, data::bow_database* bow_db) : camera_(camera), - reloc_distance_threshold_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["reloc_distance_threshold"].as(0.2)), - reloc_angle_threshold_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["reloc_angle_threshold"].as(0.45)), - init_retry_threshold_time_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["init_retry_threshold_time"].as(5.0)), - enable_auto_relocalization_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["enable_auto_relocalization"].as(true)), - enable_temporal_keyframe_only_tracking_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["enable_temporal_keyframe_only_tracking"].as(false)), - use_robust_matcher_for_relocalization_request_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["use_robust_matcher_for_relocalization_request"].as(false)), - max_num_local_keyfrms_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["max_num_local_keyfrms"].as(60)), + tracking_yaml_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")), + reloc_distance_threshold_(tracking_yaml_["reloc_distance_threshold"].as(0.2)), + reloc_angle_threshold_(tracking_yaml_["reloc_angle_threshold"].as(0.45)), + init_retry_threshold_time_(tracking_yaml_["init_retry_threshold_time"].as(5.0)), + enable_auto_relocalization_(tracking_yaml_["enable_auto_relocalization"].as(true)), + enable_temporal_keyframe_only_tracking_(tracking_yaml_["enable_temporal_keyframe_only_tracking"].as(false)), + use_robust_matcher_for_relocalization_request_(tracking_yaml_["use_robust_matcher_for_relocalization_request"].as(false)), + max_num_local_keyfrms_(tracking_yaml_["max_num_local_keyfrms"].as(60)), + margin_local_map_projection_(tracking_yaml_["margin_local_map_projection"].as(5.0)), + margin_local_map_projection_unstable_(tracking_yaml_["margin_local_map_projection_unstable"].as(20.0)), map_db_(map_db), bow_vocab_(bow_vocab), bow_db_(bow_db), initializer_(map_db, bow_db, util::yaml_optional_ref(cfg->yaml_node_, "Initializer")), - pose_optimizer_(optimize::pose_optimizer_factory::create(util::yaml_optional_ref(cfg->yaml_node_, "Tracking"))), - frame_tracker_(camera_, pose_optimizer_, 10, initializer_.get_use_fixed_seed()), + pose_optimizer_(optimize::pose_optimizer_factory::create(tracking_yaml_)), + frame_tracker_(camera_, pose_optimizer_, 10, initializer_.get_use_fixed_seed(), tracking_yaml_["margin_last_frame_projection"].as(20.0)), relocalizer_(pose_optimizer_, util::yaml_optional_ref(cfg->yaml_node_, "Relocalizer")), keyfrm_inserter_(util::yaml_optional_ref(cfg->yaml_node_, "KeyframeInserter")) { spdlog::debug("CONSTRUCT: tracking_module"); @@ -571,10 +574,8 @@ bool tracking_module::search_local_landmarks() { // acquire more 2D-3D matches by projecting the local landmarks to the current frame match::projection projection_matcher(0.8); const float margin = (curr_frm_.id_ < last_reloc_frm_id_ + 2) - ? 20.0 - : ((camera_->setup_type_ == camera::setup_type_t::RGBD) - ? 10.0 - : 5.0); + ? margin_local_map_projection_unstable_ + : margin_local_map_projection_; projection_matcher.match_frame_and_landmarks(curr_frm_, local_landmarks_, lm_to_reproj, lm_to_x_right, lm_to_scale, margin); return true; } diff --git a/src/stella_vslam/tracking_module.h b/src/stella_vslam/tracking_module.h index a9084d749..9bbcdb7a4 100644 --- a/src/stella_vslam/tracking_module.h +++ b/src/stella_vslam/tracking_module.h @@ -111,6 +111,9 @@ class tracking_module { //! camera model camera::base* camera_; + //! yaml node + YAML::Node tracking_yaml_; + //! closest keyframes thresholds (by distance and angle) to relocalize with when updating by pose double reloc_distance_threshold_ = 0.2; double reloc_angle_threshold_ = 0.45; @@ -130,6 +133,10 @@ class tracking_module { //! Max number of local keyframes for tracking unsigned int max_num_local_keyfrms_ = 60; + //! margin for projection matcher + float margin_local_map_projection_ = 5.0; + float margin_local_map_projection_unstable_ = 20.0; + //----------------------------------------- // variables