From bf30648ab733e3842bb17098d71fdaa3518b5f5d Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Wed, 3 Jul 2024 13:22:46 -0700 Subject: [PATCH] Release 0.19.1 (#799) * updated registration timeout * updated gl duration * updated brisk params * added vl runtime to msg * fixed loc nodelet read params bug * Release 0.19.1 * don't publish haz cam in sim by default * added scaling when changing threshold ratios for teblid * added rounding when casting interest point dynamic thresholds * updated teblid default thres * added toomany/toofew ratios as params * adding adjusting num similar images * fixed loc header * remove else for adjust num similar * visual landmarks adding field bmr rule * updated loc min success rate config * set valid to true * localizer parameter in test changed * fixing release spaces and # * fixed recording toomany and toofew ratios during mapping * avoid adjusting thresholds if success history size is 0 --------- Co-authored-by: Ryan Soussan --- RELEASE.md | 12 +- astrobee.doxyfile | 2 +- astrobee/CMakeLists.txt | 2 +- astrobee/config/localization.config | 38 ++-- .../localization/graph_localizer.config | 2 +- .../localization/localization_manager.config | 2 +- astrobee/config/simulation/simulation.config | 4 +- .../2023_07_01_VisualLandmarks_add_field.bmr | 205 ++++++++++++++++++ .../ff_msgs/msg/VisualLandmarks.msg | 1 + debian/changelog | 6 + .../include/interest_point/matching.h | 27 +-- localization/interest_point/src/matching.cc | 139 +++++++----- .../include/localization_node/localization.h | 8 +- .../localization_node/src/localization.cc | 39 ++-- .../src/localization_nodelet.cc | 4 + .../test_graph_localizer_parameter_reader.cc | 2 +- .../include/sparse_mapping/sparse_map.h | 4 +- localization/sparse_mapping/src/sparse_map.cc | 17 +- 18 files changed, 393 insertions(+), 121 deletions(-) create mode 100644 communications/ff_msgs/bmr/2023_07_01_VisualLandmarks_add_field.bmr diff --git a/RELEASE.md b/RELEASE.md index 6ebcf08ba5..28addb61ad 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 @@ -8,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 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/astrobee/config/localization.config b/astrobee/config/localization.config index ab46b78f90..7bfe466c07 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 @@ -42,14 +42,16 @@ 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 +brisk_success_history_size = 0 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 @@ -69,19 +71,21 @@ 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 +teblid512_too_many_ratio = 1.05 +teblid512_too_few_ratio = 0.95 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_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 @@ -101,16 +105,18 @@ 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 +teblid256_too_many_ratio = 1.05 +teblid256_too_few_ratio = 0.95 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_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/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 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, 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; 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..ab9d8c43b9 --- /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 = True + + 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. 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/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 diff --git a/localization/interest_point/include/interest_point/matching.h b/localization/interest_point/include/interest_point/matching.h index 21cb10328a..aeb38dbf80 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, @@ -43,14 +44,14 @@ 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_; } protected: - unsigned int min_features_, max_features_, max_retries_; - double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_; + int min_features_, max_features_, max_retries_; + double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_, too_few_ratio_, too_many_ratio_; int last_keypoint_count_; }; @@ -65,22 +66,22 @@ 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); 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 6d9d351cf4..4e3d3234bb 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."); @@ -62,25 +67,44 @@ 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."); +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, 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, @@ -106,10 +130,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 +150,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 +170,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 +185,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 +203,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 { @@ -197,9 +221,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,15 +228,29 @@ namespace interest_point { teblid_->compute(image, *keypoints, *keypoints_description); } virtual void TooMany(void) { - dynamic_thresh_ *= 1.1; - dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility + 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; + if (keypoint_diff < kKeypointDiff) { + threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; + } + dynamic_thresh_ *= threshold_ratio; + dynamic_thresh_ = std::lround(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; - dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility + 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; + if (keypoint_diff < kKeypointDiff) { + threshold_ratio += (1.0 - threshold_ratio)*(kKeypointDiff - keypoint_diff)/kKeypointDiff; + } + dynamic_thresh_ *= threshold_ratio; + dynamic_thresh_ = std::lround(dynamic_thresh_); // for backwards compatibility if (dynamic_thresh_ < min_thresh_) dynamic_thresh_ = min_thresh_; brisk_->setThreshold(dynamic_thresh_); @@ -226,23 +261,21 @@ 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, 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) { @@ -252,9 +285,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) { @@ -271,6 +304,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; @@ -279,6 +314,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; } @@ -287,16 +324,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/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index d4a0837f45..7a22665ad3 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 @@ -36,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 { @@ -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..a4eba47b28 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); @@ -78,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. @@ -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, @@ -111,6 +113,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 +125,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()); @@ -149,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(); @@ -159,23 +166,21 @@ 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; - } + } + 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; - } + } + 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; } } } 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_); } 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); } 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..d6c758d17e 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(); } @@ -710,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();