refactor: redesign feature detector API

src/ekf-mono-slam/CMakeLists.txt

@@ -30,14 +30,13 @@ file(GLOB_RECURSE math_src src/math/*.cpp)
 file(GLOB_RECURSE filter_src src/filter/*.cpp)
 file(GLOB_RECURSE image_src src/image/*.cpp)
 file(GLOB_RECURSE feature_src src/feature/*.cpp)
+file(GLOB_RECURSE matching_src src/matching/*.cpp)
 
-# Fetch and set the msg and srv files
+# Fetch and set the msg
 file(GLOB msg_files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg)
-file(GLOB srv_files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} srv/*.srv)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${msg_files}
-  ${srv_files}
   DEPENDENCIES
   sensor_msgs
   std_msgs
@@ -68,18 +67,13 @@ link_libraries(
 add_executable(file_sequence_image src/file_sequence_image_node.cpp ${config_src} ${image_src})
 ament_target_dependencies(file_sequence_image PUBLIC rclcpp std_msgs sensor_msgs cv_bridge image_transport)
 
-# Create the feature detector target
-add_executable(feature_detector src/feature_detector_node.cpp ${feature_src} ${math_src})
-ament_target_dependencies(feature_detector PUBLIC rclcpp std_msgs sensor_msgs cv_bridge)
-
 # Create the EKF target
-add_executable(ekf src/ekf_node.cpp ${filter_src} ${math_src} ${feature_src})
+add_executable(ekf src/ekf_node.cpp ${filter_src} ${math_src} ${feature_src} ${matching_src})
 ament_target_dependencies(ekf PUBLIC rclcpp std_msgs sensor_msgs geometry_msgs cv_bridge image_transport)
 
 install(TARGETS
   file_sequence_image
   ekf
-  feature_detector
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/ekf-mono-slam/include/ekf_node.h

@@ -5,9 +5,9 @@
 #include <rerun.hpp>
 #include <rerun/recording_stream.hpp>
 
+#include "ekf_mono_slam/msg/covariance_matrix.hpp"
 #include "ekf_mono_slam/msg/image_feature_measurement_array.hpp"
 #include "ekf_mono_slam/msg/state.hpp"
-#include "ekf_mono_slam/srv/feature_detect.hpp"
 #include "filter/ekf.h"
 #include "image_transport/image_transport.hpp"
 #include "sensor_msgs/msg/image.hpp"
@@ -18,9 +18,7 @@ class EKFNode final : public rclcpp::Node {
   ~EKFNode() override = default;
 
  private:
-  EKF ekf_;
-  rclcpp::Client<ekf_mono_slam::srv::FeatureDetect>::SharedPtr
-    feature_detect_client_;
+  EKF filter_;
   std::shared_ptr<image_transport::Subscriber> image_subscriber_;
   rclcpp::Publisher<ekf_mono_slam::msg::State>::SharedPtr state_publisher_;
   rclcpp::Publisher<ekf_mono_slam::msg::CovarianceMatrix>::SharedPtr
@@ -29,7 +27,4 @@ class EKFNode final : public rclcpp::Node {
   std::shared_ptr<rerun::RecordingStream> rec_;
 
   void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg);
-
-  void init_callback(const rclcpp::Client<
-                     ekf_mono_slam::srv::FeatureDetect>::SharedFuture& future);
 };

src/ekf-mono-slam/include/feature/feature_detector.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <opencv2/core/types.hpp>
 #include <opencv2/features2d.hpp>
 
@@ -40,6 +41,15 @@ class FeatureDetector final {
     const std::vector<std::shared_ptr<ImageFeaturePrediction>>& predictions
   );
 
+  std::vector<std::shared_ptr<ImageFeatureMeasurement>> detect_and_compute(
+    const cv::Mat& image, const cv::Mat& image_mask
+  );
+
+  static void build_image_mask(
+    const cv::Mat& image_mask,
+    const std::vector<std::shared_ptr<ImageFeaturePrediction>>& predictions
+  );
+
  private:
   std::vector<std::shared_ptr<ImageFeatureMeasurement>> image_features_;
   cv::Ptr<cv::FeatureDetector> detector_;
@@ -50,25 +60,18 @@ class FeatureDetector final {
 
   static cv::Ptr<cv::FeatureDetector> create(Type type);
 
-  static void build_image_mask(
-    const cv::Mat& image_mask,
-    const std::vector<std::shared_ptr<ImageFeaturePrediction>>& predictions
-  );
-
   void search_features_by_zone(
     const cv::Mat& image_mask,
-    const std::vector<cv::KeyPoint>& keypoints,
-    const cv::Mat& descriptors,
+    const std::vector<std::shared_ptr<ImageFeatureMeasurement>>& image_features,
     const std::vector<std::shared_ptr<ImageFeaturePrediction>>& predictions
   );
 
   [[nodiscard]] std::vector<std::shared_ptr<Zone>> create_zones() const;
 
   void group_features_and_prediction_by_zone(
     std::vector<std::shared_ptr<Zone>>& zones,
-    const std::vector<std::shared_ptr<ImageFeaturePrediction>>& predictions,
-    const std::vector<cv::KeyPoint>& keypoints,
-    const cv::Mat& descriptors
+    const std::vector<std::shared_ptr<ImageFeatureMeasurement>>& features,
+    const std::vector<std::shared_ptr<ImageFeaturePrediction>>& predictions
   ) const;
 
   void compute_image_feature_measurements(

src/ekf-mono-slam/include/feature/image_feature_prediction.h

@@ -2,6 +2,7 @@
 
 #include <eigen3/Eigen/Dense>
 
+#include "feature/ellipse.h"
 #include "image_feature.h"
 
 class ImageFeaturePrediction final : public ImageFeature {
@@ -22,6 +23,8 @@ class ImageFeaturePrediction final : public ImageFeature {
     const Eigen::Vector3d& directional_vector, int index
   );
 
+  [[nodiscard]] Ellipse ellipse() const;
+
  private:
   Eigen::Matrix2d covariance_matrix_;
 };

src/ekf-mono-slam/include/feature/map_feature.h

@@ -35,7 +35,7 @@ class MapFeature {
 
   [[nodiscard]] int index() const { return index_; }
 
-  [[nodiscard]] std::shared_ptr<ImageFeaturePrediction> prediction() {
+  [[nodiscard]] std::shared_ptr<ImageFeaturePrediction> prediction() const {
     return prediction_;
   }
 

src/ekf-mono-slam/include/filter/ekf.h

@@ -4,6 +4,7 @@
 
 #include "covariance_matrix.h"
 #include "feature/feature_detector.h"
+#include "feature/map_feature.h"
 #include "state.h"
 
 class EKF final {
@@ -27,15 +28,17 @@ class EKF final {
     return feature_detector_;
   }
 
+  [[nodiscard]] std::vector<std::shared_ptr<MapFeature>> features() const {
+    return state_->features();
+  }
+
   [[nodiscard]] bool is_initilized() const {
     return !state_->depth_features().empty() ||
            !state_->inverse_depth_features().empty();
   }
 
   void predict() const;
 
-  void match_predicted_features(const cv::Mat& image);
-
   void add_features(
     const std::vector<std::shared_ptr<ImageFeatureMeasurement>>& features
   ) const;

src/ekf-mono-slam/src/ekf_node.cpp

@@ -8,6 +8,7 @@
 #include <rerun/archetypes/text_log.hpp>
 #include <rerun/recording_stream.hpp>
 
+#include "feature/feature_detector.h"
 #include "feature/image_feature_measurement.h"
 
 EKFNode::EKFNode() : Node("ekf_node") {
@@ -21,8 +22,6 @@ EKFNode::EKFNode() : Node("ekf_node") {
       "raw"
     )
   );
-  feature_detect_client_ =
-    this->create_client<ekf_mono_slam::srv::FeatureDetect>("features/detect");
   state_publisher_ =
     this->create_publisher<ekf_mono_slam::msg::State>("filter/state", 10);
   covariance_publisher_ =
@@ -36,60 +35,25 @@ EKFNode::EKFNode() : Node("ekf_node") {
   rec_->log_file_from_path(std::filesystem::path("rerun/my_app.rbl"));
 }
 
-void EKFNode::init_callback(
-  const rclcpp::Client<ekf_mono_slam::srv::FeatureDetect>::SharedFuture& future
-) {
-  if (auto status = future.wait_for(std::chrono::milliseconds(1000));
-      status == std::future_status::ready) {
-    const auto& response = future.get();
-
-    std::vector<std::shared_ptr<ImageFeatureMeasurement>> features;
-
-    for (size_t i = 0; i < response->features.size(); i++) {
-      const auto& im_feat = response->features[i];
-      const auto descriptor_size = static_cast<int>(im_feat.descriptor.size());
-      cv::Mat descriptor = cv::Mat(1, descriptor_size, CV_8UC1);
-      std::memcpy(descriptor.data, im_feat.descriptor.data(), descriptor_size);
-
-      features.push_back(std::make_shared<ImageFeatureMeasurement>(
-        cv::Point2f(im_feat.point.x, im_feat.point.y), descriptor, i
-      ));
-    }
-
-    ekf_.add_features(features);
-  }
-}
-
 void EKFNode::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg
 ) {
-  if (!ekf_.is_initilized()) {
-    rec_->log(
-      "/",
-      rerun::TextLog("EKF is not initialized. Initializing...")
-        .with_level(rerun::TextLogLevel::Info)
-    );
+  const cv_bridge::CvImagePtr cv_ptr =
+    cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+  const cv::Mat image = cv_ptr->image;
 
-    auto request =
-      std::make_shared<ekf_mono_slam::srv::FeatureDetect::Request>();
-    request->image = *msg;
+  FeatureDetector detector(
+    FeatureDetector::Type::BRISK, cv::Size(image.rows, image.cols)
+  );
 
-    feature_detect_client_->async_send_request(
-      request,
-      [this](rclcpp::Client<ekf_mono_slam::srv::FeatureDetect>::SharedFuture
-               future  // NOLINT
-      ) { this->init_callback(future); }
-    );
+  if (!filter_.is_initilized()) {
+    detector.detect_features(image);
+    filter_.add_features(detector.image_features());
   } else {
-    const cv_bridge::CvImagePtr cv_ptr =
-      cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
-    const cv::Mat image = cv_ptr->image;
-
-    rec_->log("camera/state/x", rerun::Scalar(ekf_.state()->position()[0]));
-    rec_->log("camera/state/y", rerun::Scalar(ekf_.state()->position()[1]));
-    rec_->log("camera/state/z", rerun::Scalar(ekf_.state()->position()[2]));
+    rec_->log("camera/state/x", rerun::Scalar(filter_.state()->position()[0]));
+    rec_->log("camera/state/y", rerun::Scalar(filter_.state()->position()[1]));
+    rec_->log("camera/state/z", rerun::Scalar(filter_.state()->position()[2]));
 
-    ekf_.predict();
-    ekf_.match_predicted_features(image);
+    filter_.predict();
 
     // TODO: continue implementing
   }