visualization.cpp
Go to the documentation of this file.
1 #include "visualization.h"
2 
3 #include <vector>
4 
5 #include <Eigen/Core>
6 
7 #include <SimoxUtility/color/cmaps/colormaps.h>
8 
11 
14 
15 
17 {
18 
19  void
20  visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name, const float zOffset)
21  {
22  const auto cmap = simox::color::cmaps::viridis();
23  const float vmax = costmap.getGrid().array().maxCoeff();
24 
25  const auto asColor = [&cmap, &vmax](const float distance,
26  const bool isValid) -> viz::data::Color
27  {
28  const auto color = cmap.at(distance, 0.F, vmax);
29  const Ice::Byte alpha = isValid ? color.a : 0;
30  return {alpha, color.r, color.g, color.b};
31  };
32 
33  const std::int64_t cols = costmap.getGrid().cols();
34  const std::int64_t rows = costmap.getGrid().rows();
35 
36  auto mesh = viz::Mesh(name);
37 
38  std::vector<std::vector<Eigen::Vector3f>> vertices;
39  std::vector<std::vector<viz::data::Color>> colors;
40 
41  for (int r = 0; r < rows; r++)
42  {
43  auto& verticesRow = vertices.emplace_back(cols);
44  auto& colorsRow = colors.emplace_back(cols);
45 
46  for (int c = 0; c < cols; c++)
47  {
48  verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
49 
50  const bool isValid =
51  costmap.getMask().has_value() ? costmap.getMask().value()(r, c) : true;
52  colorsRow.at(c) = asColor(costmap.getGrid()(r, c), isValid);
53  }
54  }
55 
56  mesh.grid2D(vertices, colors);
57  mesh.position({0,0, zOffset});
58 
59  layer.add(mesh);
60  }
61 
62 
63 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::Costmap::getMask
const std::optional< Mask > & getMask() const noexcept
Definition: Costmap.cpp:392
armarx::viz::Mesh
Definition: Mesh.h:28
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:13
Layer.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::navigation::algorithms::Costmap::toPositionGlobal
Position toPositionGlobal(const Index &index) const
Definition: Costmap.cpp:71
Costmap.h
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::navigation::algorithms::Costmap::getGrid
const Grid & getGrid() const
Definition: Costmap.cpp:236
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:20
F
Definition: ExportDialogControllerTest.cpp:16
armarx::navigation::conv::to3D
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:11
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
eigen.h
armarx::viz::Layer
Definition: Layer.h:12
visualization.h
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:13