From 5bd721d3bd420d38d856d79ba4d5bad4a8fa2311 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Sat, 29 Jun 2024 10:45:45 -0700 Subject: [PATCH 01/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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/21] 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();