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

Remove num_keypts from frame_observation #554

Merged
merged 1 commit into from
Jan 28, 2024
Merged

Remove num_keypts from frame_observation #554

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
2 changes: 1 addition & 1 deletion src/stella_vslam/data/frame.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ frame::frame(const double timestamp, camera::base* camera, feature::orb_params*
: id_(next_id_++), timestamp_(timestamp), camera_(camera), orb_params_(orb_params), frm_obs_(frm_obs),
markers_2d_(markers_2d),
// Initialize association with 3D points
landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.num_keypts_, nullptr)) {}
landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.undist_keypts_.size(), nullptr)) {}

void frame::set_pose_cw(const Mat44_t& pose_cw) {
pose_is_valid_ = true;
Expand Down
6 changes: 2 additions & 4 deletions src/stella_vslam/data/frame_observation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,15 +13,13 @@ struct frame_observation {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

frame_observation() = default;
frame_observation(unsigned int num_keypts, const cv::Mat& descriptors,
frame_observation(const cv::Mat& descriptors,
const std::vector<cv::KeyPoint>& undist_keypts, const eigen_alloc_vector<Vec3_t>& bearings,
const std::vector<float>& stereo_x_right, const std::vector<float>& depths,
const std::vector<std::vector<std::vector<unsigned int>>>& keypt_indices_in_cells)
: num_keypts_(num_keypts), descriptors_(descriptors), undist_keypts_(undist_keypts), bearings_(bearings),
: descriptors_(descriptors), undist_keypts_(undist_keypts), bearings_(bearings),
stereo_x_right_(stereo_x_right), depths_(depths), keypt_indices_in_cells_(keypt_indices_in_cells) {}

//! number of keypoints
unsigned int num_keypts_ = 0;
//! descriptors
cv::Mat descriptors_;
//! undistorted keypoints of monocular or stereo left image
Expand Down
8 changes: 4 additions & 4 deletions src/stella_vslam/data/keyframe.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ keyframe::keyframe(const unsigned int id, const double timestamp,
timestamp_(timestamp), camera_(camera),
orb_params_(orb_params), frm_obs_(frm_obs),
bow_vec_(bow_vec), bow_feat_vec_(bow_feat_vec),
landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.num_keypts_, nullptr)) {
landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.undist_keypts_.size(), nullptr)) {
// set pose parameters (pose_wc_, trans_wc_) using pose_cw_
set_pose_cw(pose_cw);

Expand Down Expand Up @@ -137,7 +137,7 @@ std::shared_ptr<keyframe> keyframe::from_stmt(sqlite3_stmt* stmt,
std::vector<std::vector<std::vector<unsigned int>>> keypt_indices_in_cells;
data::assign_keypoints_to_grid(camera, undist_keypts, keypt_indices_in_cells);
// Construct frame_observation
frame_observation frm_obs{num_keypts, descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells};
frame_observation frm_obs{descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells};
// Compute BoW
data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
auto keyfrm = data::keyframe::make_keyframe(
Expand Down Expand Up @@ -180,7 +180,7 @@ nlohmann::json keyframe::to_json() const {
{"rot_cw", convert_rotation_to_json(pose_cw_.block<3, 3>(0, 0))},
{"trans_cw", convert_translation_to_json(pose_cw_.block<3, 1>(0, 3))},
// features and observations
{"n_keypts", frm_obs_.num_keypts_},
{"n_keypts", frm_obs_.undist_keypts_.size()},
{"undist_keypts", convert_keypoints_to_json(frm_obs_.undist_keypts_)},
{"x_rights", frm_obs_.stereo_x_right_},
{"depths", frm_obs_.depths_},
Expand Down Expand Up @@ -415,7 +415,7 @@ float keyframe::compute_median_depth(const bool abs) const {
}

std::vector<float> depths;
depths.reserve(frm_obs_.num_keypts_);
depths.reserve(frm_obs_.undist_keypts_.size());
const Vec3_t rot_cw_z_row = pose_cw.block<1, 3>(2, 0);
const float trans_cw_z = pose_cw(2, 3);

Expand Down
4 changes: 2 additions & 2 deletions src/stella_vslam/data/map_database.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -395,7 +395,7 @@ void map_database::register_keyframe(camera_database* cam_db, orb_params_databas
std::vector<std::vector<std::vector<unsigned int>>> keypt_indices_in_cells;
data::assign_keypoints_to_grid(camera, undist_keypts, keypt_indices_in_cells);
// Construct frame_observation
frame_observation frm_obs{num_keypts, descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells};
frame_observation frm_obs{descriptors, undist_keypts, bearings, stereo_x_right, depths, keypt_indices_in_cells};
// Compute BoW
data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
auto keyfrm = data::keyframe::make_keyframe(
Expand Down Expand Up @@ -603,7 +603,7 @@ void map_database::load_association_from_stmt(sqlite3_stmt* stmt) {
auto keyfrm_id = sqlite3_column_int64(stmt, column_id);
assert(keyframes_.count(keyfrm_id));
column_id++;
std::vector<int> lm_ids(keyframes_.at(keyfrm_id)->frm_obs_.num_keypts_, -1);
std::vector<int> lm_ids(keyframes_.at(keyfrm_id)->frm_obs_.undist_keypts_.size(), -1);
p = reinterpret_cast<const char*>(sqlite3_column_blob(stmt, column_id));
std::memcpy(lm_ids.data(), p, sqlite3_column_bytes(stmt, column_id));
column_id++;
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/global_optimization_module.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -399,7 +399,7 @@ void global_optimization_module::replace_duplicated_landmarks(const std::vector<
{
std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); ++idx) {
auto curr_match_lm_in_cand = curr_match_lms_observed_in_cand.at(idx);
if (!curr_match_lm_in_cand) {
continue;
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/match/bow_tree.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ namespace match {
unsigned int bow_tree::match_frame_and_keyframe(const std::shared_ptr<data::keyframe>& keyfrm, data::frame& frm, std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm) const {
unsigned int num_matches = 0;

matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(frm.frm_obs_.num_keypts_, nullptr);
matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(frm.frm_obs_.undist_keypts_.size(), nullptr);

const auto keyfrm_lms = keyfrm->get_landmarks();

Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/match/projection.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,7 @@ unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, co

// Reproject the 3D points associated to the keypoints of the last frame,
// then acquire the 2D-3D matches
for (unsigned int idx_last = 0; idx_last < last_frm.frm_obs_.num_keypts_; ++idx_last) {
for (unsigned int idx_last = 0; idx_last < last_frm.frm_obs_.undist_keypts_.size(); ++idx_last) {
const auto& lm = last_frm.get_landmark(idx_last);
if (!lm) {
continue;
Expand Down
16 changes: 9 additions & 7 deletions src/stella_vslam/match/robust.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,9 @@ unsigned int robust::match_for_triangulation(const std::shared_ptr<data::keyfram
// Save the matching information
// Discard the already matched keypoints in keyframe 2
// to acquire a unique association to each keypoint in keyframe 1
std::vector<bool> is_already_matched_in_keyfrm_2(keyfrm_2->frm_obs_.num_keypts_, false);
std::vector<bool> is_already_matched_in_keyfrm_2(keyfrm_2->frm_obs_.undist_keypts_.size(), false);
// Save the keypoint idx in keyframe 2 which is already associated to the keypoint idx in keyframe 1
std::vector<int> matched_indices_2_in_keyfrm_1(keyfrm_1->frm_obs_.num_keypts_, -1);
std::vector<int> matched_indices_2_in_keyfrm_1(keyfrm_1->frm_obs_.undist_keypts_.size(), -1);

data::bow_feature_vector::const_iterator itr_1 = keyfrm_1->bow_feat_vec_.begin();
data::bow_feature_vector::const_iterator itr_2 = keyfrm_2->bow_feat_vec_.begin();
Expand Down Expand Up @@ -157,7 +157,7 @@ unsigned int robust::match_keyframes(const std::shared_ptr<data::keyframe>& keyf
std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm,
bool validate_with_essential_solver, bool use_fixed_seed) const {
// Initialization
const auto num_frm_keypts = keyfrm1->frm_obs_.num_keypts_;
const auto num_frm_keypts = keyfrm1->frm_obs_.undist_keypts_.size();
const auto keyfrm_lms = keyfrm2->get_landmarks();
unsigned int num_inlier_matches = 0;
matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(num_frm_keypts, nullptr);
Expand Down Expand Up @@ -205,7 +205,7 @@ unsigned int robust::match_frame_and_keyframe(data::frame& frm, const std::share
std::vector<std::shared_ptr<data::landmark>>& matched_lms_in_frm,
bool use_fixed_seed) const {
// Initialization
const auto num_frm_keypts = frm.frm_obs_.num_keypts_;
const auto num_frm_keypts = frm.frm_obs_.undist_keypts_.size();
const auto keyfrm_lms = keyfrm->get_landmarks();
unsigned int num_inlier_matches = 0;
matched_lms_in_frm = std::vector<std::shared_ptr<data::landmark>>(num_frm_keypts, nullptr);
Expand Down Expand Up @@ -237,13 +237,15 @@ unsigned int robust::match_frame_and_keyframe(data::frame& frm, const std::share
return num_inlier_matches;
}

unsigned int robust::brute_force_match(const data::frame_observation& frm_obs, const std::shared_ptr<data::keyframe>& keyfrm, std::vector<std::pair<int, int>>& matches) const {
unsigned int robust::brute_force_match(const data::frame_observation& frm_obs,
const std::shared_ptr<data::keyframe>& keyfrm,
std::vector<std::pair<int, int>>& matches) const {
unsigned int num_matches = 0;

// 1. Acquire the frame and keyframe information

const auto num_keypts_1 = frm_obs.num_keypts_;
const auto num_keypts_2 = keyfrm->frm_obs_.num_keypts_;
const auto num_keypts_1 = frm_obs.undist_keypts_.size();
const auto num_keypts_2 = keyfrm->frm_obs_.undist_keypts_.size();
const auto keypts_1 = frm_obs.undist_keypts_;
const auto keypts_2 = keyfrm->frm_obs_.undist_keypts_;
const auto lms_2 = keyfrm->get_landmarks();
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/module/frame_tracker.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data::
unsigned int frame_tracker::discard_outliers(const std::vector<bool>& outlier_flags, data::frame& curr_frm) const {
unsigned int num_valid_matches = 0;

for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
if (curr_frm.get_landmark(idx) == nullptr) {
continue;
}
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/module/initializer.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -343,7 +343,7 @@ bool initializer::create_map_for_stereo(data::bow_vocabulary* bow_vocab, data::f
curr_frm.ref_keyfrm_ = curr_keyfrm;
map_db_->update_frame_statistics(curr_frm, false);

for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
// add a new landmark if tht corresponding depth is valid
const auto z = curr_frm.frm_obs_.depths_.at(idx);
if (z <= 0) {
Expand Down
4 changes: 2 additions & 2 deletions src/stella_vslam/module/keyframe_inserter.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,8 +143,8 @@ std::shared_ptr<data::keyframe> keyframe_inserter::create_new_keyframe(

// Save the valid depth and index pairs
std::vector<std::pair<float, unsigned int>> depth_idx_pairs;
depth_idx_pairs.reserve(curr_frm.frm_obs_.num_keypts_);
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
depth_idx_pairs.reserve(curr_frm.frm_obs_.undist_keypts_.size());
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
assert(!curr_frm.frm_obs_.depths_.empty());
const auto depth = curr_frm.frm_obs_.depths_.at(idx);
// Add if the depth is valid
Expand Down
14 changes: 5 additions & 9 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 @@ -24,17 +24,15 @@ std::shared_ptr<data::keyframe> local_map_updater::get_nearest_covisibility() co
}

bool local_map_updater::acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
const unsigned int num_keypts,
unsigned int keyframe_id_threshold) {
const auto local_keyfrms_was_found = find_local_keyframes(frm_lms, num_keypts, keyframe_id_threshold);
const auto local_lms_was_found = find_local_landmarks(frm_lms, num_keypts, 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);
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,
const unsigned int num_keypts,
unsigned int keyframe_id_threshold) {
const auto num_shared_lms_and_keyfrm = count_num_shared_lms(frm_lms, num_keypts, keyframe_id_threshold);
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;
Expand All @@ -50,15 +48,14 @@ bool local_map_updater::find_local_keyframes(const std::vector<std::shared_ptr<d

auto local_map_updater::count_num_shared_lms(
const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
const unsigned int num_keypts,
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;

// count the number of sharing landmarks between the current frame and each of the neighbor keyframes
// key: keyframe, value: number of sharing landmarks
keyframe_to_num_shared_lms_t keyfrm_to_num_shared_lms;
for (unsigned int idx = 0; idx < num_keypts; ++idx) {
for (unsigned int idx = 0; idx < frm_lms.size(); ++idx) {
auto& lm = frm_lms.at(idx);
if (!lm) {
continue;
Expand Down Expand Up @@ -179,13 +176,12 @@ auto local_map_updater::find_second_local_keyframes(const std::vector<std::share
}

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

std::unordered_set<unsigned int> already_found_lms_ids;
for (unsigned int idx = 0; idx < num_keypts; ++idx) {
for (unsigned int idx = 0; idx < frm_lms.size(); ++idx) {
auto& lm = frm_lms.at(idx);
if (!lm) {
continue;
Expand Down
4 changes: 0 additions & 4 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 @@ -34,19 +34,16 @@ class local_map_updater {

//! Acquire the new local map
bool acquire_local_map(const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
const unsigned int num_keypts,
unsigned int keyframe_id_threshold = 0);

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

//! Count the number of shared landmarks between the current frame and each of the neighbor keyframes
auto count_num_shared_lms(
const std::vector<std::shared_ptr<data::landmark>>& frm_lms,
const unsigned int num_keypts,
unsigned int keyframe_id_threshold) const
-> std::vector<std::pair<unsigned int, std::shared_ptr<data::keyframe>>>;

Expand All @@ -64,7 +61,6 @@ class local_map_updater {

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

// maximum number of the local keyframes
Expand Down
8 changes: 4 additions & 4 deletions src/stella_vslam/module/loop_detector.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -424,7 +424,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
const auto inlier_indices = util::resample_by_indices(valid_indices, pnp_solver->get_inlier_flags());

// Set 2D-3D matches for the pose optimization
auto lms_in_cand = std::vector<std::shared_ptr<data::landmark>>(cur_keyfrm_->frm_obs_.num_keypts_, nullptr);
auto lms_in_cand = std::vector<std::shared_ptr<data::landmark>>(cur_keyfrm_->frm_obs_.undist_keypts_.size(), nullptr);
for (const auto idx : inlier_indices) {
// Set only the valid 3D points to the current frame
lms_in_cand.at(idx) = curr_match_lms_observed_in_cand.at(idx);
Expand All @@ -445,7 +445,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
}

// Reject outliers
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; idx++) {
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); idx++) {
if (!outlier_flags.at(idx)) {
continue;
}
Expand Down Expand Up @@ -486,7 +486,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:

// Exclude the already-associated landmarks
std::set<std::shared_ptr<data::landmark>> already_found_landmarks1;
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); ++idx) {
if (!curr_match_lms_observed_in_cand.at(idx)) {
continue;
}
Expand Down Expand Up @@ -518,7 +518,7 @@ bool loop_detector::select_loop_candidate_via_Sim3(const std::unordered_set<std:
}

// Reject outliers
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < cur_keyfrm_->frm_obs_.undist_keypts_.size(); ++idx) {
if (!outlier_flags2.at(idx)) {
continue;
}
Expand Down
10 changes: 5 additions & 5 deletions src/stella_vslam/module/relocalizer.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -223,7 +223,7 @@ bool relocalizer::optimize_pose(data::frame& curr_frm,
}

// Reject outliers
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; idx++) {
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); idx++) {
if (!outlier_flags.at(idx)) {
continue;
}
Expand Down Expand Up @@ -256,7 +256,7 @@ bool relocalizer::refine_pose(data::frame& curr_frm,

// Exclude the already-associated landmarks
std::set<std::shared_ptr<data::landmark>> already_found_landmarks1;
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
const auto& lm = curr_frm.get_landmark(idx);
if (!lm) {
continue;
Expand Down Expand Up @@ -286,7 +286,7 @@ bool relocalizer::refine_pose(data::frame& curr_frm,
}

// Reject outliers
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
if (!outlier_flags2.at(idx)) {
continue;
}
Expand All @@ -300,7 +300,7 @@ bool relocalizer::refine_pose_by_local_map(data::frame& curr_frm,
const std::shared_ptr<stella_vslam::data::keyframe>& candidate_keyfrm) const {
// Create local map
auto local_map_updater = module::local_map_updater(max_num_local_keyfrms_);
if (!local_map_updater.acquire_local_map(curr_frm.get_landmarks(), curr_frm.frm_obs_.num_keypts_)) {
if (!local_map_updater.acquire_local_map(curr_frm.get_landmarks())) {
return false;
}
auto local_keyfrms = local_map_updater.get_local_keyframes();
Expand Down Expand Up @@ -367,7 +367,7 @@ bool relocalizer::refine_pose_by_local_map(data::frame& curr_frm,
curr_frm.set_pose_cw(optimized_pose);

// Reject outliers
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.undist_keypts_.size(); ++idx) {
if (!outlier_flags.at(idx)) {
continue;
}
Expand Down
Loading
Loading