diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index bbaaddfbaadf6252f51e698abb18326d98025be7..40931d745eb7429e2eaa7a53765e453ee98510e5 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -93,12 +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 CaptureBasePtr& _capture, const LandmarkBasePtr _lmk);
+ Eigen::Vector2d projectLandmarkHp (const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBasePtr _lmk);
LandmarkBasePtr getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
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);
+ void showLandmark (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID);
+ void showLandmarks (cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBasePtr& _cap, bool _show_landmark_ID);
};
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 1f6bb69f12078756436bdbbb7f5dcf2881a4e578..c8399c52803d86e6c40cc0bda87658da15af2e68 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -106,6 +106,9 @@ void PublisherVisionDebug::publishDerived()
bool show_track_ids = false;
showTracks(cv_img_debug, track_matrix, cap_img, CYAN, show_track_ids);
+ bool show_lmk_ids = false;
+ showLandmarks(cv_img_debug, track_matrix, cap_img, show_lmk_ids);
+
// Convert to image msg
cv_bridge::CvImagePtr cv_msg = boost::make_shared<cv_bridge::CvImage>();
cv_msg->header.stamp.sec = cap_img->getTimeStamp().getSeconds();
@@ -323,7 +326,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBasePtr _capture) const
{
// get robot position and orientation at capture's timestamp
- const auto& state = _capture->getProblem()->getState(_capture->getTimeStamp());
+ const auto& state = problem_->getState(_capture->getTimeStamp(), "PO");
const auto& pos = state.at('P');
const auto& ori = state.at('O');
@@ -343,7 +346,7 @@ Eigen::Matrix4d PublisherVisionDebug::getTcwMatrix(const CaptureBasePtr _capture
Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const CaptureBasePtr _capture) const
{
- auto k(_capture->getSensor()->getIntrinsic()->getState());
+ auto k(_capture->getSensor()->getIntrinsic()->getState()); // intrinsic vector of camera k = [u0 v0 au av]
Eigen::Matrix<double, 3, 4> P;
P << k(2), 0 , k(0), 0, //
@@ -371,45 +374,99 @@ LandmarkBasePtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _
return lmk;
}
-Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const CaptureBasePtr& _capture, const LandmarkBasePtr _lmk)
+Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBasePtr _lmk)
{
- Eigen::Vector2d lmk_position;
- const auto& pos = _lmk->getP()->getState();
- Eigen::Vector3d pixel_position3 = getProjectionMatrix(_capture)
- * getTcwMatrix(_capture)
- * pos;
+ const auto& pos = _lmk->getP()->getState();
+ Eigen::Vector3d pixel_hmg = _trf_proj_mat * pos;
- lmk_position << pixel_position3(0)/pixel_position3(2),
- pixel_position3(1)/pixel_position3(2);
+ Eigen::Vector2d pixel;
+ pixel << pixel_hmg(0)/pixel_hmg(2),
+ pixel_hmg(1)/pixel_hmg(2);
- return lmk_position;
+ return pixel;
}
-void PublisherVisionDebug::showLandmarks(cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr, bool _show_landmark_ID)
+void PublisherVisionDebug::showLandmark(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(_ftr->getCapture(), lmk);
-
- 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);
-
- if (_show_landmark_ID)
+
+ // get stuff common to all landmarks
+ const auto& capture = _ftr->getCapture();
+ const auto& Tcw = getTcwMatrix(capture);
+ const auto& prj_mat = getProjectionMatrix(capture);
+ const auto& trf_prj_mat = prj_mat * Tcw;
+
+ // get stuff of this landmark
+ const auto& lmk = getAssociatedLandmark(_track_matrix, _ftr);
+ const auto& lmk_pixel = projectLandmarkHp(trf_prj_mat, lmk);
+
+ if (lmk != nullptr)
{
- 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));
+ cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
+ CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
+ 5, 1);
+
+ if (_show_landmark_ID)
+ {
+ unsigned int lmk_id = lmk->id();
+ int shift_text_x = 7;
+ int shift_text_y = 10;
+
+ cv::putText(
+ _image,
+ std::to_string(lmk_id),
+ cv::Point(lmk_pixel(0) - shift_text_x,
+ lmk_pixel(1) - shift_text_y),
+ 1, 0.5, CV_RGB(0, 0, 0));
+ }
}
- }
}
+void PublisherVisionDebug::showLandmarks(cv::Mat _image,
+ const TrackMatrix& _track_matrix,
+ const CaptureBasePtr& _capture,
+ bool _show_landmark_ID)
+{
+ // get stuff common to all landmarks
+ const auto& Tcw = getTcwMatrix(_capture);
+ const auto& prj_mat = getProjectionMatrix(_capture);
+ const auto& trf_prj_mat = prj_mat * Tcw;
+
+ // draw one ladmark for each feature
+ const auto& ftrs_alive = _track_matrix.snapshotAsList(_capture);
+ for (const auto& ftr: ftrs_alive)
+ {
+ // get stuff of this landmark
+ const auto& lmk = getAssociatedLandmark(_track_matrix, ftr);
+
+ if (lmk != nullptr)
+ {
+ const auto& lmk_pixel = projectLandmarkHp(trf_prj_mat, lmk);
+
+ cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
+ CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
+ 5, 1);
+
+ if (_show_landmark_ID)
+ {
+ unsigned int lmk_id = lmk->id();
+ int shift_text_x = 7;
+ int shift_text_y = 10;
+
+ cv::putText(
+ _image,
+ std::to_string(lmk_id),
+ cv::Point(lmk_pixel(0) - shift_text_x,
+ lmk_pixel(1) - shift_text_y),
+ 1, 0.5, CV_RGB(0, 0, 0));
+ }
+ }
+
+ }
+
+}
+
+
}