Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Performance improvement for temporal mapping #589

Merged
merged 1 commit into from
May 2, 2024
Merged

Performance improvement for temporal mapping #589

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
81 changes: 42 additions & 39 deletions src/stella_vslam/module/local_map_updater.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,32 +23,43 @@ std::shared_ptr<data::keyframe> local_map_updater::get_nearest_covisibility() co
return nearest_covisibility_;
}

bool local_map_updater::acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& 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<std::shared_ptr<data::landmark>>& 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<std::shared_ptr<data::landmark>>& 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");
return false;
}

std::unordered_set<unsigned int> 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;
}

auto local_map_updater::count_num_shared_lms(
const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
unsigned int keyframe_id_threshold) const
const unsigned int keyframe_id_threshold) const
-> std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>> {
std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>> num_shared_lms_and_keyfrm;

Expand All @@ -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<unsigned int, data::keyframe>());
}
Expand All @@ -93,7 +105,9 @@ auto local_map_updater::count_num_shared_lms(

auto local_map_updater::find_first_local_keyframes(
const std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>& num_shared_lms_and_keyfrm,
std::unordered_set<unsigned int>& already_found_keyfrm_ids)
const unsigned int keyframe_id_threshold,
std::unordered_set<unsigned int>& already_found_keyfrm_ids,
unsigned int& num_temporal_keyfrms)
-> std::vector<std::shared_ptr<data::keyframe>> {
std::vector<std::shared_ptr<data::keyframe>> first_local_keyfrms;
first_local_keyfrms.reserve(std::min(static_cast<size_t>(max_num_local_keyfrms_), 2 * num_shared_lms_and_keyfrm.size()));
Expand All @@ -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_);
Expand All @@ -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;
}
}
Expand All @@ -127,22 +144,24 @@ auto local_map_updater::find_first_local_keyframes(
}

auto local_map_updater::find_second_local_keyframes(const std::vector<std::shared_ptr<data::keyframe>>& first_local_keyframes,
const unsigned int keyframe_id_threshold,
std::unordered_set<unsigned int>& already_found_keyfrm_ids,
unsigned int keyframe_id_threshold) const
unsigned int& num_temporal_keyfrms) const
-> std::vector<std::shared_ptr<data::keyframe>> {
std::vector<std::shared_ptr<data::keyframe>> 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<data::keyframe>& keyfrm) {
auto add_second_local_keyframe = [this, &second_local_keyfrms, &already_found_keyfrm_ids,
&num_temporal_keyfrms, keyframe_id_threshold](const std::shared_ptr<data::keyframe>& keyfrm) {
if (!keyfrm) {
return false;
}
if (keyfrm->will_be_erased()) {
return false;
}
if (keyframe_id_threshold > 0 && keyfrm->id_ >= keyframe_id_threshold) {
return false;
++num_temporal_keyfrms;
}
// avoid duplication
if (already_found_keyfrm_ids.count(keyfrm->id_)) {
Expand All @@ -153,7 +172,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
return true;
};
for (auto iter = first_local_keyframes.cbegin(); iter != first_local_keyframes.cend(); ++iter) {
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) {
break;
}

Expand All @@ -163,7 +182,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
const auto neighbors = keyfrm->graph_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;
}
}
Expand All @@ -172,7 +191,7 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
const auto spanning_children = keyfrm->graph_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;
}
}
Expand All @@ -185,12 +204,12 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
return second_local_keyfrms;
}

bool local_map_updater::find_local_landmarks(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
unsigned int keyframe_id_threshold) {
bool local_map_updater::find_local_landmarks(const std::vector<std::shared_ptr<data::landmark>>& frm_lms) {
local_lms_.clear();
local_lms_.reserve(50 * local_keyfrms_.size());

std::unordered_set<unsigned int> 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) {
Expand All @@ -202,6 +221,7 @@ bool local_map_updater::find_local_landmarks(const std::vector<std::shared_ptr<d
already_found_lms_ids.insert(lm->id_);
}
for (const auto& keyfrm : local_keyfrms_) {
// ZoneScopedN("find_local_landmarks_per_keyfrm");
const auto& lms = keyfrm->get_landmarks();

for (const auto& lm : lms) {
Expand All @@ -217,23 +237,6 @@ bool local_map_updater::find_local_landmarks(const std::vector<std::shared_ptr<d
continue;
}
already_found_lms_ids.insert(lm->id_);

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<double>(temporal_observations) / observations.size();
if (temporal_ratio > temporal_ratio_thr) {
continue;
}
}

local_lms_.push_back(lm);
}
}
Expand Down
19 changes: 12 additions & 7 deletions src/stella_vslam/module/local_map_updater.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,13 +33,16 @@ class local_map_updater {
std::shared_ptr<data::keyframe> get_nearest_covisibility() const;

//! Acquire the new local map
bool acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& frm_lms);
bool acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& 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<std::shared_ptr<data::landmark>>& 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(
Expand All @@ -50,18 +53,20 @@ class local_map_updater {
//! Find the first-order local keyframes
auto find_first_local_keyframes(
const std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>& keyfrm_weights,
std::unordered_set<unsigned int>& already_found_ids)
const unsigned int keyframe_id_threshold,
std::unordered_set<unsigned int>& already_found_keyfrm_ids,
unsigned int& num_temporal_keyfrms)
-> std::vector<std::shared_ptr<data::keyframe>>;

//! Find the second-order local keyframes
auto find_second_local_keyframes(const std::vector<std::shared_ptr<data::keyframe>>& first_local_keyframes,
std::unordered_set<unsigned int>& already_found_ids,
unsigned int keyframe_id_threshold) const
unsigned int keyframe_id_threshold,
std::unordered_set<unsigned int>& already_found_keyfrm_ids,
unsigned int& num_temporal_keyfrms) const
-> std::vector<std::shared_ptr<data::keyframe>>;

//! Find the local landmarks
bool find_local_landmarks(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
unsigned int keyframe_id_threshold);
bool find_local_landmarks(const std::vector<std::shared_ptr<data::landmark>>& frm_lms);

// maximum number of the local keyframes
const unsigned int max_num_local_keyfrms_;
Expand Down
45 changes: 30 additions & 15 deletions src/stella_vslam/tracking_module.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -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) {
Expand All @@ -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_);
Expand All @@ -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;
}
Expand Down Expand Up @@ -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);
Expand All @@ -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
Expand All @@ -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<unsigned int> curr_landmark_ids;
for (const auto& lm : curr_frm_.get_landmarks()) {
Expand Down Expand Up @@ -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<double>(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)) {
Expand Down
Loading
Loading