From 17c26687c5c71c5905d03626df9253b7036dee1e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 6 Sep 2022 12:33:54 +0200
Subject: [PATCH] landmarks 3d pose better representation
---
src/publisher_graph.cpp | 60 +++++++++++++++++++++++++++++------------
1 file changed, 43 insertions(+), 17 deletions(-)
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 5626348..480ec43 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -410,31 +410,57 @@ bool PublisherGraph::fillLandmarkMarkers(LandmarkBaseConstPtr lmk,
return false;
// SHAPE ------------------------------------------------------
+ // Pose
+ // 2d: ARROW
+ // 3d: FRAME
// Position
// 2d: CYLINDER
// 3d: SPHERE
- // Pose -> ARROW
- if (lmk->getO() != nullptr)
+ if (lmk->getO() and lmk->getO())
{
- landmark_marker_.type = visualization_msgs::Marker::ARROW;
- lmk_marker.scale.x = viz_scale_*landmark_length_;
- lmk_marker.scale.y = viz_scale_*landmark_width_;
- lmk_marker.scale.z = viz_scale_*landmark_width_;
+ if (lmk->getP()->getSize() == 2)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::ARROW;
+ lmk_marker.scale.x = viz_scale_*landmark_length_;
+ lmk_marker.scale.y = viz_scale_*landmark_width_;
+ lmk_marker.scale.z = viz_scale_*landmark_width_;
+ }
+ else if (lmk->getP()->getSize() == 3)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::LINE_LIST;
+ lmk_marker.scale.x = viz_scale_*landmark_width_;
+
+ lmk_marker.points = frame_marker_.points;
+ lmk_marker.points[1].x = viz_scale_*landmark_length_;
+ lmk_marker.points[3].y = viz_scale_*landmark_length_;
+ lmk_marker.points[5].z = viz_scale_*landmark_length_;
+
+ lmk_marker.colors = frame_marker_.colors;
+ }
+ else
+ return false;
}
- else if (lmk->getP()->getSize() == 2)
+ else if (lmk->getP())
{
- landmark_marker_.type = visualization_msgs::Marker::CYLINDER;
- lmk_marker.scale.x = viz_scale_*landmark_width_;
- lmk_marker.scale.y = viz_scale_*landmark_width_;
- lmk_marker.scale.z = viz_scale_*landmark_length_;
+ if (lmk->getP()->getSize() == 2)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::CYLINDER;
+ lmk_marker.scale.x = viz_scale_*landmark_width_;
+ lmk_marker.scale.y = viz_scale_*landmark_width_;
+ lmk_marker.scale.z = viz_scale_*landmark_length_;
+ }
+ else if (lmk->getP()->getSize() == 3)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::SPHERE;
+ lmk_marker.scale.x = viz_scale_*landmark_width_;
+ lmk_marker.scale.y = viz_scale_*landmark_width_;
+ lmk_marker.scale.z = viz_scale_*landmark_width_;
+ }
+ else
+ return false;
}
else
- {
- landmark_marker_.type = visualization_msgs::Marker::SPHERE;
- lmk_marker.scale.x = viz_scale_*landmark_width_;
- lmk_marker.scale.y = viz_scale_*landmark_width_;
- lmk_marker.scale.z = viz_scale_*landmark_width_;
- }
+ return false;
// COLOR ------------------------------------------------------
if (lmk->getHits() > landmark_max_hits_)
--
GitLab