refractor code for feture extractor generator

vio_hw/internal/feat/feature.hpp

@@ -6,9 +6,12 @@
 
 namespace viohw {
 
+class ORBSLAMExtractorConfig;
+class GoodFeature2TrackerConfig;
+
 class FeatureBase
 {
- public:
+public:
   enum FeatureType
   {
     ORB_CV,
@@ -20,27 +23,26 @@ class FeatureBase
   struct FeatureExtractorOptions
   {
     FeatureType feature_type_;
-    int max_kps_num_;
-    int kps_max_distance_;
-    float kps_quality_level_;
+    std::shared_ptr<ORBSLAMExtractorConfig> orbslamExtractorConfig;
+    std::shared_ptr<GoodFeature2TrackerConfig> goodFeature2TrackerConfig;
   };
 
   FeatureBase();
 
   virtual ~FeatureBase() = default;
 
   // TODO: the [desc] is redundant
-  virtual bool detect(const cv::Mat &image, std::vector<cv::KeyPoint> &kps,
-                      cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat()) = 0;
+  virtual bool detect( const cv::Mat &image, std::vector<cv::KeyPoint> &kps,
+                       cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat() ) = 0;
 
-  virtual std::vector<cv::Mat> DescribeBRIEF(const cv::Mat &im,
-                                             const std::vector<cv::Point2f> &vpts);
+  virtual std::vector<cv::Mat> DescribeBRIEF( const cv::Mat &im,
+                                              const std::vector<cv::Point2f> &vpts );
 
-  static std::shared_ptr<FeatureBase> Create(const FeatureExtractorOptions &options);
+  static std::shared_ptr<FeatureBase> Create( const FeatureExtractorOptions &options );
 
-  void setMaxKpsNumber(int num) { max_kps_number_ = num; }
+  void setTobeExtractKpsNumber( int num ) { tobe_extractor_kps_num_ = num; }
 
-  int max_kps_number_ = 0;
+  int tobe_extractor_kps_num_ = 0;
   cv::Ptr<cv::xfeatures2d::BriefDescriptorExtractor> brief_desc_extractor_;
 };
 

vio_hw/internal/feat/good_feature_impl.hpp

@@ -6,17 +6,27 @@
 
 namespace viohw {
 
+class GoodFeature2TrackerConfig
+{
+public:
+  int kps_min_distance_;
+  float kps_quality_level_;
+  int max_kps_num_;
+};
+
+typedef std::shared_ptr<GoodFeature2TrackerConfig> GoodFeature2TrackerConfigPtr;
+typedef std::shared_ptr<const GoodFeature2TrackerConfig> GoodFeature2TrackerConfigConstPtr;
+
 class GoodFeature2Tracker : public FeatureBase
 {
- public:
-  explicit GoodFeature2Tracker(const FeatureBase::FeatureExtractorOptions &options);
+public:
+  explicit GoodFeature2Tracker( const GoodFeature2TrackerConfig &options );
 
-  bool detect(const cv::Mat &image, std::vector<cv::KeyPoint> &kps,
-              cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat()) override;
+  bool detect( const cv::Mat &image, std::vector<cv::KeyPoint> &kps, cv::Mat mask = cv::Mat(),
+               cv::Mat desc = cv::Mat() ) override;
 
- private:
-  int kps_min_distance_;
-  float kps_quality_level_;
+private:
+  GoodFeature2TrackerConfig feature_extract_config_;
 };
 
 }  // namespace viohw

vio_hw/internal/feat/orb_slam_impl.hpp

@@ -1,16 +1,31 @@
 #ifndef VIO_HELLO_WORLD_ORB_SLAM_IMPL_HPP
 #define VIO_HELLO_WORLD_ORB_SLAM_IMPL_HPP
 
+#include "feat/orb_slam/orbextractor.hpp"
 #include "vio_hw/internal/feat/feature.hpp"
 
 namespace viohw {
 
+class ORBSLAMExtractorConfig
+{
+public:
+  int max_kps_ = 300;
+  float scale_factor_ = 1.2;
+  int level_ = 8;
+  int iniThFAST_ = 20;
+  int minThFAST_ = 7;
+};
+
+typedef std::shared_ptr<ORBSLAMExtractorConfig> ORBSLAMExtractorConfigPtr;
+typedef std::shared_ptr<const ORBSLAMExtractorConfig> ORBSLAMExtractorConfigConstPtr;
+
 class ORBSLAMExtractor : public FeatureBase
 {
- public:
-  ORBSLAMExtractor() = default;
-  virtual bool detect(const cv::Mat &image, std::vector<cv::KeyPoint> &kps,
-                      cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat());
+public:
+  explicit ORBSLAMExtractor(const ORBSLAMExtractorConfig& option);
+  virtual bool detect( const cv::Mat &image, std::vector<cv::KeyPoint> &kps,
+                       cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat() );
+  std::shared_ptr<feat::ORBextractor> orb_extractor_ = nullptr;
 };
 
 }  // namespace viohw

vio_hw/internal/world_manager.hpp

@@ -25,6 +25,7 @@ class WorldManager
   void setupCalibration();
   bool VisualizationImage();
   void VisualizationKFTraj();
+  bool GenerateFeatureExtractorBase();
 
  private:
   std::queue<cv::Mat> img_left_queen_, img_right_queen_;

vio_hw/params/kitti/kitti_00-02.yaml

@@ -24,7 +24,7 @@ FeatureAndTracker:
   feature.Type: 0
   tracker.Type: 0
   # common params
-  max.kps.num: 500
+  max.kps.num: 300
   max.kps.distance: 20
   use.clahe: 0
   clahe.val: 3

vio_hw/src/feat/feat.cpp

@@ -19,11 +19,11 @@ std::shared_ptr<FeatureBase> FeatureBase::Create(const FeatureExtractorOptions&
     }
     case FeatureType::HARRIS: {
       LOG(INFO) << "Create Feature Extractor with [HARRIS]";
-      return std::make_shared<GoodFeature2Tracker>(options);
+      return std::make_shared<GoodFeature2Tracker>(*options.goodFeature2TrackerConfig);
     }
     case FeatureType::ORB: {
       LOG(INFO) << "Create Feature Extractor with [ORB]";
-      return std::make_shared<ORBSLAMExtractor>();
+      return std::make_shared<ORBSLAMExtractor>(*options.orbslamExtractorConfig);
     }
     case FeatureType::SUPER_POINT: {
       // TODO

vio_hw/src/feat/good_feature_impl.cpp

@@ -1,18 +1,18 @@
 #include "vio_hw/internal/feat/good_feature_impl.hpp"
 
 namespace viohw {
-GoodFeature2Tracker::GoodFeature2Tracker(const FeatureBase::FeatureExtractorOptions &options) {
-  kps_min_distance_ = options.kps_max_distance_;
-  max_kps_number_ = options.kps_max_distance_;
-  kps_quality_level_ = options.kps_quality_level_;
+GoodFeature2Tracker::GoodFeature2Tracker( const GoodFeature2TrackerConfig &options ) {
+  feature_extract_config_ = options;
+  tobe_extractor_kps_num_ = options.max_kps_num_;
 }
 
-bool GoodFeature2Tracker::detect(const cv::Mat &image, std::vector<cv::KeyPoint> &kps, cv::Mat mask,
-                                 cv::Mat desc) {
+bool GoodFeature2Tracker::detect( const cv::Mat &image, std::vector<cv::KeyPoint> &kps,
+                                  cv::Mat mask, cv::Mat desc ) {
   std::vector<cv::Point2f> points;
-  feat::goodFeaturesToTrack(image, points, max_kps_number_, kps_quality_level_, kps_min_distance_,
-                            mask);
-  cv::KeyPoint::convert(points, kps);
+  feat::goodFeaturesToTrack( image, points, tobe_extractor_kps_num_,
+                             feature_extract_config_.kps_quality_level_,
+                             feature_extract_config_.kps_min_distance_, mask );
+  cv::KeyPoint::convert( points, kps );
   return true;
 }
 }  // namespace viohw

vio_hw/src/feat/orb_cv_impl.cpp

@@ -9,7 +9,7 @@ bool ORBCVExtractor::detect(const cv::Mat &image,
 }
 
 ORBCVExtractor::ORBCVExtractor(const FeatureBase::FeatureExtractorOptions &options) {
-  orb_ = feat::ORB::create(options.max_kps_num_);
+  // orb_ = feat::ORB::create(options.max_kps_num_);
 }
 
 }  // namespace viohw

vio_hw/src/feat/orb_slam_impl.cpp

@@ -1,9 +1,16 @@
 #include "vio_hw/internal/feat/orb_slam_impl.hpp"
 
 namespace viohw {
-bool ORBSLAMExtractor::detect(const cv::Mat &image,
-                              std::vector<cv::KeyPoint> &kps, cv::Mat mask,
-                              cv::Mat desc) {
+bool ORBSLAMExtractor::detect( const cv::Mat &image, std::vector<cv::KeyPoint> &kps, cv::Mat mask,
+                               cv::Mat desc ) {
+  orb_extractor_->Detect( image, mask, kps );
   return true;
 }
+
+ORBSLAMExtractor::ORBSLAMExtractor( const ORBSLAMExtractorConfig &option ) {
+  orb_extractor_.reset( new feat::ORBextractor( option.max_kps_, option.scale_factor_,
+                                                option.level_, option.iniThFAST_,
+                                                option.minThFAST_ ) );
+}
+
 }  // namespace viohw

vio_hw/src/map_manager.cpp

@@ -92,7 +92,7 @@ void MapManager::ExtractKeypoints( const cv::Mat& im, const cv::Mat& im_raw ) {
   int num_need_detect = param_->feat_tracker_setting_.max_feature_num_ - kps.size();
   if ( num_need_detect > 0 ) {
     std::vector<cv::KeyPoint> new_kps;
-    feature_extractor_->setMaxKpsNumber( num_need_detect );
+    feature_extractor_->setTobeExtractKpsNumber( num_need_detect );
     feature_extractor_->detect( im, new_kps, mask );
     if ( !new_kps.empty() ) {
       std::vector<cv::Point2f> desc_pts;

vio_hw/src/world_manager.cpp

@@ -1,18 +1,15 @@
 #include "vio_hw/internal/world_manager.hpp"
 
+#include "vio_hw/internal/feat/good_feature_impl.hpp"
+#include "vio_hw/internal/feat/orb_slam_impl.hpp"
+
 namespace viohw {
 
 WorldManager::WorldManager( std::shared_ptr<Setting>& setting ) : params_( setting ) {
   setupCalibration();
 
   // create feature extractor
-  FeatureBase::FeatureExtractorOptions feature_options{
-      .feature_type_ = FeatureBase::HARRIS,
-      .max_kps_num_ = params_->feat_tracker_setting_.max_feature_num_,
-      .kps_max_distance_ = params_->feat_tracker_setting_.max_feature_dis_,
-      .kps_quality_level_ =
-          static_cast<float>( params_->feat_tracker_setting_.feature_quality_level_ ) };
-  feature_extractor_ = FeatureBase::Create( feature_options );
+  GenerateFeatureExtractorBase();
 
   // create visualization
   VisualizationBase::VisualizationOption viz_option{ VisualizationBase::RVIZ };
@@ -82,6 +79,30 @@ void WorldManager::run() {
   }
 }
 
+bool WorldManager::GenerateFeatureExtractorBase() {
+  // TODO select feature extract type
+  FeatureBase::FeatureExtractorOptions feature_options{ .feature_type_ = FeatureBase::ORB };
+  feature_options.orbslamExtractorConfig.reset( new ORBSLAMExtractorConfig );
+  feature_options.orbslamExtractorConfig->iniThFAST_ = 20;
+  feature_options.orbslamExtractorConfig->minThFAST_ = 7;
+  feature_options.orbslamExtractorConfig->level_ = 8;
+  feature_options.orbslamExtractorConfig->scale_factor_ = 1.2;
+  feature_options.orbslamExtractorConfig->max_kps_ =
+      params_->feat_tracker_setting_.max_feature_num_;
+
+  feature_options.goodFeature2TrackerConfig.reset( new GoodFeature2TrackerConfig );
+  feature_options.goodFeature2TrackerConfig->kps_min_distance_ =
+      params_->feat_tracker_setting_.max_feature_dis_;
+  feature_options.goodFeature2TrackerConfig->kps_quality_level_ =
+      params_->feat_tracker_setting_.feature_quality_level_;
+  feature_options.goodFeature2TrackerConfig->max_kps_num_ =
+      params_->feat_tracker_setting_.max_feature_num_;
+
+  feature_extractor_ = FeatureBase::Create( feature_options );
+
+  return true;
+}
+
 void WorldManager::addNewStereoImages( const double time, cv::Mat& im0, cv::Mat& im1 ) {
   std::lock_guard<std::mutex> lock( img_mutex_ );
   img_left_queen_.push( im0 );