Fix tracking (Remove GPL infringing code)

docs/parameters.rst

@@ -178,6 +178,24 @@ Relocalizer
     * - min_num_valid_obs
       - 
 
+.. _section-parameters-keyframe-inserter:
+
+KeyframeInserter
+================
+
+.. list-table::
+    :header-rows: 1
+    :widths: 1, 3
+
+    * - Name
+      - Description
+    * - max_interval
+      - max interval to insert keyframe
+    * - lms_ratio_thr_almost_all_lms_are_tracked
+      - Threshold at which we consider that we are tracking almost all landmarks. Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe"
+    * - lms_ratio_thr_view_changed
+      - Threshold at which we consider the view to have changed. Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe"
+
 .. _section-parameters-pangolin:
 
 PangolinViewer

src/openvslam/data/map_database.cc

@@ -29,6 +29,7 @@ map_database::~map_database() {
 void map_database::add_keyframe(const std::shared_ptr<keyframe>& keyfrm) {
     std::lock_guard<std::mutex> lock(mtx_map_access_);
     keyframes_[keyfrm->id_] = keyfrm;
+    last_inserted_keyfrm_ = keyfrm;
 }
 
 void map_database::erase_keyframe(const std::shared_ptr<keyframe>& keyfrm) {
@@ -138,6 +139,11 @@ std::vector<std::shared_ptr<landmark>> map_database::get_all_landmarks() const {
     return landmarks;
 }
 
+std::shared_ptr<keyframe> map_database::get_last_inserted_keyframe() const {
+    std::lock_guard<std::mutex> lock(mtx_map_access_);
+    return last_inserted_keyfrm_;
+}
+
 unsigned int map_database::get_num_landmarks() const {
     std::lock_guard<std::mutex> lock(mtx_map_access_);
     return landmarks_.size();
@@ -148,6 +154,7 @@ void map_database::clear() {
 
     landmarks_.clear();
     keyframes_.clear();
+    last_inserted_keyfrm_ = nullptr;
     local_landmarks_.clear();
     origin_keyfrm_ = nullptr;
 
@@ -171,6 +178,8 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p
 
     landmarks_.clear();
     keyframes_.clear();
+    // When loading the map, leave last_inserted_keyfrm_ as nullptr.
+    last_inserted_keyfrm_ = nullptr;
     local_landmarks_.clear();
     origin_keyfrm_ = nullptr;
 

src/openvslam/data/map_database.h

@@ -116,6 +116,12 @@ class map_database {
      */
     std::vector<std::shared_ptr<landmark>> get_all_landmarks() const;
 
+    /**
+     * Get the last keyframe added to the database
+     * @return shared pointer to the last keyframe added to the database
+     */
+    std::shared_ptr<keyframe> get_last_inserted_keyframe() const;
+
     /**
      * Get the number of landmarks
      * @return
@@ -229,6 +235,9 @@ class map_database {
     //! IDs and landmarks
     std::unordered_map<unsigned int, std::shared_ptr<landmark>> landmarks_;
 
+    //! The last keyframe added to the database
+    std::shared_ptr<keyframe> last_inserted_keyfrm_ = nullptr;
+
     //! local landmarks
     std::vector<std::shared_ptr<landmark>> local_landmarks_;
 

src/openvslam/mapping_module.cc

@@ -135,6 +135,11 @@ void mapping_module::set_is_idle(const bool is_idle) {
     is_idle_ = is_idle;
 }
 
+bool mapping_module::is_skipping_localBA() const {
+    auto queued_keyframes = get_num_queued_keyframes();
+    return queued_keyframes >= queue_threshold_;
+}
+
 void mapping_module::abort_local_BA() {
     abort_local_BA_ = true;
 }
@@ -173,8 +178,14 @@ void mapping_module::mapping_with_new_keyframe() {
 
     // local bundle adjustment
     abort_local_BA_ = false;
+    // If the processing speed is insufficient, skip localBA.
     if (2 < map_db_->get_num_keyframes()) {
-        local_bundle_adjuster_->optimize(map_db_, cur_keyfrm_, &abort_local_BA_);
+        if (is_skipping_localBA()) {
+            spdlog::debug("Skipped localBA due to insufficient performance");
+        }
+        else {
+            local_bundle_adjuster_->optimize(map_db_, cur_keyfrm_, &abort_local_BA_);
+        }
     }
     local_map_cleaner_->remove_redundant_keyframes(cur_keyfrm_);
 }

src/openvslam/mapping_module.h

@@ -59,6 +59,9 @@ class mapping_module {
     //! True when no keyframes are being processed
     bool is_idle() const;
 
+    //! If the size of the queue exceeds this threshold, skip the localBA
+    bool is_skipping_localBA() const;
+
     //-----------------------------------------
     // management for reset process
 
@@ -256,6 +259,9 @@ class mapping_module {
 
     //! Create new landmarks if the baseline distance is greater than baseline_dist_thr_ of the reference keyframe.
     double baseline_dist_thr_ = 1.0;
+
+    //! If the size of the queue exceeds this threshold, skip the localBA
+    const unsigned int queue_threshold_ = 2;
 };
 
 } // namespace openvslam

src/openvslam/module/initializer.cc

@@ -13,10 +13,9 @@
 namespace openvslam {
 namespace module {
 
-initializer::initializer(const camera::setup_type_t setup_type,
-                         data::map_database* map_db, data::bow_database* bow_db,
+initializer::initializer(data::map_database* map_db, data::bow_database* bow_db,
                          const YAML::Node& yaml_node)
-    : setup_type_(setup_type), map_db_(map_db), bow_db_(bow_db),
+    : map_db_(map_db), bow_db_(bow_db),
       num_ransac_iters_(yaml_node["num_ransac_iterations"].as<unsigned int>(100)),
       min_num_triangulated_(yaml_node["num_min_triangulated_pts"].as<unsigned int>(50)),
       parallax_deg_thr_(yaml_node["parallax_deg_threshold"].as<float>(1.0)),
@@ -35,6 +34,7 @@ void initializer::reset() {
     initializer_.reset(nullptr);
     state_ = initializer_state_t::NotReady;
     init_frm_id_ = 0;
+    init_frm_stamp_ = 0.0;
 }
 
 initializer_state_t initializer::get_state() const {
@@ -53,8 +53,13 @@ unsigned int initializer::get_initial_frame_id() const {
     return init_frm_id_;
 }
 
-bool initializer::initialize(data::bow_vocabulary* bow_vocab, data::frame& curr_frm) {
-    switch (setup_type_) {
+double initializer::get_initial_frame_timestamp() const {
+    return init_frm_stamp_;
+}
+
+bool initializer::initialize(const camera::setup_type_t setup_type,
+                             data::bow_vocabulary* bow_vocab, data::frame& curr_frm) {
+    switch (setup_type) {
         case camera::setup_type_t::Monocular: {
             // construct an initializer if not constructed
             if (state_ == initializer_state_t::NotReady) {
@@ -94,6 +99,7 @@ bool initializer::initialize(data::bow_vocabulary* bow_vocab, data::frame& curr_
     // check the state is succeeded or not
     if (state_ == initializer_state_t::Succeeded) {
         init_frm_id_ = curr_frm.id_;
+        init_frm_stamp_ = curr_frm.timestamp_;
         return true;
     }
     else {

src/openvslam/module/initializer.h

@@ -32,8 +32,7 @@ class initializer {
     initializer() = delete;
 
     //! Constructor
-    initializer(const camera::setup_type_t setup_type,
-                data::map_database* map_db, data::bow_database* bow_db,
+    initializer(data::map_database* map_db, data::bow_database* bow_db,
                 const YAML::Node& yaml_node);
 
     //! Destructor
@@ -54,21 +53,25 @@ class initializer {
     //! Get the initial frame ID which succeeded in initialization
     unsigned int get_initial_frame_id() const;
 
+    //! Get the initial frame stamp which succeeded in initialization
+    double get_initial_frame_timestamp() const;
+
     //! Initialize with the current frame
-    bool initialize(data::bow_vocabulary* bow_vocab, data::frame& curr_frm);
+    bool initialize(const camera::setup_type_t setup_type,
+                    data::bow_vocabulary* bow_vocab, data::frame& curr_frm);
 
 private:
-    //! camera setup type
-    const camera::setup_type_t setup_type_;
     //! map database
     data::map_database* map_db_ = nullptr;
     //! BoW database
     data::bow_database* bow_db_ = nullptr;
     //! initializer status
     initializer_state_t state_ = initializer_state_t::NotReady;
 
-    //! frame ID used for initialization (will be set after succeeded)
+    //! ID of frame used for initialization (will be set after succeeded)
     unsigned int init_frm_id_ = 0;
+    //! timestamp of frame used for initialization (will be set after succeeded)
+    double init_frm_stamp_ = 0.0;
 
     //-----------------------------------------
     // parameters

src/openvslam/module/keyframe_inserter.cc

@@ -3,83 +3,67 @@
 #include "openvslam/data/map_database.h"
 #include "openvslam/module/keyframe_inserter.h"
 
+#include <spdlog/spdlog.h>
+
 namespace openvslam {
 namespace module {
 
-keyframe_inserter::keyframe_inserter(const camera::setup_type_t setup_type,
-                                     data::map_database* map_db,
-                                     const unsigned int min_num_frms, const unsigned int max_num_frms)
-    : setup_type_(setup_type), map_db_(map_db),
-      min_num_frms_(min_num_frms), max_num_frms_(max_num_frms) {}
+keyframe_inserter::keyframe_inserter(const double max_interval,
+                                     const double lms_ratio_thr_almost_all_lms_are_tracked,
+                                     const double lms_ratio_thr_view_changed)
+    : max_interval_(max_interval),
+      lms_ratio_thr_almost_all_lms_are_tracked_(lms_ratio_thr_almost_all_lms_are_tracked),
+      lms_ratio_thr_view_changed_(lms_ratio_thr_view_changed) {}
+
+keyframe_inserter::keyframe_inserter(const YAML::Node& yaml_node)
+    : keyframe_inserter(yaml_node["max_interval"].as<double>(1.0),
+                        yaml_node["lms_ratio_thr_almost_all_lms_are_tracked"].as<double>(0.95),
+                        yaml_node["lms_ratio_thr_view_changed"].as<double>(0.9)) {}
 
 void keyframe_inserter::set_mapping_module(mapping_module* mapper) {
     mapper_ = mapper;
 }
 
 void keyframe_inserter::reset() {
-    frm_id_of_last_keyfrm_ = 0;
 }
 
-bool keyframe_inserter::new_keyframe_is_needed(const data::frame& curr_frm, const unsigned int num_tracked_lms,
+bool keyframe_inserter::new_keyframe_is_needed(data::map_database* map_db, const data::frame& curr_frm,
+                                               const unsigned int num_tracked_lms,
                                                const data::keyframe& ref_keyfrm) const {
     assert(mapper_);
     // Any keyframes are not able to be added when the mapping module stops
     if (mapper_->is_paused() || mapper_->pause_is_requested()) {
         return false;
     }
 
-    const auto num_keyfrms = map_db_->get_num_keyframes();
+    const auto num_keyfrms = map_db->get_num_keyframes();
+    auto last_inserted_keyfrm = map_db->get_last_inserted_keyframe();
 
     // Count the number of the 3D points that are observed from more than two keyframes
     const unsigned int min_obs_thr = (3 <= num_keyfrms) ? 3 : 2;
     const auto num_reliable_lms = ref_keyfrm.get_num_tracked_landmarks(min_obs_thr);
 
-    // Check if the mapping is in progress or not
-    const bool mapper_is_idle = mapper_->is_idle();
+    // When the mapping module skips localBA, it does not insert keyframes
+    const auto mapper_is_skipping_localBA = mapper_->is_skipping_localBA();
 
-    // Ratio-threshold of "the number of 3D points observed in the current frame" / "that of 3D points observed in the last keyframe"
-    constexpr unsigned int num_tracked_lms_thr = 15;
-    const float lms_ratio_thr = 0.9;
+    constexpr unsigned int num_tracked_lms_thr_unstable = 15;
 
-    // Condition A1: Add a keyframe if the number of frames added after the previous keyframe insertion reaches the threshold
-    const bool cond_a1 = frm_id_of_last_keyfrm_ + max_num_frms_ <= curr_frm.id_;
-    // Condition A2: Add a keyframe if the number of added frames exceeds the minimum,
-    //               and concurrently the mapping module remains standing-by
-    const bool cond_a2 = (frm_id_of_last_keyfrm_ + min_num_frms_ <= curr_frm.id_) && mapper_is_idle;
-    // Condition A3: Add a keyframe if the field-of-view of the current frame is changed a lot
-    const bool cond_a3 = num_tracked_lms < num_reliable_lms * 0.25;
+    // New keyframe is needed if the time elapsed since the last keyframe insertion reaches the threshold
+    const bool max_interval_elapsed = last_inserted_keyfrm && last_inserted_keyfrm->timestamp_ + max_interval_ <= curr_frm.timestamp_;
+    // New keyframe is needed if the field-of-view of the current frame is changed a lot
+    const bool view_changed = num_tracked_lms < num_reliable_lms * lms_ratio_thr_view_changed_;
 
-    // Condition B: (Mandatory for keyframe insertion)
-    //              Add a keyframe if the number of 3D points exceeds the threshold,
-    //              and concurrently the ratio of the reliable 3D points larger than the threshold ratio
-    const bool cond_b = (num_tracked_lms_thr <= num_tracked_lms) && (num_tracked_lms < num_reliable_lms * lms_ratio_thr);
+    // (Mandatory for keyframe insertion)
+    // New keyframe is needed if the number of 3D points exceeds the threshold,
+    // and concurrently the ratio of the reliable 3D points larger than the threshold ratio
+    bool tracking_is_unstable = num_tracked_lms < num_tracked_lms_thr_unstable;
+    bool almost_all_lms_are_tracked = num_tracked_lms > num_reliable_lms * lms_ratio_thr_almost_all_lms_are_tracked_;
 
-    // Do not add any kerframes if the condition B is not satisfied
-    if (!cond_b) {
-        return false;
-    }
-
-    // Do not add any kerframes if all the conditions A are not satisfied
-    if (!cond_a1 && !cond_a2 && !cond_a3) {
-        return false;
-    }
-
-    // Add the keyframe if the mapping module isn't in the process
-    if (mapper_is_idle) {
-        return true;
-    }
-
-    // Stop the local bundle adjustment if the mapping module is in the process, then add a new keyframe
-    if (setup_type_ != camera::setup_type_t::Monocular
-        && mapper_->get_num_queued_keyframes() <= 2) {
-        mapper_->abort_local_BA();
-        return true;
-    }
-
-    return false;
+    return (max_interval_elapsed || view_changed) && !tracking_is_unstable && !almost_all_lms_are_tracked && !mapper_is_skipping_localBA;
 }
 
-std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::frame& curr_frm) {
+std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map_database* map_db,
+                                                                       data::frame& curr_frm) {
     // Do not pause mapping_module to let this keyframe process
     if (!mapper_->prevent_pause_if_not_paused()) {
         // If it is already paused, exit
@@ -89,8 +73,6 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::fra
     curr_frm.update_pose_params();
     auto keyfrm = data::keyframe::make_keyframe(curr_frm);
 
-    frm_id_of_last_keyfrm_ = curr_frm.id_;
-
     // Queue up the keyframe to the mapping module
     if (!keyfrm->depth_is_avaliable()) {
         queue_keyframe(keyfrm);
@@ -140,7 +122,7 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::fra
 
         // Stereo-triangulation can be performed if the 3D point is not yet associated to the keypoint index
         const Vec3_t pos_w = curr_frm.triangulate_stereo(idx);
-        auto lm = std::make_shared<data::landmark>(pos_w, keyfrm, map_db_);
+        auto lm = std::make_shared<data::landmark>(pos_w, keyfrm, map_db);
 
         lm->add_observation(keyfrm, idx);
         keyfrm->add_landmark(lm, idx);
@@ -149,7 +131,7 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::fra
         lm->compute_descriptor();
         lm->update_mean_normal_and_obs_scale_variance();
 
-        map_db_->add_landmark(lm);
+        map_db->add_landmark(lm);
     }
 
     // Queue up the keyframe to the mapping module