diff --git a/include/publisher_vision.h b/include/publisher_vision.h index 54986398d14a7fbaf00817b04adffda7874d13ef..9171f61098f34f82ef517e455ed115bf53f6cc7a 100644 --- a/include/publisher_vision.h +++ b/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_; }; diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 6864189140f99ddd1eea9432d2baa98b3b315547..a084c16733e28d6f0f2da9e6a5b62fe12c07ae16 100644 --- a/src/publisher_vision.cpp +++ b/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,