Do not update map during keyframe initialization (#516)

stella-cv · Jul 2, 2023 · e51a4b3 · e51a4b3
1 parent 655fbae
commit e51a4b3
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 27 deletions.
diff --git a/src/stella_vslam/module/keyframe_inserter.cc b/src/stella_vslam/module/keyframe_inserter.cc
@@ -106,8 +106,11 @@ bool keyframe_inserter::new_keyframe_is_needed(data::map_database* map_db,
            && !mapper_is_skipping_localBA;
 }
 
-std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map_database* map_db,
-                                                                       data::frame& curr_frm) {
+std::shared_ptr<data::keyframe> keyframe_inserter::create_new_keyframe(
+    data::map_database* map_db,
+    data::frame& curr_frm) {
+    std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);
+
     auto keyfrm = data::keyframe::make_keyframe(map_db->next_keyframe_id_++, curr_frm);
     keyfrm->update_landmarks();
 
@@ -128,7 +131,6 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map
 
     // Queue up the keyframe to the mapping module
     if (!keyfrm->depth_is_available()) {
-        add_keyframe(keyfrm);
         return keyfrm;
     }
 
@@ -146,7 +148,6 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map
 
     // Queue up the keyframe to the mapping module if any valid depth values don't exist
     if (depth_idx_pairs.empty()) {
-        add_keyframe(keyfrm);
         return keyfrm;
     }
 
@@ -188,14 +189,21 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map
     }
 
     // Queue up the keyframe to the mapping module
-    add_keyframe(keyfrm);
     return keyfrm;
 }
 
-void keyframe_inserter::add_keyframe(const std::shared_ptr<data::keyframe>& keyfrm) {
-    auto future = mapper_->async_add_keyframe(keyfrm);
+void keyframe_inserter::insert_new_keyframe(data::map_database* map_db,
+                                            data::frame& curr_frm) {
+    SPDLOG_TRACE("keyframe_inserter: insert_new_keyframe (curr_frm={})", curr_frm.id_);
+    // insert the new keyframe
+    const auto ref_keyfrm = create_new_keyframe(map_db, curr_frm);
+    auto future_add_keyframe = mapper_->async_add_keyframe(ref_keyfrm);
     if (wait_for_local_bundle_adjustment_) {
-        future.get();
+        future_add_keyframe.get();
+    }
+    // set the reference keyframe with the new keyframe
+    if (ref_keyfrm) {
+        curr_frm.ref_keyfrm_ = ref_keyfrm;
     }
 }
 

diff --git a/src/stella_vslam/module/keyframe_inserter.h b/src/stella_vslam/module/keyframe_inserter.h
@@ -48,13 +48,10 @@ class keyframe_inserter {
     /**
      * Insert the new keyframe derived from the current frame
      */
-    std::shared_ptr<data::keyframe> insert_new_keyframe(data::map_database* map_db, data::frame& curr_frm);
+    void insert_new_keyframe(data::map_database* map_db, data::frame& curr_frm);
 
 private:
-    /**
-     * Queue the new keyframe to the mapping module
-     */
-    void add_keyframe(const std::shared_ptr<data::keyframe>& keyfrm);
+    std::shared_ptr<data::keyframe> create_new_keyframe(data::map_database* map_db, data::frame& curr_frm);
 
     //! mapping module
     mapping_module* mapper_ = nullptr;

diff --git a/src/stella_vslam/tracking_module.cc b/src/stella_vslam/tracking_module.cc
@@ -137,7 +137,7 @@ std::shared_ptr<Mat44_t> tracking_module::feed_frame(data::frame curr_frm) {
 
         // check to insert the new keyframe derived from the current frame
         if (succeeded && !is_stopped_keyframe_insertion_ && new_keyframe_is_needed(num_tracked_lms, num_reliable_lms, min_num_obs_thr)) {
-            insert_new_keyframe();
+            keyfrm_inserter_.insert_new_keyframe(map_db_, curr_frm_);
         }
     }
 
@@ -593,16 +593,6 @@ bool tracking_module::new_keyframe_is_needed(unsigned int num_tracked_lms,
     return keyfrm_inserter_.new_keyframe_is_needed(map_db_, curr_frm_, num_tracked_lms, num_reliable_lms, *curr_frm_.ref_keyfrm_, min_num_obs_thr);
 }
 
-void tracking_module::insert_new_keyframe() {
-    SPDLOG_TRACE("tracking_module: insert_new_keyframe (curr_frm_={})", curr_frm_.id_);
-    // insert the new keyframe
-    const auto ref_keyfrm = keyfrm_inserter_.insert_new_keyframe(map_db_, curr_frm_);
-    // set the reference keyframe with the new keyframe
-    if (ref_keyfrm) {
-        curr_frm_.ref_keyfrm_ = ref_keyfrm;
-    }
-}
-
 std::future<void> tracking_module::async_stop_keyframe_insertion() {
     auto future_stop_keyframe_insertion = std::async(
         std::launch::async,

diff --git a/src/stella_vslam/tracking_module.h b/src/stella_vslam/tracking_module.h
@@ -190,9 +190,6 @@ class tracking_module {
                                 unsigned int num_reliable_lms,
                                 const unsigned int min_num_obs_thr) const;
 
-    //! Insert the new keyframe derived from the current frame
-    void insert_new_keyframe();
-
     //! mapping module
     mapping_module* mapper_ = nullptr;
     //! global optimization module