From 5bd721d3bd420d38d856d79ba4d5bad4a8fa2311 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sat, 29 Jun 2024 10:45:45 -0700 Subject: [PATCH 01/12] 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/12] 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/12] 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/12] 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/12] 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 aafe4f2467469ad3799d5f8b3b25e78d2d9281b0 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Mon, 1 Jul 2024 10:42:36 -0700 Subject: [PATCH 06/12] 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 07/12] 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 08/12] 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 09/12] 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 10/12] 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 11/12] 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 12/12] 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;