7 #include <SimoxUtility/color/cmaps/colormaps.h>
22 const auto cmap = simox::color::cmaps::viridis();
23 const float vmax = costmap.
getGrid().array().maxCoeff();
25 const auto asColor = [&cmap, &vmax](
const float distance,
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};
33 const std::int64_t cols = costmap.
getGrid().cols();
34 const std::int64_t rows = costmap.
getGrid().rows();
38 std::vector<std::vector<Eigen::Vector3f>> vertices;
39 std::vector<std::vector<viz::data::Color>> colors;
41 for (
int r = 0; r < rows; r++)
43 auto& verticesRow = vertices.emplace_back(cols);
44 auto& colorsRow = colors.emplace_back(cols);
46 for (
int c = 0;
c < cols;
c++)
52 colorsRow.at(
c) = asColor(costmap.
getGrid()(r,
c), isValid);
56 mesh.grid2D(vertices, colors);
57 mesh.position({0,0, zOffset});