7 #include <Eigen/Geometry>
9 #include <pcl/impl/point_types.hpp>
10 #include <pcl/point_cloud.h>
12 #include <SimoxUtility/color/Color.h>
23 #include "conversions/eigen.h"
33 const std::string& frame,
34 const Eigen::Isometry3f& globalSensorPose)
36 drawCircles(features, frame, globalSensorPose);
37 drawConvexHulls(features, frame, globalSensorPose);
38 drawEllipsoids(features, frame, globalSensorPose);
39 drawChains(features, frame, globalSensorPose);
44 const Eigen::Isometry3f& globalSensorPose,
54 .
pose(globalSensorPose));
61 const Eigen::Isometry3f& robotGlobalPose,
64 auto layer = arviz.
layer(layerName);
66 drawCircle(layer, circle, robotGlobalPose, color);
73 const Eigen::Isometry3f& globalSensorPose,
77 const Eigen::Vector3f position =
78 globalSensorPose * Eigen::Vector3f(circle.
center.x(), circle.
center.y(), 10.F);
81 .
axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
87 ArVizDrawer::drawCircles(
const std::vector<Features>& features,
88 const std::string& frame,
89 const Eigen::Isometry3f& globalSensorPose)
91 auto layer = arviz.
layer(
"circles_" + frame);
93 std::for_each(features.begin(),
109 ArVizDrawer::drawConvexHulls(
const std::vector<Features>& features,
110 const std::string& frame,
111 const Eigen::Isometry3f& globalSensorPose)
113 auto layer = arviz.layer(
"convex_hulls_" + frame);
115 std::for_each(features.begin(),
119 if (not f.convexHull)
131 ArVizDrawer::draw(
const std::string& layerName,
132 const VirtualRobot::MathTools::ConvexHull2D& robotHull,
133 const Eigen::Isometry3f& robotGlobalPose,
136 auto layer = arviz.layer(layerName);
138 drawConvexHull(layer, robotHull, robotGlobalPose, color);
143 ArVizDrawer::drawConvexHull(
viz::Layer& layer,
144 const VirtualRobot::MathTools::ConvexHull2D& hull,
145 const Eigen::Isometry3f& globalSensorPose,
153 .
pose(globalSensorPose));
157 ArVizDrawer::drawEllipsoids(
const std::vector<Features>& features,
158 const std::string& frame,
159 const Eigen::Isometry3f& globalSensorPose)
161 auto layer = arviz.layer(
"ellipsoids_" + frame);
163 std::for_each(features.begin(),
171 drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
180 const Eigen::Isometry3f& globalSensorPose)
183 const Eigen::Isometry3f pose = globalSensorPose * ellipsoid.pose;
192 ArVizDrawer::drawChains(
const std::vector<Features>& features,
193 const std::string& frame,
194 const Eigen::Isometry3f& globalSensorPose)
196 auto layer = arviz.layer(
"chains_" + frame);
198 std::for_each(features.begin(),
206 drawChain(layer, *f.chain, globalSensorPose);
221 const Eigen::Isometry3f& globalSensorPose)
223 if (chain.size() < 2)
234 .
pose(globalSensorPose));