From 287dd37562fe5d9d69baa5d0941bfcadc51fa2c2 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Mon, 11 Apr 2022 16:36:18 +0200
Subject: [PATCH] Change projectLandmarkHp function

---
 include/publisher_vision.h |  2 +-
 src/publisher_vision.cpp   | 32 +++++++++++++-------------------
 2 files changed, 14 insertions(+), 20 deletions(-)

diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 63bbc9c..c54b9fb 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -94,7 +94,7 @@ 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;
-        bool                        projectLandmarkHp   (const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, Eigen::Vector2d& _pixel_position, unsigned int &_lmk_id);       
+        Eigen::Vector2d             projectLandmarkHp(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, 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;
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index fe1b472..cdce28f 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -382,34 +382,28 @@ LandmarkBasePtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _
   return lmk;
 }
 
-bool PublisherVisionDebug::projectLandmarkHp(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, Eigen::Vector2d& _lmk_position, unsigned int& _lmk_id)
+Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, const LandmarkBasePtr _lmk)
 {
-  const auto& lmk = getAssociatedLandmark(_track_matrix, _ftr);
-
-  if (lmk != nullptr)
-  {
-    _lmk_id                                     = lmk->id();
-    const auto& pos                             = lmk->getP()->getState();
-    Eigen::Matrix<double, 3, 1> pixel_position3 = getProjectionMatrix(_ftr)
-                                                * getTcwMatrix(_ftr)
-                                                * pos;
+  Eigen::Vector2d lmk_position;
+  const auto& pos                             = _lmk->getP()->getState();
+  Eigen::Vector3d pixel_position3             = getProjectionMatrix(_ftr)
+                                              * getTcwMatrix(_ftr)
+                                              * pos;
 
-    _lmk_position << pixel_position3(0)/pixel_position3(2),
-                      pixel_position3(1)/pixel_position3(2);
+  lmk_position << pixel_position3(0)/pixel_position3(2),
+                  pixel_position3(1)/pixel_position3(2);
 
-    return true;
-  }
-
-  return false;
+  return lmk_position;
 }
 
 void PublisherVisionDebug::showLandmarks(cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID) 
 {
-  Eigen::Vector2d lmk_position;
-  unsigned int lmk_id;
+  const auto& lmk          = getAssociatedLandmark(_track_matrix, _ftr);
+  const auto& lmk_position = projectLandmarkHp(_track_matrix, _ftr, lmk);
 
-  if (projectLandmarkHp(_track_matrix, _ftr, lmk_position, lmk_id))
+  if (lmk != nullptr)
   {
+    unsigned int lmk_id = lmk->id();
     cv::drawMarker(_image, cv::Point(lmk_position(0), lmk_position(1)), 
                    CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS, 
                    5, 1);
-- 
GitLab