diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 8789a01e4509f0ff529b6277c16fdda37733fe6d..d278e777a60d5198ed06c7ae1f2ee0a22f23bb9d 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -83,7 +83,9 @@ class PublisherVisionDebug : public Publisher
int size_feature_kfs_pix_;
double thickness_lmk_;
double size_text_ID_;
+ double thickness_track_;
COLOR color_tracks_features_;
+ COLOR color_tracks_features_preprocess_;
COLOR color_landmarks_;
COLOR color_last_keypoints_;
@@ -149,6 +151,7 @@ class PublisherVisionDebug : public Publisher
*/
void showLandmarks (cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBasePtr& _cap);
+ void showTracksPreprocess (cv::Mat _image, const CaptureImagePtr& _origin, const CaptureImagePtr& _last);
};
WOLF_REGISTER_PUBLISHER(PublisherVisionDebug)
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 55e3ce7c3d4d258411d746fc2c971fee07f2a9aa..175bbd0ad2d8d3483910f3e669a0833289c9ff9d 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -57,7 +57,7 @@ COLOR PublisherVisionDebug::colorStringToEnum(const std::string _color)
return COLOR::RED;
else {
std::cout << _color << " color not available! Defaulting to CYAN." << std::endl;
- return COLOR::MAGENTA;
+ return COLOR::CYAN;
}
}
@@ -72,24 +72,27 @@ 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);
- 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_ + "/color_last_keypoints", "CYAN");
+ 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);
+ thickness_track_ = getParamWithDefault<double>(_server, prefix_ + "/thickness_track", 1.5);
+ std::string str_color_tracks_features = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features", "CYAN");
+ std::string str_color_tracks_features_preProcess = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features_preprocess", "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_ + "/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);
+ color_tracks_features_ = colorStringToEnum(str_color_tracks_features);
+ color_tracks_features_preprocess_ = colorStringToEnum(str_color_tracks_features);
+ color_landmarks_ = colorStringToEnum(str_color_landmarks);
+ color_last_keypoints_ = colorStringToEnum(str_color_last_keypoints);
// search the processor
for (auto sen : _problem->getHardware()->getSensorList())
@@ -131,8 +134,10 @@ void PublisherVisionDebug::publishDerived()
// Get capture image
auto cap_img = std::dynamic_pointer_cast<CaptureImage>(processor_vision_->getLast());
+ auto cap_img_origin = std::dynamic_pointer_cast<CaptureImage>(processor_vision_->getOrigin());
assert(cap_img != nullptr && "Received Capture is not of type CaptureImage!");
+ assert(cap_img_origin != nullptr && "Received origin Capture is not of type CaptureImage!");
auto track_matrix = processor_vision_->getTrackMatrix(); // copy track matrix
@@ -142,6 +147,7 @@ void PublisherVisionDebug::publishDerived()
// Extract cv image
cv::Mat cv_img_debug;
cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
+ cv::cvtColor(cap_img_origin->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
// Draw all tracks
showTracks(cv_img_debug, track_matrix, cap_img);
@@ -380,7 +386,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
// draw line in between keyframes (as well as last KF and current frame)
cv::line(_image, cv::Point(ftr->second->getMeasurement(0), ftr->second->getMeasurement(1)),
cv::Point(previous->second->getMeasurement(0), previous->second->getMeasurement(1)),
- CV_RGB(color[0], color[1], color[2]), 1.5);
+ CV_RGB(color[0], color[1], color[2]), thickness_track_);
}
}
@@ -530,7 +536,51 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
}
}
+}
+
+void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
+ const CaptureImagePtr& _origin,
+ const CaptureImagePtr& _last)
+{
+ std::vector<int> color_track_preprocess = primaryColor(color_tracks_features_preprocess_);
+ const auto& tracks_origin = _last->getTracksOrigin();
+ const auto& kps_last = _last->getKeyPoints();
+ const auto& kps_origin = _origin->getKeyPoints();
+
+ for(auto it = tracks_origin.begin(); it != tracks_origin.end(); it++)
+ {
+
+ const auto& it_ftr_origin = kps_origin.find(it->first);
+ const auto& it_ftr_last = kps_last.find(it->second);
+
+ if (it_ftr_origin != kps_origin.end() && it_ftr_last != kps_last.end())
+ {
+ const auto& ftr_origin = it_ftr_origin->second.getCvKeyPoint();
+ const auto& ftr_last = it_ftr_last->second.getCvKeyPoint();
+
+ const auto& pos_ftr_origin = std::make_tuple(ftr_origin.pt.x, ftr_origin.pt.y) ;
+ const auto& pos_ftr_last = std::make_tuple(ftr_last.pt.x, ftr_last.pt.y);
+
+ cv::circle(_image,
+ cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
+ size_feature_curr_pix_,
+ CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+ thickness_curr_ftr_);
+
+ cv::circle(_image,
+ cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
+ size_feature_kfs_pix_,
+ CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+ thickness_kfs_ftr_);
+
+ 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]),
+ thickness_track_);
+ }
+ }
}