Merge remote-tracking branch 'origin/1-publisher-for-visual-odometry' into...

Merge remote-tracking branch 'origin/1-publisher-for-visual-odometry' into 1-publisher-for-visual-odometry

include/publisher_vision.h

@@ -93,6 +93,7 @@ class PublisherVisionDebug : public Publisher
         struct Tracks_preprocess {
             bool    show_;
             bool    show_on_diff_topic_;
+            bool    show_features_;
             double  thickness_;
             Color   color_;
         };

src/publisher_vision.cpp

@@ -102,6 +102,7 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
     //Tracks_preprocess
     tracks_preprocess_.show_               = getParamWithDefault<bool>(_server, prefix_ + "/tracks_preprocess/show", false);
     tracks_preprocess_.show_on_diff_topic_ = getParamWithDefault<bool>(_server, prefix_ + "/tracks_preprocess/show_on_diff_topic", false);
+    tracks_preprocess_.show_features_      = getParamWithDefault<bool>(_server, prefix_ + "/tracks_preprocess/show_features", false);
     tracks_preprocess_.thickness_          = getParamWithDefault<double>(_server, prefix_ + "/tracks_preprocess/thickness", 1.5);
     std::string str_colortracks_preprocess = getParamWithDefault<std::string>(_server, prefix_ + "/tracks_preprocess/color", "CYAN");
     tracks_preprocess_.color_              = colorStringToEnum(str_colortracks_preprocess);
@@ -623,18 +624,21 @@ void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
     {
       const auto& ftr_origin     = it_ftr_origin->second.getCvKeyPoint();
       const auto& ftr_last       = it_ftr_last->second.getCvKeyPoint();
+    
+    if (tracks_preprocess_.show_features_)
+    {
+     cv::circle(_image,
+               ftr_origin.pt,
+               tracks_.feature_last_.size_pix_,
+               color_track_preprocess,
+               tracks_.feature_last_.thickness_);
 
-//      cv::circle(_image,
-//                ftr_origin.pt,
-//                tracks_.feature_last_.size_pix_,
-//                color_track_preprocess,
-//                tracks_.feature_last_.thickness_);
-//
-//      cv::circle(_image,
-//                ftr_last.pt,
-//                tracks_.feature_kfs_.size_pix_,
-//                color_track_preprocess,
-//                tracks_.feature_kfs_.thickness_);
+     cv::circle(_image,
+               ftr_last.pt,
+               tracks_.feature_kfs_.size_pix_,
+               color_track_preprocess,
+               tracks_.feature_kfs_.thickness_);
+    }
 
       cv::line(_image,
                ftr_origin.pt,