Grid.cpp

Go to the documentation of this file.

#include "Grid.h"
 
#include <cstddef>
#include <functional>
#include <optional>
#include <vector>
 
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/ColorMap.h>
 
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
#include <RobotAPI/interface/ArViz/Elements.h>
 
namespace armarx::viz
{
 
    Grid&
    Grid::grid(const GridT& grd,
               const std::optional<MaskT>& mask,
               const simox::color::ColorMap& cmap,
               const std::optional<float>& vmin,
               const std::optional<float>& vmax)
    {
        const float vMin = vmin.has_value() ? vmin.value() : grd.minCoeff();
        const float vMax = vmax.has_value() ? vmax.value() : grd.maxCoeff();
 
        const auto colorFn = [cmap, vMin, vMax](const float val) -> simox::Color
        { return cmap.at(val, vMin, vMax); };
 
        return grid(grd, mask, colorFn);
    }
 
    Grid&
    Grid::grid(const GridT& grd, const std::optional<MaskT>& mask, const ColorFuncT& colorFunc)
    {
        this->data_->sizeX = grd.cols();
        this->data_->sizeY = grd.rows();
 
        auto& colors = this->data_->colors;
        colors.clear();
        colors.reserve(grd.size());
 
        for (int r = 0; r < grd.rows(); ++r)
        {
            for (int c = 0; c < grd.cols(); ++c)
            {
                if (mask.has_value() and not mask->operator()(r, c))
                {
                    colors.emplace_back(0, 0, 0, 0); // transparent
                }
                else
                {
                    const auto color = colorFunc(grd(r, c));
                    colors.push_back(viz::Color::fromRGBA(color));
                }
            }
        }
 
 
        return *this;
    }
 
    Grid&
    Grid::origin(const Eigen::Isometry3f& origin)
    {
        auto& pose = data_->pose;
 
        // position
        pose.x = origin.translation().x();
        pose.y = origin.translation().y();
        pose.z = origin.translation().z();
 
        // orientation
        Eigen::Quaternionf q{origin.rotation()};
        pose.qw = q.w();
        pose.qx = q.x();
        pose.qy = q.y();
        pose.qz = q.z();
 
        return *this;
    }
 
    Grid&
    Grid::resolution(const float res)
    {
        data_->resolution = res;
        return *this;
    }
} // namespace armarx::viz