Parameterize margins for projection matcher (#583)

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

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
 

example/kitti/KITTI_stereo_03.yaml

@@ -32,6 +32,9 @@ Camera:
 # Tracking Parameters #
 #=====================#
 
+Tracking:
+  margin_last_frame_projection: 10.0
+
 Preprocessing:
   min_size: 800
 

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
 

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.

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.

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.

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
 

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<optimize::pose_optimizer>& 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_) {

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<optimize::pose_optimizer>& pose_optimizer, const unsigned int num_matches_thr = 20, bool use_fixed_seed = false);
+    explicit frame_tracker(camera::base* camera,
+                           const std::shared_ptr<optimize::pose_optimizer>& 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<optimize::pose_optimizer> pose_optimizer_ = nullptr;
 };

src/stella_vslam/tracking_module.cc

@@ -22,17 +22,20 @@ namespace stella_vslam {
 tracking_module::tracking_module(const std::shared_ptr<config>& 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<double>(0.2)),
-      reloc_angle_threshold_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["reloc_angle_threshold"].as<double>(0.45)),
-      init_retry_threshold_time_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["init_retry_threshold_time"].as<double>(5.0)),
-      enable_auto_relocalization_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["enable_auto_relocalization"].as<bool>(true)),
-      enable_temporal_keyframe_only_tracking_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["enable_temporal_keyframe_only_tracking"].as<bool>(false)),
-      use_robust_matcher_for_relocalization_request_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["use_robust_matcher_for_relocalization_request"].as<bool>(false)),
-      max_num_local_keyfrms_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")["max_num_local_keyfrms"].as<unsigned int>(60)),
+      tracking_yaml_(util::yaml_optional_ref(cfg->yaml_node_, "Tracking")),
+      reloc_distance_threshold_(tracking_yaml_["reloc_distance_threshold"].as<double>(0.2)),
+      reloc_angle_threshold_(tracking_yaml_["reloc_angle_threshold"].as<double>(0.45)),
+      init_retry_threshold_time_(tracking_yaml_["init_retry_threshold_time"].as<double>(5.0)),
+      enable_auto_relocalization_(tracking_yaml_["enable_auto_relocalization"].as<bool>(true)),
+      enable_temporal_keyframe_only_tracking_(tracking_yaml_["enable_temporal_keyframe_only_tracking"].as<bool>(false)),
+      use_robust_matcher_for_relocalization_request_(tracking_yaml_["use_robust_matcher_for_relocalization_request"].as<bool>(false)),
+      max_num_local_keyfrms_(tracking_yaml_["max_num_local_keyfrms"].as<unsigned int>(60)),
+      margin_local_map_projection_(tracking_yaml_["margin_local_map_projection"].as<float>(5.0)),
+      margin_local_map_projection_unstable_(tracking_yaml_["margin_local_map_projection_unstable"].as<float>(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<float>(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;
 }

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