PointCloud.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef YARP_SIG_POINTCLOUD_H
#define YARP_SIG_POINTCLOUD_H
 
#include <yarp/sig/Vector.h>
#include <yarp/sig/PointCloudBase.h>
#include <yarp/os/ConnectionReader.h>
#include <yarp/os/ConnectionWriter.h>
 
namespace yarp::sig {
 
template <class T>
class YARP_sig_API PointCloud : public PointCloudBase
{
    static_assert(std::is_same<T, DataXY>::value ||
                  std::is_same<T, DataXYZ>::value ||
                  std::is_same<T, DataNormal>::value ||
                  std::is_same<T, DataXYZRGBA>::value ||
                  std::is_same<T, DataXYZI>::value ||
                  std::is_same<T, DataInterestPointXYZ>::value ||
                  std::is_same<T, DataXYZNormal>::value ||
                  std::is_same<T, DataXYZNormalRGBA>::value, "yarp::sig::PointCloud<T>: T chosen is not supported");
 
private:
 
    static bool compZ(const T& a, const T& b);
 
public:
 
    PointCloud()
    {
        m_storage.clear();
        setPointType();
    }
 
    template <class T1>
    PointCloud(const PointCloud<T1>& alt)
    {
        setPointType();
        copy<T1>(alt);
    }
 
    virtual void resize(size_t width, size_t height);
 
    virtual void resize(size_t width);
 
    const char* getRawData() const override
    {
        return m_storage.getMemoryBlock();
    }
 
    size_t wireSizeBytes() const override
    {
        return sizeof(header) + dataSizeBytes();
    }
 
    size_t dataSizeBytes() const override
    {
        return header.width * header.height * (sizeof(T));
    }
 
    size_t size() const override
    {
        return m_storage.size();
    }
 
    inline T& operator()(size_t u, size_t v)
    {
        return m_storage[u + v * width()];
    }
 
    inline const T& operator()(size_t u, size_t v) const
    {
        return m_storage[u + v * width()];
    }
 
    inline T& operator()(size_t i)
    {
        return m_storage[i];
    }
 
    inline const T& operator()(size_t i) const
    {
        return m_storage[i];
    }
 
    template <class T1>
    const PointCloud<T>& operator=(const PointCloud<T1>& alt)
    {
        copy(alt);
        return *this;
    }
 
    PointCloud<T>& operator+=(const PointCloud<T>& rhs);
 
    const PointCloud<T>  operator+(const PointCloud<T>& rhs);
 
    void push_back(const T& pt);
 
    virtual void clear();
 
    virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true);
 
    template <class T1>
    void copy(const PointCloud<T1>& alt)
    {
        resize(alt.width(), alt.height());
        if (std::is_same<T, T1>::value) {
            yAssert(dataSizeBytes() == alt.dataSizeBytes());
            memcpy(const_cast<char*>(getRawData()), alt.getRawData(), dataSizeBytes());
        } else {
            std::vector<int> recipe = getComposition(alt.getPointType());
            copyFromRawData(getRawData(), alt.getRawData(), recipe);
        }
    }
 
    bool read(yarp::os::ConnectionReader& connection) override;
 
    bool write(yarp::os::ConnectionWriter& writer) const override;
 
    virtual std::string toString(int precision = -1, int width = -1) const;
 
    yarp::os::Bottle toBottle() const;
 
    bool fromBottle(const yarp::os::Bottle& bt);
 
    int getBottleTag() const override;
 
    bool sortDataZ();
 
    bool filterDataZ(double minZ=0, double maxZ=std::numeric_limits<double>::infinity());
 
private:
    yarp::sig::VectorOf<T> m_storage;
 
    void setPointType();
};
 
using PointCloudXY               = PointCloud<DataXY>;
using PointCloudXYZ              = PointCloud<DataXYZ>;
using PointCloudNormal           = PointCloud<DataNormal>;
using PointCloudXYZRGBA          = PointCloud<DataXYZRGBA>;
using PointCloudXYZI             = PointCloud<DataXYZI>;
using PointCloudInterestPointXYZ = PointCloud<DataInterestPointXYZ>;
using PointCloudXYZNormal        = PointCloud<DataXYZNormal>;
using PointCloudXYZNormalRGBA    = PointCloud<DataXYZNormalRGBA>;
 
//Tests if the point of the pointcloud has the z member
template <typename T, typename = void>
struct has_member_z : std::false_type {};
template <typename T>
struct has_member_z<T, std::void_t<decltype(std::declval<T>().z)>> : std::true_type {};
 
} // namespace yarp::sig
 
template <>
inline int BottleTagMap<yarp::sig::DataXY>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataXYZ>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataNormal>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataXYZRGBA>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataXYZI>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataInterestPointXYZ>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataXYZNormal>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
template <>
inline int BottleTagMap<yarp::sig::DataXYZNormalRGBA>()
{
    return BOTTLE_TAG_FLOAT64;
}
 
 
#endif // YARP_SIG_POINTCLOUD_H