diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 9171f61098f34f82ef517e455ed115bf53f6cc7a..31cd93d5d4f4071c2d66d484c746ed38a05a8a54 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -126,9 +126,9 @@ class PublisherVisionDebug : public Publisher
/*
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;
+ std::pair<int,int> minMaxTrackLength(const std::map<SizeStd, Track>& _tracks) const;
- std::pair<int, int> minMaxFeatureInAliveTrack(const TrackMatrix &_track_matrix, CaptureBasePtr _cap_img) const;
+ std::pair<int, int> minMaxAliveTrackLength(const TrackMatrix &_track_matrix, CaptureBasePtr _cap_img) const;
/*
Return a unique RGB color vector depending on the lenght of the track given,
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 6ba1556f654ad620cc37bb6f26ea0d33a1dc7069..fe13ff8cf248d0edb03bdd13074aeb99edec43f6 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -211,7 +211,7 @@ void PublisherVisionDebug::publishDerived()
}
}
-std::pair<int, int> PublisherVisionDebug::minMaxFeatureInTrack(const std::map<SizeStd, Track> &_tracks) const
+std::pair<int, int> PublisherVisionDebug::minMaxTrackLength(const std::map<SizeStd, Track> &_tracks) const
{
int max = -1;
int min = 10;
@@ -232,7 +232,7 @@ std::pair<int, int> PublisherVisionDebug::minMaxFeatureInTrack(const std::map<Si
return std::pair<int, int>(min, max);
}
-std::pair<int, int> PublisherVisionDebug::minMaxFeatureInAliveTrack(const TrackMatrix &_track_matrix, CaptureBasePtr _cap_img) const
+std::pair<int, int> PublisherVisionDebug::minMaxAliveTrackLength(const TrackMatrix &_track_matrix, CaptureBasePtr _cap_img) const
{
map<size_t, FeatureBasePtr> alive_features = _track_matrix.snapshot(_cap_img);
@@ -260,9 +260,9 @@ std::pair<int, int> PublisherVisionDebug::minMaxFeatureInAliveTrack(const TrackM
cv::Scalar PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_track,
- int _max_feature_in_tracks,
- int _min_feature_in_tracks,
- Color _color_of_features)
+ int _max_feature_in_tracks,
+ int _min_feature_in_tracks,
+ Color _color_of_features)
{
int R = 0;
int G = 0;
@@ -378,28 +378,99 @@ cv::Scalar PublisherVisionDebug::primaryColor(Color _color)
void PublisherVisionDebug::showTracks(cv::Mat _image,
const TrackMatrix &_track_matrix,
- CaptureBasePtr _cap_img)
+ CaptureBasePtr _capture_img)
{
- std::pair<int, int> min_max_feat;
+ std::pair<int, int> min_max_track_length;
if (tracks_.min_max_on_alive_tracks_)
- min_max_feat = minMaxFeatureInAliveTrack(_track_matrix, _cap_img);
+ min_max_track_length = minMaxAliveTrackLength(_track_matrix, _capture_img);
else
- min_max_feat = minMaxFeatureInTrack(_track_matrix.getTracks());
+ min_max_track_length = minMaxTrackLength(_track_matrix.getTracks());
- int min_feature_in_track = min_max_feat.first;
- int max_feature_in_track = min_max_feat.second;
+ int min_track_length = min_max_track_length.first;
+ int max_track_length = min_max_track_length.second;
- map<size_t, FeatureBasePtr> alive_features = _track_matrix.snapshot(_cap_img);
-
- if (alive_features.empty()) return;
+ // color of alive feature
+ cv::Scalar color_last_kpt = primaryColor(tracks_.feature_last_.color_);
- TimeStamp time_capture = _cap_img->getTimeStamp();
+// // 1. Draw tracks with their keyframes
+// for (auto track_id : _track_matrix.trackIds())
+// {
+// auto track = _track_matrix.trackAsVector(track_id);
+// auto track_kfs = _track_matrix.trackAtKeyframes(track_id);
+//
+// // determine color and luminosity of track
+// cv::Scalar color = colorTrackAndFeatures(track.size(),
+// max_feature_in_track,
+// min_feature_in_track,
+// tracks_.color_);
+//
+// // draw track line
+// for (auto ftr_it = track.rbegin(); ftr_it < track.rend(); ++ftr_it)
+// {
+// if (ftr_it == track.rbegin()) continue; // skip first time since prev(rbegin) is undefined
+//
+// auto ftr_prev_it = std::prev(ftr_it);
+//
+// // draw line in between keyframes (as well as last KF and current frame)
+// cv::line(_image,
+// cv::Point((*ftr_it)->getMeasurement(0),
+// (*ftr_it)->getMeasurement(1)),
+// cv::Point((*ftr_prev_it)->getMeasurement(0),
+// (*ftr_prev_it)->getMeasurement(1)),
+// color,
+// tracks_.thickness_);
+//
+// }
+//
+// // draw kfs
+// for (auto pair_ts_ftr : track_kfs)
+// {
+// // mark features of previous keyframes
+// cv::circle(_image,
+// cv::Point(pair_ts_ftr.second->getMeasurement(0),
+// pair_ts_ftr.second->getMeasurement(1)),
+// tracks_.feature_kfs_.size_pix_,
+// color,
+// tracks_.feature_kfs_.thickness_);
+//
+// }
+// }
+//
+// // 2. Draw features in last image
+// for (auto ftr : alive_features)
+// {
+// // draw current feature
+// cv::circle(_image,
+// cv::Point(ftr.second->getMeasurement(0),
+// ftr.second->getMeasurement(1)),
+// tracks_.feature_last_.size_pix_,
+// color_last_kpt,
+// tracks_.feature_last_.thickness_);
+//
+// // draw track ID close to current feature
+// if (tracks_.show_id_) //show features ID of current frame
+// {
+// int shift_text_x = 7;
+// int shift_text_y = 10;
+// int font_face = 1; // let's keep the default one
+// cv::putText(
+// _image,
+// std::to_string(ftr.second->trackId()),
+// cv::Point(ftr.second->getMeasurement(0) - shift_text_x,
+// ftr.second->getMeasurement(1) - shift_text_y),
+// font_face,
+// tracks_.size_id_,
+// color_last_kpt);
+// }
+//
+// }
- for (auto alive_feat : alive_features)
+ // loop for features alive in current capture
+ for (auto ftr_alive : _track_matrix.snapshot(_capture_img))
{
// get full track of feature
- Track track = _track_matrix.track(alive_feat.second->trackId());
+ Track track = _track_matrix.track(ftr_alive.second->trackId());
if (track.size() == 0)
{
WOLF_WARN("SIZE 0 TRACK!!");
@@ -408,20 +479,14 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
// determine color and luminosity of track
cv::Scalar color = colorTrackAndFeatures(track.size(),
- max_feature_in_track,
- min_feature_in_track,
+ max_track_length,
+ min_track_length,
tracks_.color_);
- cv::Scalar color_last_kpt = primaryColor(tracks_.feature_last_.color_);
-
-
- // iterator to current feature in track
- Track::iterator ftr_current (track.find(time_capture));
-
// draw current feature
cv::circle(_image,
- cv::Point(ftr_current->second->getMeasurement(0),
- ftr_current->second->getMeasurement(1)),
+ cv::Point(ftr_alive.second->getMeasurement(0),
+ ftr_alive.second->getMeasurement(1)),
tracks_.feature_last_.size_pix_,
color_last_kpt,
tracks_.feature_last_.thickness_);
@@ -434,17 +499,17 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
int font_face = 1; // let's keep the default one
cv::putText(
_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),
+ std::to_string(ftr_alive.second->trackId()),
+ cv::Point(ftr_alive.second->getMeasurement(0) - shift_text_x,
+ ftr_alive.second->getMeasurement(1) - shift_text_y),
font_face,
- tracks_.size_id_,
+ tracks_.size_id_,
color_last_kpt);
}
// iterate track from current time to the beginning of track
- for (Track::reverse_iterator ftr(ftr_current); ftr != track.rend(); ++ftr) // start at the feature of the capture to draw
+ for (Track::reverse_iterator ftr = track.rbegin(); ftr != track.rend(); ++ftr) // start at the feature of the capture to draw
{
auto previous = std::prev(ftr); //previous iterator
@@ -457,18 +522,18 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
cv::circle(_image,
cv::Point(ftr->second->getMeasurement(0),
ftr->second->getMeasurement(1)),
- tracks_.feature_kfs_.size_pix_,
- color,
+ tracks_.feature_kfs_.size_pix_,
+ color,
tracks_.feature_kfs_.thickness_);
}
// draw line in between keyframes (as well as last KF and current frame)
- cv::line(_image,
- cv::Point(ftr->second->getMeasurement(0),
+ cv::line(_image,
+ cv::Point(ftr->second->getMeasurement(0),
ftr->second->getMeasurement(1)),
- cv::Point(previous->second->getMeasurement(0),
+ cv::Point(previous->second->getMeasurement(0),
previous->second->getMeasurement(1)),
- color,
+ color,
tracks_.thickness_);
}