diff --git a/include/publisher_vision.h b/include/publisher_vision.h index ff6d3a6ad4dba1eabf2ceebd950b17a1ff730b01..e194fa0298f48bb44411a7b2594be9c27310a02c 100644 --- a/include/publisher_vision.h +++ b/include/publisher_vision.h @@ -81,7 +81,7 @@ class PublisherVisionDebug : public Publisher struct Tracks { bool show_id_; - int size_id_; + double size_id_; double thickness_; COLOR color_; int min_luminosity_; @@ -97,7 +97,7 @@ class PublisherVisionDebug : public Publisher }; struct Landmarks { bool show_id_; - int size_id_; + double size_id_; int size_pix_; COLOR color_; }; diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 59a1583667e9a74cdfd3da81af93b074838bb933..1e86d4abc5f28dd5d5127809e584e0f430edf612 100644 --- a/src/publisher_vision.cpp +++ b/src/publisher_vision.cpp @@ -74,7 +74,7 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name, topic_preprocessor_ = getParamWithDefault<std::string>(_server, prefix_ + "/topic_preprocessor", "/debug_image_prepocessor"); //Tracks tracks_.show_id_ = getParamWithDefault<bool>(_server, prefix_ + "/tracks/show_id", false); - tracks_.size_id_ = getParamWithDefault<int>(_server, prefix_ + "/tracks/size_id", 0.5); + tracks_.size_id_ = getParamWithDefault<double>(_server, prefix_ + "/tracks/size_id", 0.5); tracks_.thickness_ = getParamWithDefault<double>(_server, prefix_ + "/tracks/thickness", 1.5); std::string str_color_tracks = getParamWithDefault<std::string>(_server, prefix_ + "/tracks/color", "CYAN"); tracks_.color_ = colorStringToEnum(str_color_tracks); @@ -93,7 +93,7 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name, //Landmarks landmarks_.show_id_ = getParamWithDefault<bool>(_server, prefix_ + "/landmarks/show_id", false); - landmarks_.size_id_ = getParamWithDefault<int>(_server, prefix_ + "/landmarks/size_id", 0.5); + landmarks_.size_id_ = getParamWithDefault<double>(_server, prefix_ + "/landmarks/size_id", 0.5); landmarks_.size_pix_ = getParamWithDefault<int>(_server, prefix_ + "/landmarks/size_pix", 5); std::string str_color_landmarks_ = getParamWithDefault<std::string>(_server, prefix_ + "/landmarks/color", "CYAN"); landmarks_.color_ = colorStringToEnum(str_color_landmarks_); @@ -410,12 +410,14 @@ void PublisherVisionDebug::showTracks(cv::Mat _image, { int shift_text_x = 7; int shift_text_y = 10; + int font_face = 1; // let's keep the default one cv::putText( _image, std::to_string(ftr_current->second->trackId()), cv::Point(ftr_current->second->getMeasurement(0) - shift_text_x, ftr_current->second->getMeasurement(1) - shift_text_y), - 1, tracks_.size_id_, + font_face, + tracks_.size_id_, CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2])); } @@ -539,13 +541,14 @@ Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<doub // unsigned int lmk_id = lmk->id(); // int shift_text_x = 7; // int shift_text_y = 10; + // int font_face = 1; // let's keep the default one // cv::putText( // _image, // std::to_string(lmk_id), // cv::Point(lmk_pixel(0) - shift_text_x, // lmk_pixel(1) - shift_text_y), - // 1, size_text_ID_, CV_RGB(0, 0, 0)); + // font_face, size_text_ID_, CV_RGB(0, 0, 0)); // } // } // } @@ -582,12 +585,14 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image, unsigned int lmk_id = lmk->id(); int shift_text_x = 7; int shift_text_y = 10; + int font_face = 1; // let's keep the default one cv::putText(_image, std::to_string(lmk_id), cv::Point(lmk_pixel(0) - shift_text_x, lmk_pixel(1) - shift_text_y), - 1, landmarks_.size_id_, + font_face, + landmarks_.size_id_, CV_RGB(color_lmks[0], color_lmks[1], color_lmks[2])); } }