Updated Map Matching With TEBLID and CLAHE

astrobee/config/localization.config

@@ -15,17 +15,102 @@
 -- License for the specific language governing permissions and limitations
 -- under the License.
 
-num_similar = 20
-ransac_inlier_tolerance = 5
-ransac_iterations = 100
-early_break_landmarks = 100
-histogram_equalization = 1
-min_features = 200
-max_features = 800
-detection_retries = 1
-num_threads = 1
-min_brisk_threshold = 20.0
-default_brisk_threshold = 90.0
-max_brisk_threshold = 110.0
 matched_features_on = false
 all_features_on = false
+num_threads = 1
+verbose_localization = false
+visualize_localization_matches = false
+
+-- BRISK
+brisk_num_similar = 20
+brisk_min_query_score_ratio = 0
+brisk_ransac_inlier_tolerance = 5 
+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_add_similar_images = false
+brisk_add_best_previous_image = false
+brisk_hamming_distance = 90
+brisk_goodness_ratio = 0.8
+brisk_use_clahe = false 
+brisk_num_extra_localization_db_images = 0
+-- Detection Params
+brisk_min_threshold = 20.0
+brisk_default_threshold = 90.0
+brisk_max_threshold = 110.0
+brisk_min_features = 200 
+brisk_max_features = 800
+brisk_detection_retries = 1
+-- Localizer Threshold Params
+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
+
+-- TEBLID512
+teblid512_num_similar = 20
+teblid512_min_query_score_ratio = 0.45
+teblid512_ransac_inlier_tolerance = 3
+teblid512_num_ransac_iterations = 1000
+teblid512_early_break_landmarks = 100
+teblid512_histogram_equalization = 3
+teblid512_check_essential_matrix = true 
+teblid512_essential_ransac_iterations = 2000
+teblid512_add_similar_images = true 
+teblid512_add_best_previous_image = true
+teblid512_hamming_distance = 85
+teblid512_goodness_ratio = 0.8
+teblid512_use_clahe = true 
+teblid512_num_extra_localization_db_images = 0
+
+-- Detection Params
+teblid512_min_threshold = 20.0
+teblid512_default_threshold = 60.0
+teblid512_max_threshold = 110.0
+teblid512_min_features = 1000
+teblid512_max_features = 3000
+teblid512_detection_retries = 1
+
+-- Localizer Threshold Params
+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
+
+-- TEBLID256
+teblid256_num_similar = 20
+teblid256_min_query_score_ratio = 0.45
+teblid256_ransac_inlier_tolerance = 3
+teblid256_num_ransac_iterations = 1000
+teblid256_early_break_landmarks = 100
+teblid256_histogram_equalization = 3
+teblid256_check_essential_matrix = true 
+teblid256_essential_ransac_iterations = 2000
+teblid256_add_similar_images = true 
+teblid256_add_best_previous_image = true
+teblid256_hamming_distance = 85
+teblid256_goodness_ratio = 0.8
+teblid256_use_clahe = true 
+teblid256_num_extra_localization_db_images = 0
+
+-- Detection Params
+teblid256_min_threshold = 20.0
+teblid256_default_threshold = 60.0
+teblid256_max_threshold = 110.0
+teblid256_min_features = 1000
+teblid256_max_features = 3000
+teblid256_detection_retries = 1
+
+-- Localizer Threshold Params
+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

localization/interest_point/CMakeLists.txt

@@ -61,6 +61,7 @@ add_library(interest_point
   src/brisk.cc
   src/essential.cc
   src/matching.cc
+  src/BAD.cpp
 )
 add_dependencies(interest_point ${catkin_EXPORTED_TARGETS})
 target_link_libraries(interest_point ${OPENMVG_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

localization/interest_point/include/interest_point/BAD.h

@@ -0,0 +1,104 @@
+/**
+ * @copyright 2021 Xoan Iago Suarez Canosa. All rights reserved.
+ * Constact: [email protected]
+ * Software developed in the PhD: Low-level vision for resource-limited devices
+ * Repo link: https://github.com/iago-suarez/efficient-descriptors
+ * License: Apache 2
+ * TODO(rsoussan): Remove this and BAD.cpp and switch to opencv TEBLID if opencv
+ * version upgraded.
+ */
+
+#ifndef INTEREST_POINT_BAD_H_
+#define INTEREST_POINT_BAD_H_
+
+#include <opencv2/opencv.hpp>
+
+#include <vector>
+#include <string>
+
+namespace upm {
+
+/**
+ * Implementation of the Box Average Difference (BAD) descriptor. The method uses features
+ * computed from the difference of the average gray values of two boxes in the patch.
+ *
+ * Each pair of boxes is represented with a BoxPairParams struct. After obtaining the feature
+ * from them, the i'th feature is thresholded with thresholds_[i], producing the binary
+ * weak-descriptor.
+ */
+class BAD : public cv::Feature2D {
+ public:
+  /**
+     * @brief  Descriptor number of bits, each bit is a weak-descriptor.
+     * The user can choose between 512 or 256 bits.
+     */
+  enum BadSize {
+    SIZE_512_BITS = 100, SIZE_256_BITS = 101,
+  };
+
+  /**
+   * @param scale_factor Adjust the sampling window around detected keypoints:
+    - <b> 1.00f </b> should be the scale for ORB keypoints
+    - <b> 6.75f </b> should be the scale for SIFT detected keypoints
+    - <b> 6.25f </b> is default and fits for KAZE, SURF detected keypoints
+    - <b> 5.00f </b> should be the scale for AKAZE, MSD, AGAST, FAST, BRISK keypoints
+   * @param n_bits Determine the number of bits in the descriptor. Should be either
+     BAD::SIZE_512_BITS or BAD::SIZE_256_BITS.
+   */
+  explicit BAD(float scale_factor = 1.0f, BadSize n_bits = SIZE_512_BITS);
+
+  /**
+   * @brief Creates the BAD descriptor.
+   * @param scale_factor Adjust the sampling window around detected keypoints:
+    - <b> 1.00f </b> should be the scale for ORB keypoints
+    - <b> 6.75f </b> should be the scale for SIFT detected keypoints
+    - <b> 6.25f </b> is default and fits for KAZE, SURF detected keypoints
+    - <b> 5.00f </b> should be the scale for AKAZE, MSD, AGAST, FAST, BRISK keypoints
+   * @param n_bits
+   * @return
+   */
+  static cv::Ptr<BAD> create(float scale_factor = 1.0f, BadSize n_bits = SIZE_512_BITS) {
+    return cv::makePtr<upm::BAD>(scale_factor, n_bits);
+  }
+
+  /** @brief Computes the descriptors for a set of keypoints detected in an image (first variant) or image set
+  (second variant).
+
+  @param image Image.
+  @param keypoints Input collection of keypoints. Keypoints for which a descriptor cannot be
+  computed are removed. Sometimes new keypoints can be added, for example: SIFT duplicates keypoint
+  with several dominant orientations (for each orientation).
+  @param descriptors Computed descriptors. In the second variant of the method descriptors[i] are
+  descriptors computed for a keypoints[i]. Row j is the keypoints (or keypoints[i]) is the
+  descriptor for keypoint j-th keypoint.
+   */
+  void compute(cv::InputArray image,
+               CV_OUT CV_IN_OUT std::vector<cv::KeyPoint> &keypoints,
+               cv::OutputArray descriptors) override;
+
+  int descriptorSize() const override { return box_params_.size() / 8; }
+  int descriptorType() const override { return CV_8UC1; }
+  int defaultNorm() const override { return cv::NORM_HAMMING; }
+  bool empty() const override { return false; }
+
+  cv::String getDefaultName() const override { return std::string("BAD") + std::to_string(box_params_.size()); }
+
+  // Struct representing a pair of boxes in the patch
+  struct BoxPairParams {
+    int x1, x2, y1, y2, boxRadius;
+  };
+
+ protected:
+  // Computes the BADdescriptor
+  void computeBAD(const cv::Mat &integral_img,
+                  const std::vector<cv::KeyPoint> &keypoints,
+                  cv::Mat &descriptors);
+
+  std::vector<float> thresholds_;
+  std::vector<BoxPairParams> box_params_;
+  float scale_factor_ = 1;
+  cv::Size patch_size_ = {32, 32};
+};
+
+}  // namespace upm
+#endif  // INTEREST_POINT_BAD_H_

localization/interest_point/include/interest_point/essential.h

@@ -27,22 +27,17 @@ namespace interest_point {
 
   // Performs a robust, ransac, solving for the essential matrix
   // between interest point measurements in x1 and x2.
-  bool RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2,
-                       Eigen::Matrix2Xd const& x1, Eigen::Matrix2Xd const& x2,
-                       Eigen::Matrix3d * e,
-                       std::vector<size_t> * vec_inliers,
-                       std::pair<size_t, size_t> const& size1,
-                       std::pair<size_t, size_t> const& size2,
-                       double * error_max,
-                       double precision);
+bool RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, Eigen::Matrix2Xd const& x1,
+                     Eigen::Matrix2Xd const& x2, Eigen::Matrix3d* e, std::vector<size_t>* vec_inliers,
+                     std::pair<size_t, size_t> const& size1, std::pair<size_t, size_t> const& size2, double* error_max,
+                     double precision, const int ransac_iterations = 4096);
 
-  // Solves for the RT (Rotation and Translation) from the essential
-  // matrix and x1 and x2. There are 4 possible, and this returns the
-  // best of the 4 solutions.
-  bool EstimateRTFromE(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2,
-                       Eigen::Matrix2Xd const& x1, Eigen::Matrix2Xd const& x2,
-                       Eigen::Matrix3d const& e, std::vector<size_t> const& vec_inliers,
-                       Eigen::Matrix3d * r, Eigen::Vector3d * t);
+// Solves for the RT (Rotation and Translation) from the essential
+// matrix and x1 and x2. There are 4 possible, and this returns the
+// best of the 4 solutions.
+bool EstimateRTFromE(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, Eigen::Matrix2Xd const& x1,
+                     Eigen::Matrix2Xd const& x2, Eigen::Matrix3d const& e, std::vector<size_t> const& vec_inliers,
+                     Eigen::Matrix3d* r, Eigen::Vector3d* t);
 
 }  // end namespace interest_point
 

localization/interest_point/include/interest_point/matching.h

@@ -18,6 +18,7 @@
 #ifndef INTEREST_POINT_MATCHING_H_
 #define INTEREST_POINT_MATCHING_H_
 
+#include <boost/optional.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <Eigen/Core>
 
@@ -45,9 +46,12 @@ namespace interest_point {
     void GetDetectorParams(int & min_features, int & max_features, int & max_retries,
                            double & min_thresh, double & default_thresh, double & max_thresh);
 
+    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 last_keypoint_count_;
   };
 
   class FeatureDetector {
@@ -81,16 +85,20 @@ namespace interest_point {
     friend bool operator== (FeatureDetector const& A, FeatureDetector const& B) {
       return (A.detector_name_ == B.detector_name_);
     }
+
+    DynamicDetector& dynamic_detector() { return *detector_; }
   };
 
   /**
    * descriptor is what opencv descriptor was used to make the descriptors
    * the descriptor maps are the features in the two images
    * matches is output to contain the matching features between the two images
+   * Optionally pass hamming distance and goodness ratio, otherwise flag
+   * values are used.
    **/
-  void FindMatches(const cv::Mat & img1_descriptor_map,
-                   const cv::Mat & img2_descriptor_map,
-                   std::vector<cv::DMatch> * matches);
+  void FindMatches(const cv::Mat& img1_descriptor_map, const cv::Mat& img2_descriptor_map,
+                   std::vector<cv::DMatch>* matches, boost::optional<int> hamming_distance = boost::none,
+                   boost::optional<double> goodness_ratio = boost::none);
 }  // namespace interest_point
 
 #endif  // INTEREST_POINT_MATCHING_H_