PCL_adapters.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 mrpt_maps_PCL_adapters_H
#define mrpt_maps_PCL_adapters_H

#include <mrpt/config.h>
#include <mrpt/utils/adapters.h>

// NOTE: Only include this file if you have PCL installed in your system
//        and do it only after including MRPT headers...

// Make sure the essential PCL headers are included:
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

namespace mrpt
{
namespace utils
{
/** Specialization mrpt::utils::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZ>
 * > for an XYZ point cloud (without RGB) \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<pcl::PointCloud<pcl::PointXYZ>>
        : public detail::PointCloudAdapterHelperNoRGB<
                  pcl::PointCloud<pcl::PointXYZ>, float>
{
   private:
        pcl::PointCloud<pcl::PointXYZ>& m_obj;

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

        /** Constructor (accept a const ref for convenience) */
        inline PointCloudAdapter(const pcl::PointCloud<pcl::PointXYZ>& obj)
                : m_obj(*const_cast<pcl::PointCloud<pcl::PointXYZ>*>(&obj))
        {
        }
        /** Get number of points */
        inline size_t size() const { return m_obj.points.size(); }
        /** Set number of points (to uninitialized values) */
        inline void resize(const size_t N) { m_obj.points.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 pcl::PointXYZ& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.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)
        {
                pcl::PointXYZ& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
        }

        /** Set Invalid Point */
        inline void setInvalidPoint(const size_t idx)
        {
                pcl::PointXYZ& p = m_obj.points[idx];
                p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
        }
};  // end of mrpt::utils::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZ> >

/** Specialization
 * mrpt::utils::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGB> > for an XYZ
 * point cloud with RGB  \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGB>>
{
   private:
        pcl::PointCloud<pcl::PointXYZRGB>& 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 = 0;
        /** Has native RGB info (as uint8_t)? */
        static const int HAS_RGBu8 = 1;

        /** Constructor (accept a const ref for convenience) */
        inline PointCloudAdapter(const pcl::PointCloud<pcl::PointXYZRGB>& obj)
                : m_obj(*const_cast<pcl::PointCloud<pcl::PointXYZRGB>*>(&obj))
        {
        }
        /** Get number of points */
        inline size_t size() const { return m_obj.points.size(); }
        /** Set number of points (to uninitialized values) */
        inline void resize(const size_t N) { m_obj.points.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 pcl::PointXYZRGB& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.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)
        {
                pcl::PointXYZRGB& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
                p.r = p.g = p.b = 255;
        }

        /** 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 pcl::PointXYZRGB& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.z;
                r = p.r / 255.f;
                g = p.g / 255.f;
                b = p.b / 255.f;
        }
        /** 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)
        {
                pcl::PointXYZRGB& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
                p.r = r * 255;
                p.g = g * 255;
                p.b = b * 255;
        }

        /** 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 pcl::PointXYZRGB& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.z;
                r = p.r;
                g = p.g;
                b = p.b;
        }
        /** 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)
        {
                pcl::PointXYZRGB& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
                p.r = r;
                p.g = g;
                p.b = b;
        }

        /** Get RGBf color of i'th point */
        inline void getPointRGBf(
                const size_t idx, float& r, float& g, float& b) const
        {
                const pcl::PointXYZRGB& p = m_obj.points[idx];
                r = p.r / 255.f;
                g = p.g / 255.f;
                b = p.b / 255.f;
        }
        /** Set XYZ_RGBf coordinates of i'th point */
        inline void setPointRGBf(
                const size_t idx, const float r, const float g, const float b)
        {
                pcl::PointXYZRGB& p = m_obj.points[idx];
                p.r = r * 255;
                p.g = g * 255;
                p.b = b * 255;
        }

        /** Get RGBu8 color of i'th point */
        inline void getPointRGBu8(
                const size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
        {
                const pcl::PointXYZRGB& p = m_obj.points[idx];
                r = p.r;
                g = p.g;
                b = p.b;
        }
        /** 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)
        {
                pcl::PointXYZRGB& p = m_obj.points[idx];
                p.r = r;
                p.g = g;
                p.b = b;
        }

};  // end of mrpt::utils::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGB> >

/** Specialization
 * mrpt::utils::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGBA> > for an
 * XYZ point cloud with RGB  \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGBA>>
{
   private:
        pcl::PointCloud<pcl::PointXYZRGBA>& 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 = 0;
        /** Has native RGB info (as uint8_t)? */
        static const int HAS_RGBu8 = 1;

        /** Constructor (accept a const ref for convenience) */
        inline PointCloudAdapter(const pcl::PointCloud<pcl::PointXYZRGBA>& obj)
                : m_obj(*const_cast<pcl::PointCloud<pcl::PointXYZRGBA>*>(&obj))
        {
        }
        /** Get number of points */
        inline size_t size() const { return m_obj.points.size(); }
        /** Set number of points (to uninitialized values) */
        inline void resize(const size_t N) { m_obj.points.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 pcl::PointXYZRGBA& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.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)
        {
                pcl::PointXYZRGBA& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
                p.r = p.g = p.b = 255;
        }

        /** Set Invalid Point */
        inline void setInvalidPoint(const size_t idx)
        {
                pcl::PointXYZRGBA& p = m_obj.points[idx];
                p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
        }

        /** 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 pcl::PointXYZRGBA& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.z;
                r = p.r / 255.f;
                g = p.g / 255.f;
                b = p.b / 255.f;
        }
        /** 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)
        {
                pcl::PointXYZRGBA& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
                p.r = r * 255;
                p.g = g * 255;
                p.b = b * 255;
        }

        /** 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 pcl::PointXYZRGBA& p = m_obj.points[idx];
                x = p.x;
                y = p.y;
                z = p.z;
                r = p.r;
                g = p.g;
                b = p.b;
        }
        /** 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)
        {
                pcl::PointXYZRGBA& p = m_obj.points[idx];
                p.x = x;
                p.y = y;
                p.z = z;
                p.r = r;
                p.g = g;
                p.b = b;
        }

        /** Get RGBf color of i'th point */
        inline void getPointRGBf(
                const size_t idx, float& r, float& g, float& b) const
        {
                const pcl::PointXYZRGBA& p = m_obj.points[idx];
                r = p.r / 255.f;
                g = p.g / 255.f;
                b = p.b / 255.f;
        }
        /** Set XYZ_RGBf coordinates of i'th point */
        inline void setPointRGBf(
                const size_t idx, const float r, const float g, const float b)
        {
                pcl::PointXYZRGBA& p = m_obj.points[idx];
                p.r = r * 255;
                p.g = g * 255;
                p.b = b * 255;
        }

        /** Get RGBu8 color of i'th point */
        inline void getPointRGBu8(
                const size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
        {
                const pcl::PointXYZRGBA& p = m_obj.points[idx];
                r = p.r;
                g = p.g;
                b = p.b;
        }
        /** 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)
        {
                pcl::PointXYZRGBA& p = m_obj.points[idx];
                p.r = r;
                p.g = g;
                p.b = b;
        }

};  // end of mrpt::utils::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGBA>
// >
}
}  // End of namespace

#endif