visualization.cpp
Go to the documentation of this file.
1 #include "visualization.h"
2 
3 #include <cstdint>
4 #include <string>
5 #include <vector>
6 
7 #include <Eigen/Core>
8 
9 #include <Ice/Config.h>
10 
11 #include <SimoxUtility/color/cmaps/colormaps.h>
12 
16 #include <RobotAPI/interface/ArViz/Elements.h>
17 
20 
22 {
23 
24  void
26  viz::Layer& layer,
27  const std::string& name,
28  const float zOffset)
29  {
30  const auto cmap = simox::color::cmaps::viridis();
31  const float vmax = costmap.getGrid().array().maxCoeff();
32 
33  const auto asColor = [&cmap, &vmax](const float distance,
34  const bool isValid) -> viz::data::Color
35  {
36  const auto color = cmap.at(distance, 0.F, vmax);
37 
38  // Also, negative distances should be skipped (even if this would provide meaningful information).
39  const Ice::Byte alpha = isValid && distance >= 0 ? color.a : 0;
40  return {alpha, color.r, color.g, color.b};
41  };
42 
43  const std::int64_t cols = costmap.getGrid().cols();
44  const std::int64_t rows = costmap.getGrid().rows();
45 
46  auto mesh = viz::Mesh(name);
47 
48  std::vector<std::vector<Eigen::Vector3f>> vertices;
49  std::vector<std::vector<viz::data::Color>> colors;
50 
51  for (int r = 0; r < rows; r++)
52  {
53  auto& verticesRow = vertices.emplace_back(cols);
54  auto& colorsRow = colors.emplace_back(cols);
55 
56  for (int c = 0; c < cols; c++)
57  {
58  verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
59 
60  const bool isValid =
61  costmap.getMask().has_value() ? costmap.getMask().value()(r, c) : true;
62  colorsRow.at(c) = asColor(costmap.getGrid()(r, c), isValid);
63  }
64  }
65 
66  mesh.grid2D(vertices, colors);
67  mesh.position({0, 0, zOffset});
68 
69  layer.add(mesh);
70 
71  // also visualize origin
72  layer.add(viz::Pose("origin").pose(conv::to3D(costmap.origin()).matrix()));
73  }
74 
75 
76 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::Costmap::getMask
const std::optional< Mask > & getMask() const noexcept
Definition: Costmap.cpp:399
armarx::viz::Mesh
Definition: Mesh.h:28
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:25
Layer.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:31
armarx::navigation::algorithms::Costmap::toPositionGlobal
Position toPositionGlobal(const Index &index) const
Definition: Costmap.cpp:69
Elements.h
Costmap.h
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::navigation::algorithms::Costmap::getGrid
const Grid & getGrid() const
Definition: Costmap.cpp:243
armarx::viz::Pose
Definition: Elements.h:178
Mesh.h
armarx::navigation::algorithms::visualize
void visualize(const algorithms::Costmap &costmap, viz::Layer &layer, const std::string &name, const float zOffset)
Definition: visualization.cpp:25
F
Definition: ExportDialogControllerTest.cpp:18
armarx::navigation::conv::to3D
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:14
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:95
armarx::navigation::algorithms::Costmap::origin
const core::Pose2D & origin() const
Definition: Costmap.h:143
eigen.h
armarx::viz::Layer
Definition: Layer.h:12
visualization.h
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:16