From 5bd721d3bd420d38d856d79ba4d5bad4a8fa2311 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sat, 29 Jun 2024 10:45:45 -0700 Subject: [PATCH 01/49] updated registration timeout --- astrobee/config/localization/localization_manager.config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/astrobee/config/localization/localization_manager.config b/astrobee/config/localization/localization_manager.config index 7d2c7d9d9f..f909904f06 100644 --- a/astrobee/config/localization/localization_manager.config +++ b/astrobee/config/localization/localization_manager.config @@ -108,7 +108,7 @@ pipelines = { { id = "ml", name = "Sparse map", ekf_input = 0, needs_filter = true, optical_flow = true, timeout = 1.0, max_confidence = 0, enable_topic = "loc/ml/enable", enable_timeout = 20.0, - reg_topic = "loc/ml/registration", reg_timeout = 3.0, + reg_topic = "loc/ml/registration", reg_timeout = 30.0, feat_topic = "loc/ml/features", feat_timeout = 600.0, feat_threshold = 3 }, -- AR Tags { id = "ar", name = "AR Tags", ekf_input = 1, From dd7fb6f0417af92c24feebc908fd0e40f84b41e0 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sat, 29 Jun 2024 16:57:43 -0700 Subject: [PATCH 02/49] updated gl duration --- astrobee/config/localization/graph_localizer.config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/astrobee/config/localization/graph_localizer.config b/astrobee/config/localization/graph_localizer.config index 8e321984ee..e2fea3bc80 100644 --- a/astrobee/config/localization/graph_localizer.config +++ b/astrobee/config/localization/graph_localizer.config @@ -61,7 +61,7 @@ gl_na_pose_starting_prior_translation_stddev = 0.02 gl_na_pose_starting_prior_quaternion_stddev = 0.01 gl_na_pose_huber_k = world_huber_k gl_na_pose_add_priors = true -gl_na_pose_ideal_duration = 4 +gl_na_pose_ideal_duration = 15 gl_na_pose_min_num_states = 3 gl_na_pose_max_num_states = 5 gl_na_pose_model_huber_k = world_huber_k From 8ab2585d77622822fbd8e09edf0cfdf299759102 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 30 Jun 2024 11:25:39 -0700 Subject: [PATCH 03/49] updated brisk params --- astrobee/config/localization.config | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index ab46b78f90..9f33281ce8 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -29,11 +29,11 @@ brisk_num_ransac_iterations = 100 brisk_early_break_landmarks = 100 brisk_histogram_equalization = 1 brisk_check_essential_matrix = false -brisk_essential_ransac_iterations = 2000 +brisk_essential_ransac_iterations = 0 brisk_add_similar_images = false brisk_add_best_previous_image = false brisk_hamming_distance = 90 -brisk_goodness_ratio = 0.8 +brisk_goodness_ratio = 10000 brisk_use_clahe = false brisk_num_extra_localization_db_images = 0 -- Detection Params From a20db256996e1b4c760a0695da66878069229bbf Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 30 Jun 2024 12:31:23 -0700 Subject: [PATCH 04/49] added vl runtime to msg --- communications/ff_msgs/msg/VisualLandmarks.msg | 1 + .../include/localization_node/localization.h | 2 ++ localization/localization_node/src/localization.cc | 4 ++++ 3 files changed, 7 insertions(+) diff --git a/communications/ff_msgs/msg/VisualLandmarks.msg b/communications/ff_msgs/msg/VisualLandmarks.msg index bc385658f4..e5d5eeef00 100644 --- a/communications/ff_msgs/msg/VisualLandmarks.msg +++ b/communications/ff_msgs/msg/VisualLandmarks.msg @@ -22,3 +22,4 @@ Header header # header with timestamp uint32 camera_id # image ID, associated with registration pulse geometry_msgs/Pose pose # estimated camera pose from features ff_msgs/VisualLandmark[] landmarks # list of all landmarks +float32 runtime # Time it took to calculate the pose and matching landmarks diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index d4a0837f45..458c9325a1 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -54,6 +55,7 @@ class Localizer { // Success params for adjusting keypoint thresholds std::deque successes_; ThresholdParams params_; + localization_common::Timer timer_ = localization_common::Timer("VL Runtime"); }; }; // namespace localization_node diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 574fc5c614..7895310acc 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -111,6 +111,7 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa vl->header.stamp = image_ptr->header.stamp; vl->header.frame_id = "world"; + timer_.Start(); map_->DetectFeatures(image_ptr->image, multithreaded, &image_descriptors, image_keypoints); camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), @@ -122,14 +123,17 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa successes_.emplace_back(0); AdjustThresholds(); // LOG(INFO) << "Failed to localize image."; + timer_.Stop(); return false; } successes_.emplace_back(1); AdjustThresholds(); + timer_.Stop(); Eigen::Affine3d global_pose = camera.GetTransform().inverse(); Eigen::Quaterniond quat(global_pose.rotation()); + vl->runtime = timer_.last_value(); vl->pose.position = msg_conversions::eigen_to_ros_point(global_pose.translation()); vl->pose.orientation = msg_conversions::eigen_to_ros_quat(quat); assert(landmarks.size() == observations.size()); From d1a8d7ad8092ee6dfeebf265ddced0336e8a81ab Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 30 Jun 2024 12:36:53 -0700 Subject: [PATCH 05/49] fixed loc nodelet read params bug --- localization/localization_node/src/localization_nodelet.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index bb1a9f9f25..c2db553023 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -127,6 +127,10 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { } void LocalizationNodelet::ReadParams(void) { + if (!config_.ReadFiles()) { + ROS_ERROR("Failed to read config files."); + return; + } if (inst_) inst_->ReadParams(config_); } From 2f42f6d50f3bbea085a18987d7fc15c07a064c11 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Sun, 30 Jun 2024 15:23:40 -0700 Subject: [PATCH 06/49] Release 0.19.1 --- RELEASE.md | 4 ++++ astrobee.doxyfile | 2 +- astrobee/CMakeLists.txt | 2 +- debian/changelog | 6 ++++++ 4 files changed, 12 insertions(+), 2 deletions(-) diff --git a/RELEASE.md b/RELEASE.md index 6ebcf08ba5..caed17a8c7 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,5 +1,9 @@ # Releases +##Release 0.19.1 + + * Updated Map Matching with TEBLID and CLAHE + ## Release 0.19.0 * New split localizer diff --git a/astrobee.doxyfile b/astrobee.doxyfile index c651413fd1..f7c5a449a2 100644 --- a/astrobee.doxyfile +++ b/astrobee.doxyfile @@ -39,7 +39,7 @@ PROJECT_NAME = "NASA Astrobee Robot Software" # control system is used. -PROJECT_NUMBER = 0.19.0 +PROJECT_NUMBER = 0.19.1 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/astrobee/CMakeLists.txt b/astrobee/CMakeLists.txt index b34b6415fd..6247b2329b 100644 --- a/astrobee/CMakeLists.txt +++ b/astrobee/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(astrobee) -set(ASTROBEE_VERSION 0.19.0) +set(ASTROBEE_VERSION 0.19.1) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) diff --git a/debian/changelog b/debian/changelog index 7d0f4825b7..1c8ed36b93 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,9 @@ +astrobee (0.19.1) testing; urgency=medium + + * Updated Map Matching with TEBLID and CLAHE + + -- Astrobee Flight Software Sun, 30 Jun 2024 15:20:46 -0700 + astrobee (0.19.0) testing; urgency=medium * New split localizer From e755e183bdc19e8c382ce8081dd4850e25bccb8d Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Sun, 30 Jun 2024 19:31:08 -0700 Subject: [PATCH 07/49] don't publish haz cam in sim by default --- astrobee/config/simulation/simulation.config | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/astrobee/config/simulation/simulation.config b/astrobee/config/simulation/simulation.config index 51bd34bb26..e881a56ad2 100644 --- a/astrobee/config/simulation/simulation.config +++ b/astrobee/config/simulation/simulation.config @@ -30,5 +30,5 @@ disable_cameras_on_speedup = true; nav_cam_rate = 0.0; dock_cam_rate = 0.0; -perch_cam_rate = 5.0; -haz_cam_rate = 5.0; +perch_cam_rate = 0.0; +haz_cam_rate = 0.0; From aafe4f2467469ad3799d5f8b3b25e78d2d9281b0 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 10:42:36 -0700 Subject: [PATCH 08/49] added scaling when changing threshold ratios for teblid --- localization/interest_point/src/matching.cc | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index 6d9d351cf4..68cc4bac5c 100644 --- a/localization/interest_point/src/matching.cc +++ b/localization/interest_point/src/matching.cc @@ -197,9 +197,6 @@ namespace interest_point { } virtual void DetectImpl(const cv::Mat& image, std::vector* keypoints) { - // std::cout << "detect max thresh: " << max_thresh_ << ", min: " << min_thresh_ << ", def: " << default_thresh_ - // << ", min feat: " << min_features_ << ", max_feat: " << max_features_ << ", dyn thresh: " << dynamic_thresh_ << - // std::endl; brisk_->detect(image, *keypoints); } virtual void ComputeImpl(const cv::Mat& image, std::vector* keypoints, @@ -207,14 +204,28 @@ namespace interest_point { teblid_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - dynamic_thresh_ *= 1.1; + double threshold_ratio = 1.1; + const int keypoint_diff = std::abs(last_keypoint_count_ - min_features_); + // Scale ratio as get close to edge of too few keypoints to avoid undershoot + constexpr double kKeypointDiff = 500; + if (keypoint_diff < kKeypointDiff) { + threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; + } + dynamic_thresh_ *= threshold_ratio; dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ > max_thresh_) dynamic_thresh_ = max_thresh_; brisk_->setThreshold(dynamic_thresh_); } virtual void TooFew(void) { - dynamic_thresh_ *= 0.9; + double threshold_ratio = 0.9; + const int keypoint_diff = std::abs(last_keypoint_count_ - max_features_); + // Scale ratio as get close to edge of too many keypoints to avoid overshoot + constexpr double kKeypointDiff = 1000; + if (keypoint_diff < kKeypointDiff) { + threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; + } + dynamic_thresh_ *= threshold_ratio; dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; From a212bb1387e8546016a4b71e1950c3001ee7eccf Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 13:46:19 -0700 Subject: [PATCH 09/49] added rounding when casting interest point dynamic thresholds --- localization/interest_point/include/interest_point/matching.h | 2 +- localization/interest_point/src/matching.cc | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/interest_point/include/interest_point/matching.h b/localization/interest_point/include/interest_point/matching.h index 21cb10328a..c3ea62ff44 100644 --- a/localization/interest_point/include/interest_point/matching.h +++ b/localization/interest_point/include/interest_point/matching.h @@ -49,7 +49,7 @@ namespace interest_point { int last_keypoint_count(void) { return last_keypoint_count_; } protected: - unsigned int min_features_, max_features_, max_retries_; + int min_features_, max_features_, max_retries_; double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_; int last_keypoint_count_; }; diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index 68cc4bac5c..a4596719cf 100644 --- a/localization/interest_point/src/matching.cc +++ b/localization/interest_point/src/matching.cc @@ -212,7 +212,7 @@ namespace interest_point { threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; } dynamic_thresh_ *= threshold_ratio; - dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility + dynamic_thresh_ = std::lround(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ > max_thresh_) dynamic_thresh_ = max_thresh_; brisk_->setThreshold(dynamic_thresh_); @@ -226,7 +226,7 @@ namespace interest_point { threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; } dynamic_thresh_ *= threshold_ratio; - dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility + dynamic_thresh_ = std::lround(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; brisk_->setThreshold(dynamic_thresh_); From e46cc9ee077d7fee269e5814566bd4f53aec0df7 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 14:22:27 -0700 Subject: [PATCH 10/49] updated teblid default thres --- astrobee/config/localization.config | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 9f33281ce8..60bac22344 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -69,7 +69,7 @@ teblid512_num_extra_localization_db_images = 0 -- Detection Params teblid512_min_threshold = 20.0 -teblid512_default_threshold = 60.0 +teblid512_default_threshold = 72.0 teblid512_max_threshold = 110.0 teblid512_min_features = 1000 teblid512_max_features = 3000 @@ -101,7 +101,7 @@ teblid256_num_extra_localization_db_images = 0 -- Detection Params teblid256_min_threshold = 20.0 -teblid256_default_threshold = 60.0 +teblid256_default_threshold = 72.0 teblid256_max_threshold = 110.0 teblid256_min_features = 1000 teblid256_max_features = 3000 From 78e97db633449b931c5480a2a1c14034db105d50 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 15:58:07 -0700 Subject: [PATCH 11/49] added toomany/toofew ratios as params --- astrobee/config/localization.config | 6 + .../include/interest_point/matching.h | 17 +-- localization/interest_point/src/matching.cc | 107 +++++++++++------- .../localization_node/src/localization.cc | 6 +- .../include/sparse_mapping/sparse_map.h | 4 +- localization/sparse_mapping/src/sparse_map.cc | 7 +- 6 files changed, 90 insertions(+), 57 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 60bac22344..78abbe343e 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -42,6 +42,8 @@ brisk_default_threshold = 90.0 brisk_max_threshold = 110.0 brisk_min_features = 200 brisk_max_features = 800 +brisk_too_many_ratio = 1.25 +brisk_too_few_ratio = 0.8 brisk_detection_retries = 1 -- Localizer Threshold Params brisk_success_history_size = 10 @@ -73,6 +75,8 @@ teblid512_default_threshold = 72.0 teblid512_max_threshold = 110.0 teblid512_min_features = 1000 teblid512_max_features = 3000 +teblid512_too_many_ratio = 1.05 +teblid512_too_few_ratio = 0.95 teblid512_detection_retries = 1 -- Localizer Threshold Params @@ -105,6 +109,8 @@ teblid256_default_threshold = 72.0 teblid256_max_threshold = 110.0 teblid256_min_features = 1000 teblid256_max_features = 3000 +teblid256_too_many_ratio = 1.05 +teblid256_too_few_ratio = 0.95 teblid256_detection_retries = 1 -- Localizer Threshold Params diff --git a/localization/interest_point/include/interest_point/matching.h b/localization/interest_point/include/interest_point/matching.h index c3ea62ff44..4c58f47746 100644 --- a/localization/interest_point/include/interest_point/matching.h +++ b/localization/interest_point/include/interest_point/matching.h @@ -31,7 +31,8 @@ namespace interest_point { class DynamicDetector { public: DynamicDetector(int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh); + double min_thresh, double default_thresh, double max_thresh, + double too_many_ratio, double too_few_ratio); virtual ~DynamicDetector(void) {} void Detect(const cv::Mat& image, std::vector* keypoints, @@ -50,7 +51,7 @@ namespace interest_point { protected: int min_features_, max_features_, max_retries_; - double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_; + double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_, too_few_ratio_, too_many_ratio_; int last_keypoint_count_; }; @@ -65,14 +66,14 @@ namespace interest_point { public: // Here on purpose invalid values are set, so the user explicitly sets them. - FeatureDetector(std::string const& detector_name = "SURF", - int min_features = 0, int max_features = 0, int retries = 0, - double min_thresh = 0, double default_thresh = 0, double max_thresh = 0); + FeatureDetector(std::string const& detector_name = "SURF", int min_features = 0, int max_features = 0, + int retries = 0, double min_thresh = 0, double default_thresh = 0, double max_thresh = 0, + double too_many_ratio = 0, double too_few_ratio = 0); ~FeatureDetector(void); - void Reset(std::string const& detector_name, - int min_features = 0, int max_features = 0, int retries = 0, - double min_thresh = 0, double default_thresh = 0, double max_thresh = 0); + void Reset(std::string const& detector_name, int min_features = 0, int max_features = 0, int retries = 0, + double min_thresh = 0, double default_thresh = 0, double max_thresh = 0, double too_many_ratio = 0, + double too_few_ratio = 0); void Detect(const cv::Mat& image, std::vector* keypoints, cv::Mat* keypoints_description); diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index a4596719cf..4a5ee713b0 100644 --- a/localization/interest_point/src/matching.cc +++ b/localization/interest_point/src/matching.cc @@ -55,6 +55,11 @@ DEFINE_double(default_surf_threshold, 10, "Default threshold for feature detection using SURF."); DEFINE_double(max_surf_threshold, 1000, "Maximum threshold for feature detection using SURF."); +DEFINE_double(surf_too_many_ratio, 1.1, + "Ratio to increase the dynamic feature threshold by if too many features are detected."); +DEFINE_double(surf_too_few_ratio, 0.9, + "Ratio to reduce the dynamic feature threshold by if too few features are detected."); + // Binary detector DEFINE_int32(min_brisk_features, 1000, "Minimum number of features to be computed using ORGBRISK."); @@ -66,20 +71,36 @@ DEFINE_double(default_brisk_threshold, 20, "Default threshold for feature detection using ORGBRISK."); DEFINE_double(max_brisk_threshold, 110, "Maximum threshold for feature detection using ORGBRISK."); +DEFINE_double(brisk_too_many_ratio, 1.25, + "Ratio to increase the dynamic feature threshold by if too many features are detected."); +DEFINE_double(brisk_too_few_ratio, 0.8, + "Ratio to reduce the dynamic feature threshold by if too few features are detected."); -namespace interest_point { - DynamicDetector::DynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh): - min_features_(min_features), max_features_(max_features), max_retries_(max_retries), - min_thresh_(min_thresh), default_thresh_(default_thresh), max_thresh_(max_thresh), - dynamic_thresh_(default_thresh), last_keypoint_count_(0) {} - void DynamicDetector::GetDetectorParams(int & min_features, int & max_features, int & max_retries, - double & min_thresh, double & default_thresh, - double & max_thresh) { - min_features = min_features_; max_features = max_features_; max_retries = max_retries_; - min_thresh = min_thresh_; default_thresh = default_thresh_; max_thresh = max_thresh_; +namespace interest_point { + +DynamicDetector::DynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, + double default_thresh, double max_thresh, double too_many_ratio, double too_few_ratio) + : min_features_(min_features), + max_features_(max_features), + max_retries_(max_retries), + min_thresh_(min_thresh), + default_thresh_(default_thresh), + max_thresh_(max_thresh), + dynamic_thresh_(default_thresh), + too_many_ratio_(too_many_ratio), + too_few_ratio_(too_few_ratio), + last_keypoint_count_(0) {} + +void DynamicDetector::GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, + double& default_thresh, double& max_thresh) { + min_features = min_features_; + max_features = max_features_; + max_retries = max_retries_; + min_thresh = min_thresh_; + default_thresh = default_thresh_; + max_thresh = max_thresh_; } void DynamicDetector::Detect(const cv::Mat& image, @@ -106,10 +127,10 @@ namespace interest_point { class BriskDynamicDetector : public DynamicDetector { public: - BriskDynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh) - : DynamicDetector(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh) { + BriskDynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio) + : DynamicDetector(min_features, max_features, max_retries, min_thresh, default_thresh, max_thresh, + too_many_ratio, too_few_ratio) { Reset(); } @@ -126,14 +147,14 @@ namespace interest_point { brisk_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - dynamic_thresh_ *= 1.25; + dynamic_thresh_ *= too_many_ratio_; dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ > max_thresh_) dynamic_thresh_ = max_thresh_; brisk_->setThreshold(dynamic_thresh_); } virtual void TooFew(void) { - dynamic_thresh_ *= 0.8; + dynamic_thresh_ *= too_few_ratio_; dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; @@ -146,10 +167,10 @@ namespace interest_point { class SurfDynamicDetector : public DynamicDetector { public: - SurfDynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh) - : DynamicDetector(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh) { + SurfDynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio) + : DynamicDetector(min_features, max_features, max_retries, min_thresh, default_thresh, max_thresh, + too_many_ratio, too_few_ratio) { surf_ = cv::xfeatures2d::SURF::create(dynamic_thresh_); } @@ -161,13 +182,13 @@ namespace interest_point { surf_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - dynamic_thresh_ *= 1.1; + dynamic_thresh_ *= too_many_ratio_; if (dynamic_thresh_ > max_thresh_) dynamic_thresh_ = max_thresh_; surf_->setHessianThreshold(static_cast(dynamic_thresh_)); } virtual void TooFew(void) { - dynamic_thresh_ *= 0.9; + dynamic_thresh_ *= too_few_ratio_; if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; surf_->setHessianThreshold(static_cast(dynamic_thresh_)); @@ -179,10 +200,10 @@ namespace interest_point { class TeblidDynamicDetector : public DynamicDetector { public: - TeblidDynamicDetector(int min_features, int max_features, int max_retries, - double min_thresh, double default_thresh, double max_thresh, bool use_512 = true) - : DynamicDetector(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh) { + TeblidDynamicDetector(int min_features, int max_features, int max_retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio, bool use_512 = true) + : DynamicDetector(min_features, max_features, max_retries, min_thresh, default_thresh, max_thresh, + too_many_ratio, too_few_ratio) { if (use_512) { teblid_ = upm::BAD::create(5.0, upm::BAD::SIZE_512_BITS); } else { @@ -204,7 +225,7 @@ namespace interest_point { teblid_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - double threshold_ratio = 1.1; + double threshold_ratio = too_many_ratio_; const int keypoint_diff = std::abs(last_keypoint_count_ - min_features_); // Scale ratio as get close to edge of too few keypoints to avoid undershoot constexpr double kKeypointDiff = 500; @@ -218,7 +239,7 @@ namespace interest_point { brisk_->setThreshold(dynamic_thresh_); } virtual void TooFew(void) { - double threshold_ratio = 0.9; + double threshold_ratio = too_few_ratio_; const int keypoint_diff = std::abs(last_keypoint_count_ - max_features_); // Scale ratio as get close to edge of too many keypoints to avoid overshoot constexpr double kKeypointDiff = 1000; @@ -237,14 +258,12 @@ namespace interest_point { cv::Ptr brisk_; }; - - - FeatureDetector::FeatureDetector(std::string const& detector_name, - int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh) { + FeatureDetector::FeatureDetector(std::string const& detector_name, int min_features, int max_features, int retries, + double min_thresh, double default_thresh, double max_thresh, double too_many_ratio, + double too_few_ratio) { detector_ = NULL; Reset(detector_name, min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); } void FeatureDetector::GetDetectorParams(int & min_features, int & max_features, int & max_retries, @@ -263,9 +282,9 @@ namespace interest_point { } } - void FeatureDetector::Reset(std::string const& detector_name, - int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh) { + void FeatureDetector::Reset(std::string const& detector_name, int min_features, int max_features, int retries, + double min_thresh, double default_thresh, double max_thresh, double too_many_ratio, + double too_few_ratio) { detector_name_ = detector_name; if (detector_ != NULL) { @@ -282,6 +301,8 @@ namespace interest_point { min_thresh = FLAGS_min_surf_threshold; default_thresh = FLAGS_default_surf_threshold; max_thresh = FLAGS_max_surf_threshold; + too_many_ratio = FLAGS_surf_too_many_ratio; + too_few_ratio = FLAGS_surf_too_few_ratio; } else if (detector_name == "ORGBRISK" || detector_name == "TEBLID512" || detector_name == "TEBLID256") { min_features = FLAGS_min_brisk_features; @@ -290,6 +311,8 @@ namespace interest_point { min_thresh = FLAGS_min_brisk_threshold; default_thresh = FLAGS_default_brisk_threshold; max_thresh = FLAGS_max_brisk_threshold; + too_many_ratio = FLAGS_brisk_too_many_ratio; + too_few_ratio = FLAGS_brisk_too_few_ratio; } else { LOG(FATAL) << "Unimplemented feature detector: " << detector_name; } @@ -298,16 +321,16 @@ namespace interest_point { // Loading the detector if (detector_name == "ORGBRISK") detector_ = new BriskDynamicDetector(min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); else if (detector_name == "SURF") detector_ = new SurfDynamicDetector(min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); else if (detector_name == "TEBLID512") detector_ = new TeblidDynamicDetector(min_features, max_features, retries, - min_thresh, default_thresh, max_thresh, true); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio, true); else if (detector_name == "TEBLID256") detector_ = new TeblidDynamicDetector(min_features, max_features, retries, - min_thresh, default_thresh, max_thresh, false); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio, false); else LOG(FATAL) << "Unimplemented feature detector: " << detector_name; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 7895310acc..455945fd43 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -63,7 +63,7 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { LOAD_PARAM(loc_params.visualize_localization_matches, config, ""); // Detector Params - double min_threshold, default_threshold, max_threshold, goodness_ratio; + double min_threshold, default_threshold, max_threshold, goodness_ratio, too_many_ratio, too_few_ratio; int min_features, max_features, detection_retries; LOAD_PARAM(min_threshold, config, prefix); LOAD_PARAM(default_threshold, config, prefix); @@ -71,6 +71,8 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { LOAD_PARAM(detection_retries, config, prefix); LOAD_PARAM(min_features, config, prefix); LOAD_PARAM(max_features, config, prefix); + LOAD_PARAM(too_many_ratio, config, prefix); + LOAD_PARAM(too_few_ratio, config, prefix); // Localizer threshold params LOAD_PARAM(params_.success_history_size, config, prefix); @@ -94,7 +96,7 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { map_->SetCameraParameters(cam_params); map_->SetLocParams(loc_params); map_->SetDetectorParams(min_features, max_features, detection_retries, - min_threshold, default_threshold, max_threshold); + min_threshold, default_threshold, max_threshold, too_many_ratio, too_few_ratio); } bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index c33d588d9e..370e95dbe5 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -99,8 +99,8 @@ struct SparseMap { const LocalizationParameters& loc_params() const { return loc_params_; } LocalizationParameters& loc_params() { return loc_params_; } - void SetDetectorParams(int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh); + void SetDetectorParams(int min_features, int max_features, int retries, double min_thresh, double default_thresh, + double max_thresh, double too_many_ratio, double too_few_ratio); /** * Detect features in given images diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 1325929338..045900b675 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -506,11 +506,12 @@ void SparseMap::LoadKeypoints(const std::string & protobuf_file) { close(input_fd); } -void SparseMap::SetDetectorParams(int min_features, int max_features, int retries, - double min_thresh, double default_thresh, double max_thresh) { +void SparseMap::SetDetectorParams(int min_features, int max_features, int retries, double min_thresh, + double default_thresh, double max_thresh, double too_many_ratio, + double too_few_ratio) { mutex_detector_.lock(); detector_.Reset(detector_.GetDetectorName(), min_features, max_features, retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); mutex_detector_.unlock(); } From 438c8252b998ac651198f32031faeffd5463aa64 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 16:07:03 -0700 Subject: [PATCH 12/49] adding adjusting num similar images --- astrobee/config/localization.config | 18 ++++++------- .../localization_node/src/localization.cc | 26 ++++++++----------- 2 files changed, 20 insertions(+), 24 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 78abbe343e..1c30442f45 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -49,9 +49,9 @@ brisk_detection_retries = 1 brisk_success_history_size = 10 brisk_min_success_rate = 0 brisk_max_success_rate = 1 -brisk_adjust_hamming_distance = false -brisk_min_hamming_distance = 90 -brisk_max_hamming_distance = 90 +brisk_adjust_num_similar = false +brisk_min_num_similar = 20 +brisk_max_num_similar = 20 -- TEBLID512 teblid512_num_similar = 20 @@ -83,9 +83,9 @@ teblid512_detection_retries = 1 teblid512_success_history_size = 10 teblid512_min_success_rate = 0.3 teblid512_max_success_rate = 0.9 -teblid512_adjust_hamming_distance = false -teblid512_min_hamming_distance = 85 -teblid512_max_hamming_distance = 85 +teblid512_adjust_num_similar = true +teblid512_min_num_similar = 15 +teblid512_max_num_similar = 20 -- TEBLID256 teblid256_num_similar = 20 @@ -117,6 +117,6 @@ teblid256_detection_retries = 1 teblid256_success_history_size = 10 teblid256_min_success_rate = 0.3 teblid256_max_success_rate = 0.9 -teblid256_adjust_hamming_distance = false -teblid256_min_hamming_distance = 85 -teblid256_max_hamming_distance = 85 +teblid256_adjust_num_similar = true +teblid256_min_num_similar = 15 +teblid256_max_num_similar = 20 diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 455945fd43..35e28fe9a0 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -80,9 +80,9 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { LOAD_PARAM(params_.max_success_rate, config, prefix); LOAD_PARAM(params_.min_features, config, prefix); LOAD_PARAM(params_.max_features, config, prefix); - LOAD_PARAM(params_.adjust_hamming_distance, config, prefix); - LOAD_PARAM(params_.min_hamming_distance, config, prefix); - LOAD_PARAM(params_.max_hamming_distance, config, prefix); + LOAD_PARAM(params_.adjust_num_similar, config, prefix); + LOAD_PARAM(params_.min_num_similar, config, prefix); + LOAD_PARAM(params_.max_num_similar, config, prefix); // This check must happen before the histogram_equalization flag is set into the map // to compare with what is there already. @@ -165,23 +165,19 @@ void Localizer::AdjustThresholds() { if (average < params_.min_success_rate) { if (last_keypoint_count < params_.max_features) { map_->detector().dynamic_detector().TooFew(); - } else if (params_.adjust_hamming_distance) { - const int current_hamming_distance = map_->loc_params().hamming_distance; - const int new_hamming_distance = current_hamming_distance + 3; - if (new_hamming_distance <= params_.max_hamming_distance) { - map_->loc_params().hamming_distance = new_hamming_distance; - } + } else if (params_.adjust_num_similar) { + const int current_num_similar = map_->loc_params().num_similar; + const int new_num_similar = std::min(current_num_similar + 1, params_.max_num_similar); + map_->loc_params().num_similar = new_num_similar; } } if (average > params_.max_success_rate) { if (last_keypoint_count > params_.min_features) { map_->detector().dynamic_detector().TooMany(); - } else if (params_.adjust_hamming_distance) { - const int current_hamming_distance = map_->loc_params().hamming_distance; - const int new_hamming_distance = current_hamming_distance - 3; - if (new_hamming_distance >= params_.min_hamming_distance) { - map_->loc_params().hamming_distance = new_hamming_distance; - } + } else if (params_.adjust_num_similar) { + const int current_num_similar = map_->loc_params().num_similar; + const int new_num_similar = std::max(current_num_similar - 1, params_.min_num_similar); + map_->loc_params().num_similar = new_num_similar; } } } From f22f1fe404dbfdea0ed2da9feee5b17dcaa4a050 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 16:28:50 -0700 Subject: [PATCH 13/49] fixed loc header --- .../include/localization_node/localization.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index 458c9325a1..7a22665ad3 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -37,9 +37,9 @@ struct ThresholdParams { double max_success_rate; int min_features; int max_features; - bool adjust_hamming_distance; - int min_hamming_distance; - int max_hamming_distance; + bool adjust_num_similar; + int min_num_similar; + int max_num_similar; }; class Localizer { From 5f57b478159b29ea8f2b6464bb416ddb79d9aabd Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 17:02:02 -0700 Subject: [PATCH 14/49] remove else for adjust num similar --- localization/localization_node/src/localization.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 35e28fe9a0..e0abf207db 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -165,7 +165,8 @@ void Localizer::AdjustThresholds() { if (average < params_.min_success_rate) { if (last_keypoint_count < params_.max_features) { map_->detector().dynamic_detector().TooFew(); - } else if (params_.adjust_num_similar) { + } + if (params_.adjust_num_similar) { const int current_num_similar = map_->loc_params().num_similar; const int new_num_similar = std::min(current_num_similar + 1, params_.max_num_similar); map_->loc_params().num_similar = new_num_similar; @@ -174,7 +175,8 @@ void Localizer::AdjustThresholds() { if (average > params_.max_success_rate) { if (last_keypoint_count > params_.min_features) { map_->detector().dynamic_detector().TooMany(); - } else if (params_.adjust_num_similar) { + } + if (params_.adjust_num_similar) { const int current_num_similar = map_->loc_params().num_similar; const int new_num_similar = std::max(current_num_similar - 1, params_.min_num_similar); map_->loc_params().num_similar = new_num_similar; From 9604467d8c93a68e56630cf132044cd0f0d81d95 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 1 Jul 2024 17:52:15 -0700 Subject: [PATCH 15/49] visual landmarks adding field bmr rule --- .../2023_07_01_VisualLandmarks_add_field.bmr | 205 ++++++++++++++++++ 1 file changed, 205 insertions(+) create mode 100644 communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr diff --git a/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr b/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr new file mode 100644 index 0000000000..14dfb89dee --- /dev/null +++ b/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr @@ -0,0 +1,205 @@ +class update_ff_msgs_VisualLandmarks_bc9fc09f69d7758225ed41ee26f6375e(MessageUpdateRule): + old_type = "ff_msgs/VisualLandmarks" + old_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# The observation from a camera image, of image coordinates +# and associated 3D coordinates. Used for sparse mapping and AR tags. + +Header header # header with timestamp +uint32 camera_id # image ID, associated with registration pulse +geometry_msgs/Pose pose # estimated camera pose from features +ff_msgs/VisualLandmark[] landmarks # list of all landmarks + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: ff_msgs/VisualLandmark +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# A single observation of a feature, with an image coordinate +# and known 3D coordinate + +float32 x # known 3D position x +float32 y # known 3D position y +float32 z # known 3D position z +float32 u # feature image coordinate x +float32 v # feature image coordinate y +""" + + new_type = "ff_msgs/VisualLandmarks" + new_full_text = """ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# The observation from a camera image, of image coordinates +# and associated 3D coordinates. Used for sparse mapping and AR tags. + +Header header # header with timestamp +uint32 camera_id # image ID, associated with registration pulse +geometry_msgs/Pose pose # estimated camera pose from features +ff_msgs/VisualLandmark[] landmarks # list of all landmarks +float32 runtime # Time it took to calculate the pose and matching landmarks + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') +# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +string frame_id + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of position and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: ff_msgs/VisualLandmark +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +# A single observation of a feature, with an image coordinate +# and known 3D coordinate + +float32 x # known 3D position x +float32 y # known 3D position y +float32 z # known 3D position z +float32 u # feature image coordinate x +float32 v # feature image coordinate y +""" + + order = 0 + migrated_types = [ + ("Header","Header"), + ("geometry_msgs/Pose","geometry_msgs/Pose"), + ("VisualLandmark","VisualLandmark"),] + + valid = False + + def update(self, old_msg, new_msg): + self.migrate(old_msg.header, new_msg.header) + new_msg.camera_id = old_msg.camera_id + self.migrate(old_msg.pose, new_msg.pose) + self.migrate_array(old_msg.landmarks, new_msg.landmarks, "ff_msgs/VisualLandmark") + #No matching field name in old message + new_msg.runtime = 0. From 508d85ba4f90649c606760bb0c636ff1ab8c573b Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 18:16:26 -0700 Subject: [PATCH 16/49] updated loc min success rate config --- astrobee/config/localization.config | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 1c30442f45..9348c84e2d 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -81,7 +81,7 @@ teblid512_detection_retries = 1 -- Localizer Threshold Params teblid512_success_history_size = 10 -teblid512_min_success_rate = 0.3 +teblid512_min_success_rate = 0.5 teblid512_max_success_rate = 0.9 teblid512_adjust_num_similar = true teblid512_min_num_similar = 15 @@ -115,7 +115,7 @@ teblid256_detection_retries = 1 -- Localizer Threshold Params teblid256_success_history_size = 10 -teblid256_min_success_rate = 0.3 +teblid256_min_success_rate = 0.5 teblid256_max_success_rate = 0.9 teblid256_adjust_num_similar = true teblid256_min_num_similar = 15 From 7691d1f5aa3209e34fcde312b4c851059bca8d1e Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 1 Jul 2024 21:31:19 -0700 Subject: [PATCH 17/49] set valid to true --- .../ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr b/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr index 14dfb89dee..ab9d8c43b9 100644 --- a/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr +++ b/communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr @@ -194,7 +194,7 @@ float32 v # feature image coordinate y ("geometry_msgs/Pose","geometry_msgs/Pose"), ("VisualLandmark","VisualLandmark"),] - valid = False + valid = True def update(self, old_msg, new_msg): self.migrate(old_msg.header, new_msg.header) From 017aa42420405067913fd6441002b3545581fc54 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Mon, 1 Jul 2024 23:17:01 -0700 Subject: [PATCH 18/49] localizer parameter in test changed --- .../test/test_graph_localizer_parameter_reader.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc b/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc index c3c050e603..5390a89d55 100644 --- a/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc +++ b/localization/parameter_reader/test/test_graph_localizer_parameter_reader.cc @@ -89,7 +89,7 @@ TEST_F(GraphLocalizerParameterReaderTest, PoseNodeAdderParams) { EXPECT_MATRIX_NEAR(na::Covariance(pose_noise), na::Covariance(params.start_noise_models[0]), 1e-6); EXPECT_NEAR(params.huber_k, 1.345, 1e-6); EXPECT_EQ(params.add_priors, true); - EXPECT_NEAR(params.ideal_duration, 4, 1e-6); + EXPECT_NEAR(params.ideal_duration, 15, 1e-6); EXPECT_EQ(params.min_num_states, 3); EXPECT_EQ(params.max_num_states, 5); } From ccbc4bc6c3bfd39062ff949fd8c724d9d0dde804 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 2 Jul 2024 14:31:09 -0700 Subject: [PATCH 19/49] fixing release spaces and # --- RELEASE.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/RELEASE.md b/RELEASE.md index caed17a8c7..28addb61ad 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,6 +1,6 @@ # Releases -##Release 0.19.1 +## Release 0.19.1 * Updated Map Matching with TEBLID and CLAHE @@ -12,23 +12,23 @@ * Astrobee now using OpenCV 4! Please see install updates in INSTALL.md -# Release 0.17.3 +## Release 0.17.3 * Comms bridge added rate feature * Bug fixes * User feedback improvements -# Release 0.17.2 +## Release 0.17.2 * Adding Generic comms bridge * Multiple bug fixes * Ubuntu 20 only supported now -# Release 0.17.1 +## Release 0.17.1 * Multiple bug fixes -# Release 0.17.0 +## Release 0.17.0 * Full compatibility and debians built for Ubuntu 20 * Deleted all simulink autocode for ctl and fam From 07e874a0e2e6a6217ab40a46e049253e2aaf8e2d Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 2 Jul 2024 16:14:03 -0700 Subject: [PATCH 20/49] fixed recording toomany and toofew ratios during mapping --- .../include/interest_point/matching.h | 8 ++++---- localization/interest_point/src/matching.cc | 13 ++++++++----- localization/sparse_mapping/src/sparse_map.cc | 10 +++++----- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/localization/interest_point/include/interest_point/matching.h b/localization/interest_point/include/interest_point/matching.h index 4c58f47746..aeb38dbf80 100644 --- a/localization/interest_point/include/interest_point/matching.h +++ b/localization/interest_point/include/interest_point/matching.h @@ -44,8 +44,8 @@ namespace interest_point { cv::Mat* keypoints_description) = 0; virtual void TooFew(void) = 0; virtual void TooMany(void) = 0; - void GetDetectorParams(int & min_features, int & max_features, int & max_retries, - double & min_thresh, double & default_thresh, double & max_thresh); + void GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, + double& default_thresh, double& max_thresh, double& too_many_ratio, double& too_few_ratio); int last_keypoint_count(void) { return last_keypoint_count_; } @@ -80,8 +80,8 @@ namespace interest_point { std::string GetDetectorName() const {return detector_name_;} - void GetDetectorParams(int & min_features, int & max_features, int & max_retries, - double & min_thresh, double & default_thresh, double & max_thresh); + void GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, + double& default_thresh, double& max_thresh, double& too_many_ratio, double& too_few_ratio); friend bool operator== (FeatureDetector const& A, FeatureDetector const& B) { return (A.detector_name_ == B.detector_name_); diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index 4a5ee713b0..4e3d3234bb 100644 --- a/localization/interest_point/src/matching.cc +++ b/localization/interest_point/src/matching.cc @@ -67,7 +67,7 @@ DEFINE_int32(max_brisk_features, 5000, "Maximum number of features to be computed using ORGBRISK."); DEFINE_double(min_brisk_threshold, 10, "Minimum threshold for feature detection using ORGBRISK."); -DEFINE_double(default_brisk_threshold, 20, +DEFINE_double(default_brisk_threshold, 90, "Default threshold for feature detection using ORGBRISK."); DEFINE_double(max_brisk_threshold, 110, "Maximum threshold for feature detection using ORGBRISK."); @@ -94,14 +94,17 @@ DynamicDetector::DynamicDetector(int min_features, int max_features, int max_ret last_keypoint_count_(0) {} void DynamicDetector::GetDetectorParams(int& min_features, int& max_features, int& max_retries, double& min_thresh, - double& default_thresh, double& max_thresh) { + double& default_thresh, double& max_thresh, double& too_many_ratio, + double& too_few_ratio) { min_features = min_features_; max_features = max_features_; max_retries = max_retries_; min_thresh = min_thresh_; default_thresh = default_thresh_; max_thresh = max_thresh_; - } + too_many_ratio = too_many_ratio_; + too_few_ratio = too_few_ratio_; +} void DynamicDetector::Detect(const cv::Mat& image, std::vector* keypoints, @@ -268,11 +271,11 @@ void DynamicDetector::GetDetectorParams(int& min_features, int& max_features, in void FeatureDetector::GetDetectorParams(int & min_features, int & max_features, int & max_retries, double & min_thresh, double & default_thresh, - double & max_thresh) { + double & max_thresh, double & too_many_ratio, double & too_few_ratio) { if (detector_ == NULL) LOG(FATAL) << "The detector was not set."; detector_->GetDetectorParams(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); } FeatureDetector::~FeatureDetector(void) { diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 045900b675..d6c758d17e 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -711,12 +711,12 @@ void SparseMap::DetectFeatures(const cv::Mat& image, // map-building, rather than in localization which is more // performance-sensitive. int min_features, max_features, max_retries; - double min_thresh, default_thresh, max_thresh; + double min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio; detector_.GetDetectorParams(min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh); - interest_point::FeatureDetector local_detector(detector_.GetDetectorName(), - min_features, max_features, max_retries, - min_thresh, default_thresh, max_thresh); + min_thresh, default_thresh, max_thresh, too_many_ratio, too_few_ratio); + interest_point::FeatureDetector local_detector(detector_.GetDetectorName(), min_features, max_features, max_retries, + min_thresh, default_thresh, max_thresh, too_many_ratio, + too_few_ratio); local_detector.Detect(*image_ptr, &storage, descriptors); } mutex_detector_.unlock(); From 4a29e631b79ed9ec74451c2532b0cfa70c5feffd Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 2 Jul 2024 16:17:55 -0700 Subject: [PATCH 21/49] avoid adjusting thresholds if success history size is 0 --- astrobee/config/localization.config | 2 +- localization/localization_node/src/localization.cc | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 9348c84e2d..7bfe466c07 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -46,7 +46,7 @@ brisk_too_many_ratio = 1.25 brisk_too_few_ratio = 0.8 brisk_detection_retries = 1 -- Localizer Threshold Params -brisk_success_history_size = 10 +brisk_success_history_size = 0 brisk_min_success_rate = 0 brisk_max_success_rate = 1 brisk_adjust_num_similar = false diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index e0abf207db..a4eba47b28 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -155,6 +155,7 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa } void Localizer::AdjustThresholds() { + if (successes_.size() == 0 || params_.success_history_size == 0) return; if (successes_.size() < params_.success_history_size) return; while (successes_.size() > params_.success_history_size) { successes_.pop_front(); From 0d6c470d74d74fd1b1c34f67569d4983e2381b26 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Wed, 3 Jul 2024 13:45:15 -0700 Subject: [PATCH 22/49] Removed essential matrix check and adjusted hamming distances --- astrobee/config/localization.config | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 7bfe466c07..33611f79fc 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -60,11 +60,11 @@ teblid512_ransac_inlier_tolerance = 3 teblid512_num_ransac_iterations = 1000 teblid512_early_break_landmarks = 100 teblid512_histogram_equalization = 3 -teblid512_check_essential_matrix = true +teblid512_check_essential_matrix = false teblid512_essential_ransac_iterations = 2000 teblid512_add_similar_images = true teblid512_add_best_previous_image = true -teblid512_hamming_distance = 85 +teblid512_hamming_distance = 90 teblid512_goodness_ratio = 0.8 teblid512_use_clahe = true teblid512_num_extra_localization_db_images = 0 @@ -94,11 +94,11 @@ teblid256_ransac_inlier_tolerance = 3 teblid256_num_ransac_iterations = 1000 teblid256_early_break_landmarks = 100 teblid256_histogram_equalization = 3 -teblid256_check_essential_matrix = true +teblid256_check_essential_matrix = false teblid256_essential_ransac_iterations = 2000 teblid256_add_similar_images = true teblid256_add_best_previous_image = true -teblid256_hamming_distance = 85 +teblid256_hamming_distance = 90 teblid256_goodness_ratio = 0.8 teblid256_use_clahe = true teblid256_num_extra_localization_db_images = 0 From 339046b1b0bddcc799b05c0583ada6c3b71fc5d1 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Thu, 4 Jul 2024 00:22:36 -0700 Subject: [PATCH 23/49] updated loc configs --- astrobee/config/localization.config | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 33611f79fc..e5ef8e651a 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -54,7 +54,7 @@ brisk_min_num_similar = 20 brisk_max_num_similar = 20 -- TEBLID512 -teblid512_num_similar = 20 +teblid512_num_similar = 18 teblid512_min_query_score_ratio = 0.45 teblid512_ransac_inlier_tolerance = 3 teblid512_num_ransac_iterations = 1000 @@ -70,25 +70,25 @@ teblid512_use_clahe = true teblid512_num_extra_localization_db_images = 0 -- Detection Params -teblid512_min_threshold = 20.0 +teblid512_min_threshold = 30.0 teblid512_default_threshold = 72.0 teblid512_max_threshold = 110.0 teblid512_min_features = 1000 -teblid512_max_features = 3000 -teblid512_too_many_ratio = 1.05 +teblid512_max_features = 2750 +teblid512_too_many_ratio = 1.07 teblid512_too_few_ratio = 0.95 teblid512_detection_retries = 1 -- Localizer Threshold Params teblid512_success_history_size = 10 -teblid512_min_success_rate = 0.5 +teblid512_min_success_rate = 0.4 teblid512_max_success_rate = 0.9 teblid512_adjust_num_similar = true teblid512_min_num_similar = 15 -teblid512_max_num_similar = 20 +teblid512_max_num_similar = 18 -- TEBLID256 -teblid256_num_similar = 20 +teblid256_num_similar = 18 teblid256_min_query_score_ratio = 0.45 teblid256_ransac_inlier_tolerance = 3 teblid256_num_ransac_iterations = 1000 @@ -104,19 +104,19 @@ teblid256_use_clahe = true teblid256_num_extra_localization_db_images = 0 -- Detection Params -teblid256_min_threshold = 20.0 +teblid256_min_threshold = 30.0 teblid256_default_threshold = 72.0 teblid256_max_threshold = 110.0 teblid256_min_features = 1000 -teblid256_max_features = 3000 -teblid256_too_many_ratio = 1.05 +teblid256_max_features = 2750 +teblid256_too_many_ratio = 1.07 teblid256_too_few_ratio = 0.95 teblid256_detection_retries = 1 -- Localizer Threshold Params teblid256_success_history_size = 10 -teblid256_min_success_rate = 0.5 +teblid256_min_success_rate = 0.4 teblid256_max_success_rate = 0.9 teblid256_adjust_num_similar = true teblid256_min_num_similar = 15 -teblid256_max_num_similar = 20 +teblid256_max_num_similar = 18 From fd1d44c1008624e63d844b489ca05ad5e76f13e3 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Thu, 4 Jul 2024 00:25:56 -0700 Subject: [PATCH 24/49] Avoid storing two maps in memory at once --- localization/localization_node/src/localization_nodelet.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index c2db553023..afebbd8c46 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -59,7 +59,9 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) { while (processing_image_) { usleep(100000); } - map_.reset(new sparse_mapping::SparseMap(map_file, true)); + // Reset map before creating new one to avoid storing two maps in memory at the same time + map_.reset(); + map_ = std::make_shared(map_file, true); inst_.reset(new Localizer(map_.get())); // Check to see if any params were changed when map was reset ReadParams(); @@ -83,7 +85,8 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { // Reset all internal shared pointers it_.reset(new image_transport::ImageTransport(*nh)); - map_.reset(new sparse_mapping::SparseMap(map_file, true)); + map_.reset(); + map_ = std::make_shared(map_file, true); inst_.reset(new Localizer(map_.get())); registration_publisher_ = nh->advertise( From 10693cb0ff16351c78853c53c4054d96580dd757 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 8 Jul 2024 15:50:28 -0700 Subject: [PATCH 25/49] added check for feature descriptor during a map switch --- .../localization_node/src/localization_nodelet.cc | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index afebbd8c46..12ce4a4675 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -63,6 +63,16 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) { map_.reset(); map_ = std::make_shared(map_file, true); inst_.reset(new Localizer(map_.get())); + // Sanity check that map contains a valid detector type + const auto detector_name = map_->GetDetectorName(); + if (detector_name != "ORGBRISK" || detector_name != "TEBLID512" || detector_name != "TEBLID256") { + ROS_ERROR_STREAM("Failed to switch to map file " << map_file + << " due to invalid feature detector type: " << detector_name); + map_.reset(); + inst_.reset(); + return false; + } + // Check to see if any params were changed when map was reset ReadParams(); enabled_ = true; @@ -87,6 +97,8 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { it_.reset(new image_transport::ImageTransport(*nh)); map_.reset(); map_ = std::make_shared(map_file, true); + + inst_.reset(new Localizer(map_.get())); registration_publisher_ = nh->advertise( @@ -192,6 +204,7 @@ void LocalizationNodelet::ImageCallback(const sensor_msgs::ImageConstPtr& msg) { } void LocalizationNodelet::Localize(void) { + if (!inst_) return; ff_msgs::VisualLandmarks vl; Eigen::Matrix2Xd image_keypoints; From 60df48acf752db5328187bf4c01ee665c8ac9922 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 8 Jul 2024 16:31:43 -0700 Subject: [PATCH 26/49] added protection against switching to an invalid map --- .../include/localization_node/localization.h | 2 +- .../localization_node/localization_nodelet.h | 6 ++- .../localization_node/src/localization.cc | 22 +++++++--- .../src/localization_nodelet.cc | 44 +++++++++++++------ .../include/sparse_mapping/sparse_mapping.h | 2 +- .../sparse_mapping/src/sparse_mapping.cc | 14 ++++-- 6 files changed, 65 insertions(+), 25 deletions(-) diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index 7a22665ad3..9278f3aa00 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -45,7 +45,7 @@ struct ThresholdParams { class Localizer { public: explicit Localizer(sparse_mapping::SparseMap* map); - void ReadParams(config_reader::ConfigReader& config); + bool ReadParams(config_reader::ConfigReader& config, bool fatal_failure = true); bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, Eigen::Matrix2Xd* image_keypoints = NULL); private: diff --git a/localization/localization_node/include/localization_node/localization_nodelet.h b/localization/localization_node/include/localization_node/localization_nodelet.h index 6a13e1a467..4593a72bb5 100644 --- a/localization/localization_node/include/localization_node/localization_nodelet.h +++ b/localization/localization_node/include/localization_node/localization_nodelet.h @@ -44,7 +44,10 @@ class LocalizationNodelet : public ff_util::FreeFlyerNodelet { virtual void Initialize(ros::NodeHandle* nh); private: - void ReadParams(void); + // Wrapper function that calls ReadParams but does not take a param, + // required for configuring with a config timer. + void ReadParamsWrapper(); + bool ReadParams(bool fatal_failure = true); bool ResetMap(const std::string& map_file); void Run(void); void Localize(void); @@ -56,6 +59,7 @@ class LocalizationNodelet : public ff_util::FreeFlyerNodelet { std::shared_ptr map_; std::shared_ptr thread_; config_reader::ConfigReader config_; + std::string last_valid_map_file_; ros::Timer config_timer_; std::shared_ptr it_; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index a4eba47b28..7ba01f1265 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -29,7 +29,7 @@ namespace localization_node { Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {} -void Localizer::ReadParams(config_reader::ConfigReader& config) { +bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) { camera::CameraParameters cam_params(&config, "nav_cam"); std::string prefix; const auto detector_name = map_->GetDetectorName(); @@ -40,7 +40,12 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { } else if (detector_name == "TEBLID256") { prefix = "teblid256_"; } else { - ROS_FATAL_STREAM("Invalid detector: " << detector_name); + if (fatal_failure) { + ROS_FATAL_STREAM("Invalid detector: " << detector_name); + } else { + ROS_ERROR_STREAM("Invalid detector: " << detector_name); + } + return false; } // Loc params @@ -86,17 +91,24 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) { // This check must happen before the histogram_equalization flag is set into the map // to compare with what is there already. - sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(), - loc_params.histogram_equalization); + if (!sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(), + loc_params.histogram_equalization, fatal_failure)) return false; + // Check consistency between clahe params if (loc_params.use_clahe && (loc_params.histogram_equalization != 3 || map_->GetHistogramEqualization() != 3)) { - ROS_FATAL("Invalid clahe and histogram equalization settings."); + if (fatal_failure) { + ROS_FATAL("Invalid clahe and histogram equalization settings."); + } else { + ROS_ERROR("Invalid clahe and histogram equalization settings."); + } + return false; } map_->SetCameraParameters(cam_params); map_->SetLocParams(loc_params); map_->SetDetectorParams(min_features, max_features, detection_retries, min_threshold, default_threshold, max_threshold, too_many_ratio, too_few_ratio); + return true; } bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index 12ce4a4675..acb22b4b5a 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -63,18 +63,27 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) { map_.reset(); map_ = std::make_shared(map_file, true); inst_.reset(new Localizer(map_.get())); - // Sanity check that map contains a valid detector type - const auto detector_name = map_->GetDetectorName(); - if (detector_name != "ORGBRISK" || detector_name != "TEBLID512" || detector_name != "TEBLID256") { + + // Check to see if any params were changed when map was reset. + // Make sure they are valid. + if (!ReadParams(false)) { ROS_ERROR_STREAM("Failed to switch to map file " << map_file - << " due to invalid feature detector type: " << detector_name); - map_.reset(); - inst_.reset(); - return false; + << " due to invalid params."); + if (!last_valid_map_file_.empty()) { + ROS_ERROR_STREAM("Reverting to last map file " << last_valid_map_file_); + map_.reset(); + map_ = std::make_shared(last_valid_map_file_, true); + inst_.reset(new Localizer(map_.get())); + } else { + ROS_ERROR_STREAM("No last valid map, please reset with a valid map. Not localizing."); + map_.reset(); + inst_.reset(); + } + return false; + } else { + last_valid_map_file_ = map_file; } - // Check to see if any params were changed when map was reset - ReadParams(); enabled_ = true; return true; } @@ -97,6 +106,7 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { it_.reset(new image_transport::ImageTransport(*nh)); map_.reset(); map_ = std::make_shared(map_file, true); + last_valid_map_file_ = map_file; inst_.reset(new Localizer(map_.get())); @@ -133,7 +143,7 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { cv::setNumThreads(num_threads); config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) { - config_.CheckFilesUpdated(std::bind(&LocalizationNodelet::ReadParams, this));}, false, true); + config_.CheckFilesUpdated(std::bind(&LocalizationNodelet::ReadParamsWrapper, this));}, false, true); enable_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_ML_ENABLE, &LocalizationNodelet::EnableService, this); reset_map_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_RESET_MAP, &LocalizationNodelet::ResetMapService, this); @@ -141,12 +151,20 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { SERVICE_LOCALIZATION_RESET_MAP_LOC); } -void LocalizationNodelet::ReadParams(void) { + +void LocalizationNodelet::ReadParamsWrapper() { + ReadParams(true); +} + +bool LocalizationNodelet::ReadParams(bool fatal_failure) { if (!config_.ReadFiles()) { ROS_ERROR("Failed to read config files."); - return; + return false; + } + if (inst_) { + return inst_->ReadParams(config_, fatal_failure); } - if (inst_) inst_->ReadParams(config_); + return true; } bool LocalizationNodelet::EnableService(ff_msgs::SetBool::Request & req, ff_msgs::SetBool::Response & res) { diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h index f573acc273..c9ec62ae19 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h @@ -76,7 +76,7 @@ Eigen::Quaternion slerp_n(std::vector const& W, std::vector const& cid_to_keypoint_map, std::vector const& cid_to_filename, diff --git a/localization/sparse_mapping/src/sparse_mapping.cc b/localization/sparse_mapping/src/sparse_mapping.cc index 94642e2150..8cfda8af8b 100644 --- a/localization/sparse_mapping/src/sparse_mapping.cc +++ b/localization/sparse_mapping/src/sparse_mapping.cc @@ -115,15 +115,21 @@ namespace sparse_mapping { // Logic for implementing if two histogram equalization flags are compatible. // This flag can be either 0 (false), 1 (true), or 2 (unknown). Be tolerant // of unknown values, but intolerant when true and false are mixed. -void sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1, - int histogram_equalization2) { +bool sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1, + int histogram_equalization2, bool fatal_failure) { // Ignore if either has unknown equalization value if (histogram_equalization1 == HistogramEqualizationType::kUnknown || histogram_equalization2 == HistogramEqualizationType::kUnknown) { - return; + return true; } else if (histogram_equalization1 != histogram_equalization2) { - LOG(FATAL) << "Incompatible values of histogram equalization detected."; + if (fatal_failure) { + LOG(FATAL) << "Incompatible values of histogram equalization detected."; + } else { + LOG(ERROR) << "Incompatible values of histogram equalization detected."; + } + return false; } + return true; } bool sparse_mapping::IsBinaryDescriptor(std::string const& descriptor) { From 2c919e8536c8a9375cfeca03470069f7880c202b Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 10:51:26 -0700 Subject: [PATCH 27/49] added set start pose tool --- .../localization_node/tools/set_start_pose.py | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100755 localization/localization_node/tools/set_start_pose.py diff --git a/localization/localization_node/tools/set_start_pose.py b/localization/localization_node/tools/set_start_pose.py new file mode 100755 index 0000000000..12b7dbd796 --- /dev/null +++ b/localization/localization_node/tools/set_start_pose.py @@ -0,0 +1,89 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +""" +Initializes the localizer with a defined pose by sending a ff_msgs VisualLandmarks message +with the pose value. +""" + +import argparse +import os +import sys + +import rospy +import std_msgs.msg +from ff_msgs.msg import VisualLandmark, VisualLandmarks + + +def publish_pose(pose): + # Init publisher + pub = rospy.Publisher("/loc/ml/features", VisualLandmarks, queue_size=1) + rospy.init_node("PosePublisher") + + msg = VisualLandmarks() + msg.header = std_msgs.msg.Header() + msg.header.stamp = rospy.Time.now() + msg.camera_id = 0 + # Set pose values + msg.pose.position.x = pose[0] + msg.pose.position.y = pose[1] + msg.pose.position.z = pose[2] + msg.pose.orientation.x = pose[3] + msg.pose.orientation.y = pose[4] + msg.pose.orientation.z = pose[5] + msg.pose.orientation.w = pose[6] + # Set empty landmark features, make sure to have > 5 so msg is considered valid + for i in range(0, 5): + landmark = VisualLandmark() + landmark.x = 0 + landmark.y = 0 + landmark.z = 0 + landmark.u = 0 + landmark.v = 0 + msg.landmarks.append(landmark) + + pub.publish(msg) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter + ) + parser.add_argument( + "pose", + nargs="+", + type=float, + help="Start pose in the format: x y z qx qy qz qw. If --position-only used, only x y z are required.", + ) + parser.add_argument( + "-p", + "--position-only", + dest="initialize_position_only", + action="store_true", + help="Initialize a pose using only the position, the orientation will be set to identity.", + ) + args = parser.parse_args() + if not args.initialize_position_only and len(args.pose) != 7: + print("Pose requires 7 fields.") + sys.exit() + if args.initialize_position_only and len(args.pose) != 3: + print("Position only pose requires 3 fields.") + sys.exit() + if args.initialize_position_only: + args.pose.extend([0, 0, 0, 1]) + publish_pose(args.pose) From 804695739fac7aa8ecc1f301035e85c5b1212931 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 11:14:14 -0700 Subject: [PATCH 28/49] enable loc after revert to last map --- localization/localization_node/src/localization_nodelet.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index acb22b4b5a..72a4444d38 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -74,12 +74,13 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) { map_.reset(); map_ = std::make_shared(last_valid_map_file_, true); inst_.reset(new Localizer(map_.get())); + enabled_ = true; } else { ROS_ERROR_STREAM("No last valid map, please reset with a valid map. Not localizing."); map_.reset(); inst_.reset(); } - return false; + return false; } else { last_valid_map_file_ = map_file; } From 95b6d9eaa0ea9eb3b45acb9a137e2013f363b04c Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 11:20:40 -0700 Subject: [PATCH 29/49] added missing read params after revert from map switch --- localization/localization_node/src/localization_nodelet.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index 72a4444d38..5363bafadb 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -74,6 +74,7 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) { map_.reset(); map_ = std::make_shared(last_valid_map_file_, true); inst_.reset(new Localizer(map_.get())); + ReadParams(); enabled_ = true; } else { ROS_ERROR_STREAM("No last valid map, please reset with a valid map. Not localizing."); From 920c2e5dc084f468aebd35e4448a1147191faca6 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 11:33:56 -0700 Subject: [PATCH 30/49] added sleep for subscribing to clock --- localization/localization_node/tools/set_start_pose.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/localization/localization_node/tools/set_start_pose.py b/localization/localization_node/tools/set_start_pose.py index 12b7dbd796..d25ad617b7 100755 --- a/localization/localization_node/tools/set_start_pose.py +++ b/localization/localization_node/tools/set_start_pose.py @@ -34,10 +34,13 @@ def publish_pose(pose): # Init publisher pub = rospy.Publisher("/loc/ml/features", VisualLandmarks, queue_size=1) rospy.init_node("PosePublisher") + # Sleep so node can subscribe to /clock topic and produce a valid header time + rospy.sleep(5) msg = VisualLandmarks() msg.header = std_msgs.msg.Header() msg.header.stamp = rospy.Time.now() + msg.header.frame_id = "world" msg.camera_id = 0 # Set pose values msg.pose.position.x = pose[0] From 6d88dc859864bf981c28c601d86c8f43c76799be Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 11:57:31 -0700 Subject: [PATCH 31/49] added time delay to set start pose --- localization/localization_node/tools/set_start_pose.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/localization/localization_node/tools/set_start_pose.py b/localization/localization_node/tools/set_start_pose.py index d25ad617b7..97f915507c 100755 --- a/localization/localization_node/tools/set_start_pose.py +++ b/localization/localization_node/tools/set_start_pose.py @@ -18,7 +18,7 @@ # under the License. """ Initializes the localizer with a defined pose by sending a ff_msgs VisualLandmarks message -with the pose value. +with the pose value. Note the pose is expected in the robot nav camera frame. """ import argparse @@ -40,6 +40,9 @@ def publish_pose(pose): msg = VisualLandmarks() msg.header = std_msgs.msg.Header() msg.header.stamp = rospy.Time.now() + # Subtract 2 seconds from stamp to simulate localization time, + # allows VIO messages to accrue that span before and after the vl message + msg.header.stamp.secs = msg.header.stamp.secs - 2 msg.header.frame_id = "world" msg.camera_id = 0 # Set pose values From 5958a30b65414056cfabac0467db0fbb87af3122 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 17:20:47 -0700 Subject: [PATCH 32/49] upgraded loc analysis scripts to python3 --- .../scripts/depth_odometry_parameter_sweep.py | 2 +- tools/localization_analysis/scripts/groundtruth_sweep.py | 2 +- tools/localization_analysis/scripts/imu_analyzer.py | 2 +- tools/localization_analysis/scripts/make_groundtruth.py | 2 +- tools/localization_analysis/scripts/make_map.py | 2 +- tools/localization_analysis/scripts/plot_helpers.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py b/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py index faa4474eb9..5029e64f3e 100755 --- a/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py +++ b/tools/localization_analysis/scripts/depth_odometry_parameter_sweep.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/groundtruth_sweep.py b/tools/localization_analysis/scripts/groundtruth_sweep.py index f0479a58f7..ebc440022e 100755 --- a/tools/localization_analysis/scripts/groundtruth_sweep.py +++ b/tools/localization_analysis/scripts/groundtruth_sweep.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/imu_analyzer.py b/tools/localization_analysis/scripts/imu_analyzer.py index 499fbcbd13..b66959cb3e 100755 --- a/tools/localization_analysis/scripts/imu_analyzer.py +++ b/tools/localization_analysis/scripts/imu_analyzer.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/make_groundtruth.py b/tools/localization_analysis/scripts/make_groundtruth.py index 5d2274f71b..576dfc1e9f 100755 --- a/tools/localization_analysis/scripts/make_groundtruth.py +++ b/tools/localization_analysis/scripts/make_groundtruth.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/make_map.py b/tools/localization_analysis/scripts/make_map.py index ede8dd16f7..073bcd551e 100755 --- a/tools/localization_analysis/scripts/make_map.py +++ b/tools/localization_analysis/scripts/make_map.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/tools/localization_analysis/scripts/plot_helpers.py b/tools/localization_analysis/scripts/plot_helpers.py index 6d879b2c0c..faf22c3276 100644 --- a/tools/localization_analysis/scripts/plot_helpers.py +++ b/tools/localization_analysis/scripts/plot_helpers.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. From ad761ea37b862416794ec1291f530f044046d0a6 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 17:22:16 -0700 Subject: [PATCH 33/49] upgraded sparse map scripts to python3 --- localization/sparse_mapping/scripts/make_surf_map.py | 2 +- localization/sparse_mapping/scripts/make_surf_maps.py | 2 +- localization/sparse_mapping/scripts/make_teblid512_map.py | 2 +- .../sparse_mapping/scripts/process_sequential_images.py | 2 +- .../sparse_mapping/scripts/prune_partitioned_directories.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/sparse_mapping/scripts/make_surf_map.py b/localization/sparse_mapping/scripts/make_surf_map.py index f5deb5f33a..338e8c512e 100755 --- a/localization/sparse_mapping/scripts/make_surf_map.py +++ b/localization/sparse_mapping/scripts/make_surf_map.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/make_surf_maps.py b/localization/sparse_mapping/scripts/make_surf_maps.py index 1e6d011451..28cc50d4c5 100755 --- a/localization/sparse_mapping/scripts/make_surf_maps.py +++ b/localization/sparse_mapping/scripts/make_surf_maps.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/make_teblid512_map.py b/localization/sparse_mapping/scripts/make_teblid512_map.py index 75766ab10f..56d5cc7eb7 100755 --- a/localization/sparse_mapping/scripts/make_teblid512_map.py +++ b/localization/sparse_mapping/scripts/make_teblid512_map.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/process_sequential_images.py b/localization/sparse_mapping/scripts/process_sequential_images.py index aafe1c1586..dbe5c05b03 100755 --- a/localization/sparse_mapping/scripts/process_sequential_images.py +++ b/localization/sparse_mapping/scripts/process_sequential_images.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. diff --git a/localization/sparse_mapping/scripts/prune_partitioned_directories.py b/localization/sparse_mapping/scripts/prune_partitioned_directories.py index f986d57697..ff5efd6889 100755 --- a/localization/sparse_mapping/scripts/prune_partitioned_directories.py +++ b/localization/sparse_mapping/scripts/prune_partitioned_directories.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/python3 # # Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. From afd3abe925d4eba2c88f129a9e57b007b491da95 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Tue, 9 Jul 2024 17:24:55 -0700 Subject: [PATCH 34/49] updated min faetuers in make teblid map --- localization/sparse_mapping/scripts/make_teblid512_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/sparse_mapping/scripts/make_teblid512_map.py b/localization/sparse_mapping/scripts/make_teblid512_map.py index 56d5cc7eb7..a26b3a5b52 100755 --- a/localization/sparse_mapping/scripts/make_teblid512_map.py +++ b/localization/sparse_mapping/scripts/make_teblid512_map.py @@ -53,7 +53,7 @@ def make_teblid512_map(surf_map, world, robot_name, map_directory=None): if map_directory: os.chdir(map_directory) build_teblid512_map_command = ( - "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -output_map " + "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -min_brisk_features 3000 -output_map " + teblid512_map_full_path ) lu.run_command_and_save_output( From 2e824871dba1843da668e3019056321d1d36fa8b Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Wed, 10 Jul 2024 14:34:24 -0700 Subject: [PATCH 35/49] updated make map script --- tools/localization_analysis/scripts/make_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/localization_analysis/scripts/make_map.py b/tools/localization_analysis/scripts/make_map.py index 073bcd551e..5f248e7c77 100755 --- a/tools/localization_analysis/scripts/make_map.py +++ b/tools/localization_analysis/scripts/make_map.py @@ -133,7 +133,7 @@ def make_map( if merge_with_base_map: os.chdir("maps") rebuild_map_command = ( - "rosrun sparse_mapping build_map -rebuild -rebuild_detector TEBLID512 -output_map " + "rosrun sparse_mapping build_map -rebuild -rebuild_detector TEBLID512 -min_brisk_features 3000 -output_map " + bag_teblid512_map_full_path ) if histogram_equalization: From c17345268849bb9a482dca85812ed2f4bb3393d6 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Wed, 17 Jul 2024 12:28:14 -0700 Subject: [PATCH 36/49] added loc coverage display tool for rviz --- .../localization_rviz_plugins/CMakeLists.txt | 17 ++-- .../nodelet_plugins.xml | 7 ++ tools/localization_rviz_plugins/readme.md | 3 + .../src/localization_coverage_display.cc | 96 +++++++++++++++++++ .../src/localization_coverage_display.h | 61 ++++++++++++ 5 files changed, 176 insertions(+), 8 deletions(-) create mode 100644 tools/localization_rviz_plugins/src/localization_coverage_display.cc create mode 100644 tools/localization_rviz_plugins/src/localization_coverage_display.h diff --git a/tools/localization_rviz_plugins/CMakeLists.txt b/tools/localization_rviz_plugins/CMakeLists.txt index d93d1ff85b..715bf644d5 100644 --- a/tools/localization_rviz_plugins/CMakeLists.txt +++ b/tools/localization_rviz_plugins/CMakeLists.txt @@ -18,7 +18,7 @@ cmake_minimum_required(VERSION 3.0) project(localization_rviz_plugins) -if (FALSE AND BUILD_LOC_RVIZ_PLUGINS) +if (BUILD_LOC_RVIZ_PLUGINS) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) @@ -61,13 +61,14 @@ if (FALSE AND BUILD_LOC_RVIZ_PLUGINS) # Declare C++ libraries add_library(${PROJECT_NAME} - src/depth_odometry_display.cc - src/localization_graph_display.cc - src/localization_graph_panel.cc - src/pose_display.cc - src/utilities.cc - src/imu_augmentor_display.cc - src/slider_property.cc + #src/depth_odometry_display.cc + src/localization_coverage_display.cc + #src/localization_graph_display.cc + #src/localization_graph_panel.cc + #src/pose_display.cc + #src/utilities.cc + #src/imu_augmentor_display.cc + #src/slider_property.cc src/sparse_mapping_display.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/tools/localization_rviz_plugins/nodelet_plugins.xml b/tools/localization_rviz_plugins/nodelet_plugins.xml index c4b3f276d7..7636b79bca 100644 --- a/tools/localization_rviz_plugins/nodelet_plugins.xml +++ b/tools/localization_rviz_plugins/nodelet_plugins.xml @@ -6,6 +6,13 @@ Depth Odometry Display + + + Localization Coverage Display + + diff --git a/tools/localization_rviz_plugins/readme.md b/tools/localization_rviz_plugins/readme.md index f749a9e6b5..8a444fa848 100644 --- a/tools/localization_rviz_plugins/readme.md +++ b/tools/localization_rviz_plugins/readme.md @@ -16,6 +16,9 @@ The depth odometry display publishes source and target point clouds and correspo ## Imu Augmentor Display The imu augmentor display draws imu augmentor poses. This is useful when comparing with graph localizer poses and sparse mapping poses, as ideally these are all alligned. +## Localization Coverage Display +The localization coverage display draws map pose positions from a bagfile. To use this, use: export COVERAGE_BAG=/path/to/bagfile before launching rviz to set the bagfile to use for plotting. The bag file should have poses stored using the /sparse_mapping/pose topic, which is the output for the localization_analysis map matcher and offline replay tools for map matched poses. Subscribe to the output topic called /localization_coverage/cloud using the rviz PointCloud2 visualization plugin to view the pose positions. + ## Localization Graph Display The localization graph display draws the full history of poses in the latest graph localization message. It also draws imu estimates between poses as arrows, and publishes optical flow feature track images using the feature tracks in the localizer. diff --git a/tools/localization_rviz_plugins/src/localization_coverage_display.cc b/tools/localization_rviz_plugins/src/localization_coverage_display.cc new file mode 100644 index 0000000000..027ee7e92a --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_coverage_display.cc @@ -0,0 +1,96 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include "localization_coverage_display.h" // NOLINT + +namespace localization_rviz_plugins { +LocalizationCoverageDisplay::LocalizationCoverageDisplay() { + int ff_argc = 1; + char argv[] = "localization_coverage_display"; + char* argv_ptr = &argv[0]; + char** argv_ptr_ptr = &argv_ptr; + cloud_publisher_ = nh_.advertise("localization_coverage/cloud", 1); + // Load positions from bagfile + const std::string coverage_bag = std::getenv("COVERAGE_BAG"); + rosbag::Bag bag(coverage_bag, rosbag::bagmode::Read); + rosbag::View view(bag, rosbag::TopicQuery("/sparse_mapping/pose")); + for (const rosbag::MessageInstance msg : view) { + geometry_msgs::PoseStampedPtr pose_msg = msg.instantiate(); + if (pose_msg) { + positions_.emplace_back(localization_common::PoseFromMsg(*pose_msg).translation().cast()); + } + } +} + +void LocalizationCoverageDisplay::onInitialize() { drawMap(); } + +void LocalizationCoverageDisplay::drawMap() { + // TODO(rsoussan): Use pcl point cloud when pcl_ros dependency added + sensor_msgs::PointCloud2 cloud; + cloud.header = std_msgs::Header(); + cloud.header.frame_id = "world"; + cloud.height = 1; + cloud.width = positions_.size(); + cloud.fields.resize(3); + cloud.fields[0].name = "x"; + cloud.fields[0].offset = 0; + cloud.fields[0].datatype = 7; + cloud.fields[0].count = 1; + cloud.fields[1].name = "y"; + cloud.fields[1].offset = 4; + cloud.fields[1].datatype = 7; + cloud.fields[1].count = 1; + cloud.fields[2].name = "z"; + cloud.fields[2].offset = 8; + cloud.fields[2].datatype = 7; + cloud.fields[2].count = 1; + cloud.is_bigendian = false; + cloud.point_step = 12; + cloud.row_step = cloud.point_step * cloud.width; + cloud.is_dense = true; + cloud.data.resize(cloud.row_step); + + for (int i = 0; i < static_cast(cloud.width); ++i) { + const Eigen::Vector3f& point = positions_[i]; + memcpy(&cloud.data[cloud.point_step * i + 0], &point.x(), 4); + memcpy(&cloud.data[cloud.point_step * i + 4], &point.y(), 4); + memcpy(&cloud.data[cloud.point_step * i + 8], &point.z(), 4); + } + + cloud.header.stamp = ros::Time::now(); + // TODO(rsoussan): Use ros point_cloud_common instead of publishing when rviz_default_plugin linker issue fixed + cloud_publisher_.publish(cloud); +} + +void LocalizationCoverageDisplay::reset() {} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::LocalizationCoverageDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/localization_coverage_display.h b/tools/localization_rviz_plugins/src/localization_coverage_display.h new file mode 100644 index 0000000000..21a0d1bad8 --- /dev/null +++ b/tools/localization_rviz_plugins/src/localization_coverage_display.h @@ -0,0 +1,61 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Header file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#endif + +#include +#include +#include + +// Forward declarations for ogre and rviz +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { + +class LocalizationCoverageDisplay : public rviz::Display { + Q_OBJECT // NOLINT + public : // NOLINT + LocalizationCoverageDisplay(); + ~LocalizationCoverageDisplay() = default; + + protected: + void onInitialize() final; + void reset() final; + + private: + void drawMap(); + + // TODO(rosussan): Remove publishing and use point_cloud_common from rviz/default_plugins + // when linking error is fixed + std::vector positions_; + ros::Publisher cloud_publisher_; + ros::NodeHandle nh_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_LOCALIZATION_COVERAGE_DISPLAY_H_ NOLINT From 1699eed591a97695fd9d74a1342207018c16429c Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sat, 20 Jul 2024 22:03:32 -0700 Subject: [PATCH 37/49] added draft for camera_params by camera id --- .../include/sparse_mapping/sparse_map.h | 13 +++++++++--- localization/sparse_mapping/src/sparse_map.cc | 20 ++++++++----------- .../sparse_mapping/src/sparse_mapping.cc | 7 +++++-- 3 files changed, 23 insertions(+), 17 deletions(-) diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index 370e95dbe5..29ca546dba 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -177,8 +177,14 @@ struct SparseMap { /** * Return the parameters of the camera used to construct the map. **/ - camera::CameraParameters GetCameraParameters(void) const {return camera_params_;} - void SetCameraParameters(camera::CameraParameters camera_params) {camera_params_ = camera_params;} + const camera::CameraParameters& camera_params(int cid) const { + return camera_id_to_camera_params_[cid_to_camera_id[cid]]; + } + camera::CameraParameters GetCameraParameters(int cid) const { + return camera_id_to_camera_params_[cid_to_camera_id[cid]]; + } + // void SetCameraParameters(int cid, camera::CameraParameters& camera_params) {cid_to_camera_params_[cid] = + // camera_params;} /** * Return the number of observations. Use this number to divide the final error to find the average pixel error. **/ @@ -239,7 +245,8 @@ struct SparseMap { std::vector> cid_to_matching_cid_counts_; interest_point::FeatureDetector detector_; - camera::CameraParameters camera_params_; + std::vector cid_to_camera_id_; + std::vector camera_id_to_camera_params_; mutable sparse_mapping::VocabDB vocab_db_; // TODO(oalexan1): Mutable means someone is doing something wrong. LocalizationParameters loc_params_; std::string protobuf_file_; diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index d6c758d17e..c058b2b49c 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -661,21 +661,17 @@ void SparseMap::InitializeCidFidToPid() { &cid_fid_to_pid_); } -void SparseMap::DetectFeaturesFromFile(std::string const& filename, - bool multithreaded, - cv::Mat* descriptors, - Eigen::Matrix2Xd* keypoints) { +void SparseMap::DetectFeaturesFromFile(std::string const& filename, const camera::CameraParameters& camera_params, + bool multithreaded, cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) { cv::Mat image = cv::imread(filename, cv::IMREAD_GRAYSCALE); if (image.rows == 0 || image.cols == 0) LOG(FATAL) << "Found empty image in file: " << filename; - DetectFeatures(image, multithreaded, descriptors, keypoints); + DetectFeatures(image, camera_params, multithreaded, descriptors, keypoints); } -void SparseMap::DetectFeatures(const cv::Mat& image, - bool multithreaded, - cv::Mat* descriptors, - Eigen::Matrix2Xd* keypoints) { +void SparseMap::DetectFeatures(const cv::Mat& image, const camera::CameraParameters& camera_params, bool multithreaded, + cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) { // If using histogram equalization, need an extra image to store it cv::Mat * image_ptr = const_cast(&image); cv::Mat hist_image; @@ -728,7 +724,7 @@ void SparseMap::DetectFeatures(const cv::Mat& image, Eigen::Vector2d output; for (size_t j = 0; j < storage.size(); j++) { - camera_params_.Convert + camera_params.Convert (Eigen::Vector2d(storage[j].pt.x, storage[j].pt.y), &output); keypoints->col(j) = output; } @@ -816,7 +812,7 @@ bool SparseMap::Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const if (map_image.empty()) { LOG(ERROR) << "Failed to load map image: " << map_filename; } else { - ViewMatches(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params_, image, map_image); + ViewMatches(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params(cid), image, map_image); } } @@ -828,7 +824,7 @@ bool SparseMap::Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const std::vector inlier_matches; std::vector vec_inliers; Eigen::Matrix3d essential_matrix; - FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params_, + FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params(cid), &inlier_matches, &vec_inliers, &essential_matrix, loc_params_.essential_ransac_iterations); all_matches[i] = inlier_matches; diff --git a/localization/sparse_mapping/src/sparse_mapping.cc b/localization/sparse_mapping/src/sparse_mapping.cc index 8cfda8af8b..93ff43f313 100644 --- a/localization/sparse_mapping/src/sparse_mapping.cc +++ b/localization/sparse_mapping/src/sparse_mapping.cc @@ -1054,7 +1054,8 @@ double sparse_mapping::ComputeRaysAngle(int pid, } void sparse_mapping::FilterPID(double reproj_thresh, - camera::CameraParameters const& camera_params, + std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector > * pid_to_cid_fid, @@ -1077,7 +1078,6 @@ void sparse_mapping::FilterPID(double reproj_thresh, s.total = (*pid_to_xyz).size(); std::vector is_bad((*pid_to_xyz).size(), false); - Eigen::Vector2d half_size = camera_params.GetUndistortedHalfSize(); for (size_t pid = 0; pid < (*pid_to_xyz).size(); pid++) { bool small_angle = false, behind_cam = false, invalid_reproj = false; @@ -1090,11 +1090,13 @@ void sparse_mapping::FilterPID(double reproj_thresh, } for (std::pair cid_fid : (*pid_to_cid_fid)[pid]) { + const auto& camera_params = camera_id_to_camera_params[cid_to_camera_id[cid_fid.first]]; Eigen::Vector2d pix = (cid_to_cam_t_global[cid_fid.first] * (*pid_to_xyz)[pid]).hnormalized() * camera_params.GetFocalLength(); errors.push_back((cid_to_keypoint_map[cid_fid.first].col(cid_fid.second) - pix).norm()); // Mark points which don't project at valid camera pixels // TODO(zmoratto) : This can probably be done with a Eigen Array reduction + Eigen::Vector2d half_size = camera_params.GetUndistortedHalfSize(); if (pix[0] < -half_size[0] || pix[0] >= half_size[0] || pix[1] < -half_size[1] || pix[1] >= half_size[1]) { invalid_reproj = true; is_bad[pid] = true; @@ -1133,6 +1135,7 @@ void sparse_mapping::FilterPID(double reproj_thresh, std::map::iterator itr = cid_fid.begin(); while (itr != cid_fid.end()) { s.num_features++; + const auto& camera_params = camera_id_to_camera_params[cid_to_camera_id[itr->first]]; Eigen::Vector2d pix = (cid_to_cam_t_global[itr->first] * (*pid_to_xyz)[pid]).hnormalized() * camera_params.GetFocalLength(); double err From 0a8930dde4fb9c8cc4f34ec50f2a30ac14e1e11e Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 13:04:27 -0700 Subject: [PATCH 38/49] more cam params updates --- .../sparse_mapping/src/reprojection.cc | 6 +-- localization/sparse_mapping/src/tensor.cc | 50 +++++++++++-------- 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/localization/sparse_mapping/src/reprojection.cc b/localization/sparse_mapping/src/reprojection.cc index 5bf243f717..7eeafaf83f 100644 --- a/localization/sparse_mapping/src/reprojection.cc +++ b/localization/sparse_mapping/src/reprojection.cc @@ -198,7 +198,7 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, // This is a very specialized function void BundleAdjustSmallSet(std::vector const& features_n, - double focal_length, + std::vector focal_length_n, std::vector * cam_t_global_n, Eigen::Matrix3Xd * pid_to_xyz, ceres::LossFunction * loss, @@ -232,10 +232,10 @@ void BundleAdjustSmallSet(std::vector const& features_n, &cam_t_global_n->at(cid).translation()[0], &aa.at(cid)[0], &pid_to_xyz->col(pid)[0], - &focal_length); + &focal_length[cid]); } } - problem.SetParameterBlockConstant(&focal_length); + problem.SetParameterBlockConstant(&focal_length[cid]); // Solve the problem ceres::Solve(options, &problem, summary); diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index e51fa3f94b..80e284f389 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -156,13 +156,16 @@ void WriteMatches(openMVG::matching::PairWiseMatches const& match_map, void BuildMapPerformMatching(openMVG::matching::PairWiseMatches * match_map, std::vector const& cid_to_keypoint_map, std::vector const& cid_to_descriptor_map, - camera::CameraParameters const& camera_params, + std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, CIDPairAffineMap * relative_affines, std::mutex * match_mutex, int i /*query cid index*/, int j /*train cid index*/, bool compute_rays_angle, double * rays_angle) { Eigen::Matrix2Xd const& keypoints1 = cid_to_keypoint_map[i]; Eigen::Matrix2Xd const& keypoints2 = cid_to_keypoint_map[j]; + const auto& camera_params1 = camera_id_to_camera_params[cid_to_camera_id[i]]; + const auto& camera_params2 = camera_id_to_camera_params[cid_to_camera_id[j]]; std::vector matches, inlier_matches; interest_point::FindMatches(cid_to_descriptor_map[i], @@ -178,7 +181,7 @@ void BuildMapPerformMatching(openMVG::matching::PairWiseMatches * match_map, bool compute_inliers_only = false; BuildMapFindEssentialAndInliers(keypoints1, keypoints2, matches, - camera_params, compute_inliers_only, + camera_params1, camera_params2, compute_inliers_only, i, j, match_mutex, relative_affines, @@ -284,7 +287,8 @@ void MatchFeatures(const std::string & essential_file, &match_map, s->cid_to_keypoint_map_, s->cid_to_descriptor_map_, - std::cref(s->camera_params_), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, &relative_affines, &match_mutex, cid, indices[j], @@ -1935,9 +1939,10 @@ void ReadAffineCSV(std::string const& input_filename, // Filter the matches by a geometric constraint. Compute the essential matrix. void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, - std::vector const& matches, camera::CameraParameters const& camera_params, - std::vector* inlier_matches, std::vector* vec_inliers, - Eigen::Matrix3d* essential_matrix, const int ransac_iterations) { + std::vector const& matches, camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, std::vector* inlier_matches, + std::vector* vec_inliers, Eigen::Matrix3d* essential_matrix, + const int ransac_iterations) { // Initialize the outputs inlier_matches->clear(); @@ -1951,13 +1956,14 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X std::pair image_size(camera_params.GetUndistortedSize()[0], camera_params.GetUndistortedSize()[1]); - Eigen::Matrix3d k = camera_params.GetIntrinsicMatrix(); + Eigen::Matrix3d k1 = camera_params1.GetIntrinsicMatrix(); + Eigen::Matrix3d k2 = camera_params2.GetIntrinsicMatrix(); // Calculate the essential matrix double error_max = std::numeric_limits::max(); double max_expected_error = 2.5; - if (!interest_point::RobustEssential(k, k, observationsa, observationsb, + if (!interest_point::RobustEssential(k1, k2, observationsa, observationsb, essential_matrix, vec_inliers, image_size, image_size, &error_max, @@ -1979,7 +1985,8 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, std::vector const& matches, - camera::CameraParameters const& camera_params, + camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, bool compute_inliers_only, size_t cam_a_idx, size_t cam_b_idx, std::mutex * match_mutex, @@ -1993,7 +2000,8 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, std::vector vec_inliers; Eigen::Matrix3d e; - FindEssentialAndInliers(keypoints1, keypoints2, matches, camera_params, inlier_matches, &vec_inliers, &e); + FindEssentialAndInliers(keypoints1, keypoints2, matches, camera_params1, camera_params2, inlier_matches, &vec_inliers, + &e); if (!inlier_matches) { VLOG(2) << cam_a_idx << " " << cam_b_idx << " | Estimation of essential matrix failed!\n"; @@ -2028,8 +2036,9 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, observationsa.col(i) = keypoints1.col(matches[i].queryIdx); observationsb.col(i) = keypoints2.col(matches[i].trainIdx); } - Eigen::Matrix3d k = camera_params.GetIntrinsicMatrix(); - if (!interest_point::EstimateRTFromE(k, k, observationsa, observationsb, + Eigen::Matrix3d k1 = camera_params1.GetIntrinsicMatrix(); + Eigen::Matrix3d k2 = camera_params2.GetIntrinsicMatrix(); + if (!interest_point::EstimateRTFromE(k1, k2, observationsa, observationsb, e, vec_inliers, &r, &t)) { VLOG(2) << cam_a_idx << " " << cam_b_idx @@ -2064,13 +2073,9 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, double error; int num_pts_behind_camera = 0; for (ptrdiff_t i = 0; i < observations2[0].cols(); i++) { - pid_to_xyz.col(i) = - sparse_mapping::TriangulatePoint - (Eigen::Vector3d(observations2[0](0, i), observations2[0](1, i), - camera_params.GetFocalLength()), - Eigen::Vector3d(observations2[1](0, i), observations2[1](1, i), - camera_params.GetFocalLength()), - r, t, &error); + pid_to_xyz.col(i) = sparse_mapping::TriangulatePoint + (Eigen::Vector3d(observations2[0](0, i), observations2[0](1, i), camera_params1.GetFocalLength()), + Eigen::Vector3d(observations2[1](0, i), observations2[1](1, i), camera_params2.GetFocalLength()), r, t, &error); Eigen::Vector3d P = pid_to_xyz.col(i); Eigen::Vector3d Q = r*P + t; if (P[2] <= 0 || Q[2] <= 0) { @@ -2083,7 +2088,8 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, << " (" << round((100.0*num_pts_behind_camera) / observations2[0].cols()) << "%)"; - sparse_mapping::BundleAdjustSmallSet(observations2, camera_params.GetFocalLength(), &cameras, + std::vector focal_lengths{camera_params1.GetFocalLength(), camera_params2.GetFocalLength()}; + sparse_mapping::BundleAdjustSmallSet(observations2, focal_lengths, &cameras, &pid_to_xyz, new ceres::CauchyLoss(0.5), options, &summary); @@ -2103,9 +2109,9 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Vector3d P = sparse_mapping::TriangulatePoint (Eigen::Vector3d(observations2[0](0, i), observations2[0](1, i), - camera_params.GetFocalLength()), + camera_params1.GetFocalLength()), Eigen::Vector3d(observations2[1](0, i), observations2[1](1, i), - camera_params.GetFocalLength()), + camera_params2.GetFocalLength()), cameras[1].linear(), cameras[1].translation(), &error); Eigen::Vector3d X0 = ctr0 - P; From 8d1de759d4d0c7157d9bd8e789371d64d4cdb4c2 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 13:17:26 -0700 Subject: [PATCH 39/49] updated triangulate calls --- localization/sparse_mapping/src/tensor.cc | 36 +++++++++++++---------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index 80e284f389..d1dfbf98f4 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -356,7 +356,8 @@ void BuildTracks(bool rm_invalid_xyz, // Triangulate. The results should be quite inaccurate, we'll redo this // later. This step is mostly for consistency. sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), @@ -442,7 +443,8 @@ void IncrementalBA(std::string const& essential_file, pid_to_xyz_local.clear(); std::vector > cid_fid_to_pid_local; sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, cid_to_cam_t_local, s->cid_to_keypoint_map_, &pid_to_cid_fid_local, @@ -492,7 +494,8 @@ void IncrementalBA(std::string const& essential_file, // Triangulate all points sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), @@ -604,7 +607,8 @@ void CloseLoop(sparse_mapping::SparseMap * s) { // sparse_mapping::PrintPidStats(s->pid_to_cid_fid_); bool rm_invalid_xyz = true; sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), @@ -1422,7 +1426,8 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, std::vector > new_cid_fid_to_pid; bool rm_invalid_xyz = true; // don't remove anything, as cameras are pretty unreliable now sparse_mapping::Triangulate(rm_invalid_xyz, - C.camera_params_.GetFocalLength(), + C.cid_to_camera_id_, + C.camera_id_to_camera_params_, C.cid_to_cam_t_global_, C.cid_to_keypoint_map_, &new_pid_to_cid_fid, @@ -1722,7 +1727,8 @@ double RegistrationOrVerification(std::vector const& data_files, std::vector > cid_fid_to_pid_local; bool rm_invalid_xyz = false; // there should be nothing to remove hopefully sparse_mapping::Triangulate(rm_invalid_xyz, - map->camera_params_.GetFocalLength(), + map->cid_to_camera_id_, + map->camera_id_to_camera_params_, map->cid_to_cam_t_global_, map->user_cid_to_keypoint_map_, &(map->user_pid_to_cid_fid_), @@ -2187,21 +2193,21 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, } } -void Triangulate(bool rm_invalid_xyz, double focal_length, +void Triangulate(bool rm_invalid_xyz, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, - std::vector > * pid_to_cid_fid, - std::vector * pid_to_xyz, - std::vector > * cid_fid_to_pid) { - Eigen::Matrix3d k; - k << focal_length, 0, 0, - 0, focal_length, 0, - 0, 0, 1; - + std::vector>* pid_to_cid_fid, std::vector* pid_to_xyz, + std::vector>* cid_fid_to_pid) { // Build p matrices for all of the cameras. openMVG::Triangulation // will be holding pointers to all of the cameras. std::vector cid_to_p(cid_to_cam_t_global.size()); for (size_t cid = 0; cid < cid_to_p.size(); cid++) { + const double focal_length = camera_id_to_camera_params[cid_to_camera_id[cid]].GetFocalLength(); + Eigen::Matrix3d k; + k << focal_length, 0, 0, + 0, focal_length, 0, + 0, 0, 1; openMVG::P_From_KRt(k, cid_to_cam_t_global[cid].linear(), cid_to_cam_t_global[cid].translation(), &cid_to_p[cid]); } From cf9719f287ff213345ea861550856cdfa9805e46 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 13:26:49 -0700 Subject: [PATCH 40/49] updated bundle adjustment calls --- .../sparse_mapping/src/reprojection.cc | 12 ++++++++---- localization/sparse_mapping/src/tensor.cc | 18 ++++++++---------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/localization/sparse_mapping/src/reprojection.cc b/localization/sparse_mapping/src/reprojection.cc index 7eeafaf83f..789a293925 100644 --- a/localization/sparse_mapping/src/reprojection.cc +++ b/localization/sparse_mapping/src/reprojection.cc @@ -94,7 +94,8 @@ struct ReprojectionError { }; void BundleAdjust(std::vector > const& pid_to_cid_fid, - std::vector const& cid_to_keypoint_map, double focal_length, + std::vector const& cid_to_keypoint_map, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector* cid_to_cam_t_global, std::vector* pid_to_xyz, std::vector > const& user_pid_to_cid_fid, std::vector const& user_cid_to_keypoint_map, @@ -109,12 +110,14 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, // Allocate space for the angle axis representation of rotation std::vector camera_aa_storage(3 * cid_to_cam_t_global->size()); + std::vector focal_lengths; for (size_t cid = 0; cid < cid_to_cam_t_global->size(); cid++) { Eigen::Map aa_storage(camera_aa_storage.data() + 3 * cid); Eigen::Vector3d vec; camera::RotationToRodrigues(cid_to_cam_t_global->at(cid).linear(), &vec); aa_storage = vec; + focal_lengths.emplace_back(camera_id_to_camera_params[cid_to_camera_id[cid]].GetFocalLength()); } // Build problem @@ -158,13 +161,12 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, for (std::map::value_type const& cid_fid : (*p_pid_to_cid_fid)[pid]) { ceres::CostFunction* cost_function = ReprojectionError::Create((*p_cid_to_keypoint_map)[cid_fid.first].col(cid_fid.second)); - problem.AddResidualBlock(cost_function, local_loss, &cid_to_cam_t_global->at(cid_fid.first).translation()[0], &camera_aa_storage[3 * cid_fid.first], &p_pid_to_xyz->at(pid)[0], - &focal_length); + &focal_length[cid_fid.first]); if (fix_all_cameras || (cid_fid.first < first || cid_fid.first > last) || fixed_cameras.find(cid_fid.first) != fixed_cameras.end()) { @@ -180,7 +182,9 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, problem.SetParameterBlockConstant(&p_pid_to_xyz->at(pid)[0]); } } - problem.SetParameterBlockConstant(&focal_length); + for (const auto& focal_length : focal_lengths) { + problem.SetParameterBlockConstant(&focal_length); + } } // Solve the problem diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index d1dfbf98f4..c184601163 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -478,14 +478,10 @@ void IncrementalBA(std::string const& essential_file, LOG(INFO) << "Optimizing cameras from " << start << " to " << cid << " (total: " << cid-start+1 << ")"; - sparse_mapping::BundleAdjust(pid_to_cid_fid_local, s->cid_to_keypoint_map_, - s->camera_params_.GetFocalLength(), - &cid_to_cam_t_local, &pid_to_xyz_local, - s->user_pid_to_cid_fid_, - s->user_cid_to_keypoint_map_, - &(s->user_pid_to_xyz_), - loss, options, &summary, - start, cid); + sparse_mapping::BundleAdjust(pid_to_cid_fid_local, s->cid_to_keypoint_map_, s->cid_to_camera_id_, + s->camera_id_to_camera_params_, &cid_to_cam_t_local, &pid_to_xyz_local, + s->user_pid_to_cid_fid_, s->user_cid_to_keypoint_map_, &(s->user_pid_to_xyz_), loss, + options, &summary, start, cid); // Copy back for (int c = 0; c <= cid; c++) @@ -651,7 +647,8 @@ void BundleAdjustment(sparse_mapping::SparseMap * s, int first, int last, bool fix_all_cameras, std::set const& fixed_cameras) { sparse_mapping::BundleAdjust(s->pid_to_cid_fid_, s->cid_to_keypoint_map_, - s->camera_params_.GetFocalLength(), &(s->cid_to_cam_t_global_), + s->cid_to_camera_id_, s->camera_id_to_camera_params_, + &(s->cid_to_cam_t_global_), &(s->pid_to_xyz_), s->user_pid_to_cid_fid_, s->user_cid_to_keypoint_map_, &(s->user_pid_to_xyz_), @@ -1709,8 +1706,9 @@ double RegistrationOrVerification(std::vector const& data_files, // Shift the keypoints. Undistort if necessary. Eigen::Vector2d output; for (size_t cid = 0; cid < map->user_cid_to_keypoint_map_.size(); cid++) { + const auto& camera_params = map->camera_params(cid); for (int i = 0; i < map->user_cid_to_keypoint_map_[cid].cols(); i++) { - map->camera_params_.Convert + camera_params.Convert (map->user_cid_to_keypoint_map_[cid].col(i), &output); map->user_cid_to_keypoint_map_[cid].col(i) = output; } From 96f7d30a4e616c2e7e339887417f62926838ee92 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 13:45:53 -0700 Subject: [PATCH 41/49] updated merge script --- localization/sparse_mapping/src/tensor.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index c184601163..27e910965f 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -657,7 +657,7 @@ void BundleAdjustment(sparse_mapping::SparseMap * s, // First do BA, and only afterwards remove outliers. if (!FLAGS_skip_filtering) { - FilterPID(FLAGS_reproj_thresh, s->camera_params_, s->cid_to_cam_t_global_, + FilterPID(FLAGS_reproj_thresh, s->cid_to_camera_id_, s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), &(s->pid_to_xyz_)); s->InitializeCidFidToPid(); } @@ -1206,6 +1206,7 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, int num_acid = A.cid_to_filename_.size(); int num_bcid = B.cid_to_filename_.size(); int num_ccid = num_acid + num_bcid; + int num_A_camera_ids = A.camera_id_to_camera_params_.size(); C.cid_to_filename_ .resize(num_ccid); C.cid_to_keypoint_map_ .resize(num_ccid); C.cid_to_cam_t_global_ .resize(num_ccid); @@ -1216,8 +1217,14 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, C.cid_to_filename_[c] = B.cid_to_filename_[cid]; C.cid_to_keypoint_map_[c] = B.cid_to_keypoint_map_[cid]; C.cid_to_descriptor_map_[c] = B.cid_to_descriptor_map_[cid]; + C.cid_to_camera_id_[c] = B.cid_to_camera_id_[cid] + num_A_camera_ids; // We will have to deal with cid_to_cam_t_global_ later } + // Append B's camera params to A's. + // TODO(rsoussan): Check for equality of camera params and consolidate + for (const auto& camera_params : B.camera_ids_to_camera_params_) { + A.camera_ids_to_camera_params_.append(camera_params); + } // Create cid_fid_to_pid_ for both maps, to be able to go from cid_fid to pid. A.InitializeCidFidToPid(); From 3226419fd1700b250a489712df3ee2e96bbb4184 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 13:50:56 -0700 Subject: [PATCH 42/49] more updates --- .../sparse_mapping/tools/partition_image_sequences.cc | 2 +- localization/sparse_mapping/tools/utilities.cc | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/localization/sparse_mapping/tools/partition_image_sequences.cc b/localization/sparse_mapping/tools/partition_image_sequences.cc index 6a10500404..6d0c639800 100644 --- a/localization/sparse_mapping/tools/partition_image_sequences.cc +++ b/localization/sparse_mapping/tools/partition_image_sequences.cc @@ -108,7 +108,7 @@ Result RotationOnlyImage(const vc::FeatureMatches& matches, const camera::Camera return result; } std::vector inliers; - const auto source_T_target = sm::EstimateAffine3d(matches, camera_params, inliers); + const auto source_T_target = sm::EstimateAffine3d(matches, camera_params, camera_params, inliers); const double relative_pose_inliers_ratio = static_cast(inliers.size()) / static_cast(matches.size()); if (relative_pose_inliers_ratio < min_relative_pose_inliers_ratio) { LogDebug("Too few inliers found. Inliers: " << inliers.size() << ", total matches: " << matches.size() diff --git a/localization/sparse_mapping/tools/utilities.cc b/localization/sparse_mapping/tools/utilities.cc index aeae09c8c5..aa1c67f16b 100644 --- a/localization/sparse_mapping/tools/utilities.cc +++ b/localization/sparse_mapping/tools/utilities.cc @@ -58,7 +58,8 @@ boost::optional Matches(const vc::FeatureImage& current_imag return matches; } -Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera::CameraParameters& camera_params, +Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera::CameraParameters& camera_params1, + const camera::CameraParameters& camera_params2, std::vector& inliers) { Eigen::Matrix2Xd source_image_points(2, matches.size()); Eigen::Matrix2Xd target_image_points(2, matches.size()); @@ -67,8 +68,8 @@ Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera const auto& match = matches[i]; Eigen::Vector2d undistorted_source_point; Eigen::Vector2d undistorted_target_point; - camera_params.Convert(match.source_point, &undistorted_source_point); - camera_params.Convert(match.target_point, &undistorted_target_point); + camera_params1.Convert(match.source_point, &undistorted_source_point); + camera_params2.Convert(match.target_point, &undistorted_target_point); source_image_points.col(i) = undistorted_source_point; target_image_points.col(i) = undistorted_target_point; cv_matches.emplace_back(cv::DMatch(i, i, i, 0)); @@ -76,8 +77,8 @@ Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera std::mutex mutex; CIDPairAffineMap affines; - BuildMapFindEssentialAndInliers(source_image_points, target_image_points, cv_matches, camera_params, false, 0, 0, - &mutex, &affines, &inliers, false, nullptr); + BuildMapFindEssentialAndInliers(source_image_points, target_image_points, cv_matches, camera_params1, camera_params2, + false, 0, 0, &mutex, &affines, &inliers, false, nullptr); const Eigen::Affine3d target_T_source = affines[std::make_pair(0, 0)]; return target_T_source.inverse(); } From 5c537bd8a1f1f5153af2ef61a76144e91f6b7594 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 23:26:49 -0700 Subject: [PATCH 43/49] set loading/saving --- .../sparse_mapping/protobuf/sparse_map.proto | 2 + localization/sparse_mapping/src/sparse_map.cc | 110 ++++++++++++------ .../sparse_mapping/tools/export_map.cc | 4 +- .../sparse_mapping/tools/nvm_visualize.cc | 1 - 4 files changed, 80 insertions(+), 37 deletions(-) diff --git a/localization/sparse_mapping/protobuf/sparse_map.proto b/localization/sparse_mapping/protobuf/sparse_map.proto index 2f42d0998e..9735277b54 100644 --- a/localization/sparse_mapping/protobuf/sparse_map.proto +++ b/localization/sparse_mapping/protobuf/sparse_map.proto @@ -36,6 +36,7 @@ message Frame { optional string name = 1; repeated Feature feature = 2; optional Affine3d pose = 3; + optional int32 camera_id = 4; } message Vector3d { @@ -124,5 +125,6 @@ message Map { optional int32 orgbrisk_threshold = 8; // this is no longer used but remains for compatability // histogram_equalization 1 means true, 0 means false, 2 means not known optional int32 histogram_equalization = 9 [default = 2]; + optional int32 num_cameras = 10; } diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index c058b2b49c..e149d3b245 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -89,16 +89,16 @@ namespace sparse_mapping { SparseMap::SparseMap(const std::vector& filenames, const std::string& detector, const camera::CameraParameters& params) : cid_to_filename_(filenames), - detector_(detector), - camera_params_(params) { + detector_(detector) { SetDefaultLocParams(); cid_to_descriptor_map_.resize(cid_to_filename_.size()); cid_to_keypoint_map_.resize(cid_to_filename_.size()); + cid_to_camera_id_.resize(cid_to_filename_.size()); + camera_id_to_camera_params_.emplace_back(params); } SparseMap::SparseMap(const std::string& protobuf_file, bool localization) - : camera_params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), Eigen::Vector2d(-1, -1)), - protobuf_file_(protobuf_file) { + : protobuf_file_(protobuf_file) { SetDefaultLocParams(); // The above camera params used bad values because we are expected to reload // later. @@ -108,8 +108,7 @@ SparseMap::SparseMap(const std::string& protobuf_file, bool localization) // Form a sparse map with given cameras/images, and no features SparseMap::SparseMap(const std::vector& cid_to_cam_t, const std::vector& filenames, const std::string& detector, const camera::CameraParameters& params) - : detector_(detector), - camera_params_(params) { + : detector_(detector) { SetDefaultLocParams(); if (filenames.size() != cid_to_cam_t.size()) LOG(FATAL) << "Expecting as many images as cameras"; @@ -127,6 +126,8 @@ SparseMap::SparseMap(const std::vector& cid_to_cam_t, const std // Initialize other data expected in the map cid_to_keypoint_map_.resize(num_cams); cid_to_descriptor_map_.resize(num_cams); + cid_to_camera_id_.resize(cid_to_filename_.size(), 0); + camera_id_to_camera_params_.emplace_back(params); } // Form a sparse map by reading a text file from disk. This is for comparing @@ -317,30 +318,64 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { detector_.Reset(map.detector_name()); // Check that the maps is correctly formed - assert(map.camera().focal_length_size() == 2); - assert(map.camera().optical_offset_size() == 2); - assert(map.camera().distorted_image_size_size() == 2); - assert(map.camera().undistorted_image_size_size() == 2); - typedef Eigen::Vector2d V2d; - typedef Eigen::Vector2i V2i; - camera_params_.SetFocalLength(V2d(map.camera().focal_length(0), - map.camera().focal_length(1))); - camera_params_.SetOpticalOffset(V2d(map.camera().optical_offset(0), - map.camera().optical_offset(1))); - camera_params_.SetDistortedSize(V2i(map.camera().distorted_image_size(0), - map.camera().distorted_image_size(1))); - camera_params_.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0), - map.camera().undistorted_image_size(1))); - Eigen::VectorXd distortion(map.camera().distortion_size()); - for (int i = 0; i < map.camera().distortion_size(); i++) { - distortion[i] = map.camera().distortion(i); + // For backwards compatibility, if num_cameras not set just load map.camera. + if (map.has_num_cameras()) { + for (int i = 0; i < map.num_cameras(); ++i) { + sparse_mapping_protobuf::CameraModel camera; + if (!ReadProtobufFrom(input, &camera)) { + LOG(FATAL) << "Failed to parse camera."; + } + camera::CameraParams camera_params; + assert(camera.focal_length_size() == 2); + assert(camera.optical_offset_size() == 2); + assert(camera.distorted_image_size_size() == 2); + assert(camera.undistorted_image_size_size() == 2); + typedef Eigen::Vector2d V2d; + typedef Eigen::Vector2i V2i; + camera_params.SetFocalLength(V2d(camera.focal_length(0), + camera.focal_length(1))); + camera_params.SetOpticalOffset(V2d(camera.optical_offset(0), + camera.optical_offset(1))); + camera_params.SetDistortedSize(V2i(camera.distorted_image_size(0), + camera.distorted_image_size(1))); + camera_params.SetUndistortedSize(V2i(camera.undistorted_image_size(0), + camera.undistorted_image_size(1))); + Eigen::VectorXd distortion(camera.distortion_size()); + for (int i = 0; i < camera.distortion_size(); i++) { + distortion[i] = camera.distortion(i); + } + camera_params.SetDistortion(distortion); + camera_id_to_camera_params_.emplace_back(camera_params); + } + } else { + camera::CameraParams camera_params; + assert(map.camera().focal_length_size() == 2); + assert(map.camera().optical_offset_size() == 2); + assert(map.camera().distorted_image_size_size() == 2); + assert(map.camera().undistorted_image_size_size() == 2); + typedef Eigen::Vector2d V2d; + typedef Eigen::Vector2i V2i; + camera_params.SetFocalLength(V2d(map.camera().focal_length(0), + map.camera().focal_length(1))); + camera_params.SetOpticalOffset(V2d(map.camera().optical_offset(0), + map.camera().optical_offset(1))); + camera_params.SetDistortedSize(V2i(map.camera().distorted_image_size(0), + map.camera().distorted_image_size(1))); + camera_params.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0), + map.camera().undistorted_image_size(1))); + Eigen::VectorXd distortion(map.camera().distortion_size()); + for (int i = 0; i < map.camera().distortion_size(); i++) { + distortion[i] = map.camera().distortion(i); + } + camera_params.SetDistortion(distortion); + camera_id_to_camera_params_.emplace_back(camera_params); } - camera_params_.SetDistortion(distortion); int num_frames = map.num_frames(); int num_landmarks = map.num_landmarks(); cid_to_filename_.resize(num_frames); + cid_to_camera_id_.resize(num_frames, 0); cid_to_descriptor_map_.resize(num_frames); if (!localization) { cid_to_keypoint_map_.resize(num_frames); @@ -358,6 +393,7 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { else cid_to_filename_[cid] = ""; + if (frame.has_camera_id()) cid_to_camera_id_[cid] = frame.camera_id(); // load keypoints if (!localization) @@ -531,17 +567,20 @@ void SparseMap::Save(const std::string & protobuf_file) const { else map.set_descriptor_depth(0); - sparse_mapping_protobuf::CameraModel* camera = map.mutable_camera(); - camera->add_focal_length(camera_params_.GetFocalVector()[0]); - camera->add_focal_length(camera_params_.GetFocalVector()[1]); - camera->add_optical_offset(camera_params_.GetOpticalOffset()[0]); - camera->add_optical_offset(camera_params_.GetOpticalOffset()[1]); - camera->add_distorted_image_size(camera_params_.GetDistortedSize()[0]); - camera->add_distorted_image_size(camera_params_.GetDistortedSize()[1]); - camera->add_undistorted_image_size(camera_params_.GetUndistortedSize()[0]); - camera->add_undistorted_image_size(camera_params_.GetUndistortedSize()[1]); - for (int i = 0; i < camera_params_.GetDistortion().size(); i++) { - camera->add_distortion(camera_params_.GetDistortion()[i]); + map.set_num_cameras(camera_id_to_camera_params_.size()); + for (const auto& camera_params : camera_id_to_camera_params_) { + sparse_mapping_protobuf::CameraModel* camera = map.add_camera(); + camera->add_focal_length(camera_params.GetFocalVector()[0]); + camera->add_focal_length(camera_params.GetFocalVector()[1]); + camera->add_optical_offset(camera_params.GetOpticalOffset()[0]); + camera->add_optical_offset(camera_params.GetOpticalOffset()[1]); + camera->add_distorted_image_size(camera_params.GetDistortedSize()[0]); + camera->add_distorted_image_size(camera_params.GetDistortedSize()[1]); + camera->add_undistorted_image_size(camera_params.GetUndistortedSize()[0]); + camera->add_undistorted_image_size(camera_params.GetUndistortedSize()[1]); + for (int i = 0; i < camera_params.GetDistortion().size(); i++) { + camera->add_distortion(camera_params.GetDistortion()[i]); + } } CHECK(cid_to_filename_.size() == cid_to_keypoint_map_.size()) @@ -575,6 +614,7 @@ void SparseMap::Save(const std::string & protobuf_file) const { if (!cid_to_filename_[cid].empty()) { frame.set_name(cid_to_filename_[cid]); } + frame.set_camera_id(cid_to_camera_id_[cid]); // set the features, required for (int fid = 0; fid < cid_to_keypoint_map_[cid].cols(); fid++) { diff --git a/localization/sparse_mapping/tools/export_map.cc b/localization/sparse_mapping/tools/export_map.cc index 44db27e0e1..6b37848e10 100644 --- a/localization/sparse_mapping/tools/export_map.cc +++ b/localization/sparse_mapping/tools/export_map.cc @@ -52,7 +52,9 @@ int main(int argc, char** argv) { sparse_mapping::SparseMap map(FLAGS_input_map); - camera::CameraParameters camera_params = map.GetCameraParameters(); + // Note: this assumes only one camera exists in the map, since the NVM + // format only supports one camera! + camera::CameraParameters camera_params = map.camera_id_to_camera_params_[0]; // This is very important, std::cout << "Saving the nvm file with interest point matches that "; diff --git a/localization/sparse_mapping/tools/nvm_visualize.cc b/localization/sparse_mapping/tools/nvm_visualize.cc index 7bc5e2888a..472dd53a67 100644 --- a/localization/sparse_mapping/tools/nvm_visualize.cc +++ b/localization/sparse_mapping/tools/nvm_visualize.cc @@ -22,7 +22,6 @@ #include #include #include -#include #include #include From 1de99470f86a0a3a7c6d76127905f5e841c6376e Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 23:31:30 -0700 Subject: [PATCH 44/49] updated headers --- .../sparse_mapping/include/sparse_mapping/reprojection.h | 3 ++- .../sparse_mapping/include/sparse_mapping/sparse_mapping.h | 3 ++- localization/sparse_mapping/include/sparse_mapping/tensor.h | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/localization/sparse_mapping/include/sparse_mapping/reprojection.h b/localization/sparse_mapping/include/sparse_mapping/reprojection.h index 3382ff9d98..379f9994e3 100644 --- a/localization/sparse_mapping/include/sparse_mapping/reprojection.h +++ b/localization/sparse_mapping/include/sparse_mapping/reprojection.h @@ -60,7 +60,8 @@ namespace sparse_mapping { **/ void BundleAdjust(std::vector > const& pid_to_cid_fid, std::vector const& cid_to_keypoint_map, - double focal_length, +std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector * cid_to_cam_t_global, std::vector * pid_to_xyz, std::vector > const& user_pid_to_cid_fid, diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h index c9ec62ae19..fb9fbfd26d 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h @@ -180,7 +180,8 @@ double ComputeRaysAngle(int pid, std::vector > const& pid_to_ std::vector const& cam_ctrs, std::vector const& pid_to_xyz); // Filter points by reprojection error and other criteria -void FilterPID(double reproj_thresh, camera::CameraParameters const& camera_params, +void FilterPID(double reproj_thresh, std::vector const& camera_id_to_camera_params, + std::vector const& cid_to_cam_t_global, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector >* pid_to_cid_fid, std::vector* pid_to_xyz, diff --git a/localization/sparse_mapping/include/sparse_mapping/tensor.h b/localization/sparse_mapping/include/sparse_mapping/tensor.h index e72cfc60ef..58381fbaa2 100644 --- a/localization/sparse_mapping/include/sparse_mapping/tensor.h +++ b/localization/sparse_mapping/include/sparse_mapping/tensor.h @@ -176,7 +176,8 @@ namespace sparse_mapping { // Triangulates all points given camera positions. This is better // than what is in sparse map as it uses multiple view information. - void Triangulate(bool rm_invalid_xyz, double focal_length, + void Triangulate(bool rm_invalid_xyz, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector > * pid_to_cid_fid, From bce16c87f7b97ddae026a05c54b8f7078e1c25d0 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sun, 21 Jul 2024 23:33:50 -0700 Subject: [PATCH 45/49] formatting --- .../sparse_mapping/include/sparse_mapping/reprojection.h | 2 +- .../sparse_mapping/include/sparse_mapping/sparse_mapping.h | 4 ++-- localization/sparse_mapping/include/sparse_mapping/tensor.h | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/localization/sparse_mapping/include/sparse_mapping/reprojection.h b/localization/sparse_mapping/include/sparse_mapping/reprojection.h index 379f9994e3..5e0243c8f4 100644 --- a/localization/sparse_mapping/include/sparse_mapping/reprojection.h +++ b/localization/sparse_mapping/include/sparse_mapping/reprojection.h @@ -60,7 +60,7 @@ namespace sparse_mapping { **/ void BundleAdjust(std::vector > const& pid_to_cid_fid, std::vector const& cid_to_keypoint_map, -std::vector const& cid_to_camera_id, + std::vector const& cid_to_camera_id, std::vector const& camera_id_to_camera_params, std::vector * cid_to_cam_t_global, std::vector * pid_to_xyz, diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h index fb9fbfd26d..1dfd3d2d00 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h @@ -180,8 +180,8 @@ double ComputeRaysAngle(int pid, std::vector > const& pid_to_ std::vector const& cam_ctrs, std::vector const& pid_to_xyz); // Filter points by reprojection error and other criteria -void FilterPID(double reproj_thresh, std::vector const& camera_id_to_camera_params, - std::vector const& cid_to_cam_t_global, +void FilterPID(double reproj_thresh, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector >* pid_to_cid_fid, std::vector* pid_to_xyz, diff --git a/localization/sparse_mapping/include/sparse_mapping/tensor.h b/localization/sparse_mapping/include/sparse_mapping/tensor.h index 58381fbaa2..cf382361f5 100644 --- a/localization/sparse_mapping/include/sparse_mapping/tensor.h +++ b/localization/sparse_mapping/include/sparse_mapping/tensor.h @@ -176,8 +176,8 @@ namespace sparse_mapping { // Triangulates all points given camera positions. This is better // than what is in sparse map as it uses multiple view information. - void Triangulate(bool rm_invalid_xyz, std::vector const& cid_to_camera_id, - std::vector const& camera_id_to_camera_params, + void Triangulate(bool rm_invalid_xyz, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector > * pid_to_cid_fid, From 93ab5452703f917a878c7dc434d1eeda26d9ad58 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 22 Jul 2024 00:02:13 -0700 Subject: [PATCH 46/49] more compile updates --- .../include/sparse_mapping/reprojection.h | 3 +- .../include/sparse_mapping/sparse_map.h | 9 +- .../include/sparse_mapping/tensor.h | 6 +- .../sparse_mapping/src/reprojection.cc | 10 +- localization/sparse_mapping/src/sparse_map.cc | 101 +++++++++--------- localization/sparse_mapping/src/tensor.cc | 14 +-- 6 files changed, 77 insertions(+), 66 deletions(-) diff --git a/localization/sparse_mapping/include/sparse_mapping/reprojection.h b/localization/sparse_mapping/include/sparse_mapping/reprojection.h index 5e0243c8f4..00b381706b 100644 --- a/localization/sparse_mapping/include/sparse_mapping/reprojection.h +++ b/localization/sparse_mapping/include/sparse_mapping/reprojection.h @@ -30,6 +30,7 @@ namespace camera { class CameraModel; + class CameraParameters; } namespace cv { @@ -83,7 +84,7 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, * **/ void BundleAdjustSmallSet(std::vector const& features_n, - double focal_length, + std::vector focal_length_n, std::vector * cam_t_global_n, Eigen::Matrix3Xd * pid_to_xyz, ceres::LossFunction * loss, diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index 29ca546dba..b6b1b9d547 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -178,13 +178,12 @@ struct SparseMap { * Return the parameters of the camera used to construct the map. **/ const camera::CameraParameters& camera_params(int cid) const { - return camera_id_to_camera_params_[cid_to_camera_id[cid]]; + return camera_id_to_camera_params_[cid_to_camera_id_[cid]]; } camera::CameraParameters GetCameraParameters(int cid) const { - return camera_id_to_camera_params_[cid_to_camera_id[cid]]; + return camera_id_to_camera_params_[cid_to_camera_id_[cid]]; } - // void SetCameraParameters(int cid, camera::CameraParameters& camera_params) {cid_to_camera_params_[cid] = - // camera_params;} + /** * Return the number of observations. Use this number to divide the final error to find the average pixel error. **/ @@ -218,10 +217,12 @@ struct SparseMap { // detect features with opencv void DetectFeaturesFromFile(std::string const& filename, + const camera::CameraParameters& camera_params, bool multithreaded, cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints); void DetectFeatures(cv::Mat const& image, + const camera::CameraParameters& camera_params, bool multithreaded, cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints); diff --git a/localization/sparse_mapping/include/sparse_mapping/tensor.h b/localization/sparse_mapping/include/sparse_mapping/tensor.h index cf382361f5..6159ad8a63 100644 --- a/localization/sparse_mapping/include/sparse_mapping/tensor.h +++ b/localization/sparse_mapping/include/sparse_mapping/tensor.h @@ -146,14 +146,16 @@ namespace sparse_mapping { // Filter the matches by a geometric constraint. Compute the essential matrix. void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, - std::vector const& matches, camera::CameraParameters const& camera_params, + std::vector const& matches, camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, std::vector* inlier_matches, std::vector* vec_inliers, Eigen::Matrix3d* essential_matrix, const int ransac_iterations = 4096); void BuildMapFindEssentialAndInliers(const Eigen::Matrix2Xd & keypoints1, const Eigen::Matrix2Xd & keypoints2, const std::vector & matches, - camera::CameraParameters const& camera_params, + camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, bool compute_inliers_only, size_t cam_a_idx, size_t cam_b_idx, std::mutex * match_mutex, diff --git a/localization/sparse_mapping/src/reprojection.cc b/localization/sparse_mapping/src/reprojection.cc index 789a293925..2eb4cd2217 100644 --- a/localization/sparse_mapping/src/reprojection.cc +++ b/localization/sparse_mapping/src/reprojection.cc @@ -166,7 +166,7 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, &cid_to_cam_t_global->at(cid_fid.first).translation()[0], &camera_aa_storage[3 * cid_fid.first], &p_pid_to_xyz->at(pid)[0], - &focal_length[cid_fid.first]); + &focal_lengths[cid_fid.first]); if (fix_all_cameras || (cid_fid.first < first || cid_fid.first > last) || fixed_cameras.find(cid_fid.first) != fixed_cameras.end()) { @@ -182,7 +182,7 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, problem.SetParameterBlockConstant(&p_pid_to_xyz->at(pid)[0]); } } - for (const auto& focal_length : focal_lengths) { + for (auto& focal_length : focal_lengths) { problem.SetParameterBlockConstant(&focal_length); } } @@ -236,10 +236,12 @@ void BundleAdjustSmallSet(std::vector const& features_n, &cam_t_global_n->at(cid).translation()[0], &aa.at(cid)[0], &pid_to_xyz->col(pid)[0], - &focal_length[cid]); + &focal_length_n[cid]); } } - problem.SetParameterBlockConstant(&focal_length[cid]); + for (size_t cid = 0; cid < n_cameras; ++cid) { + problem.SetParameterBlockConstant(&focal_length_n[cid]); + } // Solve the problem ceres::Solve(options, &problem, summary); diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index e149d3b245..0fc4056417 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -132,9 +132,8 @@ SparseMap::SparseMap(const std::vector& cid_to_cam_t, const std // Form a sparse map by reading a text file from disk. This is for comparing // bundler, nvm or theia maps. -SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vector const& all_image_files) - : camera_params_(Eigen::Vector2i(640, 480), Eigen::Vector2d::Constant(300), - Eigen::Vector2d(320, 240)) /* these are placeholders and must be changed */ { +SparseMap::SparseMap(bool bundler_format, std::string const& filename, + std::vector const& all_image_files) { SetDefaultLocParams(); std::string ext = ff_common::file_extension(filename); boost::to_lower(ext); @@ -325,50 +324,52 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { if (!ReadProtobufFrom(input, &camera)) { LOG(FATAL) << "Failed to parse camera."; } - camera::CameraParams camera_params; assert(camera.focal_length_size() == 2); assert(camera.optical_offset_size() == 2); assert(camera.distorted_image_size_size() == 2); assert(camera.undistorted_image_size_size() == 2); typedef Eigen::Vector2d V2d; typedef Eigen::Vector2i V2i; - camera_params.SetFocalLength(V2d(camera.focal_length(0), - camera.focal_length(1))); - camera_params.SetOpticalOffset(V2d(camera.optical_offset(0), - camera.optical_offset(1))); - camera_params.SetDistortedSize(V2i(camera.distorted_image_size(0), - camera.distorted_image_size(1))); - camera_params.SetUndistortedSize(V2i(camera.undistorted_image_size(0), - camera.undistorted_image_size(1))); + const V2d focal_length(camera.focal_length(0), + camera.focal_length(1)); + const V2d optical_center(camera.optical_offset(0), + camera.optical_offset(1)); + const V2i distorted_image_size(camera.distorted_image_size(0), + camera.distorted_image_size(1)); Eigen::VectorXd distortion(camera.distortion_size()); for (int i = 0; i < camera.distortion_size(); i++) { distortion[i] = camera.distortion(i); } - camera_params.SetDistortion(distortion); + camera::CameraParameters camera_params(distorted_image_size, focal_length, optical_center, distortion); + camera_params.SetDistortedSize(V2i(camera.distorted_image_size(0), + camera.distorted_image_size(1))); + camera_params.SetUndistortedSize(V2i(camera.undistorted_image_size(0), + camera.undistorted_image_size(1))); camera_id_to_camera_params_.emplace_back(camera_params); } } else { - camera::CameraParams camera_params; - assert(map.camera().focal_length_size() == 2); - assert(map.camera().optical_offset_size() == 2); - assert(map.camera().distorted_image_size_size() == 2); - assert(map.camera().undistorted_image_size_size() == 2); - typedef Eigen::Vector2d V2d; - typedef Eigen::Vector2i V2i; - camera_params.SetFocalLength(V2d(map.camera().focal_length(0), - map.camera().focal_length(1))); - camera_params.SetOpticalOffset(V2d(map.camera().optical_offset(0), - map.camera().optical_offset(1))); - camera_params.SetDistortedSize(V2i(map.camera().distorted_image_size(0), - map.camera().distorted_image_size(1))); - camera_params.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0), - map.camera().undistorted_image_size(1))); - Eigen::VectorXd distortion(map.camera().distortion_size()); - for (int i = 0; i < map.camera().distortion_size(); i++) { - distortion[i] = map.camera().distortion(i); - } - camera_params.SetDistortion(distortion); - camera_id_to_camera_params_.emplace_back(camera_params); + assert(map.camera().focal_length_size() == 2); + assert(map.camera().optical_offset_size() == 2); + assert(map.camera().distorted_image_size_size() == 2); + assert(map.camera().undistorted_image_size_size() == 2); + typedef Eigen::Vector2d V2d; + typedef Eigen::Vector2i V2i; + const V2d focal_length(map.camera().focal_length(0), + map.camera().focal_length(1)); + const V2d optical_center(map.camera().optical_offset(0), + map.camera().optical_offset(1)); + const V2i distorted_image_size(map.camera().distorted_image_size(0), + map.camera().distorted_image_size(1)); + Eigen::VectorXd distortion(map.camera().distortion_size()); + for (int i = 0; i < map.camera().distortion_size(); i++) { + distortion[i] = map.camera().distortion(i); + } + camera::CameraParameters camera_params(distorted_image_size, focal_length, optical_center, distortion); + camera_params.SetDistortedSize(V2i(map.camera().distorted_image_size(0), + map.camera().distorted_image_size(1))); + camera_params.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0), + map.camera().undistorted_image_size(1))); + camera_id_to_camera_params_.emplace_back(camera_params); } int num_frames = map.num_frames(); @@ -568,20 +569,6 @@ void SparseMap::Save(const std::string & protobuf_file) const { map.set_descriptor_depth(0); map.set_num_cameras(camera_id_to_camera_params_.size()); - for (const auto& camera_params : camera_id_to_camera_params_) { - sparse_mapping_protobuf::CameraModel* camera = map.add_camera(); - camera->add_focal_length(camera_params.GetFocalVector()[0]); - camera->add_focal_length(camera_params.GetFocalVector()[1]); - camera->add_optical_offset(camera_params.GetOpticalOffset()[0]); - camera->add_optical_offset(camera_params.GetOpticalOffset()[1]); - camera->add_distorted_image_size(camera_params.GetDistortedSize()[0]); - camera->add_distorted_image_size(camera_params.GetDistortedSize()[1]); - camera->add_undistorted_image_size(camera_params.GetUndistortedSize()[0]); - camera->add_undistorted_image_size(camera_params.GetUndistortedSize()[1]); - for (int i = 0; i < camera_params.GetDistortion().size(); i++) { - camera->add_distortion(camera_params.GetDistortion()[i]); - } - } CHECK(cid_to_filename_.size() == cid_to_keypoint_map_.size()) << "Number of CIDs in filenames and keypoint map do not match"; @@ -606,6 +593,24 @@ void SparseMap::Save(const std::string & protobuf_file) const { LOG(FATAL) << "Failed to write map to file."; } + for (const auto& camera_params : camera_id_to_camera_params_) { + sparse_mapping_protobuf::CameraModel camera; + camera.add_focal_length(camera_params.GetFocalVector()[0]); + camera.add_focal_length(camera_params.GetFocalVector()[1]); + camera.add_optical_offset(camera_params.GetOpticalOffset()[0]); + camera.add_optical_offset(camera_params.GetOpticalOffset()[1]); + camera.add_distorted_image_size(camera_params.GetDistortedSize()[0]); + camera.add_distorted_image_size(camera_params.GetDistortedSize()[1]); + camera.add_undistorted_image_size(camera_params.GetUndistortedSize()[0]); + camera.add_undistorted_image_size(camera_params.GetUndistortedSize()[1]); + for (int i = 0; i < camera_params.GetDistortion().size(); i++) { + camera.add_distortion(camera_params.GetDistortion()[i]); + } + if (!WriteProtobufTo(camera, output)) { + LOG(FATAL) << "Failed to write camera to file."; + } + } + // write the frames for (size_t cid = 0; cid < cid_to_filename_.size(); cid++) { sparse_mapping_protobuf::Frame frame; diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index 27e910965f..4fdc45a5ea 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -1183,8 +1183,6 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, sparse_mapping::SparseMap & C = *C_out; // Basic sanity checks (not exhaustive) - if ( !(A.GetCameraParameters() == B.GetCameraParameters()) ) - LOG(FATAL) << "The input maps don't have the same camera parameters."; if ( !(A.detector_ == B.detector_) ) LOG(FATAL) << "The input maps don't have the same detector and/or descriptor."; @@ -1222,8 +1220,8 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, } // Append B's camera params to A's. // TODO(rsoussan): Check for equality of camera params and consolidate - for (const auto& camera_params : B.camera_ids_to_camera_params_) { - A.camera_ids_to_camera_params_.append(camera_params); + for (const auto& camera_params : B.camera_id_to_camera_params_) { + A.camera_id_to_camera_params_.emplace_back(camera_params); } // Create cid_fid_to_pid_ for both maps, to be able to go from cid_fid to pid. @@ -1965,8 +1963,10 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X observationsb.col(i) = keypoints2.col(matches[i].trainIdx); } - std::pair image_size(camera_params.GetUndistortedSize()[0], - camera_params.GetUndistortedSize()[1]); + std::pair image_size1(camera_params1.GetUndistortedSize()[0], + camera_params1.GetUndistortedSize()[1]); + std::pair image_size2(camera_params2.GetUndistortedSize()[0], + camera_params2.GetUndistortedSize()[1]); Eigen::Matrix3d k1 = camera_params1.GetIntrinsicMatrix(); Eigen::Matrix3d k2 = camera_params2.GetIntrinsicMatrix(); @@ -1976,7 +1976,7 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X if (!interest_point::RobustEssential(k1, k2, observationsa, observationsb, essential_matrix, vec_inliers, - image_size, image_size, + image_size1, image_size2, &error_max, max_expected_error, ransac_iterations)) { VLOG(2) << " | Estimation of essential matrix failed!\n"; From 7e1602359ec505f67f3a46bfcea411ed6015d5d3 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 22 Jul 2024 11:01:32 -0700 Subject: [PATCH 47/49] more tool updates --- .../include/localization_node/localization.h | 1 + .../localization_node/src/localization.cc | 7 +++-- .../include/sparse_mapping/sparse_map.h | 18 ++++++++++--- localization/sparse_mapping/src/sparse_map.cc | 27 ++++++++++++------- .../sparse_mapping/tools/build_map.cc | 21 ++++++++------- .../tools/evaluate_localization.cc | 3 ++- .../tools/generate_localization_test.cc | 5 ++-- .../sparse_mapping/tools/import_map.cc | 4 +-- localization/sparse_mapping/tools/localize.cc | 3 ++- .../sparse_mapping/tools/localize_cams.cc | 10 +++---- .../sparse_mapping/tools/nvm_visualize.cc | 14 ++++++---- localization/sparse_mapping/tools/utilities.h | 3 ++- 12 files changed, 74 insertions(+), 42 deletions(-) diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index 9278f3aa00..f4e6b729e5 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -52,6 +52,7 @@ class Localizer { void AdjustThresholds(); sparse_mapping::SparseMap* map_; + std::shared_ptr camera::CameraParameters cam_params_; // Success params for adjusting keypoint thresholds std::deque successes_; ThresholdParams params_; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 7ba01f1265..7ed5da6ed3 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -30,7 +30,7 @@ namespace localization_node { Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {} bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) { - camera::CameraParameters cam_params(&config, "nav_cam"); + cam_params_.reset(camera::CameraParameters(&config, "nav_cam")); std::string prefix; const auto detector_name = map_->GetDetectorName(); if (detector_name == "ORGBRISK") { @@ -104,7 +104,6 @@ bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failu return false; } - map_->SetCameraParameters(cam_params); map_->SetLocParams(loc_params); map_->SetDetectorParams(min_features, max_features, detection_retries, min_threshold, default_threshold, max_threshold, too_many_ratio, too_few_ratio); @@ -126,10 +125,10 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa vl->header.frame_id = "world"; timer_.Start(); - map_->DetectFeatures(image_ptr->image, multithreaded, &image_descriptors, image_keypoints); + map_->DetectFeatures(image_ptr->image, *cam_params_, multithreaded, &image_descriptors, image_keypoints); camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - map_->GetCameraParameters()); + *cam_params_); std::vector landmarks; std::vector observations; if (!map_->Localize(image_descriptors, *image_keypoints, diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index b6b1b9d547..72760a3217 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -65,14 +65,22 @@ struct SparseMap { /** * Constructs a new sparse map from a list of image files and their - * associate keypoint and descriptor files. If use_cached_features - * is set to false, it reads the image files and performs feature - * detection instead. Does not perform bundle adjustment. + * associate keypoint and descriptor files. Assumes a single camera model + * used for all the images. **/ SparseMap(const std::vector & filenames, const std::string & detector, const camera::CameraParameters & params); + /** + * Constructs a new sparse map from a list of image files and their + * associate keypoint and descriptor files. Adds camera models + * and a map associating each image with its respective camera model. + **/ + SparseMap(const std::vector& filenames, const std::string& detector, + const std::vector& cid_to_camera_id, + const std::vector& camera_id_to_camera_params); + /** * Constructs a new sparse map from a protobuf file, with specified * vocabulary tree and optional parameters. @@ -184,6 +192,10 @@ struct SparseMap { return camera_id_to_camera_params_[cid_to_camera_id_[cid]]; } + void OverwriteCameraParameters(const camera::CameraParameters& camera_params, int camera_id = 0) { + camera_id_to_camera_params_[camera_id] = camera_params; + } + /** * Return the number of observations. Use this number to divide the final error to find the average pixel error. **/ diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 0fc4056417..92230e0a2f 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -97,6 +97,18 @@ SparseMap::SparseMap(const std::vector& filenames, const std::strin camera_id_to_camera_params_.emplace_back(params); } +SparseMap::SparseMap(const std::vector& filenames, const std::string& detector, + const std::vector& cid_to_camera_id, + const std::vector& camera_id_to_camera_params) + : cid_to_filename_(filenames), + detector_(detector), + cid_to_camera_id_(cid_to_camera_id), + camera_id_to_camera_params_(camera_id_to_camera_params) { + SetDefaultLocParams(); + cid_to_descriptor_map_.resize(cid_to_filename_.size()); + cid_to_keypoint_map_.resize(cid_to_filename_.size()); +} + SparseMap::SparseMap(const std::string& protobuf_file, bool localization) : protobuf_file_(protobuf_file) { SetDefaultLocParams(); @@ -276,11 +288,8 @@ void SparseMap::DetectFeatures() { for (size_t cid = 0; cid < num_files; cid++) { ff_common::PrintProgressBar(stdout, static_cast(cid) / static_cast(num_files - 1)); - pool.AddTask(&SparseMap::DetectFeaturesFromFile, this, - std::ref(cid_to_filename_[cid]), - multithreaded, - &cid_to_descriptor_map_[cid], - &cid_to_keypoint_map_[cid]); + pool.AddTask(&SparseMap::DetectFeaturesFromFile, this, std::ref(cid_to_filename_[cid]), + std::ref(camera_params(cid)), multithreaded, &cid_to_descriptor_map_[cid], &cid_to_keypoint_map_[cid]); } pool.Join(); @@ -869,8 +878,8 @@ bool SparseMap::Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const std::vector inlier_matches; std::vector vec_inliers; Eigen::Matrix3d essential_matrix; - FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params(cid), - &inlier_matches, &vec_inliers, &essential_matrix, + FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], pose->GetParameters(), + camera_params(cid), &inlier_matches, &vec_inliers, &essential_matrix, loc_params_.essential_ransac_iterations); all_matches[i] = inlier_matches; if (loc_params_.verbose_localization) @@ -986,7 +995,7 @@ bool SparseMap::Localize(std::string const& img_file, cv::Mat test_descriptors; Eigen::Matrix2Xd test_keypoints; bool multithreaded = false; - DetectFeaturesFromFile(img_file, multithreaded, &test_descriptors, &test_keypoints); + DetectFeaturesFromFile(img_file, pose->GetParameters(), multithreaded, &test_descriptors, &test_keypoints); return Localize(test_descriptors, test_keypoints, pose, inlier_landmarks, inlier_observations, @@ -1164,7 +1173,7 @@ bool SparseMap::Localize(const cv::Mat & image, camera::CameraModel* pose, bool multithreaded = false; cv::Mat test_descriptors; Eigen::Matrix2Xd test_keypoints; - DetectFeatures(image, multithreaded, &test_descriptors, &test_keypoints); + DetectFeatures(image, pose->GetParameters(), multithreaded, &test_descriptors, &test_keypoints); return Localize(test_descriptors, test_keypoints, pose, inlier_landmarks, inlier_observations, diff --git a/localization/sparse_mapping/tools/build_map.cc b/localization/sparse_mapping/tools/build_map.cc index bfe1fe002a..50f77e4ad7 100644 --- a/localization/sparse_mapping/tools/build_map.cc +++ b/localization/sparse_mapping/tools/build_map.cc @@ -236,11 +236,20 @@ void Rebuild() { LOG(INFO) << "Rebuilding map with " << FLAGS_rebuild_detector << " detector."; sparse_mapping::SparseMap original(FLAGS_output_map); - camera::CameraParameters params = original.GetCameraParameters(); + std::vector files(original.GetNumFrames()); + for (size_t i = 0; i < original.GetNumFrames(); i++) { + files[i] = original.GetFrameFilename(i); + } + + sparse_mapping::SparseMap map(files, FLAGS_rebuild_detector, original.cid_to_camera_id_, original.camera_id_to_camera_params_); + + // Replace camera model, assumes only one is used for the map if (FLAGS_rebuild_replace_camera) { char * bot_ptr = getenv("ASTROBEE_ROBOT"); if (bot_ptr == NULL) LOG(FATAL) << "Must set ASTROBEE_ROBOT."; + if (map.camera_id_to_camera_params_.size() != 1) + LOG(FATAL) << "When replacing camera, the original map must have been build with one camera, instead is used" << map.camera_id_to_camera_params_.size(); LOG(INFO) << "Using camera for robot: " << bot_ptr << "."; config_reader::ConfigReader config; config.AddFile("cameras.config"); @@ -249,16 +258,10 @@ void Rebuild() { exit(1); return; } - params = camera::CameraParameters(&config, FLAGS_robot_camera.c_str()); + const camera::CameraParameters params(&config, FLAGS_robot_camera.c_str()); + map.OverwriteCameraParameters(params); } - std::vector files(original.GetNumFrames()); - for (size_t i = 0; i < original.GetNumFrames(); i++) { - files[i] = original.GetFrameFilename(i); - } - - sparse_mapping::SparseMap map(files, FLAGS_rebuild_detector, params); - LOG(INFO) << "Detecting features."; map.DetectFeatures(); diff --git a/localization/sparse_mapping/tools/evaluate_localization.cc b/localization/sparse_mapping/tools/evaluate_localization.cc index a2a4ec6f93..6bc5ebe20c 100644 --- a/localization/sparse_mapping/tools/evaluate_localization.cc +++ b/localization/sparse_mapping/tools/evaluate_localization.cc @@ -65,7 +65,8 @@ int main(int argc, char** argv) { Eigen::Vector3d pos(x, y, z); trials++; - camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), map.GetCameraParameters()); + // Assume first camera model + camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), map.GetCameraParameters(0)); struct timeval a, b; gettimeofday(&a, NULL); if (!map.Localize(name, &camera)) { diff --git a/localization/sparse_mapping/tools/generate_localization_test.cc b/localization/sparse_mapping/tools/generate_localization_test.cc index 93df0e9416..c5c97e3f6d 100644 --- a/localization/sparse_mapping/tools/generate_localization_test.cc +++ b/localization/sparse_mapping/tools/generate_localization_test.cc @@ -39,8 +39,9 @@ int main(int argc, char** argv) { // print results for (size_t i = 0; i < map.GetNumFrames() - 1; i++) { - camera::CameraModel m1(map.GetFrameGlobalTransform(i), map.GetCameraParameters()); - camera::CameraModel m2(map.GetFrameGlobalTransform(i), map.GetCameraParameters()); + // Assume first camera model + camera::CameraModel m1(map.GetFrameGlobalTransform(i), map.GetCameraParameters(0)); + camera::CameraModel m2(map.GetFrameGlobalTransform(i), map.GetCameraParameters(0)); std::string name1 = map.GetFrameFilename(i); std::string name2 = map.GetFrameFilename(i+1); std::string base = name1.substr(0, name1.length() - 11); diff --git a/localization/sparse_mapping/tools/import_map.cc b/localization/sparse_mapping/tools/import_map.cc index c1a204aef7..b746b94cde 100644 --- a/localization/sparse_mapping/tools/import_map.cc +++ b/localization/sparse_mapping/tools/import_map.cc @@ -78,7 +78,7 @@ namespace { // Save the camera parameters for the given robot. camera::CameraParameters camera_params(&config, "nav_cam"); - map.SetCameraParameters(camera_params); + map.OverwriteCameraParameters(camera_params); // Replace the undistorted images with distorted ones std::vector distorted_images; @@ -191,7 +191,7 @@ int main(int argc, char** argv) { = camera::CameraParameters(Eigen::Vector2i(widx, widy), Eigen::Vector2d::Constant(f), Eigen::Vector2d(cx, cy)); - map.SetCameraParameters(camera_params); + map.OverwriteCameraParameters(camera_params); } else { // Replace the images and robot camera params with distorted (original) ones diff --git a/localization/sparse_mapping/tools/localize.cc b/localization/sparse_mapping/tools/localize.cc index 728315381e..363799e9b9 100644 --- a/localization/sparse_mapping/tools/localize.cc +++ b/localization/sparse_mapping/tools/localize.cc @@ -54,8 +54,9 @@ int main(int argc, char** argv) { FLAGS_histogram_equalization); // localize frame + // Use first camera model in map camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - map.GetCameraParameters()); + map.GetCameraParameters(0)); if (!map.Localize(img_file, &camera)) { LOG(ERROR) << "Failed to localize image."; return 1; diff --git a/localization/sparse_mapping/tools/localize_cams.cc b/localization/sparse_mapping/tools/localize_cams.cc index f9b9b70e4c..3294b2fad6 100644 --- a/localization/sparse_mapping/tools/localize_cams.cc +++ b/localization/sparse_mapping/tools/localize_cams.cc @@ -82,20 +82,20 @@ int main(int argc, char** argv) { sparse_mapping::SparseMap source(FLAGS_source_map); - if ( !(source.GetCameraParameters() == reference.GetCameraParameters()) ) - LOG(FATAL) << "The source and reference maps don't have the same camera parameters."; - int num_good_errors = 0; size_t num_frames = source.GetNumFrames(); for (size_t cid = 0; cid < num_frames; cid++) { + if ( !(source.GetCameraParameters(cid) == reference.GetCameraParameters(cid))) + LOG(FATAL) << "The source and reference maps don't have the same camera parameters."; + std::string img_file = source.GetFrameFilename(cid); // localize frame camera::CameraModel localized_cam(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - reference.GetCameraParameters()); + reference.GetCameraParameters(cid)); camera::CameraModel source_cam(source.GetFrameGlobalTransform(cid), - source.GetCameraParameters()); + source.GetCameraParameters(cid)); std::cout << "Source map position: " << source_cam.GetPosition().transpose() << "\n"; if (!reference.Localize(img_file, &localized_cam)) { diff --git a/localization/sparse_mapping/tools/nvm_visualize.cc b/localization/sparse_mapping/tools/nvm_visualize.cc index 472dd53a67..2095589830 100644 --- a/localization/sparse_mapping/tools/nvm_visualize.cc +++ b/localization/sparse_mapping/tools/nvm_visualize.cc @@ -100,7 +100,7 @@ void Draw3DFrame(cv::viz::Viz3d* window, const sparse_mapping::SparseMap & map, int cols; int rows; if (!image.empty()) { - f = map.GetCameraParameters().GetFocalLength(); + f = map.GetCameraParameters(cid).GetFocalLength(); cols = image.cols; rows = image.rows; } else { @@ -136,8 +136,9 @@ bool LocalizeFrame(MapViewerState* state, int frame) { cv::Mat image; // display localized images if (state->num_frames > 0) { + // Assume first camera model camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - state->map->GetCameraParameters()); + state->map->GetCameraParameters(0)); image = cv::imread(state->frames[frame], cv::IMREAD_GRAYSCALE); if (!state->map->Localize(state->frames[frame], &camera, &landmarks)) { LOG(ERROR) << "Failed to localize image."; @@ -165,7 +166,8 @@ bool LocalizeFrame(MapViewerState* state, int frame) { state->window->setViewerPose(new_pose); // state->window->getViewerPose().concatenate( // state->old_pose.inv()).concatenate(camera_pose)); } - double f = state->map->GetCameraParameters().GetFocalLength(); + // Assume first camera model + double f = state->map->GetCameraParameters(0).GetFocalLength(); cv::viz::WCameraPosition im_pose(cv::Vec2f(2 * atan(image.cols * 0.5 / f), 2 * atan(image.rows * 0.5 / f)), image, 0.5, cv::viz::Color::red()); im_pose.setRenderingProperty(cv::viz::LINE_WIDTH, 6.0); @@ -378,7 +380,7 @@ static void onMouse(int event, int x, int y, int, void*) { return; } - camera::CameraParameters camera_param = map.GetCameraParameters(); + camera::CameraParameters camera_param = map.GetCameraParameters(cid); const Eigen::Matrix2Xd & keypoint_map = map.GetFrameKeypoints(cid); // Locate that point in the image @@ -529,7 +531,9 @@ int main(int argc, char** argv) { LOG(INFO) << "\t" << map.GetNumFrames() << " cameras and " << map.GetNumLandmarks() << " points"; - camera::CameraParameters camera_param = map.GetCameraParameters(); + // Assume first camera model + // TODO(rsoussan): Add option to use robot camera parameters or pass camera parameters manually + camera::CameraParameters camera_param = map.GetCameraParameters(0); int cid = FLAGS_first; int num_images = map.GetNumFrames(); diff --git a/localization/sparse_mapping/tools/utilities.h b/localization/sparse_mapping/tools/utilities.h index 6a019f95e4..75f1388c4b 100644 --- a/localization/sparse_mapping/tools/utilities.h +++ b/localization/sparse_mapping/tools/utilities.h @@ -38,7 +38,8 @@ boost::optional Matches( vision_common::LKOpticalFlowFeatureDetectorAndMatcher& detector_and_matcher); Eigen::Affine3d EstimateAffine3d(const vision_common::FeatureMatches& matches, - const camera::CameraParameters& camera_params, std::vector& inliers); + const camera::CameraParameters& camera_params1, + const camera::CameraParameters& camera_params2, std::vector& inliers); vision_common::FeatureImage LoadImage(const int index, const std::vector& image_names, cv::Feature2D& detector); From c627c797f93c92e6e4c53f9cda627760f08c94c1 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 22 Jul 2024 11:10:34 -0700 Subject: [PATCH 48/49] updated localization node --- .../include/localization_node/localization.h | 3 ++- localization/localization_node/src/localization.cc | 2 +- localization/localization_node/src/localization_nodelet.cc | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index f4e6b729e5..37acce00d5 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -48,11 +48,12 @@ class Localizer { bool ReadParams(config_reader::ConfigReader& config, bool fatal_failure = true); bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, Eigen::Matrix2Xd* image_keypoints = NULL); + const camera::CameraParameters& camera_params() const { return *cam_params_; } private: void AdjustThresholds(); sparse_mapping::SparseMap* map_; - std::shared_ptr camera::CameraParameters cam_params_; + std::shared_ptr cam_params_; // Success params for adjusting keypoint thresholds std::deque successes_; ThresholdParams params_; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 7ed5da6ed3..5c7b218da4 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -30,7 +30,7 @@ namespace localization_node { Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {} bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) { - cam_params_.reset(camera::CameraParameters(&config, "nav_cam")); + cam_params_ = std::make_shared(&config, "nav_cam"); std::string prefix; const auto detector_name = map_->GetDetectorName(); if (detector_name == "ORGBRISK") { diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index 5363bafadb..0f2260083c 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -249,7 +249,7 @@ void LocalizationNodelet::Localize(void) { Eigen::Vector2d undistorted, distorted; undistorted[0] = vl.landmarks[i].u; undistorted[1] = vl.landmarks[i].v; - (map_->GetCameraParameters()).Convert(undistorted, &distorted); + (inst_->camera_params()).Convert(undistorted, &distorted); cv::circle(used_image->image, cv::Point(distorted[0], distorted[1]), 10, CV_RGB(255, 255, 255), 3, 8); cv::circle(used_image->image, cv::Point(distorted[0], distorted[1]), 6, CV_RGB(0, 0, 0), 3, 8); } @@ -261,7 +261,7 @@ void LocalizationNodelet::Localize(void) { Eigen::Vector2d undistorted, distorted; undistorted[0] = image_keypoints.col(i)[0]; undistorted[1] = image_keypoints.col(i)[1]; - (map_->GetCameraParameters()).Convert(undistorted, &distorted); + (inst_->camera_params()).Convert(undistorted, &distorted); cv::circle(detected_image->image, cv::Point(distorted[0], distorted[1]), 10, CV_RGB(255, 255, 255), 3, 8); cv::circle(detected_image->image, cv::Point(distorted[0], distorted[1]), 6, CV_RGB(0, 0, 0), 2, 8); } From 3f74d718f57827a3f99f62c966d27c7412d3cf46 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 29 Jul 2024 16:37:27 -0700 Subject: [PATCH 49/49] fixed merge bugs --- localization/sparse_mapping/src/tensor.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index 4fdc45a5ea..86906554af 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -1209,6 +1209,7 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, C.cid_to_keypoint_map_ .resize(num_ccid); C.cid_to_cam_t_global_ .resize(num_ccid); C.cid_to_descriptor_map_.resize(num_ccid); + C.cid_to_camera_id_ .resize(num_ccid); for (int cid = 0; cid < num_bcid; cid++) { // C.cid_to_filename_ already contains A.cid_to_filename_, etc. int c = num_acid + cid; @@ -1221,7 +1222,7 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, // Append B's camera params to A's. // TODO(rsoussan): Check for equality of camera params and consolidate for (const auto& camera_params : B.camera_id_to_camera_params_) { - A.camera_id_to_camera_params_.emplace_back(camera_params); + C.camera_id_to_camera_params_.emplace_back(camera_params); } // Create cid_fid_to_pid_ for both maps, to be able to go from cid_fid to pid. @@ -1962,7 +1963,6 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X observationsa.col(i) = keypoints1.col(matches[i].queryIdx); observationsb.col(i) = keypoints2.col(matches[i].trainIdx); } - std::pair image_size1(camera_params1.GetUndistortedSize()[0], camera_params1.GetUndistortedSize()[1]); std::pair image_size2(camera_params2.GetUndistortedSize()[0],