visualization.cpp

Go to the documentation of this file.

#include "visualization.h"
 
#include <cstdint>
#include <string>
#include <vector>
 
#include <Eigen/Core>
 
#include <Ice/Config.h>
 
#include <SimoxUtility/color/cmaps/colormaps.h>
 
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
#include <RobotAPI/interface/ArViz/Elements.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/conversions/eigen.h>
 
namespace armarx::navigation::algorithms
{
 
    void
    visualize(const algorithms::Costmap& costmap,
              viz::Layer& layer,
              const std::string& name,
              const float zOffset)
    {
        const auto cmap = simox::color::cmaps::viridis();
        const float vmax = costmap.getGrid().array().maxCoeff();
 
        const auto asColor = [&cmap, &vmax](const float distance,
                                            const bool isValid) -> viz::data::Color
        {
            const auto color = cmap.at(distance, 0.F, vmax);
 
            // Also, negative distances should be skipped (even if this would provide meaningful information).
            const Ice::Byte alpha = isValid && distance >= 0 ? color.a : 0;
            return {alpha, color.r, color.g, color.b};
        };
 
        const std::int64_t cols = costmap.getGrid().cols();
        const std::int64_t rows = costmap.getGrid().rows();
 
        auto mesh = viz::Mesh(name);
 
        std::vector<std::vector<Eigen::Vector3f>> vertices;
        std::vector<std::vector<viz::data::Color>> colors;
 
        for (int r = 0; r < rows; r++)
        {
            auto& verticesRow = vertices.emplace_back(cols);
            auto& colorsRow = colors.emplace_back(cols);
 
            for (int c = 0; c < cols; c++)
            {
                verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
 
                const bool isValid =
                    costmap.getMask().has_value() ? costmap.getMask().value()(r, c) : true;
                colorsRow.at(c) = asColor(costmap.getGrid()(r, c), isValid);
            }
        }
 
        mesh.grid2D(vertices, colors);
        mesh.position({0, 0, zOffset});
 
        layer.add(mesh);
 
        // also visualize origin
        layer.add(viz::Pose("origin").pose(conv::to3D(costmap.origin()).matrix()));
    }
 
 
} // namespace armarx::navigation::algorithms