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/Grid.h>
#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
#include <RobotAPI/interface/ArViz/Elements.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/conversions/eigen.h>
 
namespace armarx::navigation::algorithms
{
    namespace
    {
 
        // helper function to avoid code duplication
        void
        visualizeGrid(
            const algorithms::Costmap::Grid& grid,
            const std::optional<algorithms::Costmap::Mask>& mask,
            std::function<algorithms::Costmap::Position(const algorithms::Costmap::Index&)>
                indexToGlobal,
            viz::Layer& layer,
            const std::string& name,
            const float zOffset)
        {
            const auto cmap = simox::color::cmaps::viridis();
            const float vmax = grid.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 = grid.cols();
            const std::int64_t rows = grid.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(indexToGlobal({r, c}));
 
                    const bool isValid = mask.has_value() ? mask.value()(r, c) : true;
                    colorsRow.at(c) = asColor(grid(r, c), isValid);
                }
            }
 
            mesh.grid2D(vertices, colors);
            mesh.position({0, 0, zOffset});
 
            layer.add(mesh);
        }
    } // namespace
 
    void
    visualize(const algorithms::Costmap& costmap,
              viz::Layer& layer,
              const std::string& name,
              const float zOffset)
    {
        // visualizeGrid(
        //     costmap.getGrid(),
        //     costmap.getMask(),
        //     [&costmap](const auto& idx) { return costmap.toPositionGlobal(idx); },
        //     layer,
        //     name,
        //     zOffset);
 
        // // also visualize origin
        // layer.add(viz::Pose("origin").pose(conv::to3D(costmap.origin()).matrix()));
 
        auto e = viz::Grid(name);
 
        core::Pose origin = conv::to3D(costmap.origin());
        origin.translation().z() += zOffset;
 
        const Eigen::Translation3f origin_T_costmap{
            costmap.getLocalSceneBounds().min.x(), costmap.getLocalSceneBounds().min.y(), 0.F};
 
        origin *= origin_T_costmap;
 
        e.origin(origin);
 
        e.resolution(costmap.params().cellSize);
 
        e.grid(costmap.getGrid(), costmap.getMask());
 
        layer.add(e);
    }
 
    void
    visualize(const algorithms::orientation_aware::Costmap3D& costmap,
              std::vector<viz::Layer>& layers,
              const std::string& name,
              const float zOffset,
              bool initializeMask)
    {
        for (int i = 0; i < costmap.params().orientations; ++i)
        {
            std::stringstream ori_name;
            ori_name << name << "_" << i;
 
            visualize(costmap.costmapForOrientation(i, initializeMask),
                      layers[i],
                      ori_name.str(),
                      zOffset + 0.01F * i);
 
            /*visualizeGrid(
                costmap.getGrid()[i],
                costmap.getMask(),
                [&costmap](const auto& idx) { return costmap.toPositionGlobal(idx); },
                layers[i],
                ori_name.str(),
                zOffset + 0.001F*i);*/
        }
 
 
        // also visualize origin
        //layers[0].add(viz::Pose("origin").pose(conv::to3D(costmap.origin()).matrix()));
    }
 
 
} // namespace armarx::navigation::algorithms