diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 2c507182dae0b48ef493c2fb55da9b5d7c80f749..5bcb9339b1753b776822e238f9fa3bf3da54e545 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -73,10 +73,23 @@ class PublisherVisionDebug : public Publisher
ProcessorVisualOdometryPtr processor_vision_;
CaptureBasePtr last_capture_;
+ bool show_tracks_ID_;
+ bool show_landmarks_ID_;
+ int min_luminosity_;
+ int max_luminosity_;
+ double thickness_curr_ftr_;
+ int size_feature_curr_pix_;
+ double thickness_kfs_ftr_;
+ int size_feature_kfs_pix_;
+ double thickness_lmk_;
+ double size_text_ID_;
+ int color_tracks_features_;
+
image_transport::Publisher publisher_image_;
image_transport::ImageTransport img_transport_;
private:
+ void stringToEnum(const std::string color);
/*
Function that search for the maximum and minimum of features in one track in the track matrix
*/
@@ -86,12 +99,12 @@ 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 _min_lum, int _max_lum, 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, int _color_of_features);
/*
Function that change an image to show tracks and features within it
*/
- void showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBasePtr _cap_img, COLORFEATURES _color_of_features, bool _show_track_ID);
+ void showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBasePtr _cap_img);
/*
Function that return the transformation from world to camera
@@ -121,12 +134,12 @@ class PublisherVisionDebug : public Publisher
/*
Function that print one landmark into the capture
*/
- void showLandmark (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID);
+ void showLandmark (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
/*
Function that print all of the landmarks associated with the capture
*/
- void showLandmarks (cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBasePtr& _cap, bool _show_landmark_ID);
+ 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 c8399c52803d86e6c40cc0bda87658da15af2e68..f8b60a3a4400d15bd53514d9d149516403e3af7d 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -39,6 +39,26 @@
namespace wolf
{
+void PublisherVisionDebug::stringToEnum(const std::string _color)
+{
+ if (_color == "BLUE")
+ color_tracks_features_ = 0;
+ else if (_color == "GREEN")
+ color_tracks_features_ = 1;
+ else if (_color == "YELLOW")
+ color_tracks_features_ = 2;
+ else if (_color == "MAGENTA")
+ color_tracks_features_ = 3;
+ else if (_color == "CYAN")
+ color_tracks_features_ = 4;
+ else if (_color == "GREY")
+ color_tracks_features_ = 5;
+ else if (_color == "RED")
+ color_tracks_features_ = 6;
+ else
+ color_tracks_features_ = 4;
+}
+
PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
const ParamsServer &_server,
const ProblemPtr _problem) :
@@ -48,7 +68,20 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
img_transport_(ros::NodeHandle())
{
// 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", "");
+ 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);
+ std::string str_color_tracks_features = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features", "CYAN");
+ stringToEnum(str_color_tracks_features);
// search the processor
for (auto sen : _problem->getHardware()->getSensorList())
@@ -103,11 +136,8 @@ void PublisherVisionDebug::publishDerived()
cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
// Draw all tracks
- bool show_track_ids = false;
- showTracks(cv_img_debug, track_matrix, cap_img, CYAN, show_track_ids);
-
- bool show_lmk_ids = false;
- showLandmarks(cv_img_debug, track_matrix, cap_img, show_lmk_ids);
+ showTracks(cv_img_debug, track_matrix, cap_img);
+ showLandmarks(cv_img_debug, track_matrix, cap_img);
// Convert to image msg
cv_bridge::CvImagePtr cv_msg = boost::make_shared<cv_bridge::CvImage>();
@@ -151,11 +181,9 @@ std::pair<int, int> PublisherVisionDebug::minMaxFeatureInTrack(const std::map<Si
}
std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_track,
- int _min_lum,
- int _max_lum,
int _max_feature_in_tracks,
int _min_feature_in_tracks,
- COLORFEATURES _color_of_features)
+ int _color_of_features)
{
// std::cout << "\n\ncolorTrackAndFeatures" << '\n';
@@ -179,7 +207,7 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
// alpha = 0.5; // put "half luminosity as a default value"
}
- double lum = (_min_lum + (_max_lum - _min_lum) * (1 - alpha));
+ double lum = (min_luminosity_ + (max_luminosity_ - min_luminosity_) * (1 - alpha));
if (lum > 255)
{
color[0] = 255;
@@ -237,14 +265,9 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
void PublisherVisionDebug::showTracks(cv::Mat _image,
const TrackMatrix &_track_matrix,
- CaptureBasePtr _cap_img,
- COLORFEATURES _color_of_features,
- bool _show_track_ID)
+ CaptureBasePtr _cap_img)
{
- int size_feature_kf_pix_ = 1;
- int size_feature_current_pix_ = 2;
-
std::pair<int, int> min_max_feat = minMaxFeatureInTrack(_track_matrix.getTracks());
int min_feature_in_track = min_max_feat.first;
int max_feature_in_track = min_max_feat.second;
@@ -268,25 +291,23 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
// determine color and luminosity of track
std::vector<int> color = colorTrackAndFeatures(track.size(),
- 100, 450,
max_feature_in_track,
min_feature_in_track,
- _color_of_features);
+ color_tracks_features_);
// iterator to current feature in track
Track::iterator ftr_current (track.find(time_capture));
// draw current feature
- int thickness_curr = 0.1; // -1 -> filled empty
cv::circle(_image,
cv::Point(ftr_current->second->getMeasurement(0),
ftr_current->second->getMeasurement(1)),
- size_feature_current_pix_,
+ size_feature_curr_pix_,
CV_RGB(color[0], color[1], color[2]),
- thickness_curr);
+ thickness_curr_ftr_);
// draw track ID close to current feature
- if (_show_track_ID) //show features ID of current frame
+ if (show_tracks_ID_) //show features ID of current frame
{
int shift_text_x = 7;
int shift_text_y = 10;
@@ -295,7 +316,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _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, 0.5, CV_RGB(color[0], color[1], color[2]));
+ 1, size_text_ID_, CV_RGB(color[0], color[1], color[2]));
}
@@ -311,7 +332,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)),
- size_feature_kf_pix_, CV_RGB(color[0], color[1], color[2]), 0.1);
+ size_feature_kfs_pix_, CV_RGB(color[0], color[1], color[2]), thickness_kfs_ftr_);
}
// draw line in between keyframes (as well as last KF and current frame)
@@ -388,8 +409,7 @@ Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<doub
void PublisherVisionDebug::showLandmark(cv::Mat _image,
const TrackMatrix& _track_matrix,
- const FeatureBasePtr& _ftr,
- bool _show_landmark_ID)
+ const FeatureBasePtr& _ftr)
{
// get stuff common to all landmarks
@@ -406,9 +426,9 @@ void PublisherVisionDebug::showLandmark(cv::Mat _image,
{
cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
- 5, 1);
+ thickness_lmk_, 1);
- if (_show_landmark_ID)
+ if (show_landmarks_ID_)
{
unsigned int lmk_id = lmk->id();
int shift_text_x = 7;
@@ -419,15 +439,14 @@ void PublisherVisionDebug::showLandmark(cv::Mat _image,
std::to_string(lmk_id),
cv::Point(lmk_pixel(0) - shift_text_x,
lmk_pixel(1) - shift_text_y),
- 1, 0.5, CV_RGB(0, 0, 0));
+ 1, size_text_ID_, CV_RGB(0, 0, 0));
}
}
}
void PublisherVisionDebug::showLandmarks(cv::Mat _image,
const TrackMatrix& _track_matrix,
- const CaptureBasePtr& _capture,
- bool _show_landmark_ID)
+ const CaptureBasePtr& _capture)
{
// get stuff common to all landmarks
const auto& Tcw = getTcwMatrix(_capture);
@@ -449,7 +468,7 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
5, 1);
- if (_show_landmark_ID)
+ if (show_landmarks_ID_)
{
unsigned int lmk_id = lmk->id();
int shift_text_x = 7;
@@ -460,7 +479,7 @@ 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, 0.5, CV_RGB(0, 0, 0));
+ 1, size_text_ID_, CV_RGB(0, 0, 0));
}
}