diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 6b00b2fba15e94bb6ae8e24399f300aada6f3491..b36566da3e53a76b6183b6efa83d8d0ec2e7af1a 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -45,13 +45,13 @@
namespace wolf
{
-enum COLORFEATURES {BLUE = 0,
- GREEN,
- YELLOW,
- MAGENTA,
- CYAN,
- GREY,
- RED
+enum COLOR {BLUE = 0,
+ GREEN,
+ YELLOW,
+ MAGENTA,
+ CYAN,
+ GREY,
+ RED
};
class PublisherVisionDebug : public Publisher
@@ -83,13 +83,14 @@ class PublisherVisionDebug : public Publisher
int size_feature_kfs_pix_;
double thickness_lmk_;
double size_text_ID_;
- COLORFEATURES color_tracks_features_;
+ COLOR color_tracks_features_;
+ COLOR color_lmks_;
image_transport::Publisher publisher_image_;
image_transport::ImageTransport img_transport_;
private:
- void stringToEnum(const std::string color);
+ void stringToEnum(const std::string _color, bool _isALandmark);
/*
Function that search for the maximum and minimum of features in one track in the track matrix
*/
@@ -99,7 +100,9 @@ class PublisherVisionDebug : public Publisher
Function that return a unique RGB color vector depending on the lenght of the track given,
so the longer is the track the darker it is
*/
- std::vector<int> colorTrackAndFeatures(int _nb_feature_in_track, int _max_feature_in_tracks, int _min_feature_in_tracks, COLORFEATURES _color_of_features);
+ std::vector<int> colorTrackAndFeatures(int _nb_feature_in_track, int _max_feature_in_tracks, int _min_feature_in_tracks, COLOR _color_of_features);
+
+ std::vector<int> colorLandmarks(COLOR _color_of_lmks);
/*
Function that change an image to show tracks and features within it
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 32a8e0b975223559cfa54875e53d0c0c2c362d0d..387e252559649f058f1de15cf7fdf8864115417c 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -39,24 +39,27 @@
namespace wolf
{
-void PublisherVisionDebug::stringToEnum(const std::string _color)
+void PublisherVisionDebug::stringToEnum(const std::string _color, bool _isALandmark)
{
+
+ COLOR colors[2] = {color_tracks_features_, color_lmks_};
+
if (_color == "BLUE")
- color_tracks_features_ = COLORFEATURES::BLUE;
+ colors[_isALandmark] = COLOR::BLUE;
else if (_color == "GREEN")
- color_tracks_features_ = COLORFEATURES::GREEN;
+ colors[_isALandmark] = COLOR::GREEN;
else if (_color == "YELLOW")
- color_tracks_features_ = COLORFEATURES::YELLOW;
+ colors[_isALandmark] = COLOR::YELLOW;
else if (_color == "MAGENTA")
- color_tracks_features_ = COLORFEATURES::MAGENTA;
+ colors[_isALandmark] = COLOR::MAGENTA;
else if (_color == "CYAN")
- color_tracks_features_ = COLORFEATURES::CYAN;
+ colors[_isALandmark] = COLOR::CYAN;
else if (_color == "GREY")
- color_tracks_features_ = COLORFEATURES::GREY;
+ colors[_isALandmark] = COLOR::GREY;
else if (_color == "RED")
- color_tracks_features_ = COLORFEATURES::RED;
+ colors[_isALandmark] = COLOR::RED;
else
- color_tracks_features_ = COLORFEATURES::MAGENTA;
+ colors[_isALandmark] = COLOR::MAGENTA;
}
PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
@@ -70,18 +73,20 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
// if user do not provide processor's name, first processor of type PublisherVisionDebug is taken
auto processor_name = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", "");
- show_tracks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_tracks_ID", false);
- show_landmarks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_landmarks_ID", false);
- min_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/min_luminosity", 100);
- max_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/max_luminosity", 450);
- thickness_curr_ftr_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_curr_ftr", -1);
- size_feature_curr_pix_ = getParamWithDefault<int>(_server, prefix_ + "/size_feature_curr_pix", 2);
- thickness_kfs_ftr_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_kfs_ftr", 0.1);
- size_feature_kfs_pix_ = getParamWithDefault<int>(_server, prefix_ + "/size_feature_kfs_pix", 1);
- thickness_lmk_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_lmk", 5);
- size_text_ID_ = getParamWithDefault<double>(_server, prefix_ + "/size_text_ID", 0.5);
+ show_tracks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_tracks_ID", false);
+ show_landmarks_ID_ = getParamWithDefault<bool>(_server, prefix_ + "/show_landmarks_ID", false);
+ min_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/min_luminosity", 100);
+ max_luminosity_ = getParamWithDefault<int>(_server, prefix_ + "/max_luminosity", 450);
+ thickness_curr_ftr_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_curr_ftr", -1);
+ size_feature_curr_pix_ = getParamWithDefault<int>(_server, prefix_ + "/size_feature_curr_pix", 2);
+ thickness_kfs_ftr_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_kfs_ftr", 0.1);
+ size_feature_kfs_pix_ = getParamWithDefault<int>(_server, prefix_ + "/size_feature_kfs_pix", 1);
+ thickness_lmk_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_lmk", 5);
+ size_text_ID_ = getParamWithDefault<double>(_server, prefix_ + "/size_text_ID", 0.5);
std::string str_color_tracks_features = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features", "CYAN");
- stringToEnum(str_color_tracks_features);
+ std::string str_color_lmks = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
+ stringToEnum(str_color_tracks_features, false);
+
// search the processor
for (auto sen : _problem->getHardware()->getSensorList())
@@ -183,7 +188,7 @@ std::pair<int, int> PublisherVisionDebug::minMaxFeatureInTrack(const std::map<Si
std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_track,
int _max_feature_in_tracks,
int _min_feature_in_tracks,
- COLORFEATURES _color_of_features)
+ COLOR _color_of_features)
{
// std::cout << "\n\ncolorTrackAndFeatures" << '\n';
@@ -263,6 +268,46 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
return color;
}
+std::vector<int> PublisherVisionDebug::colorLandmarks(COLOR _color_of_lmks)
+{
+ std::vector<int> color = {0, 0, 0};
+
+ switch (_color_of_lmks)
+ {
+ case BLUE:
+ color[2] = 255;
+ break;
+ case GREEN:
+ color[1] = 255;
+ break;
+ case RED:
+ color[0] = 255;
+ break;
+ case YELLOW:
+ color[0] = 255;
+ color[1] = 255;
+ break;
+ case MAGENTA:
+ color[0] = 255;
+ color[2] = 255;
+ break;
+ case CYAN:
+ color[1] = 255;
+ color[2] = 255;
+ break;
+ case GREY:
+ color[0] = 128;
+ color[1] = 128;
+ color[2] = 128;
+ break;
+ default:
+ break;
+ }
+
+ return color;
+}
+
+
void PublisherVisionDebug::showTracks(cv::Mat _image,
const TrackMatrix &_track_matrix,
CaptureBasePtr _cap_img)
@@ -449,6 +494,8 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
const CaptureBasePtr& _capture)
{
// get stuff common to all landmarks
+ std::vector<int> colorLmks = colorLandmarks(color_lmks_);
+
const auto& Tcw = getTcwMatrix(_capture);
const auto& prj_mat = getProjectionMatrix(_capture);
const auto& trf_prj_mat = prj_mat * Tcw;
@@ -465,7 +512,8 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
const auto& lmk_pixel = projectLandmarkHp(trf_prj_mat, lmk);
cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
- CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
+ CV_RGB(colorLmks[0], colorLmks[1], colorLmks[2]),
+ cv::MARKER_TILTED_CROSS,
5, 1);
if (show_landmarks_ID_)
@@ -479,7 +527,8 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _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));
+ 1, size_text_ID_,
+ CV_RGB(colorLmks[0], colorLmks[1], colorLmks[2]));
}
}