Merge pull request #3159 from robotology/pointcloud_sorting

Pointcloud sorting

src/libYARP_sig/src/CMakeLists.txt

@@ -43,6 +43,7 @@ set(YARP_sig_SRCS
   yarp/sig/LaserMeasurementData.cpp
   yarp/sig/LayeredImage.cpp
   yarp/sig/Matrix.cpp
+  yarp/sig/PointCloud.cpp
   yarp/sig/PointCloudBase.cpp
   yarp/sig/PointCloudUtils.cpp
   yarp/sig/Sound.cpp

src/libYARP_sig/src/yarp/sig/PointCloud.cpp

@@ -0,0 +1,315 @@
+/*
+ * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/sig/PointCloud.h>
+
+#include <algorithm>
+#include <type_traits>
+#include <cmath>
+
+namespace yarp::sig {
+
+template <class T>
+bool PointCloud<T>::compZ(const T& a, const T& b)
+{
+    if constexpr (has_member_z<T>::value)
+    {
+        return (a.z < b.z);
+    }
+    return false;
+}
+
+template <class T>
+void PointCloud<T>::resize(size_t width, size_t height)
+{
+    header.width = width;
+    header.height = height;
+    m_storage.resize(width * height);
+}
+
+template <class T>
+void PointCloud<T>::resize(size_t width)
+{
+    header.width = width;
+    header.height = 1;
+    m_storage.resize(width);
+}
+
+template <class T>
+void PointCloud<T>::clear()
+{
+    m_storage.clear();
+    header.width = 0;
+    header.height = 0;
+}
+
+template <class T>
+void PointCloud<T>::fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense)
+{
+    yAssert(source);
+    header.isDense = isDense;
+    resize(width, height);
+    if (this->getPointType() == type) {
+        memcpy(const_cast<char*>(getRawData()), source, dataSizeBytes());
+    } else {
+        std::vector<int> recipe = getComposition(type);
+        copyFromRawData(getRawData(), source, recipe);
+    }
+}
+
+template <class T>
+bool PointCloud<T>::read(yarp::os::ConnectionReader& connection)
+{
+    connection.convertTextMode();
+    yarp::sig::PointCloudNetworkHeader _header;
+    bool ok = connection.expectBlock((char*)&_header, sizeof(_header));
+    if (!ok) {
+        return false;
+    }
+
+    m_storage.resize(_header.height * _header.width);
+    std::memset((void*)m_storage.data(), 0, m_storage.size() * sizeof(T));
+
+    header.height = _header.height;
+    header.width = _header.width;
+    header.isDense = _header.isDense;
+
+    if (header.pointType == _header.pointType) {
+        return m_storage.read(connection);
+    }
+
+    T* tmp = m_storage.data();
+
+    yAssert(tmp != nullptr);
+
+    // Skip the vector header....
+    connection.expectInt32();
+    connection.expectInt32();
+
+    std::vector<int> recipe = getComposition(_header.pointType);
+
+    yarp::os::ManagedBytes dummy;
+    for (size_t i = 0; i < m_storage.size(); i++) {
+        for (size_t j = 0; j < recipe.size(); j++) {
+            size_t sizeToRead = pointType2Size(recipe[j]);
+            if ((header.pointType & recipe[j])) {
+                size_t offset = getOffset(header.pointType, recipe[j]);
+                connection.expectBlock((char*)&tmp[i] + offset, sizeToRead);
+            } else {
+                dummy.allocateOnNeed(sizeToRead, sizeToRead);
+                connection.expectBlock(dummy.bytes().get(), sizeToRead);
+            }
+        }
+    }
+
+    connection.convertTextMode();
+    return true;
+}
+
+template <class T>
+bool PointCloud<T>::write(yarp::os::ConnectionWriter& writer) const
+{
+    writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader));
+    return m_storage.write(writer);
+}
+
+template <class T>
+std::string PointCloud<T>::toString(int precision, int width) const
+{
+    std::string ret;
+    if (isOrganized()) {
+        for (size_t r = 0; r < this->width(); r++) {
+            for (size_t c = 0; c < this->height(); c++) {
+                ret += (*this)(r, c).toString(precision, width);
+            }
+            if (r < this->width() - 1) // if it is not the last row
+            {
+                ret += "\n";
+            }
+        }
+
+    } else {
+        for (size_t i = 0; i < this->size(); i++) {
+            ret += (*this)(i).toString(precision, width);
+        }
+    }
+    return ret;
+}
+
+template <class T>
+yarp::os::Bottle PointCloud<T>::toBottle() const
+{
+    yarp::os::Bottle ret;
+    ret.addInt32(width());
+    ret.addInt32(height());
+    ret.addInt32(getPointType());
+    ret.addInt32(isDense());
+
+    for (size_t i = 0; i < this->size(); i++) {
+        ret.addList().append((*this)(i).toBottle());
+    }
+    return ret;
+}
+
+template <class T>
+bool PointCloud<T>::fromBottle(const yarp::os::Bottle& bt)
+{
+    if (bt.isNull()) {
+        return false;
+    }
+
+    if (this->getPointType() != bt.get(2).asInt32()) {
+        return false;
+    }
+
+    this->resize(bt.get(0).asInt32(), bt.get(1).asInt32());
+    this->header.isDense = bt.get(3).asInt32();
+
+    if ((size_t)bt.size() != 4 + width() * height()) {
+        return false;
+    }
+
+    for (size_t i = 0; i < this->size(); i++) {
+        (*this)(i).fromBottle(bt, i + 4);
+    }
+
+    return true;
+}
+
+template <class T>
+bool PointCloud<T>::sortDataZ()
+{
+    if constexpr (has_member_z<T>::value)
+    {
+        //remove the points whose z value is nan
+        m_storage.erase(std::remove_if(m_storage.begin(), m_storage.end(), [](const T& point)
+                                       { return std::isnan(point.z); }),
+                        m_storage.end());
+        //sort the points
+        std::sort(m_storage.begin(), m_storage.end(), compZ);
+        return true;
+    }
+
+    return false;
+}
+
+template <class T>
+bool PointCloud<T>::filterDataZ(double minZ, double maxZ)
+{
+    if constexpr (has_member_z<T>::value)
+    {
+        m_storage.erase(
+            std::remove_if(m_storage.begin(), m_storage.end(), [minZ, maxZ](const T& data) {
+                return data.z < minZ || data.z > maxZ;
+            }),
+            m_storage.end()
+        );
+        return true;
+    }
+
+    return false;
+}
+
+template <class T>
+void PointCloud<T>::setPointType()
+{
+    if (std::is_same<T, DataXY>::value) {
+        header.pointType = PCL_POINT2D_XY;
+        return;
+    }
+
+    if (std::is_same<T, DataXYZ>::value) {
+        header.pointType = PCL_POINT_XYZ;
+        return;
+    }
+
+    if (std::is_same<T, DataNormal>::value) {
+        header.pointType = PCL_NORMAL;
+        return;
+    }
+
+    if (std::is_same<T, DataXYZRGBA>::value) {
+        header.pointType = PCL_POINT_XYZ_RGBA;
+        return;
+    }
+
+    if (std::is_same<T, DataXYZI>::value) {
+        header.pointType = PCL_POINT_XYZ_I;
+        return;
+    }
+
+    if (std::is_same<T, DataInterestPointXYZ>::value) {
+        header.pointType = PCL_INTEREST_POINT_XYZ;
+        return;
+    }
+
+    if (std::is_same<T, DataXYZNormal>::value) {
+        header.pointType = PCL_POINT_XYZ_NORMAL;
+        return;
+    }
+
+    if (std::is_same<T, DataXYZNormalRGBA>::value) {
+        header.pointType = PCL_POINT_XYZ_NORMAL_RGBA;
+        return;
+    }
+
+    //        DataRGBA       has sense to implement them?
+    //        intensity       has sense to implement them?
+    //        DataViewpoint  has sense to implement them?
+
+    header.pointType = 0;
+}
+
+template <class T>
+int PointCloud<T>::getBottleTag() const
+{
+    return BottleTagMap<T>();
+}
+
+template <class T>
+PointCloud<T>& PointCloud<T>::operator+=(const PointCloud<T>& rhs)
+{
+    yAssert(getPointType() == rhs.getPointType());
+
+    size_t nr_points = m_storage.size();
+    m_storage.resize(nr_points + rhs.size());
+    for (size_t i = nr_points; i < m_storage.size(); ++i) {
+        m_storage[i] = rhs.m_storage[i - nr_points];
+    }
+
+    header.width = m_storage.size();
+    header.height = 1;
+    if (rhs.isDense() && isDense()) {
+        header.isDense = 1;
+    } else {
+        header.isDense = 0;
+    }
+    return (*this);
+}
+
+template <class T>
+const PointCloud<T> PointCloud<T>::operator+(const PointCloud<T>& rhs)
+{
+    return (PointCloud<T>(*this) += rhs);
+}
+
+template <class T>
+void PointCloud<T>::push_back(const T& pt)
+{
+    m_storage.push_back(pt);
+    header.width = m_storage.size();
+    header.height = 1;
+}
+
+} //namespace yarp::sig
+
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXY>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZ>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZI>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataNormal>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZRGBA>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZNormal>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA>;
+template class YARP_sig_API yarp::sig::PointCloud<yarp::sig::DataInterestPointXYZ>;