Skip to content

Commit

Permalink
Remove num_keypts from frame_observation
Browse files Browse the repository at this point in the history
  • Loading branch information
ymd-stella committed Jan 28, 2024
1 parent 1dbadcc commit 01b4fa8
Show file tree
Hide file tree
Showing 19 changed files with 49 additions and 60 deletions.
2 changes: 1 addition & 1 deletion src/stella_vslam/data/frame.cc
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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

0 comments on commit 01b4fa8

Please sign in to comment.