From c973991299f6abc91d83071c8afce4d7a919ea1f Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Wed, 26 Jun 2024 13:30:50 -0700 Subject: [PATCH] added separete fcn to load keypoints --- .../include/sparse_mapping/sparse_map.h | 8 ++- localization/sparse_mapping/src/sparse_map.cc | 55 +++++++++++++++++-- 2 files changed, 57 insertions(+), 6 deletions(-) diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index 48256a0012..cefa4b089f 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -95,7 +95,7 @@ struct SparseMap { void SetDefaultLocParams(); // Set loc params - void SetLocParams(const LocalizationParameters& loc_params) { loc_params_ = loc_params; } + void SetLocParams(const LocalizationParameters& loc_params); void SetDetectorParams(int min_features, int max_features, int retries, double min_thresh, double default_thresh, double max_thresh); @@ -200,6 +200,11 @@ struct SparseMap { // needed for localization. void Load(const std::string & protobuf_file, bool localization = false); + // Optionally load keypoints. Call this after loading the map, if for example + // the user needs to use the keypoints for visualization or essential matrix + // estimation and those params differ from the default FLAG values. + void LoadKeypoints(const std::string & protobuf_file); + // construct from pid_to_cid_fid void InitializeCidFidToPid(); @@ -235,6 +240,7 @@ struct SparseMap { camera::CameraParameters camera_params_; mutable sparse_mapping::VocabDB vocab_db_; // TODO(oalexan1): Mutable means someone is doing something wrong. LocalizationParameters loc_params_; + std::string protobuf_file_; // e.g, 10th db image is 3rd image in cid_to_filename_ std::map db_to_cid_map_; diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 080c22fc98..5d26396c78 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -89,7 +89,8 @@ SparseMap::SparseMap(const std::vector& filenames, const std::strin } SparseMap::SparseMap(const std::string& protobuf_file, bool localization) - : camera_params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), Eigen::Vector2d(-1, -1)) { + : camera_params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), Eigen::Vector2d(-1, -1)), + protobuf_file_(protobuf_file) { SetDefaultLocParams(); // The above camera params used bad values because we are expected to reload // later. @@ -248,6 +249,14 @@ void SparseMap::SetDefaultLocParams() { } } +void SparseMap::SetLocParams(const LocalizationParameters& loc_params) { + loc_params_ = loc_params; + // Load keypoints if required since these aren't loaded by default for localization + if (!protobuf_file_.empty() && (loc_params_.check_essential_matrix || loc_params_.visualize_localization_matches)) { + LoadKeypoints(protobuf_file_); + } +} + // Detect features in given images void SparseMap::DetectFeatures() { ff_common::ThreadPool pool; @@ -322,8 +331,8 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { cid_to_filename_.resize(num_frames); cid_to_descriptor_map_.resize(num_frames); - cid_to_keypoint_map_.resize(num_frames); if (!localization) { + cid_to_keypoint_map_.resize(num_frames); cid_to_cam_t_global_.resize(num_frames); } @@ -340,7 +349,8 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { // load keypoints - cid_to_keypoint_map_[cid].resize(Eigen::NoChange_t(), frame.feature_size()); + if (!localization) + cid_to_keypoint_map_[cid].resize(Eigen::NoChange_t(), frame.feature_size()); // Poke the first frame's first descriptor to see how long the // descriptor is. @@ -357,8 +367,8 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { for (int fid = 0; fid < frame.feature_size(); fid++) { sparse_mapping_protobuf::Feature feature = frame.feature(fid); - - cid_to_keypoint_map_[cid].col(fid) << feature.x(), feature.y(); + if (!localization) + cid_to_keypoint_map_[cid].col(fid) << feature.x(), feature.y(); // Copy the descriptors memcpy(cid_to_descriptor_map_[cid].ptr(fid), // Destination @@ -452,6 +462,41 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { close(input_fd); } +void SparseMap::LoadKeypoints(const std::string & protobuf_file) { + sparse_mapping_protobuf::Map map; + int input_fd = open(protobuf_file.c_str(), O_RDONLY); + if (input_fd < 0) + LOG(FATAL) << "Failed to open map file: " << protobuf_file; + + google::protobuf::io::ZeroCopyInputStream* input = + new google::protobuf::io::FileInputStream(input_fd); + if (!ReadProtobufFrom(input, &map)) { + LOG(FATAL) << "Failed to parse map file."; + } + + int num_frames = map.num_frames(); + cid_to_keypoint_map_.resize(num_frames); + + // load each frame + for (int cid = 0; cid < num_frames; cid++) { + sparse_mapping_protobuf::Frame frame; + if (!ReadProtobufFrom(input, &frame)) { + LOG(FATAL) << "Failed to parse frame."; + } + + // load keypoints + cid_to_keypoint_map_[cid].resize(Eigen::NoChange_t(), frame.feature_size()); + + for (int fid = 0; fid < frame.feature_size(); fid++) { + sparse_mapping_protobuf::Feature feature = frame.feature(fid); + cid_to_keypoint_map_[cid].col(fid) << feature.x(), feature.y(); + } + } + + delete input; + close(input_fd); +} + void SparseMap::SetDetectorParams(int min_features, int max_features, int retries, double min_thresh, double default_thresh, double max_thresh) { mutex_detector_.lock();