Change feature dependencies in functions into capture dependencies

include/publisher_vision.h

@@ -93,13 +93,13 @@ class PublisherVisionDebug : public Publisher
         */
         void showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBasePtr _cap_img, COLORFEATURES _color_of_features, bool _show_track_ID);
 
-        Eigen::Vector2d             projectLandmarkHp   (const LandmarkBasePtr _lmk, const CaptureBasePtr _capture) const;
-        Eigen::Vector2d             projectLandmarkHp(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, const LandmarkBasePtr _lmk);
+        Eigen::Vector2d             projectLandmarkHp    (const LandmarkBasePtr _lmk, const CaptureBasePtr _capture) const;
+        Eigen::Vector2d             projectLandmarkHp    (const CaptureBasePtr& _capture, const LandmarkBasePtr _lmk);
         LandmarkBasePtr             getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
-        Eigen::Matrix<double, 3, 4> getProjectionMatrix (const FeatureBasePtr _ftr) const;
-        Eigen::Isometry3d           getTcw              (const FeatureBasePtr _ftr) const;
-        Eigen::Matrix4d             getTcwMatrix        (const FeatureBasePtr _ftr) const;
-        void                        showLandmarks       (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID);
+        Eigen::Matrix<double, 3, 4> getProjectionMatrix  (const CaptureBasePtr _capture) const;
+        Eigen::Isometry3d           getTcw               (const CaptureBasePtr _capture) const;
+        Eigen::Matrix4d             getTcwMatrix         (const CaptureBasePtr _capture) const;
+        void                        showLandmarks        (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID);
 
 };
 

src/publisher_vision.cpp

@@ -320,30 +320,30 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
     }
 }
 
-Eigen::Isometry3d PublisherVisionDebug::getTcw(const FeatureBasePtr _ftr) const
+Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBasePtr _capture) const
 {
     // get robot position and orientation at capture's timestamp
-    const auto& state = _ftr->getCapture()->getProblem()->getState(_ftr->getCapture()->getTimeStamp());
+    const auto& state = _capture->getProblem()->getState(_capture->getTimeStamp());
     const auto& pos     = state.at('P');
     const auto& ori     = state.at('O');
 
     // compute world-to-camera transform
     Eigen::Isometry3d T_w_r = Translation3d(pos) * Quaterniond(ori.data());
-    Eigen::Isometry3d T_r_c = Translation3d(_ftr->getCapture()->getSensorP()->getState())
-            * Quaterniond(_ftr->getCapture()->getSensorO()->getState().data());
+    Eigen::Isometry3d T_r_c = Translation3d(_capture->getSensorP()->getState())
+            * Quaterniond(_capture->getSensorO()->getState().data());
 
     // invert transform to camera-to-world and return
     return (T_w_r * T_r_c).inverse();
 }
 
-Eigen::Matrix4d PublisherVisionDebug::getTcwMatrix(const FeatureBasePtr _ftr) const
+Eigen::Matrix4d PublisherVisionDebug::getTcwMatrix(const CaptureBasePtr _capture) const
 {
-    return getTcw(_ftr).matrix();
+    return getTcw(_capture).matrix();
 }
 
-Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const FeatureBasePtr _ftr) const
+Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const CaptureBasePtr _capture) const
 {
-    auto k(_ftr->getCapture()->getSensor()->getIntrinsic()->getState());
+    auto k(_capture->getSensor()->getIntrinsic()->getState());
     Eigen::Matrix<double, 3, 4> P;
 
     P <<    k(2), 0   , k(0), 0, //
@@ -382,12 +382,12 @@ LandmarkBasePtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _
   return lmk;
 }
 
-Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, const LandmarkBasePtr _lmk)
+Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const CaptureBasePtr& _capture, const LandmarkBasePtr _lmk)
 {
   Eigen::Vector2d lmk_position;
   const auto& pos                             = _lmk->getP()->getState();
-  Eigen::Vector3d pixel_position3             = getProjectionMatrix(_ftr)
-                                              * getTcwMatrix(_ftr)
+  Eigen::Vector3d pixel_position3             = getProjectionMatrix(_capture)
+                                              * getTcwMatrix(_capture)
                                               * pos;
 
   lmk_position << pixel_position3(0)/pixel_position3(2),
@@ -399,7 +399,7 @@ Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const TrackMatrix& _trac
 void PublisherVisionDebug::showLandmarks(cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID) 
 {
   const auto& lmk          = getAssociatedLandmark(_track_matrix, _ftr);
-  const auto& lmk_position = projectLandmarkHp(_track_matrix, _ftr, lmk);
+  const auto& lmk_position = projectLandmarkHp(_ftr->getCapture(), lmk);
 
   if (lmk != nullptr)
   {