diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 63bbc9c3373b61fb5c92eadb98315286eecd0822..c54b9fb8a039955f622a5ecae61bfd7e75f5df0a 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 fe1b472693066c55f32a8e24d5b744f2cfd0cbcf..cdce28facf258c2eac73477b555bc3a89dda8370 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);