34 use observer design for feature tracker

config/euroc/camera_config.yaml

@@ -0,0 +1,47 @@
+%YAML:1.0
+
+#camera calibration 
+model_type: PINHOLE
+camera_name: camera
+image_width: 752
+image_height: 480
+distortion_parameters:
+   k1: -2.917e-01
+   k2: 8.228e-02
+   p1: 5.333e-05
+   p2: -1.578e-04
+projection_parameters:
+   fx: 4.616e+02
+   fy: 4.603e+02
+   cx: 3.630e+02
+   cy: 2.481e+02
+
+# Extrinsic parameter between IMU and Camera.
+estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
+                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
+                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
+#If you choose 0 or 1, you should write down the following matrix.
+#Rotation from camera frame to imu frame, imu^R_cam
+extrinsicRotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [0.0148655429818, -0.999880929698, 0.00414029679422,
+           0.999557249008, 0.0149672133247, 0.025715529948, 
+           -0.0257744366974, 0.00375618835797, 0.999660727178]
+#Translation from camera frame to imu frame, imu^T_cam
+extrinsicTranslation: !!opencv-matrix
+   rows: 3
+   cols: 1
+   dt: d
+   data: [-0.0216401454975,-0.064676986768, 0.00981073058949]
+
+#feature traker paprameters
+max_cnt: 150            # max feature number in feature tracking
+min_dist: 30            # min distance between two features 
+freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
+F_threshold: 1.0        # ransac threshold (pixel)
+show_track: 1           # publish tracking image as topic
+equalize: 1             # if image is too dark or light, trun on equalize to find enough features
+fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
+

config/euroc/feature_tracker_configs.yaml

@@ -0,0 +1,7 @@
+#feature traker paprameters
+max_cnt: 150            # max feature number in feature tracking
+min_dist: 30            # min distance between two features 
+freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
+F_threshold: 1.0        # ransac threshold (pixel)
+equalize: true             # if image is too dark or light, trun on equalize to find enough features
+fisheye: false              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

feature_tracker/CMakeLists.txt

@@ -5,17 +5,16 @@ set(CMAKE_CXX_FLAGS "-std=c++17")
 find_package(OpenCV REQUIRED)
 find_package(camodocal REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(spdlog REQUIRED)
 
 # Define your library target
-add_library(feature_tracker STATIC src/feature_tracker.cpp)
-target_link_libraries(feature_tracker PRIVATE ${OpenCV_LIBS} ${CERES_LIBRARIES} spdlog::spdlog_header_only camodocal::camodocal )
+add_library(feature_tracker STATIC src/feature_tracker.cpp src/feature_tracker_observer.cpp)
+target_link_libraries(feature_tracker PRIVATE ${OpenCV_LIBS} ${CERES_LIBRARIES} camodocal::camodocal)
 
 # Set include directories
 target_include_directories(feature_tracker
     PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
 )
 
 # Install the library
@@ -59,4 +58,4 @@ install(FILES
     ${CMAKE_CURRENT_BINARY_DIR}/feature_trackerConfig.cmake
     ${CMAKE_CURRENT_BINARY_DIR}/feature_trackerConfigVersion.cmake
     DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/feature_tracker
-)
+)

feature_tracker/cmake/feature_trackerConfig.cmake.in

@@ -14,4 +14,3 @@ if(NOT TARGET feature_tracker::feature_tracker)
 endif()
 include(CMakeFindDependencyMacro)
 find_dependency(camodocal REQUIRED)
-find_dependency(spdlog REQUIRED)

feature_tracker/include/feature_tracker/feature_tracker.hpp

@@ -0,0 +1,90 @@
+#ifndef FEATURE_TRACKER_HPP
+#define FEATURE_TRACKER_HPP
+
+#include <opencv2/opencv.hpp>
+#include "camodocal/camera_models/CameraFactory.h"
+#include "feature_tracker/feature_tracker_observer.hpp"
+
+class FeatureTracker {
+ public:
+  FeatureTracker(std::string camera_config_file, bool fisheye,
+                 bool run_histogram_equilisation,
+                 uint max_feature_count_per_image,
+                 uint min_distance_between_features,
+                 double fundemental_matrix_ransac_threshold, double fx,
+                 double fy, double feature_pruning_frequency,
+                 double max_time_difference);
+
+  void RegisterEventObserver(
+      std::shared_ptr<FeatureTrackerObserver> event_observer);
+
+  void ProcessNewFrame(const cv::Mat &img, const double time_s);
+
+ private:
+  void RestartTracker(const cv::Mat &pre_processed_img, double current_time);
+
+  void AddPoints(const cv::Mat image, const cv::Mat mask,
+                 const int max_number_new_of_points,
+                 const int min_distance_between_points,
+                 const camodocal::CameraPtr m_camera,
+                 std::vector<cv::Point2f> &points,
+                 std::vector<cv::Point2f> &undistorted_points,
+                 std::vector<int> &track_length, std::vector<int> &feature_ids);
+
+  void PrunePoints(std::vector<cv::Point2f> &curr_points,
+                   std::vector<cv::Point2f> &curr_un_points,
+                   std::vector<cv::Point2f> &prev_points,
+                   std::vector<cv::Point2f> &prev_un_points,
+                   std::vector<int> &ids, std::vector<int> &track_counts,
+                   const std::vector<uchar> &status);
+
+  cv::Mat CreateMask(std::vector<cv::Point2f> &curr_pts,
+                     std::vector<int> &track_length,
+                     std::vector<uchar> &status_out);
+
+  void RejectUsingRansac(const std::vector<cv::Point2f> &cur_un_pts,
+                         const std::vector<cv::Point2f> &prev_un_pts,
+                         std::vector<uchar> &status_out) const;
+
+  void GetPointVelocty(double dt, const std::vector<cv::Point2f> &cur_un_pts,
+                       const std::vector<cv::Point2f> &prev_un_pts,
+                       std::vector<cv::Point2f> &pts_velocity_out) const;
+
+  cv::Point2f UndistortPoint(const cv::Point2f point,
+                             const camodocal::CameraPtr camera) const;
+
+  std::string GenerateStateString() const;
+
+  cv::Mat base_mask_;
+  cv::Mat previous_pre_processed_image_;
+  double previous_frame_time_;
+
+  std::vector<cv::Point2f> previous_undistorted_pts_;
+  std::vector<cv::Point2f> previous_points_;
+  std::vector<int> feature_ids_;
+  std::vector<int> feature_track_lengh_;
+  double prev_prune_time_;
+
+  double fx_;
+  double fy_;
+  camodocal::CameraPtr camera_model_;
+
+  static int feature_counter_;  // Static to ensure unique id between different
+                                // instances
+
+  cv::Ptr<cv::CLAHE> clahe_;
+
+  bool fisheye_;
+  bool run_histogram_equilisation_;
+  uint max_feature_count_per_image_;
+  uint min_distance_between_features_;
+  double fundemental_matrix_ransac_threshold_;
+
+  double max_time_difference_;
+  double feature_pruning_frequency_;
+  double feature_pruning_period_;
+  std::shared_ptr<FeatureTrackerObserver> event_observer_;
+};
+
+
+#endif /* FEATURE_TRACKER_HPP */

feature_tracker/include/feature_tracker/feature_tracker_observer.hpp

@@ -0,0 +1,45 @@
+#ifndef FEATURE_TRACKER_OBSERVER_HPP
+#define FEATURE_TRACKER_OBSERVER_HPP
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+class FeatureTrackerObserver
+    : public std::enable_shared_from_this<FeatureTrackerObserver> {
+ public:
+  FeatureTrackerObserver() = default;
+  virtual ~FeatureTrackerObserver() = default;
+
+  virtual void OnRegistered() = 0;
+  virtual void OnRestart() = 0;
+  virtual void OnDurationBetweenFrameTooLarge(double current_image_time_s,
+                                              double previous_image_time_s) = 0;
+  virtual void OnImageTimeMovingBackwards(double current_image_time_s,
+                                          double previous_image_time_s) = 0;
+
+  virtual void OnImageRecieved(const cv::Mat &new_frame,
+                               double current_image_time_s) = 0;
+  virtual void OnHistogramEqualisation(const cv::Mat &new_frame,
+                                    double current_image_time_s) = 0;
+
+  virtual void OnProcessedImage(const cv::Mat &new_frame,
+                                double current_image_time_s,
+                                std::vector<cv::Point2f> features,
+                                std::vector<cv::Point2f> undistorted_features,
+
+                                std::vector<int> ids,
+                                std::vector<int> track_count,
+                                std::vector<cv::Point2f> points_velocity) = 0;
+
+  cv::Mat CreateTrackedFeatureImage(cv::Mat image,
+                                    std::vector<cv::Point2f> features,
+                                    std::vector<int> track_cnt,
+                                    uint max_track_count);
+
+  cv::Mat CreateOpticalFlowImage(cv::Mat image,
+                                 std::vector<cv::Point2f> features,
+                                 std::vector<int> track_cnt,
+                                 uint max_track_count,
+                                 std::vector<cv::Point2f> points_velocity);
+};
+#endif /* FEATURE_TRACKER_OBSERVER_HPP */