From 6fcba833569b26bfa100d95a0dff61018a8c91d3 Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Wed, 13 Apr 2022 15:28:19 +0200
Subject: [PATCH] Different color for last key points
---
include/publisher_vision.h | 2 +-
src/publisher_vision.cpp | 22 ++++++++--------------
2 files changed, 9 insertions(+), 15 deletions(-)
diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 83230ca..8789a01 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -107,7 +107,7 @@ class PublisherVisionDebug : public Publisher
*/
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);
/*
Change an image to show tracks and features within it
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 15e6721..95d2c54 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -83,8 +83,8 @@ 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_landmarks = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
- std::string str_color_last_keypoints = getParamWithDefault<std::string>(_server, prefix_ + "/str_color_last_keypoints", "CYAN");
+ std::string str_color_landmarks = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
+ std::string str_color_last_keypoints = getParamWithDefault<std::string>(_server, prefix_ + "/str_color_last_keypoints", "CYAN");
// set color attributes
color_tracks_features_ = colorStringToEnum(str_color_tracks_features);
@@ -229,14 +229,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:
@@ -271,11 +263,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;
@@ -346,12 +338,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
@@ -497,7 +491,7 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
const CaptureBasePtr& _capture)
{
// get stuff common to all landmarks
- std::vector<int> colorLmks = colorLandmarks(color_landmarks_);
+ std::vector<int> colorLmks = primaryColor(color_landmarks_);
const auto& Tcw = getTcwMatrix(_capture);
const auto& prj_mat = getProjectionMatrix(_capture);
--
GitLab