diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index be1670dfc97cd286357e72472ee5db8bf5099cd5..2daca69cf7bae45e2d838645d650d5bd4ec6e6c1 100644 --- a/src/publisher_vision.cpp +++ b/src/publisher_vision.cpp @@ -516,11 +516,12 @@ void PublisherVisionDebug::showTracks(cv::Mat _image, int min_feature_in_track = min_max_feat.first; int max_feature_in_track = min_max_feat.second; - // std::cout << "\n\n\n Number of tracks: " << _tracks.size() << '\n'; // for (auto track = _tracks.begin(); track != _tracks.end(); ++track) //la map trackS map<size_t, FeatureBasePtr> alive_features = _track_matrix.snapshot(_cap_img); + if (alive_features.empty()) return; + TimeStamp time_capture = _cap_img->getTimeStamp(); for (auto alive_feat : alive_features) @@ -529,7 +530,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image, Track track = _track_matrix.track(alive_feat.second->trackId()); if (track.size() == 0) { - std::cout << "SIZE 0 TRACK!!" << '\n'; + WOLF_WARN("SIZE 0 TRACK!!"); continue; } @@ -714,6 +715,9 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image, // draw one ladmark for each feature const auto& ftrs_alive = _track_matrix.snapshotAsList(_capture); + + if (ftrs_alive.empty()) return; + for (const auto& ftr: ftrs_alive) { // get stuff of this landmark