ArVizDrawer.cpp
Go to the documentation of this file.
1 #include "ArVizDrawer.h"
2 
3 #include <iterator>
4 #include <string>
5 
6 #include <Eigen/Core>
7 #include <Eigen/Geometry>
8 
9 #include <pcl/impl/point_types.hpp>
10 #include <pcl/point_cloud.h>
11 
12 #include <SimoxUtility/color/Color.h>
13 
21 
22 #include "FeatureExtractor.h"
23 #include "conversions/eigen.h"
24 #include "conversions/pcl.h"
25 #include "conversions/pcl_eigen.h"
27 
29 {
30 
31  void
32  ArVizDrawer::draw(const std::vector<Features>& features,
33  const std::string& frame,
34  const Eigen::Isometry3f& globalSensorPose)
35  {
36  drawCircles(features, frame, globalSensorPose);
37  drawConvexHulls(features, frame, globalSensorPose);
38  drawEllipsoids(features, frame, globalSensorPose);
39  drawChains(features, frame, globalSensorPose);
40  }
41 
42  void
44  const Eigen::Isometry3f& globalSensorPose,
45  const simox::Color& color)
46  {
47  auto layer = arviz.layer("points_" + msg.header.frame);
48 
49  const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.data));
50 
51  layer.add(viz::PointCloud("points_" + std::to_string(layer.size()))
52  .pointCloud(pointCloud, viz::Color(color))
54  .pose(globalSensorPose));
55  arviz.commit(layer);
56  }
57 
58  void
59  ArVizDrawer::draw(const std::string& layerName,
60  const Circle& circle,
61  const Eigen::Isometry3f& robotGlobalPose,
62  const simox::Color& color)
63  {
64  auto layer = arviz.layer(layerName);
65 
66  drawCircle(layer, circle, robotGlobalPose, color);
67  arviz.commit(layer);
68  }
69 
70  void
71  ArVizDrawer::drawCircle(viz::Layer& layer,
72  const Circle& circle,
73  const Eigen::Isometry3f& globalSensorPose,
74  const simox::Color& color)
75  {
76 
77  const Eigen::Vector3f position =
78  globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), 10.F);
79 
80  layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
81  .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
82  .position(position)
83  .color(color));
84  }
85 
86  void
87  ArVizDrawer::drawCircles(const std::vector<Features>& features,
88  const std::string& frame,
89  const Eigen::Isometry3f& globalSensorPose)
90  {
91  auto layer = arviz.layer("circles_" + frame);
92 
93  std::for_each(features.begin(),
94  features.end(),
95  [&](const Features& f)
96  {
97  if (not f.circle)
98  {
99  return;
100  }
101  drawCircle(
102  layer, *f.circle, globalSensorPose, simox::Color::red(200, 100));
103  });
104 
105  arviz.commit(layer);
106  }
107 
108  void
109  ArVizDrawer::drawConvexHulls(const std::vector<Features>& features,
110  const std::string& frame,
111  const Eigen::Isometry3f& globalSensorPose)
112  {
113  auto layer = arviz.layer("convex_hulls_" + frame);
114 
115  std::for_each(features.begin(),
116  features.end(),
117  [&](const Features& f)
118  {
119  if (not f.convexHull)
120  {
121  return;
122  }
123  drawConvexHull(
124  layer, *f.convexHull, globalSensorPose, simox::Color::red(100, 80));
125  });
126 
127  arviz.commit(layer);
128  }
129 
130  void
131  ArVizDrawer::draw(const std::string& layerName,
132  const VirtualRobot::MathTools::ConvexHull2D& robotHull,
133  const Eigen::Isometry3f& robotGlobalPose,
134  const simox::Color& color)
135  {
136  auto layer = arviz.layer(layerName);
137 
138  drawConvexHull(layer, robotHull, robotGlobalPose, color);
139  arviz.commit(layer);
140  }
141 
142  void
143  ArVizDrawer::drawConvexHull(viz::Layer& layer,
144  const VirtualRobot::MathTools::ConvexHull2D& hull,
145  const Eigen::Isometry3f& globalSensorPose,
146  const simox::Color& color)
147  {
148  const auto points = conversions::to3D(hull.vertices);
149 
150  layer.add(viz::Polygon("convex_hull_" + std::to_string(layer.size()))
151  .points(points)
152  .color(color)
153  .pose(globalSensorPose));
154  }
155 
156  void
157  ArVizDrawer::drawEllipsoids(const std::vector<Features>& features,
158  const std::string& frame,
159  const Eigen::Isometry3f& globalSensorPose)
160  {
161  auto layer = arviz.layer("ellipsoids_" + frame);
162 
163  std::for_each(features.begin(),
164  features.end(),
165  [&](const Features& f)
166  {
167  if (not f.ellipsoid)
168  {
169  return;
170  }
171  drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
172  });
173 
174  arviz.commit(layer);
175  }
176 
177  void
178  ArVizDrawer::drawEllipsoid(viz::Layer& layer,
179  const Ellipsoid& ellipsoid,
180  const Eigen::Isometry3f& globalSensorPose)
181  {
182 
183  const Eigen::Isometry3f pose = globalSensorPose * ellipsoid.pose;
184 
185  layer.add(viz::Ellipsoid("ellipsoid_" + std::to_string(layer.size()))
186  .axisLengths(conversions::to3D(ellipsoid.radii))
187  .pose(pose)
188  .color(simox::Color(255, 102, 0, 128))); // orange, but a bit more shiny
189  }
190 
191  void
192  ArVizDrawer::drawChains(const std::vector<Features>& features,
193  const std::string& frame,
194  const Eigen::Isometry3f& globalSensorPose)
195  {
196  auto layer = arviz.layer("chains_" + frame);
197 
198  std::for_each(features.begin(),
199  features.end(),
200  [&](const Features& f)
201  {
202  if (not f.chain)
203  {
204  return;
205  }
206  drawChain(layer, *f.chain, globalSensorPose);
207 
208  // const auto ellipsoids = f.linesAsEllipsoids(50);
209  // for (const auto& ellipsoid : ellipsoids)
210  // {
211  // drawEllipsoid(layer, ellipsoid, globalSensorPose);
212  // }
213  });
214 
215  arviz.commit(layer);
216  }
217 
218  void
219  ArVizDrawer::drawChain(viz::Layer& layer,
220  const Points& chain,
221  const Eigen::Isometry3f& globalSensorPose)
222  {
223  if (chain.size() < 2)
224  {
225  return;
226  }
227 
228  const auto cloud = conversions::to3D(chain);
229 
230  layer.add(viz::Path("chain_" + std::to_string(layer.size()))
231  .points(cloud)
232  .width(7.F)
234  .pose(globalSensorPose));
235  }
236 } // namespace armarx::navigation::components::laser_scanner_feature_extraction
Client.h
pcl.h
armarx::armem::laser_scans::LaserScanStamped
Definition: types.h:40
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
armarx::conversions::to3D
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:8
armarx::viz::Polygon::points
Polygon & points(std::vector< Eigen::Vector3f > const &ps)
Definition: Elements.cpp:120
Path.h
armarx::navigation::components::laser_scanner_feature_extraction::Ellipsoid
memory::Ellipsoid Ellipsoid
Definition: EnclosingEllipsoid.h:42
armarx::armem::laser_scans::LaserScanStamped::data
LaserScan data
Definition: types.h:43
armarx::viz::Color::blue
static Color blue(int b=255, int a=255)
Definition: Color.h:89
visionx::Points
std::vector< Point > Points
Definition: ObjectShapeClassification.h:70
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::armem::laser_scans::SensorHeader::frame
std::string frame
Definition: types.h:36
Elements.h
armarx::viz::ElementOps::position
DerivedT & position(float x, float y, float z)
Definition: ElementOps.h:127
FeatureExtractor.h
Color
uint32_t Color
RGBA color.
Definition: color.h:8
Line.h
armarx::viz::PointCloud::pointSizeInPixels
PointCloud & pointSizeInPixels(float s)
Definition: PointCloud.h:55
Color.h
armarx::conversions::eigen2pcl
pcl::PointXY eigen2pcl(const Eigen::Vector2f &pt)
Definition: pcl_eigen.h:62
armarx::navigation::memory::Circle::center
Eigen::Vector2f center
Definition: types.h:43
armarx::viz::Color
Definition: Color.h:13
pcl_eigen.h
armarx::viz::Ellipsoid
Definition: Elements.h:147
armarx::viz::PointCloud
Definition: PointCloud.h:21
armarx::viz::Ellipsoid::axisLengths
Ellipsoid & axisLengths(const Eigen::Vector3f &axisLengths)
Definition: Elements.h:157
armarx::viz::Polygon
Definition: Elements.h:258
armarx::navigation::memory::Circle
Definition: types.h:41
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::red
QColor red()
Definition: StyleSheets.h:76
armarx::conversions::Features
armarx::navigation::components::laser_scanner_feature_extraction::Features Features
Definition: features.cpp:9
armarx::viz::ElementOps::pose
DerivedT & pose(Eigen::Matrix4f const &pose)
Definition: ElementOps.h:159
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::viz::Path::points
Path & points(std::vector< Eigen::Vector3f > const &ps)
Definition: Path.cpp:25
armarx::armem::laser_scans::LaserScanStamped::header
SensorHeader header
Definition: types.h:42
armarx::viz::Layer::size
std::size_t size() const noexcept
Definition: Layer.h:48
ArVizDrawer.h
armarx::viz::Path
Definition: Path.h:31
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
laser_scanner_conversion.h
armarx::navigation::components::laser_scanner_feature_extraction
Definition: ArVizDrawer.cpp:28
armarx::viz::PointCloud::pointCloud
PointCloud & pointCloud(const PointCloudT &cloud)
Draw a point cloud.
Definition: PointCloud.h:194
armarx::viz::Layer
Definition: Layer.h:12
StyleSheets.h
armarx::viz::Path::width
Path & width(float w)
Definition: Path.cpp:18
PointCloud.h
armarx::navigation::components::laser_scanner_feature_extraction::ArVizDrawer::draw
void draw(const std::vector< Features > &features, const std::string &frame, const Eigen::Isometry3f &globalSensorPose)
Definition: ArVizDrawer.cpp:32