From dc742ba5d742b7d8db2cea7bb343279cd48aa218 Mon Sep 17 00:00:00 2001 From: ymd-stella Date: Thu, 2 May 2024 10:38:26 +0900 Subject: [PATCH] Performance improvement for temporal mapping --- src/stella_vslam/module/local_map_updater.cc | 81 ++++++++++---------- src/stella_vslam/module/local_map_updater.h | 19 +++-- src/stella_vslam/tracking_module.cc | 45 +++++++---- src/stella_vslam/tracking_module.h | 9 ++- 4 files changed, 90 insertions(+), 64 deletions(-) diff --git a/src/stella_vslam/module/local_map_updater.cc b/src/stella_vslam/module/local_map_updater.cc index eede847e..cd2e3307 100644 --- a/src/stella_vslam/module/local_map_updater.cc +++ b/src/stella_vslam/module/local_map_updater.cc @@ -23,15 +23,26 @@ std::shared_ptr local_map_updater::get_nearest_covisibility() co return nearest_covisibility_; } +bool local_map_updater::acquire_local_map(const std::vector>& frm_lms) { + constexpr unsigned int keyframe_id_threshold = 0; + unsigned int num_temporal_keyfrms = 0; + const auto local_keyfrms_was_found = find_local_keyframes(frm_lms, keyframe_id_threshold, num_temporal_keyfrms); + const auto local_lms_was_found = find_local_landmarks(frm_lms); + return local_keyfrms_was_found && local_lms_was_found; +} + bool local_map_updater::acquire_local_map(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold) { - const auto local_keyfrms_was_found = find_local_keyframes(frm_lms, keyframe_id_threshold); - const auto local_lms_was_found = find_local_landmarks(frm_lms, keyframe_id_threshold); + const unsigned int keyframe_id_threshold, + unsigned int& num_temporal_keyfrms) { + num_temporal_keyfrms = 0; + const auto local_keyfrms_was_found = find_local_keyframes(frm_lms, keyframe_id_threshold, num_temporal_keyfrms); + const auto local_lms_was_found = find_local_landmarks(frm_lms); return local_keyfrms_was_found && local_lms_was_found; } bool local_map_updater::find_local_keyframes(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold) { + const unsigned int keyframe_id_threshold, + unsigned int& num_temporal_keyfrms) { const auto num_shared_lms_and_keyfrm = count_num_shared_lms(frm_lms, keyframe_id_threshold); if (num_shared_lms_and_keyfrm.empty()) { SPDLOG_TRACE("find_local_keyframes: empty"); @@ -39,8 +50,8 @@ bool local_map_updater::find_local_keyframes(const std::vector already_found_keyfrm_ids; - const auto first_local_keyfrms = find_first_local_keyframes(num_shared_lms_and_keyfrm, already_found_keyfrm_ids); - const auto second_local_keyfrms = find_second_local_keyframes(first_local_keyfrms, already_found_keyfrm_ids, keyframe_id_threshold); + const auto first_local_keyfrms = find_first_local_keyframes(num_shared_lms_and_keyfrm, keyframe_id_threshold, already_found_keyfrm_ids, num_temporal_keyfrms); + const auto second_local_keyfrms = find_second_local_keyframes(first_local_keyfrms, keyframe_id_threshold, already_found_keyfrm_ids, num_temporal_keyfrms); local_keyfrms_ = first_local_keyfrms; std::copy(second_local_keyfrms.begin(), second_local_keyfrms.end(), std::back_inserter(local_keyfrms_)); return true; @@ -48,7 +59,7 @@ bool local_map_updater::find_local_keyframes(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold) const + const unsigned int keyframe_id_threshold) const -> std::vector>> { std::vector>> num_shared_lms_and_keyfrm; @@ -66,19 +77,20 @@ auto local_map_updater::count_num_shared_lms( const auto observations = lm->get_observations(); for (auto obs : observations) { auto keyfrm = obs.first.lock(); - if (keyframe_id_threshold > 0 && keyfrm->id_ >= keyframe_id_threshold) { - continue; - } ++keyfrm_to_num_shared_lms[keyfrm]; } } + int num_temporal_keyfrms = 0; for (auto& it : keyfrm_to_num_shared_lms) { + if (keyframe_id_threshold > 0 && it.first->id_ >= keyframe_id_threshold) { + ++num_temporal_keyfrms; + } num_shared_lms_and_keyfrm.emplace_back(it.second, it.first); } constexpr int margin = 5; // Keep a little more than max_num_local_keyfrms_, as keyframes may be deleted. - if (num_shared_lms_and_keyfrm.size() > max_num_local_keyfrms_ + margin) { + if (num_shared_lms_and_keyfrm.size() > max_num_local_keyfrms_ + num_temporal_keyfrms + margin) { std::partial_sort(num_shared_lms_and_keyfrm.begin(), - num_shared_lms_and_keyfrm.begin() + max_num_local_keyfrms_ + margin, + num_shared_lms_and_keyfrm.begin() + max_num_local_keyfrms_ + num_temporal_keyfrms + margin, num_shared_lms_and_keyfrm.end(), greater_number_and_id_object_pairs()); } @@ -93,7 +105,9 @@ auto local_map_updater::count_num_shared_lms( auto local_map_updater::find_first_local_keyframes( const std::vector>>& num_shared_lms_and_keyfrm, - std::unordered_set& already_found_keyfrm_ids) + const unsigned int keyframe_id_threshold, + std::unordered_set& already_found_keyfrm_ids, + unsigned int& num_temporal_keyfrms) -> std::vector> { std::vector> first_local_keyfrms; first_local_keyfrms.reserve(std::min(static_cast(max_num_local_keyfrms_), 2 * num_shared_lms_and_keyfrm.size())); @@ -108,6 +122,9 @@ auto local_map_updater::find_first_local_keyframes( } first_local_keyfrms.push_back(keyfrm); + if (keyframe_id_threshold > 0 && keyfrm->id_ >= keyframe_id_threshold) { + ++num_temporal_keyfrms; + } // avoid duplication already_found_keyfrm_ids.insert(keyfrm->id_); @@ -118,7 +135,7 @@ auto local_map_updater::find_first_local_keyframes( nearest_covisibility_ = keyfrm; } - if (max_num_local_keyfrms_ <= first_local_keyfrms.size()) { + if (max_num_local_keyfrms_ <= first_local_keyfrms.size() + num_temporal_keyfrms) { break; } } @@ -127,14 +144,16 @@ auto local_map_updater::find_first_local_keyframes( } auto local_map_updater::find_second_local_keyframes(const std::vector>& first_local_keyframes, + const unsigned int keyframe_id_threshold, std::unordered_set& already_found_keyfrm_ids, - unsigned int keyframe_id_threshold) const + unsigned int& num_temporal_keyfrms) const -> std::vector> { std::vector> second_local_keyfrms; second_local_keyfrms.reserve(4 * first_local_keyframes.size()); // add the second-order keyframes to the local landmarks - auto add_second_local_keyframe = [this, &second_local_keyfrms, &already_found_keyfrm_ids, keyframe_id_threshold](const std::shared_ptr& keyfrm) { + auto add_second_local_keyframe = [this, &second_local_keyfrms, &already_found_keyfrm_ids, + &num_temporal_keyfrms, keyframe_id_threshold](const std::shared_ptr& keyfrm) { if (!keyfrm) { return false; } @@ -142,7 +161,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector 0 && keyfrm->id_ >= keyframe_id_threshold) { - return false; + ++num_temporal_keyfrms; } // avoid duplication if (already_found_keyfrm_ids.count(keyfrm->id_)) { @@ -153,7 +172,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vectorgraph_node_->get_top_n_covisibilities(10); for (const auto& neighbor : neighbors) { add_second_local_keyframe(neighbor); - if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size()) { + if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size() + num_temporal_keyfrms) { return second_local_keyfrms; } } @@ -172,7 +191,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vectorgraph_node_->get_spanning_children(); for (const auto& child : spanning_children) { add_second_local_keyframe(child); - if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size()) { + if (max_num_local_keyfrms_ <= first_local_keyframes.size() + second_local_keyfrms.size() + num_temporal_keyfrms) { return second_local_keyfrms; } } @@ -185,12 +204,12 @@ auto local_map_updater::find_second_local_keyframes(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold) { +bool local_map_updater::find_local_landmarks(const std::vector>& frm_lms) { local_lms_.clear(); local_lms_.reserve(50 * local_keyfrms_.size()); std::unordered_set already_found_lms_ids; + already_found_lms_ids.reserve(frm_lms.size()); for (unsigned int idx = 0; idx < frm_lms.size(); ++idx) { auto& lm = frm_lms.at(idx); if (!lm) { @@ -202,6 +221,7 @@ bool local_map_updater::find_local_landmarks(const std::vectorid_); } for (const auto& keyfrm : local_keyfrms_) { + // ZoneScopedN("find_local_landmarks_per_keyfrm"); const auto& lms = keyfrm->get_landmarks(); for (const auto& lm : lms) { @@ -217,23 +237,6 @@ bool local_map_updater::find_local_landmarks(const std::vectorid_); - - if (keyframe_id_threshold > 0) { - const auto observations = lm->get_observations(); - unsigned int temporal_observations = 0; - for (auto obs : observations) { - auto keyfrm = obs.first.lock(); - if (keyfrm->id_ >= keyframe_id_threshold) { - ++temporal_observations; - } - } - const double temporal_ratio_thr = 0.5; - double temporal_ratio = static_cast(temporal_observations) / observations.size(); - if (temporal_ratio > temporal_ratio_thr) { - continue; - } - } - local_lms_.push_back(lm); } } diff --git a/src/stella_vslam/module/local_map_updater.h b/src/stella_vslam/module/local_map_updater.h index 2be2c607..f0c6bcf5 100644 --- a/src/stella_vslam/module/local_map_updater.h +++ b/src/stella_vslam/module/local_map_updater.h @@ -33,13 +33,16 @@ class local_map_updater { std::shared_ptr get_nearest_covisibility() const; //! Acquire the new local map + bool acquire_local_map(const std::vector>& frm_lms); bool acquire_local_map(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold = 0); + unsigned int keyframe_id_threshold, + unsigned int& num_temporal_keyfrms); private: //! Find the local keyframes bool find_local_keyframes(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold); + unsigned int keyframe_id_threshold, + unsigned int& num_temporal_keyfrms); //! Count the number of shared landmarks between the current frame and each of the neighbor keyframes auto count_num_shared_lms( @@ -50,18 +53,20 @@ class local_map_updater { //! Find the first-order local keyframes auto find_first_local_keyframes( const std::vector>>& keyfrm_weights, - std::unordered_set& already_found_ids) + const unsigned int keyframe_id_threshold, + std::unordered_set& already_found_keyfrm_ids, + unsigned int& num_temporal_keyfrms) -> std::vector>; //! Find the second-order local keyframes auto find_second_local_keyframes(const std::vector>& first_local_keyframes, - std::unordered_set& already_found_ids, - unsigned int keyframe_id_threshold) const + unsigned int keyframe_id_threshold, + std::unordered_set& already_found_keyfrm_ids, + unsigned int& num_temporal_keyfrms) const -> std::vector>; //! Find the local landmarks - bool find_local_landmarks(const std::vector>& frm_lms, - unsigned int keyframe_id_threshold); + bool find_local_landmarks(const std::vector>& frm_lms); // maximum number of the local keyframes const unsigned int max_num_local_keyfrms_; diff --git a/src/stella_vslam/tracking_module.cc b/src/stella_vslam/tracking_module.cc index 9a307569..22cea216 100644 --- a/src/stella_vslam/tracking_module.cc +++ b/src/stella_vslam/tracking_module.cc @@ -218,13 +218,14 @@ bool tracking_module::track(bool relocalization_is_needed, } // update the local map and optimize current camera pose + unsigned int fixed_keyframe_id_threshold = map_db_->get_fixed_keyframe_id_threshold(); + unsigned int num_temporal_keyfrms = 0; if (succeeded) { - succeeded = track_local_map(num_tracked_lms, num_reliable_lms, min_num_obs_thr); + succeeded = track_local_map(num_tracked_lms, num_reliable_lms, num_temporal_keyfrms, min_num_obs_thr, fixed_keyframe_id_threshold); } // update the local map and optimize current camera pose without temporal keyframes - unsigned int fixed_keyframe_id_threshold = map_db_->get_fixed_keyframe_id_threshold(); - if (fixed_keyframe_id_threshold > 0 && succeeded) { + if (fixed_keyframe_id_threshold > 0 && succeeded && num_temporal_keyfrms > 0) { succeeded = track_local_map_without_temporal_keyframes(num_tracked_lms, num_reliable_lms, min_num_obs_thr, fixed_keyframe_id_threshold); } @@ -243,13 +244,15 @@ bool tracking_module::track(bool relocalization_is_needed, bool tracking_module::track_local_map(unsigned int& num_tracked_lms, unsigned int& num_reliable_lms, - const unsigned int min_num_obs_thr) { + unsigned int& num_temporal_keyfrms, + const unsigned int min_num_obs_thr, + const unsigned int fixed_keyframe_id_threshold) { bool succeeded = false; SPDLOG_TRACE("tracking_module: update_local_map (curr_frm_={})", curr_frm_.id_); - succeeded = update_local_map(); + succeeded = update_local_map(fixed_keyframe_id_threshold, num_temporal_keyfrms); if (succeeded) { - succeeded = search_local_landmarks(); + succeeded = search_local_landmarks(fixed_keyframe_id_threshold); } if (succeeded) { @@ -269,11 +272,7 @@ bool tracking_module::track_local_map_without_temporal_keyframes(unsigned int& n const unsigned int fixed_keyframe_id_threshold) { bool succeeded = false; SPDLOG_TRACE("tracking_module: update_local_map without temporal keyframes (curr_frm_={})", curr_frm_.id_); - succeeded = update_local_map(fixed_keyframe_id_threshold); - - if (succeeded) { - succeeded = search_local_landmarks(); - } + succeeded = search_local_landmarks(fixed_keyframe_id_threshold); if (enable_temporal_keyframe_only_tracking_ && !succeeded) { SPDLOG_TRACE("temporal keyframe only tracking (curr_frm_={})", curr_frm_.id_); @@ -286,7 +285,7 @@ bool tracking_module::track_local_map_without_temporal_keyframes(unsigned int& n } if (!succeeded) { - spdlog::info("local map tracking failed (curr_frm_={})", curr_frm_.id_); + spdlog::info("local map tracking (without temporal keyframes) failed (curr_frm_={})", curr_frm_.id_); } return succeeded; } @@ -488,7 +487,8 @@ bool tracking_module::optimize_current_frame_with_local_map(unsigned int& num_tr return true; } -bool tracking_module::update_local_map(unsigned int fixed_keyframe_id_threshold) { +bool tracking_module::update_local_map(unsigned int fixed_keyframe_id_threshold, + unsigned int& num_temporal_keyfrms) { // clean landmark associations for (unsigned int idx = 0; idx < curr_frm_.frm_obs_.undist_keypts_.size(); ++idx) { const auto& lm = curr_frm_.get_landmark(idx); @@ -504,7 +504,7 @@ bool tracking_module::update_local_map(unsigned int fixed_keyframe_id_threshold) // acquire the current local map local_landmarks_.clear(); auto local_map_updater = module::local_map_updater(max_num_local_keyfrms_); - if (!local_map_updater.acquire_local_map(curr_frm_.get_landmarks(), fixed_keyframe_id_threshold)) { + if (!local_map_updater.acquire_local_map(curr_frm_.get_landmarks(), fixed_keyframe_id_threshold, num_temporal_keyfrms)) { return false; } // update the variables @@ -520,7 +520,7 @@ bool tracking_module::update_local_map(unsigned int fixed_keyframe_id_threshold) return true; } -bool tracking_module::search_local_landmarks() { +bool tracking_module::search_local_landmarks(unsigned int fixed_keyframe_id_threshold) { // select the landmarks which can be reprojected from the ones observed in the current frame std::unordered_set curr_landmark_ids; for (const auto& lm : curr_frm_.get_landmarks()) { @@ -554,6 +554,21 @@ bool tracking_module::search_local_landmarks() { if (lm->will_be_erased()) { continue; } + if (fixed_keyframe_id_threshold > 0) { + const auto observations = lm->get_observations(); + unsigned int temporal_observations = 0; + for (auto obs : observations) { + auto keyfrm = obs.first.lock(); + if (keyfrm->id_ >= fixed_keyframe_id_threshold) { + ++temporal_observations; + } + } + const double temporal_ratio_thr = 0.5; + double temporal_ratio = static_cast(temporal_observations) / observations.size(); + if (temporal_ratio > temporal_ratio_thr) { + continue; + } + } // check the observability if (curr_frm_.can_observe(lm, 0.5, reproj, x_right, pred_scale_level)) { diff --git a/src/stella_vslam/tracking_module.h b/src/stella_vslam/tracking_module.h index 9bbcdb7a..2ce64178 100644 --- a/src/stella_vslam/tracking_module.h +++ b/src/stella_vslam/tracking_module.h @@ -160,7 +160,9 @@ class tracking_module { const unsigned int min_num_obs_thr); bool track_local_map(unsigned int& num_tracked_lms, unsigned int& num_reliable_lms, - unsigned int min_num_obs_thr); + unsigned int& num_temporal_keyfrms, + unsigned int min_num_obs_thr, + unsigned int fixed_keyframe_id_threshold); bool track_local_map_without_temporal_keyframes(unsigned int& num_tracked_lms, unsigned int& num_reliable_lms, unsigned int min_num_obs_thr, @@ -187,10 +189,11 @@ class tracking_module { const unsigned int min_num_obs_thr); //! Update the local map - bool update_local_map(unsigned int fixed_keyframe_id_threshold = 0); + bool update_local_map(unsigned int fixed_keyframe_id_threshold, + unsigned int& num_temporal_keyfrms); //! Acquire more 2D-3D matches using initial camera pose estimation - bool search_local_landmarks(); + bool search_local_landmarks(unsigned int fixed_keyframe_id_threshold); //! Check the new keyframe is needed or not bool new_keyframe_is_needed(unsigned int num_tracked_lms,