diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index c131620d667ec411d70d85d6d167995cd0079010..54986398d14a7fbaf00817b04adffda7874d13ef 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -112,6 +112,11 @@ class PublisherVisionDebug : public Publisher
image_transport::ImageTransport img_transport_;
private:
+ cv::Scalar primaryColor(Color _color);
+ cv::Scalar colorTrackAndFeatures(int _nb_feature_in_track,
+ int _max_feature_in_tracks,
+ int _min_feature_in_tracks,
+ Color _color_of_features);
/*
Convert a string corresponding to a color to its corresponding Color enum value
*/
@@ -128,12 +133,12 @@ class PublisherVisionDebug : public Publisher
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> colorTrackAndFeatures(int _nb_feature_in_track, int _max_feature_in_tracks, int _min_feature_in_tracks, Color _color_of_features);
/*
Return a RGB color vector coresponding to the color asked
*/
- std::vector<int> primaryColor(Color _color_of_lmks);
+ // std::vector<int> primaryColor(Color _color_of_lmks);
/*
Change an image to show tracks and features within it
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 6f05af4fb79a3aa01401e3dbd488bf0d3013d8d0..be1670dfc97cd286357e72472ee5db8bf5099cd5 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -271,22 +271,92 @@ std::pair<int, int> PublisherVisionDebug::minMaxFeatureInAliveTrack(const TrackM
return std::pair<int, int>(min, max);
}
-std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_track,
+// std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_track,
+ // int _max_feature_in_tracks,
+ // int _min_feature_in_tracks,
+ // Color _color_of_features)
+// {
+
+ // // std::cout << "\n\ncolorTrackAndFeatures" << '\n';
+
+ // //_color_of_features will be useful with other types of sensor -> change color of tracks and features
+ // std::vector<int> color = {0, 0, 0};
+
+ // // alpha corresponds to the relative luminosity between tracks
+ // // 0 -> min luminosity to represent a short track
+ // // 1 -> max luminosity to represent a long track
+ // double alpha;
+// // std::cout << "_max_feature_in_tracks: " << _max_feature_in_tracks << '\n';
+ // if (_max_feature_in_tracks != _min_feature_in_tracks)
+ // {
+ // alpha = (double)(_nb_feature_in_track - _min_feature_in_tracks)
+ // / (double)(_max_feature_in_tracks - _min_feature_in_tracks); // to interval [0,1]
+ // }
+ // else
+ // {
+ // alpha = 0; // put "full luminosity as a default value"
+// // alpha = 0.5; // put "half luminosity as a default value"
+ // }
+
+ // double lum = (tracks_.min_luminosity_ + (tracks_.max_luminosity_ - tracks_.min_luminosity_) * (1 - alpha));
+ // if (lum > 255)
+ // {
+ // color[0] = 255;
+ // color[1] = round(lum) - 255;
+ // color[2] = round(lum) - 255;
+ // }
+ // else
+ // {
+ // color[0] = round(lum);
+ // color[1] = 0;
+ // color[2] = 0;
+ // }
+
+ // switch (_color_of_features)
+ // {
+ // case BLUE:
+ // color[2] = color[0];
+ // color[0] = color[1];
+ // break;
+ // case GREEN:
+ // color[1] = color[0];
+ // color[0] = color[2];
+ // break;
+ // case YELLOW:
+ // color[1] = color[0];
+ // break;
+ // case MAGENTA:
+ // color[2] = color[0];
+ // break;
+ // case CYAN:
+ // color[2] = color[0];
+ // color[0] = color[1];
+ // color[1] = color[2];
+ // break;
+ // case GREY:
+ // color[2] = color[0];
+ // color[1] = color[0];
+ // break;
+ // case RED:
+ // break;
+ // default:
+ // break;
+ // }
+
+ // return color;
+// }
+
+cv::Scalar PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_track,
int _max_feature_in_tracks,
int _min_feature_in_tracks,
Color _color_of_features)
{
+ int R = 0;
+ int G = 0;
+ int B = 0;
- // std::cout << "\n\ncolorTrackAndFeatures" << '\n';
-
- //_color_of_features will be useful with other types of sensor -> change color of tracks and features
- std::vector<int> color = {0, 0, 0};
-
- // alpha corresponds to the relative luminosity between tracks
- // 0 -> min luminosity to represent a short track
- // 1 -> max luminosity to represent a long track
double alpha;
-// std::cout << "_max_feature_in_tracks: " << _max_feature_in_tracks << '\n';
+
if (_max_feature_in_tracks != _min_feature_in_tracks)
{
alpha = (double)(_nb_feature_in_track - _min_feature_in_tracks)
@@ -295,47 +365,46 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
else
{
alpha = 0; // put "full luminosity as a default value"
-// alpha = 0.5; // put "half luminosity as a default value"
}
double lum = (tracks_.min_luminosity_ + (tracks_.max_luminosity_ - tracks_.min_luminosity_) * (1 - alpha));
if (lum > 255)
{
- color[0] = 255;
- color[1] = round(lum) - 255;
- color[2] = round(lum) - 255;
+ R = 255;
+ G = round(lum) - 255;
+ B = round(lum) - 255;
}
else
{
- color[0] = round(lum);
- color[1] = 0;
- color[2] = 0;
+ R = round(lum);
+ G = 0;
+ B = 0;
}
switch (_color_of_features)
{
case BLUE:
- color[2] = color[0];
- color[0] = color[1];
+ B = R;
+ R = G;
break;
case GREEN:
- color[1] = color[0];
- color[0] = color[2];
+ G = R;
+ R = B;
break;
case YELLOW:
- color[1] = color[0];
+ G = R;
break;
case MAGENTA:
- color[2] = color[0];
+ B = R;
break;
case CYAN:
- color[2] = color[0];
- color[0] = color[1];
- color[1] = color[2];
+ B = R;
+ R = G;
+ G = B;
break;
case GREY:
- color[2] = color[0];
- color[1] = color[0];
+ B = R;
+ G = R;
break;
case RED:
break;
@@ -343,49 +412,96 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
break;
}
+ cv::Scalar color(B,G,R,0);
+
return color;
}
-std::vector<int> PublisherVisionDebug::primaryColor(Color _color)
+
+// std::vector<int> PublisherVisionDebug::primaryColor(Color _color)
+// {
+// std::vector<int> color = {0, 0, 0};
+
+// switch (_color)
+// {
+// 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;
+// }
+
+cv::Scalar PublisherVisionDebug::primaryColor(Color _color)
{
- std::vector<int> color = {0, 0, 0};
+ int R = 0;
+ int G = 0;
+ int B = 0;
switch (_color)
{
case BLUE:
- color[2] = 255;
+ B = 255;
break;
case GREEN:
- color[1] = 255;
+ G = 255;
break;
case RED:
- color[0] = 255;
+ R = 255;
break;
case YELLOW:
- color[0] = 255;
- color[1] = 255;
+ R = 255;
+ G = 255;
break;
case MAGENTA:
- color[0] = 255;
- color[2] = 255;
+ R = 255;
+ B = 255;
break;
case CYAN:
- color[1] = 255;
- color[2] = 255;
+ G = 255;
+ B = 255;
break;
case GREY:
- color[0] = 128;
- color[1] = 128;
- color[2] = 128;
+ R = 128;
+ G = 128;
+ B = 128;
break;
default:
break;
}
+ cv::Scalar color(B,G,R,0);
+
return color;
}
+
void PublisherVisionDebug::showTracks(cv::Mat _image,
const TrackMatrix &_track_matrix,
CaptureBasePtr _cap_img)
@@ -418,12 +534,12 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
}
// determine color and luminosity of track
- std::vector<int> color = colorTrackAndFeatures(track.size(),
+ cv::Scalar color = colorTrackAndFeatures(track.size(),
max_feature_in_track,
min_feature_in_track,
tracks_.color_);
- std::vector<int> color_last_kpt = primaryColor(tracks_.feature_last_.color_);
+ cv::Scalar color_last_kpt = primaryColor(tracks_.feature_last_.color_);
// iterator to current feature in track
@@ -434,7 +550,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
cv::Point(ftr_current->second->getMeasurement(0),
ftr_current->second->getMeasurement(1)),
tracks_.feature_last_.size_pix_,
- CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]),
+ color_last_kpt,
tracks_.feature_last_.thickness_);
// draw track ID close to current feature
@@ -450,7 +566,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
ftr_current->second->getMeasurement(1) - shift_text_y),
font_face,
tracks_.size_id_,
- CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]));
+ color_last_kpt);
}
@@ -467,7 +583,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
// mark features of previous keyframes
cv::circle(_image, cv::Point(ftr->second->getMeasurement(0), ftr->second->getMeasurement(1)),
tracks_.feature_kfs_.size_pix_,
- CV_RGB(color[0], color[1], color[2]),
+ color,
tracks_.feature_kfs_.thickness_);
}
@@ -477,7 +593,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
ftr->second->getMeasurement(1)),
cv::Point(previous->second->getMeasurement(0),
previous->second->getMeasurement(1)),
- CV_RGB(color[0], color[1], color[2]),
+ color,
tracks_.thickness_);
}
@@ -590,7 +706,7 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
const CaptureBasePtr& _capture)
{
// get stuff common to all landmarks
- std::vector<int> color_lmks = primaryColor(landmarks_.color_);
+ cv::Scalar color_lmks = primaryColor(landmarks_.color_);
const auto& Tcw = getTcwMatrix(_capture);
const auto& prj_mat = getProjectionMatrix(_capture);
@@ -608,7 +724,7 @@ 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(color_lmks[0], color_lmks[1], color_lmks[2]),
+ color_lmks,
cv::MARKER_TILTED_CROSS,
landmarks_.size_pix_, 1);
@@ -625,7 +741,7 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
lmk_pixel(1) - shift_text_y),
font_face,
landmarks_.size_id_,
- CV_RGB(color_lmks[0], color_lmks[1], color_lmks[2]));
+ color_lmks);
}
}
@@ -636,7 +752,7 @@ void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
const CaptureImagePtr& _origin,
const CaptureImagePtr& _last)
{
- std::vector<int> color_track_preprocess = primaryColor(tracks_preprocess_.color_);
+ cv::Scalar color_track_preprocess = primaryColor(tracks_preprocess_.color_);
const auto& tracks_origin = _last->getTracksOrigin();
const auto& kps_last = _last->getKeyPoints();
@@ -659,19 +775,19 @@ void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
cv::circle(_image,
cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
tracks_.feature_last_.size_pix_,
- CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+ color_track_preprocess,
tracks_.feature_last_.thickness_);
cv::circle(_image,
cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
tracks_.feature_kfs_.size_pix_,
- CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+ color_track_preprocess,
tracks_.feature_kfs_.thickness_);
cv::line(_image,
cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
- CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+ color_track_preprocess,
tracks_preprocess_.thickness_);
}
}