Add showTracksPreprocess function and a parameter for the thickness of tracks

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)

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_);
+    }
+  }
 }
 
 

[Joan Solà Ortega]

why these tuples? A little weird. Can't you just use the x and y coordinates in lines below?