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;
    }
 
 
} // namespace visionx::voxelgrid::LabelDensity