diff --git a/vio_hw/internal/feat/feature.hpp b/vio_hw/internal/feat/feature.hpp index b299faf..f98be81 100644 --- a/vio_hw/internal/feat/feature.hpp +++ b/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,9 +23,8 @@ class FeatureBase struct FeatureExtractorOptions { FeatureType feature_type_; - int max_kps_num_; - int kps_max_distance_; - float kps_quality_level_; + std::shared_ptr orbslamExtractorConfig; + std::shared_ptr goodFeature2TrackerConfig; }; FeatureBase(); @@ -30,17 +32,17 @@ class FeatureBase virtual ~FeatureBase() = default; // TODO: the [desc] is redundant - virtual bool detect(const cv::Mat &image, std::vector &kps, - cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat()) = 0; + virtual bool detect( const cv::Mat &image, std::vector &kps, + cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat() ) = 0; - virtual std::vector DescribeBRIEF(const cv::Mat &im, - const std::vector &vpts); + virtual std::vector DescribeBRIEF( const cv::Mat &im, + const std::vector &vpts ); - static std::shared_ptr Create(const FeatureExtractorOptions &options); + static std::shared_ptr 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 brief_desc_extractor_; }; diff --git a/vio_hw/internal/feat/good_feature_impl.hpp b/vio_hw/internal/feat/good_feature_impl.hpp index 80c0140..ecc8987 100644 --- a/vio_hw/internal/feat/good_feature_impl.hpp +++ b/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 GoodFeature2TrackerConfigPtr; +typedef std::shared_ptr 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 &kps, - cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat()) override; + bool detect( const cv::Mat &image, std::vector &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 diff --git a/vio_hw/internal/feat/orb_slam_impl.hpp b/vio_hw/internal/feat/orb_slam_impl.hpp index a1e84f6..cb37f7a 100644 --- a/vio_hw/internal/feat/orb_slam_impl.hpp +++ b/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 ORBSLAMExtractorConfigPtr; +typedef std::shared_ptr ORBSLAMExtractorConfigConstPtr; + class ORBSLAMExtractor : public FeatureBase { - public: - ORBSLAMExtractor() = default; - virtual bool detect(const cv::Mat &image, std::vector &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 &kps, + cv::Mat mask = cv::Mat(), cv::Mat desc = cv::Mat() ); + std::shared_ptr orb_extractor_ = nullptr; }; } // namespace viohw diff --git a/vio_hw/internal/world_manager.hpp b/vio_hw/internal/world_manager.hpp index bec56bb..17b4fa0 100644 --- a/vio_hw/internal/world_manager.hpp +++ b/vio_hw/internal/world_manager.hpp @@ -25,6 +25,7 @@ class WorldManager void setupCalibration(); bool VisualizationImage(); void VisualizationKFTraj(); + bool GenerateFeatureExtractorBase(); private: std::queue img_left_queen_, img_right_queen_; diff --git a/vio_hw/params/kitti/kitti_00-02.yaml b/vio_hw/params/kitti/kitti_00-02.yaml index 074fd7c..93cde56 100644 --- a/vio_hw/params/kitti/kitti_00-02.yaml +++ b/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 diff --git a/vio_hw/src/feat/feat.cpp b/vio_hw/src/feat/feat.cpp index c4efacc..420b388 100644 --- a/vio_hw/src/feat/feat.cpp +++ b/vio_hw/src/feat/feat.cpp @@ -19,11 +19,11 @@ std::shared_ptr FeatureBase::Create(const FeatureExtractorOptions& } case FeatureType::HARRIS: { LOG(INFO) << "Create Feature Extractor with [HARRIS]"; - return std::make_shared(options); + return std::make_shared(*options.goodFeature2TrackerConfig); } case FeatureType::ORB: { LOG(INFO) << "Create Feature Extractor with [ORB]"; - return std::make_shared(); + return std::make_shared(*options.orbslamExtractorConfig); } case FeatureType::SUPER_POINT: { // TODO diff --git a/vio_hw/src/feat/good_feature_impl.cpp b/vio_hw/src/feat/good_feature_impl.cpp index c9a8536..588cf0d 100644 --- a/vio_hw/src/feat/good_feature_impl.cpp +++ b/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 &kps, cv::Mat mask, - cv::Mat desc) { +bool GoodFeature2Tracker::detect( const cv::Mat &image, std::vector &kps, + cv::Mat mask, cv::Mat desc ) { std::vector 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 diff --git a/vio_hw/src/feat/orb_cv_impl.cpp b/vio_hw/src/feat/orb_cv_impl.cpp index 296c8aa..bd6b33f 100644 --- a/vio_hw/src/feat/orb_cv_impl.cpp +++ b/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 \ No newline at end of file diff --git a/vio_hw/src/feat/orb_slam_impl.cpp b/vio_hw/src/feat/orb_slam_impl.cpp index fa11535..291f075 100644 --- a/vio_hw/src/feat/orb_slam_impl.cpp +++ b/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 &kps, cv::Mat mask, - cv::Mat desc) { +bool ORBSLAMExtractor::detect( const cv::Mat &image, std::vector &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 \ No newline at end of file diff --git a/vio_hw/src/map_manager.cpp b/vio_hw/src/map_manager.cpp index f00e022..10bf828 100644 --- a/vio_hw/src/map_manager.cpp +++ b/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 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 desc_pts; diff --git a/vio_hw/src/world_manager.cpp b/vio_hw/src/world_manager.cpp index b514837..ffbaf61 100644 --- a/vio_hw/src/world_manager.cpp +++ b/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 ) : 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( 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 lock( img_mutex_ ); img_left_queen_.push( im0 );