Skip to content

Commit

Permalink
Performance improvement for temporal mapping
Browse files Browse the repository at this point in the history
  • Loading branch information
@ymd-stella
ymd-stella committed May 2, 2024
1 parent 6c9504b commit dc742ba
Show file tree
Hide file tree
Showing 4 changed files with 90 additions and 64 deletions.
81 changes: 42 additions & 39 deletions src/stella_vslam/module/local_map_updater.cc
View file
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
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
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

0 comments on commit dc742ba

Please sign in to comment.