From fd25ca9685bafead04a6552a214075e29289a38d Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Mon, 11 Apr 2022 15:48:29 +0200
Subject: [PATCH] Add function getAssociatedLandmark
---
include/publisher_vision.h | 5 +--
src/publisher_vision.cpp | 72 +++++++++++++++++++++++---------------
2 files changed, 46 insertions(+), 31 deletions(-)
diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index c6d778b..63bbc9c 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -94,11 +94,12 @@ 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, Eigen::Matrix<double, 2, 1>& _pixel_position, const FeatureBasePtr& _ftr, unsigned int &_lmk_id);
+ bool projectLandmarkHp (const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, Eigen::Vector2d& _pixel_position, unsigned int &_lmk_id);
+ 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);
+ void showLandmarks (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID);
};
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index f0fa770..fe1b472 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -364,53 +364,67 @@ Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const Feat
// return pixel_position;
// }
-bool PublisherVisionDebug::projectLandmarkHp(const TrackMatrix& _track_matrix, Eigen::Matrix<double, 2, 1>& _pixel_position, const FeatureBasePtr& _ftr, unsigned int &_lmk_id)
+LandmarkBasePtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr)
{
- auto track_at_keyframe = _track_matrix.trackAtKeyframes(_ftr->trackId());
+ const auto& track_at_kfs = _track_matrix.trackAtKeyframes(_ftr->trackId());
- if (track_at_keyframe.empty())
- return false;
+ if (track_at_kfs.empty())
+ return nullptr;
- auto feature_at_last_keyframe = track_at_keyframe.rbegin()->second;
- auto factor_list = feature_at_last_keyframe->getFactorList();
+ const auto& feature_at_last_kf = track_at_kfs.rbegin()->second;
+ const auto& factor_list = feature_at_last_kf->getFactorList();
if (factor_list.empty())
- return false;
+ return nullptr;
+
+ const auto& lmk = factor_list.front()->getLandmarkOther();
- auto lmk = factor_list.front()->getLandmarkOther();
- _lmk_id = lmk->id();
- //
- auto pos = lmk->getP()->getState();
- Eigen::Matrix<double, 3, 1> pixel_position3 = getProjectionMatrix(_ftr)
- * getTcwMatrix(_ftr)
- * pos;
+ return lmk;
+}
- _pixel_position << pixel_position3(0)/pixel_position3(2),
- pixel_position3(1)/pixel_position3(2);
+bool PublisherVisionDebug::projectLandmarkHp(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, Eigen::Vector2d& _lmk_position, unsigned int& _lmk_id)
+{
+ 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;
+
+ _lmk_position << pixel_position3(0)/pixel_position3(2),
+ pixel_position3(1)/pixel_position3(2);
+
+ return true;
+ }
- return true;
+ return false;
}
void PublisherVisionDebug::showLandmarks(cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID)
{
- Eigen::Matrix<double, 2, 1> lmk_position(0,0);
+ Eigen::Vector2d lmk_position;
unsigned int lmk_id;
- if (projectLandmarkHp(_track_matrix, lmk_position, _ftr, lmk_id)) //track_at_keyframe or factor_list is not empty
+ if (projectLandmarkHp(_track_matrix, _ftr, lmk_position, lmk_id))
{
- cv::drawMarker(_image, cv::Point(lmk_position(0), lmk_position(1)), CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS, 5, 1);
+ cv::drawMarker(_image, cv::Point(lmk_position(0), lmk_position(1)),
+ CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
+ 5, 1);
if (_show_landmark_ID)
{
- int shift_text_x = 7;
- int shift_text_y = 10;
-
- cv::putText(
- _image,
- std::to_string(lmk_id),
- cv::Point(lmk_position(0) - shift_text_x,
- lmk_position(1) - shift_text_y),
- 1, 0.5, CV_RGB(0, 0, 0));
+ int shift_text_x = 7;
+ int shift_text_y = 10;
+
+ cv::putText(
+ _image,
+ std::to_string(lmk_id),
+ cv::Point(lmk_position(0) - shift_text_x,
+ lmk_position(1) - shift_text_y),
+ 1, 0.5, CV_RGB(0, 0, 0));
}
}
}
--
GitLab