PointCloud.h

Go to the documentation of this file.

#pragma once
 
#include <type_traits>
 
#include <SimoxUtility/color/ColorMap.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/interface/ArViz/Elements.h>
 
#include "ElementOps.h"
#include "point_cloud_type_traits.hpp"
 
namespace armarx::viz
{
 
    using data::ColoredPoint;
 
    class PointCloud : public ElementOps<PointCloud, data::ElementPointCloud>
    {
    public:
        using ElementOps::ElementOps;
 
        // Settings
 
        /**
         * @brief Enable or disable checking whether points are finite when adding them
         * (disabled by default).
         *
         * Call this function (with no argument or `true`) before adding points to enable checking.
         *
         * Non-finite points can break visualization (e.g. produce a white screen),
         * so enable this if your point cloud may contain non-finite points.
         *
         * @see isfinite()
         */
        PointCloud&
        checkFinite(bool enabled = true)
        {
            this->_checkFinite = enabled;
            return *this;
        }
 
        PointCloud&
        transparency(float t)
        {
            data_->transparency = t;
 
            return *this;
        }
 
        PointCloud&
        pointSizeInPixels(float s)
        {
            data_->pointSizeInPixels = s;
            return *this;
        }
 
        // Adding points
 
        PointCloud&
        clear()
        {
            data_->points.clear();
            return *this;
        }
 
        PointCloud&
        points(std::vector<ColoredPoint> const& ps)
        {
            std::size_t memorySize = ps.size() * sizeof(ps[0]);
            const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(ps.data());
            const Ice::Byte* const end = begin + memorySize;
            data_->points.assign(begin, end);
            return *this;
        }
 
        // Adding single points
 
        PointCloud&
        addPoint(ColoredPoint const& p)
        {
            if (isfinite(p))
            {
                addPointUnchecked(p);
            }
 
            return *this;
        }
 
        PointCloud&
        addPointUnchecked(ColoredPoint const& p)
        {
            const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(&p);
            const Ice::Byte* const end = begin + sizeof(p);
            data_->points.insert(data_->points.end(), begin, end);
            return *this;
        }
 
        PointCloud&
        addPoint(float x, float y, float z, const data::Color& color)
        {
            ColoredPoint p;
            p.x = x;
            p.y = y;
            p.z = z;
            p.color = color;
            return addPoint(p);
        }
 
        PointCloud&
        addPoint(float x, float y, float z, const simox::Color& color)
        {
            return addPoint(x, y, z, Color{color});
        }
 
        template <typename ColorCoeff = int>
        PointCloud&
        addPoint(float x,
                 float y,
                 float z,
                 ColorCoeff r,
                 ColorCoeff g,
                 ColorCoeff b,
                 ColorCoeff a = 255)
        {
            return addPoint(x, y, z, simox::Color(r, g, b, a));
        }
 
        PointCloud&
        addPoint(float x, float y, float z)
        {
            return addPoint(x, y, z, simox::Color::black(255));
        }
 
        PointCloud&
        addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
        {
            return addPoint(x, y, z, simox::color::GlasbeyLUT::at(id, alpha));
        }
 
        // Templated setters for usage with PCL point types (`pcl::Point*`).
 
        /**
         * @brief Add a point in the given color.
         */
        template <class PointT>
        PointCloud&
        addPoint(const PointT& p, Color color)
        {
            return addPoint(p.x, p.y, p.z, color);
        }
 
        /**
         * @brief Add a point with its "natural" color.
         *
         * If the point has `label`, the corresponding Glasbey color is used.
         * If the point has `r, g, b, a` (but no `label`, its RGBA is used.
         * Otherwise, its color will be grey.
         *
         * @param p Point with members `x, y, z`, optionally `label`, optionally `r, g, b, a`.
         */
        template <class PointT>
        PointCloud&
        addPoint(const PointT& p)
        {
            if constexpr (detail::has_member_label<PointT>::value)
            {
                return addPoint(p.x, p.y, p.z, simox::color::GlasbeyLUT::at(p.label));
            }
            else if constexpr (detail::has_members_rgba<PointT>::value)
            {
                return addPoint(p.x, p.y, p.z, simox::Color(p.r, p.g, p.b, p.a));
            }
            else
            {
                return addPoint(p.x, p.y, p.z, Color::gray());
            }
        }
 
        // Label and RGBA, offer additional flag to choose which
        /**
         * @brief Add a colored or labeled point.
         * @param p Point with members `x, y, z, r, g, b, a, label`.
         * @param colorByLabel Whether to color the point by label (true) or by its RGBA (false).
         */
        template <class PointT>
        PointCloud&
        addPoint(const PointT& p, bool colorByLabel)
        {
            if (colorByLabel)
            {
                return addPoint(p.x, p.y, p.z, simox::color::GlasbeyLUT::at(p.label));
            }
            else
            {
                return addPoint(p.x, p.y, p.z, simox::Color(p.r, p.g, p.b, p.a));
            }
        }
 
        // Setters for point clouds (for usage with `pcl::Point*`).
 
        /// Draw a point cloud.
        template <class PointCloudT>
        PointCloud&
        pointCloud(const PointCloudT& cloud)
        {
            return this->setPointCloud(cloud, [this](const auto& p) { this->addPoint(p); });
        }
 
        /// Draw a point cloud with given indices.
        template <class PointCloudT>
        PointCloud&
        pointCloud(const PointCloudT& cloud, const std::vector<int>& indices)
        {
            return this->setPointCloud(cloud, indices, [this](int, const auto& p) { addPoint(p); });
        }
 
        /// Draw a unicolored point cloud with given color.
        template <class PointCloudT>
        PointCloud&
        pointCloud(const PointCloudT& cloud, Color color)
        {
            return this->setPointCloud(cloud,
                                       [this, color](const auto& p) { this->addPoint(p, color); });
        }
 
        /// Draw a point cloud.
        template <class PointCloudT>
        PointCloud&
        pointCloud(const PointCloudT& cloud, bool colorByLabel)
        {
            return this->setPointCloud(
                cloud, [this, colorByLabel](const auto& p) { this->addPoint(p, colorByLabel); });
        }
 
        /// Draw a point cloud with given indices.
        template <class PointCloudT>
        PointCloud&
        pointCloud(const PointCloudT& cloud, const std::vector<int>& indices, bool colorByLabel)
        {
            return this->setPointCloud(cloud,
                                       indices,
                                       [this, colorByLabel](int, const auto& p)
                                       { addPoint(p, colorByLabel); });
        }
 
        /// Draw a unicolored point cloud with given color and indices.
        template <class PointCloudT>
        PointCloud&
        pointCloud(const PointCloudT& cloud, const std::vector<int>& indices, Color color)
        {
            return this->setPointCloud(
                cloud, indices, [this, color](int, const auto& p) { addPoint(p, color); });
            return *this;
        }
 
        /**
         * @brief Draw a colored point cloud with custom colors.
         *
         * The color of a point is specified by `colorFunc`, which must be
         * a callable taking an element of `pointCloud` and returning its
         * color as `viz::Color` or `simox::Color`.
         */
        template <class PointCloudT, class ColorFuncT>
        PointCloud&
        pointCloud(const PointCloudT& cloud, const ColorFuncT& colorFunc)
        {
            return this->setPointCloud(
                cloud, [this, &colorFunc](const auto& p) { addPoint(p, colorFunc(p)); });
        }
 
        /**
         * @brief Draw a colored point cloud with custom colors and given indices.
         */
        template <class PointCloudT, class ColorFuncT>
        PointCloud&
        pointCloud(const PointCloudT& cloud,
                   const std::vector<int>& indices,
                   const ColorFuncT& colorFunc)
        {
            return this->setPointCloud(cloud,
                                       indices,
                                       [this, &colorFunc](int, const auto& p)
                                       { addPoint(p, colorFunc(p)); });
        }
 
        /**
         * @brief Draw a colored point cloud with using a color map.
         *
         * The color of a point is specified by `colorMap` and `scalarFunc`.
         * `scalarFunc` must be a callable taking an element of `pointCloud` and returning
         * a scalar value which is passed to `colorMap` to retrieve the point's color.
         */
        template <class PointCloudT, class ScalarFuncT>
        PointCloud&
        pointCloud(const PointCloudT& pointCloud,
                   const simox::ColorMap& colorMap,
                   const ScalarFuncT& scalarFunc)
        {
            return this->pointCloud(pointCloud,
                                    [&colorMap, scalarFunc](const auto& p)
                                    { return colorMap(scalarFunc(p)); });
        }
 
        /**
         * @brief Draw a colored point cloud with using a color map and given indices.
         */
        template <class PointCloudT, class ScalarFuncT>
        PointCloud&
        pointCloud(const PointCloudT& pointCloud,
                   const std::vector<int>& indices,
                   const simox::ColorMap& colorMap,
                   const ScalarFuncT& scalarFunc)
        {
            return this->pointCloud(pointCloud,
                                    indices,
                                    [colorMap, scalarFunc](const auto& p)
                                    { return colorMap(scalarFunc(p)); });
        }
 
        // Setters taking processing functions and handling iteration.
 
        /**
         * @brief Set the point cloud from a `pcl::PointCloud`.
         *
         * @param cloud The point cloud.
         * @param pointFunc A function processing each point, taking an element of `cloud`.
         * @param checkFinite
         *   Enable or disable checking whether points are finite (enabled by default).
         *   Points containing infinity can break visualization (e.g. produce a white screen).
         * @param clear Whether to clear the point cloud beforehand (true by default).
         *
         * @return `*this`
         */
        template <class PointCloudT, class PointFunc>
        PointCloud&
        setPointCloud(const PointCloudT& cloud, const PointFunc& pointFunc, bool clear = true)
        {
            if (clear)
            {
                this->clear();
            }
            for (const auto& p : cloud)
            {
                pointFunc(p);
            }
            return *this;
        }
 
        /**
         * @brief Set the point cloud from a `pcl::PointCloud`.
         *
         * @param cloud The source point cloud.
         * @param indices The indices to add.
         * @param pointFunc
         *  A function processing each point, taking an element of each `indices` and `cloud`.
         * @param checkFinite
         *   Enable or disable checking whether points are finite (enabled by default).
         *   Points containing infinity can break visualization (e.g. produce a white screen).
         * @param clear Whether to clear the point cloud beforehand (true by default).
         *
         * @return `*this`
         */
        template <class PointCloudT, class PointFunc>
        PointCloud&
        setPointCloud(const PointCloudT& cloud,
                      const std::vector<int>& indices,
                      const PointFunc& pointFunc,
                      bool clear = true)
        {
            if (clear)
            {
                this->clear();
            }
            for (int i : indices)
            {
                ARMARX_CHECK_FITS_SIZE(i, cloud.size());
                const auto& p = cloud.at(size_t(i));
                pointFunc(i, p);
            }
            return *this;
        }
 
 
    private:
        template <class PointT>
        bool
        isfinite(const PointT& p) const
        {
            return !_checkFinite ||
                   (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z));
        }
 
        /// Whether to check whether points are finite when adding them.
        bool _checkFinite = false;
    };
 
} // namespace armarx::viz