Visualizer.cpp

Go to the documentation of this file.

#include "Visualizer.h"
 
#include <pcl/common/colors.h>
#include <pcl/point_types.h>  // for pcl::RGB
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 
namespace visionx::voxelgrid::LabelDensity
{
 
    Visualizer::Visualizer()
        = default;
 
    Visualizer::Visualizer(
        const armarx::DebugDrawerTopic& debugDrawer, const std::string& layer) :
        voxelgrid::Visualizer<Voxel>(debugDrawer, layer)
    {}
 
    Visualizer::~Visualizer()
        = default;
 
 
    bool Visualizer::isVisible(const Visualizer::VoxelVisuData& visu) const
    {
        return !visu.voxel.isFree();
    }
 
    void Visualizer::drawVoxel(const Visualizer::VoxelVisuData& visu)
    {
        armarx::DrawColor color {1, 1, 1, 1};
 
        const std::map<Label, float> density = visu.voxel.getDensity();
 
        const Eigen::Vector3f y = (visu.ori * Eigen::Vector3f::UnitY()) * visu.extents.y();
        const Eigen::Vector3f posBase = visu.pos - y / 2;
 
        float sum = 0;
        for (const auto& [label, fraction] : density)
        {
            const pcl::RGB rgb = pcl::GlasbeyLUT::at(static_cast<unsigned int>(label)
                                 % pcl::GlasbeyLUT::size());
 
            color = drawer().toDrawColor(rgb, alpha, true);
 
            Eigen::Vector3f extents = visu.extents;
            extents.y() *= fraction;
 
            const Eigen::Vector3f position = posBase + y * (sum + fraction / 2);
            sum += fraction;
 
            ARMARX_CHECK_LESS_EQUAL(sum, 1 + 1e-6f);
 
            drawer().drawBox({getVoxelLayer(), visu.name + "_" + std::to_string(label)},
                             position, visu.ori, extents, color);
        }
    }
 
    float Visualizer::getAlpha() const
    {
        return alpha;
    }
 
    void Visualizer::setAlpha(float value)
    {
        ARMARX_CHECK_LESS_EQUAL(0, value);
        ARMARX_CHECK_LESS_EQUAL(value, 1);
 
        this->alpha = value;
    }
 
 
}