CPointCloudColoured.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#ifndef opengl_CPointCloudColoured_H
#define opengl_CPointCloudColoured_H

#include <mrpt/opengl/CRenderizable.h>
#include <mrpt/opengl/COctreePointRenderer.h>
#include <mrpt/utils/PLY_import_export.h>
#include <mrpt/utils/adapters.h>
#include <mrpt/utils/color_maps.h>

namespace mrpt
{
namespace opengl
{
/** A cloud of points, each one with an individual colour (R,G,B). The alpha
 * component is shared by all the points and is stored in the base member
 * m_color_A.
  *
  *  To load from a points-map, CPointCloudColoured::loadFromPointsMap().
  *
  *   This class uses smart optimizations while rendering to efficiently draw
 * clouds of millions of points,
  *   as described in this page:
 * http://www.mrpt.org/Efficiently_rendering_point_clouds_of_millions_of_points
  *
  *  \sa opengl::COpenGLScene, opengl::CPointCloud
  *
  *  <div align="center">
  *  <table border="0" cellspan="4" cellspacing="4" style="border-width: 1px;
 * border-style: solid;">
  *   <tr> <td> mrpt::opengl::CPointCloudColoured </td> <td> \image html
 * preview_CPointCloudColoured.png </td> </tr>
  *  </table>
  *  </div>
  *
  * \ingroup mrpt_opengl_grp
  */
class CPointCloudColoured : public CRenderizable,
                                                        public COctreePointRenderer<CPointCloudColoured>,
                                                        public mrpt::utils::PLY_Importer,
                                                        public mrpt::utils::PLY_Exporter
{
        DEFINE_SERIALIZABLE(CPointCloudColoured)

   public:
        struct TPointColour
        {
                inline TPointColour() {}
                inline TPointColour(
                        float _x, float _y, float _z, float _R, float _G, float _B)
                        : x(_x), y(_y), z(_z), R(_R), G(_G), B(_B)
                {
                }
                float x, y, z, R, G, B;  // Float is precission enough for rendering
        };

   private:
        typedef std::vector<TPointColour> TListPointColour;
        TListPointColour m_points;

        typedef TListPointColour::iterator iterator;
        typedef TListPointColour::const_iterator const_iterator;
        inline iterator begin() { return m_points.begin(); }
        inline const_iterator begin() const { return m_points.begin(); }
        inline iterator end() { return m_points.end(); }
        inline const_iterator end() const { return m_points.end(); }
        /** By default is 1.0 */
        float m_pointSize;
        /** Default: false */
        bool m_pointSmooth;
        mutable volatile size_t m_last_rendered_count,
                m_last_rendered_count_ongoing;

   public:
        /** Constructor
          */
        CPointCloudColoured()
                : m_points(),
                  m_pointSize(1),
                  m_pointSmooth(false),
                  m_last_rendered_count(0),
                  m_last_rendered_count_ongoing(0)
        {
        }
        /** Private, virtual destructor: only can be deleted from smart pointers */
        virtual ~CPointCloudColoured() {}
        /** Do needed internal work if all points are new (octree rebuilt,...) */
        void markAllPointsAsNew();

   public:
        /** Evaluates the bounding box of this object (including possible children)
         * in the coordinate frame of the object parent. */
        virtual void getBoundingBox(
                mrpt::math::TPoint3D& bb_min,
                mrpt::math::TPoint3D& bb_max) const override
        {
                this->octree_getBoundingBox(bb_min, bb_max);
        }

        /** @name Read/Write of the list of points to render
                @{ */

        /** Inserts a new point into the point cloud. */
        void push_back(float x, float y, float z, float R, float G, float B);

        /** Set the number of points, with undefined contents */
        inline void resize(size_t N)
        {
                m_points.resize(N);
                markAllPointsAsNew();
        }

        /** Like STL std::vector's reserve */
        inline void reserve(size_t N) { m_points.reserve(N); }
        /** Read access to each individual point (checks for "i" in the valid range
         * only in Debug). */
        inline const TPointColour& operator[](size_t i) const
        {
#ifdef _DEBUG
                ASSERT_BELOW_(i, size())
#endif
                return m_points[i];
        }

        /** Read access to each individual point (checks for "i" in the valid range
         * only in Debug). */
        inline const TPointColour& getPoint(size_t i) const
        {
#ifdef _DEBUG
                ASSERT_BELOW_(i, size())
#endif
                return m_points[i];
        }

        /** Read access to each individual point (checks for "i" in the valid range
         * only in Debug). */
        inline mrpt::math::TPoint3Df getPointf(size_t i) const
        {
#ifdef _DEBUG
                ASSERT_BELOW_(i, size())
#endif
                return mrpt::math::TPoint3Df(
                        m_points[i].x, m_points[i].y, m_points[i].z);
        }

        /** Write an individual point (checks for "i" in the valid range only in
         * Debug). */
        void setPoint(size_t i, const TPointColour& p);

        /** Like \a setPoint() but does not check for index out of bounds */
        inline void setPoint_fast(const size_t i, const TPointColour& p)
        {
                m_points[i] = p;
                markAllPointsAsNew();
        }

        /** Like \a setPoint() but does not check for index out of bounds */
        inline void setPoint_fast(
                const size_t i, const float x, const float y, const float z)
        {
                TPointColour& p = m_points[i];
                p.x = x;
                p.y = y;
                p.z = z;
                markAllPointsAsNew();
        }

        /** Like \c setPointColor but without checking for out-of-index erors */
        inline void setPointColor_fast(size_t index, float R, float G, float B)
        {
                m_points[index].R = R;
                m_points[index].G = G;
                m_points[index].B = B;
        }
        /** Like \c getPointColor but without checking for out-of-index erors */
        inline void getPointColor_fast(
                size_t index, float& R, float& G, float& B) const
        {
                R = m_points[index].R;
                G = m_points[index].G;
                B = m_points[index].B;
        }

        /** Return the number of points */
        inline size_t size() const { return m_points.size(); }
        /** Erase all the points */
        inline void clear()
        {
                m_points.clear();
                markAllPointsAsNew();
        }

        /** Load the points from any other point map class supported by the adapter
         * mrpt::utils::PointCloudAdapter. */
        template <class POINTSMAP>
        void loadFromPointsMap(const POINTSMAP* themap);
        // Must be implemented at the end of the header.

        /** Get the number of elements actually rendered in the last render event.
         */
        size_t getActuallyRendered() const { return m_last_rendered_count; }
        /** @} */

        /** @name Modify the appearance of the rendered points
                @{ */

        inline void setPointSize(float pointSize) { m_pointSize = pointSize; }
        inline float getPointSize() const { return m_pointSize; }
        inline void enablePointSmooth(bool enable = true)
        {
                m_pointSmooth = enable;
        }
        inline void disablePointSmooth() { m_pointSmooth = false; }
        inline bool isPointSmoothEnabled() const { return m_pointSmooth; }
        /** Regenerates the color of each point according the one coordinate
         * (coord_index:0,1,2 for X,Y,Z) and the given color map. */
        void recolorizeByCoordinate(
                const float coord_min, const float coord_max, const int coord_index = 2,
                const mrpt::utils::TColormap color_map = mrpt::utils::cmJET);
        /** @} */

        /** Render */
        void render() const override;

        /** Render a subset of points (required by octree renderer) */
        void render_subset(
                const bool all, const std::vector<size_t>& idxs,
                const float render_area_sqpixels) const;

   protected:
        /** @name PLY Import virtual methods to implement in base classes
                @{ */
        /** In a base class, reserve memory to prepare subsequent calls to
         * PLY_import_set_vertex */
        virtual void PLY_import_set_vertex_count(const size_t N) override;
        /** In a base class, reserve memory to prepare subsequent calls to
         * PLY_import_set_face */
        virtual void PLY_import_set_face_count(const size_t N) override
        {
                MRPT_UNUSED_PARAM(N);
        }
        /** In a base class, will be called after PLY_import_set_vertex_count() once
         * for each loaded point.
          *  \param pt_color Will be nullptr if the loaded file does not provide
         * color info.
          */
        virtual void PLY_import_set_vertex(
                const size_t idx, const mrpt::math::TPoint3Df& pt,
                const mrpt::utils::TColorf* pt_color = nullptr) override;
        /** @} */

        /** @name PLY Export virtual methods to implement in base classes
                @{ */
        size_t PLY_export_get_vertex_count() const override;
        size_t PLY_export_get_face_count() const override { return 0; }
        void PLY_export_get_vertex(
                const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
                mrpt::utils::TColorf& pt_color) const override;
        /** @} */
};

mrpt::utils::CStream& operator>>(
        mrpt::utils::CStream& in, CPointCloudColoured::TPointColour& o);
mrpt::utils::CStream& operator<<(
        mrpt::utils::CStream& out, const CPointCloudColoured::TPointColour& o);

}  // end namespace

namespace utils
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NAMESPACE(
        CPointCloudColoured::TPointColour, mrpt::opengl)
}

namespace utils
{
/** Specialization
 * mrpt::utils::PointCloudAdapter<mrpt::opengl::CPointCloudColoured>  \ingroup
 * mrpt_adapters_grp*/
template <>
class PointCloudAdapter<mrpt::opengl::CPointCloudColoured>
{
   private:
        mrpt::opengl::CPointCloudColoured& m_obj;

   public:
        /** The type of each point XYZ coordinates */
        typedef float coords_t;
        /** Has any color RGB info? */
        static const int HAS_RGB = 1;
        /** Has native RGB info (as floats)? */
        static const int HAS_RGBf = 1;
        /** Has native RGB info (as uint8_t)? */
        static const int HAS_RGBu8 = 0;

        /** Constructor (accept a const ref for convenience) */
        inline PointCloudAdapter(const mrpt::opengl::CPointCloudColoured& obj)
                : m_obj(*const_cast<mrpt::opengl::CPointCloudColoured*>(&obj))
        {
        }
        /** Get number of points */
        inline size_t size() const { return m_obj.size(); }
        /** Set number of points (to uninitialized values) */
        inline void resize(const size_t N) { m_obj.resize(N); }
        /** Get XYZ coordinates of i'th point */
        template <typename T>
        inline void getPointXYZ(const size_t idx, T& x, T& y, T& z) const
        {
                const mrpt::opengl::CPointCloudColoured::TPointColour& pc = m_obj[idx];
                x = pc.x;
                y = pc.y;
                z = pc.z;
        }
        /** Set XYZ coordinates of i'th point */
        inline void setPointXYZ(
                const size_t idx, const coords_t x, const coords_t y, const coords_t z)
        {
                m_obj.setPoint_fast(idx, x, y, z);
        }

        inline void setInvalidPoint(const size_t idx)
        {
                THROW_EXCEPTION("mrpt::opengl::CPointCloudColoured needs to be dense");
        }

        /** Get XYZ_RGBf coordinates of i'th point */
        template <typename T>
        inline void getPointXYZ_RGBf(
                const size_t idx, T& x, T& y, T& z, float& r, float& g, float& b) const
        {
                const mrpt::opengl::CPointCloudColoured::TPointColour& pc = m_obj[idx];
                x = pc.x;
                y = pc.y;
                z = pc.z;
                r = pc.R;
                g = pc.G;
                b = pc.B;
        }
        /** Set XYZ_RGBf coordinates of i'th point */
        inline void setPointXYZ_RGBf(
                const size_t idx, const coords_t x, const coords_t y, const coords_t z,
                const float r, const float g, const float b)
        {
                m_obj.setPoint_fast(
                        idx,
                        mrpt::opengl::CPointCloudColoured::TPointColour(x, y, z, r, g, b));
        }

        /** Get XYZ_RGBu8 coordinates of i'th point */
        template <typename T>
        inline void getPointXYZ_RGBu8(
                const size_t idx, T& x, T& y, T& z, uint8_t& r, uint8_t& g,
                uint8_t& b) const
        {
                const mrpt::opengl::CPointCloudColoured::TPointColour& pc = m_obj[idx];
                x = pc.x;
                y = pc.y;
                z = pc.z;
                r = pc.R * 255;
                g = pc.G * 255;
                b = pc.B * 255;
        }
        /** Set XYZ_RGBu8 coordinates of i'th point */
        inline void setPointXYZ_RGBu8(
                const size_t idx, const coords_t x, const coords_t y, const coords_t z,
                const uint8_t r, const uint8_t g, const uint8_t b)
        {
                m_obj.setPoint_fast(
                        idx, mrpt::opengl::CPointCloudColoured::TPointColour(
                                         x, y, z, r / 255.f, g / 255.f, b / 255.f));
        }

        /** Get RGBf color of i'th point */
        inline void getPointRGBf(
                const size_t idx, float& r, float& g, float& b) const
        {
                m_obj.getPointColor_fast(idx, r, g, b);
        }
        /** Set XYZ_RGBf coordinates of i'th point */
        inline void setPointRGBf(
                const size_t idx, const float r, const float g, const float b)
        {
                m_obj.setPointColor_fast(idx, r, g, b);
        }

        /** Get RGBu8 color of i'th point */
        inline void getPointRGBu8(
                const size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
        {
                float R, G, B;
                m_obj.getPointColor_fast(idx, R, G, B);
                r = R * 255;
                g = G * 255;
                b = B * 255;
        }
        /** Set RGBu8 coordinates of i'th point */
        inline void setPointRGBu8(
                const size_t idx, const uint8_t r, const uint8_t g, const uint8_t b)
        {
                m_obj.setPointColor_fast(idx, r / 255.f, g / 255.f, b / 255.f);
        }

};  // end of PointCloudAdapter<mrpt::opengl::CPointCloudColoured>
}
namespace opengl
{
// After declaring the adapter we can here implement this method:
template <class POINTSMAP>
void CPointCloudColoured::loadFromPointsMap(const POINTSMAP* themap)
{
        mrpt::utils::PointCloudAdapter<CPointCloudColoured> pc_dst(*this);
        const mrpt::utils::PointCloudAdapter<POINTSMAP> pc_src(*themap);
        const size_t N = pc_src.size();
        pc_dst.resize(N);
        for (size_t i = 0; i < N; i++)
        {
                float x, y, z, r, g, b;
                pc_src.getPointXYZ_RGBf(i, x, y, z, r, g, b);
                pc_dst.setPointXYZ_RGBf(i, x, y, z, r, g, b);
        }
}
}
}  // End of namespace

#endif