diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index b36566da3e53a76b6183b6efa83d8d0ec2e7af1a..8789a01e4509f0ff529b6277c16fdda37733fe6d 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -84,63 +84,68 @@ class PublisherVisionDebug : public Publisher
double thickness_lmk_;
double size_text_ID_;
COLOR color_tracks_features_;
- COLOR color_lmks_;
+ COLOR color_landmarks_;
+ COLOR color_last_keypoints_;
image_transport::Publisher publisher_image_;
image_transport::ImageTransport img_transport_;
private:
- 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
+ Convert a string corresponding to a color to its corresponding COLOR enum value
+ */
+ COLOR colorStringToEnum(const std::string _color);
+
+ /*
+ Search for the maximum and minimum of features in one track in the track matrix
*/
std::pair<int,int> minMaxFeatureInTrack(const std::map<SizeStd, Track>& _tracks) const;
/*
- Function that return a unique RGB color vector depending on the lenght of the track given,
+ 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, COLOR _color_of_features);
- std::vector<int> colorLandmarks(COLOR _color_of_lmks);
+ std::vector<int> primaryColor(COLOR _color_of_lmks);
/*
- Function that change an image to show tracks and features within it
+ Change an image to show tracks and features within it
*/
void showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBasePtr _cap_img);
/*
- Function that return the transformation from world to camera
+ Return the transformation from world to camera
*/
Eigen::Isometry3d getTcw (const CaptureBasePtr _capture) const;
/*
- Function that transform Tcw into matrix for multiplication purpose
+ Transform Tcw into matrix for multiplication purpose
*/
Eigen::Matrix4d getTcwMatrix (const CaptureBasePtr _capture) const;
/*
- Function that return the projection matrix of a sensor (linked to a capture)
+ Return the projection matrix of a sensor (linked to a capture)
*/
Eigen::Matrix<double, 3, 4> getProjectionMatrix (const CaptureBasePtr _capture) const;
/*
- Function that return only the landmark associated to a feature in last keyframes
+ Return only the landmark associated to a feature in last keyframes
*/
LandmarkBasePtr getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
/*
- Function that return the projection of a landmark in the frame
+ Return the projection of a landmark in the frame
*/
Eigen::Vector2d projectLandmarkHp (const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBasePtr _lmk);
/*
- Function that print one landmark into the capture
+ Print one landmark into the capture
*/
void showLandmark (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
/*
- Function that print all of the landmarks associated with the capture
+ Print all of the landmarks associated with the capture
*/
void showLandmarks (cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBasePtr& _cap);
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 909bba38cf5b29ac07bf9c7d53484cc257cd39f1..aea1dc2ec26cce76d149210122caf10389333ea8 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -39,27 +39,26 @@
namespace wolf
{
-void PublisherVisionDebug::stringToEnum(const std::string _color, bool _isALandmark)
+COLOR PublisherVisionDebug::colorStringToEnum(const std::string _color)
{
-
- COLOR colors[2] = {color_tracks_features_, color_lmks_};
-
if (_color == "BLUE")
- colors[_isALandmark] = COLOR::BLUE;
+ return COLOR::BLUE;
else if (_color == "GREEN")
- colors[_isALandmark] = COLOR::GREEN;
+ return COLOR::GREEN;
else if (_color == "YELLOW")
- colors[_isALandmark] = COLOR::YELLOW;
+ return COLOR::YELLOW;
else if (_color == "MAGENTA")
- colors[_isALandmark] = COLOR::MAGENTA;
+ return COLOR::MAGENTA;
else if (_color == "CYAN")
- colors[_isALandmark] = COLOR::CYAN;
+ return COLOR::CYAN;
else if (_color == "GREY")
- colors[_isALandmark] = COLOR::GREY;
+ return COLOR::GREY;
else if (_color == "RED")
- colors[_isALandmark] = COLOR::RED;
- else
- colors[_isALandmark] = COLOR::MAGENTA;
+ return COLOR::RED;
+ else {
+ std::cout << _color << " color not available! Defaulting to CYAN." << std::endl;
+ return COLOR::MAGENTA;
+ }
}
PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
@@ -84,10 +83,13 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
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");
- std::string str_color_lmks = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
- stringToEnum(str_color_tracks_features, false);
- stringToEnum(str_color_lmks, true);
-
+ std::string str_color_landmarks = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
+ std::string str_color_last_keypoints = getParamWithDefault<std::string>(_server, prefix_ + "/color_last_keypoints", "CYAN");
+
+ // set color attributes
+ color_tracks_features_ = colorStringToEnum(str_color_tracks_features);
+ color_landmarks_ = colorStringToEnum(str_color_landmarks);
+ color_last_keypoints_ = colorStringToEnum(str_color_last_keypoints);
// search the processor
if (processor_name == "")
@@ -238,14 +240,6 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
color[2] = 0;
}
-// std::cout << "color HERE " << '\n';
-// std::cout << "alpha: " << alpha << '\n';
-// for (int c : color)
-// {
-// std::cout << c << " ";
-// }
-// std::cout << std::endl;
-
switch (_color_of_features)
{
case BLUE:
@@ -280,11 +274,11 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
return color;
}
-std::vector<int> PublisherVisionDebug::colorLandmarks(COLOR _color_of_lmks)
+std::vector<int> PublisherVisionDebug::primaryColor(COLOR _color)
{
std::vector<int> color = {0, 0, 0};
- switch (_color_of_lmks)
+ switch (_color)
{
case BLUE:
color[2] = 255;
@@ -355,12 +349,14 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
// iterator to current feature in track
Track::iterator ftr_current (track.find(time_capture));
+ std::vector<int> color_last_kpt = primaryColor(color_last_keypoints_);
+
// draw current feature
cv::circle(_image,
cv::Point(ftr_current->second->getMeasurement(0),
ftr_current->second->getMeasurement(1)),
size_feature_curr_pix_,
- CV_RGB(color[0], color[1], color[2]),
+ CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]),
thickness_curr_ftr_);
// draw track ID close to current feature
@@ -506,7 +502,7 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
const CaptureBasePtr& _capture)
{
// get stuff common to all landmarks
- std::vector<int> colorLmks = colorLandmarks(color_lmks_);
+ std::vector<int> colorLmks = primaryColor(color_landmarks_);
const auto& Tcw = getTcwMatrix(_capture);
const auto& prj_mat = getProjectionMatrix(_capture);