From 6bc52b838410a0dde77f201722b1b65cca7bdcae Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Tue, 14 Jan 2025 15:32:45 +0100 Subject: [PATCH] Pointcloud template now splitted in .cpp and .h files Added method pointcloud::sortDataZ to order points from 0 to inf Added method pointcloud::filterDataZ to keep zmin + +#include +#include + +namespace yarp::sig { + +template +bool PointCloud::compZ(const T& a, const T& b) +{ + if constexpr (has_member_z::value) + { + return (a.z < b.z); + } + return false; +} + +template +void PointCloud::resize(size_t width, size_t height) +{ + header.width = width; + header.height = height; + m_storage.resize(width * height); +} + +template +void PointCloud::resize(size_t width) +{ + header.width = width; + header.height = 1; + m_storage.resize(width); +} + +template +void PointCloud::clear() +{ + m_storage.clear(); + header.width = 0; + header.height = 0; +} + +template +void PointCloud::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(getRawData()), source, dataSizeBytes()); + } else { + std::vector recipe = getComposition(type); + copyFromRawData(getRawData(), source, recipe); + } +} + +template +bool PointCloud::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 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 +bool PointCloud::write(yarp::os::ConnectionWriter& writer) const +{ + writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader)); + return m_storage.write(writer); +} + +template +std::string PointCloud::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 +yarp::os::Bottle PointCloud::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 +bool PointCloud::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 +bool PointCloud::sortDataZ() +{ + if constexpr (has_member_z::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 +bool PointCloud::filterDataZ(double minZ, double maxZ) +{ + if constexpr (has_member_z::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 +void PointCloud::setPointType() +{ + if (std::is_same::value) { + header.pointType = PCL_POINT2D_XY; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_NORMAL; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_RGBA; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_I; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_INTEREST_POINT_XYZ; + return; + } + + if (std::is_same::value) { + header.pointType = PCL_POINT_XYZ_NORMAL; + return; + } + + if (std::is_same::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 +int PointCloud::getBottleTag() const +{ + return BottleTagMap(); +} + +template +PointCloud& PointCloud::operator+=(const PointCloud& 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 +const PointCloud PointCloud::operator+(const PointCloud& rhs) +{ + return (PointCloud(*this) += rhs); +} + +template +void PointCloud::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::PointCloud; +template class yarp::sig::PointCloud; +template class yarp::sig::PointCloud; +template class yarp::sig::PointCloud; +template class yarp::sig::PointCloud; +template class yarp::sig::PointCloud; +template class yarp::sig::PointCloud; +template class yarp::sig::PointCloud; diff --git a/src/libYARP_sig/src/yarp/sig/PointCloud.h b/src/libYARP_sig/src/yarp/sig/PointCloud.h index f94811d1584..06df1a0f3ee 100644 --- a/src/libYARP_sig/src/yarp/sig/PointCloud.h +++ b/src/libYARP_sig/src/yarp/sig/PointCloud.h @@ -11,14 +11,13 @@ #include #include - namespace yarp::sig { template /** * @brief The PointCloud class. */ -class PointCloud : public PointCloudBase +class YARP_sig_API PointCloud : public PointCloudBase { static_assert(std::is_same::value || std::is_same::value || @@ -28,6 +27,11 @@ class PointCloud : public PointCloudBase std::is_same::value || std::is_same::value || std::is_same::value, "yarp::sig::PointCloud: T chosen is not supported"); + +private: + + static bool compZ(const T& a, const T& b); + public: /** @@ -56,12 +60,7 @@ class PointCloud : public PointCloudBase * @param width. * @param height. */ - virtual void resize(size_t width, size_t height) - { - header.width = width; - header.height = height; - m_storage.resize(width * height); - } + virtual void resize(size_t width, size_t height); /** * @brief Resize the PointCloud. @@ -70,12 +69,7 @@ class PointCloud : public PointCloudBase * becomes unorganized. * @param width. */ - virtual void resize(size_t width) - { - header.width = width; - header.height = 1; - m_storage.resize(width); - } + virtual void resize(size_t width); const char* getRawData() const override { @@ -159,66 +153,31 @@ class PointCloud : public PointCloudBase return *this; } - /** * @brief Concatenate a point cloud to the current cloud. * @param rhs the cloud to add to the current cloud * @return the new cloud as a concatenation of the current cloud and the new given cloud */ - inline PointCloud& - operator+=(const PointCloud& 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); - } + PointCloud& operator+=(const PointCloud& rhs); /** * @brief Concatenate a point cloud to another cloud. * @param rhs the cloud to add to the current cloud * @return the new cloud as a concatenation of the current cloud and the new given cloud */ - inline const PointCloud - operator+(const PointCloud& rhs) - { - return (PointCloud(*this) += rhs); - } + const PointCloud operator+(const PointCloud& rhs); /** * @brief Insert a new point in the cloud, at the end of the container. * @note This breaks the organized structure of the cloud by setting the height to 1. * @param[in] pt the point to insert. */ - inline void push_back(const T& pt) - { - m_storage.push_back(pt); - header.width = m_storage.size(); - header.height = 1; - } + void push_back(const T& pt); /** * @brief Clear the data. */ - virtual inline void clear() - { - m_storage.clear(); - header.width = 0; - header.height = 0; - } + virtual void clear(); /** * @brief Copy the content of an external PointCloud. @@ -228,26 +187,14 @@ class PointCloud : public PointCloudBase * @param height, height of the source cloud. * @param isDense */ - virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true) - { - yAssert(source); - header.isDense = isDense; - resize(width, height); - if (this->getPointType() == type) { - memcpy(const_cast(getRawData()), source, dataSizeBytes()); - } else { - std::vector recipe = getComposition(type); - copyFromRawData(getRawData(), source, recipe); - } - } - + virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true); - template /** * Copy operator * @brief clones the content of another point cloud * @param alt the point cloud to clone */ + template void copy(const PointCloud& alt) { resize(alt.width(), alt.height()); @@ -260,99 +207,17 @@ class PointCloud : public PointCloudBase } } - bool read(yarp::os::ConnectionReader& connection) override - { - 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 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; - } - - bool write(yarp::os::ConnectionWriter& writer) const override - { - writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader)); - return m_storage.write(writer); - } + bool read(yarp::os::ConnectionReader& connection) override; - virtual std::string toString(int precision = -1, int width = -1) 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"; - } - } + bool write(yarp::os::ConnectionWriter& writer) const override; - } else { - for (size_t i = 0; i < this->size(); i++) { - ret += (*this)(i).toString(precision, width); - } - } - return ret; - } + virtual std::string toString(int precision = -1, int width = -1) const; /** * @brief Generate a yarp::os::Bottle filled with the PointCloud data. * @return the yarp::os::Bottle generated */ - yarp::os::Bottle 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; - } + yarp::os::Bottle toBottle() const; /** * @brief Populate the PointCloud from a yarp::os::Bottle @@ -360,87 +225,28 @@ class PointCloud : public PointCloudBase * the same way it is generated by the toBottle() method. * @return true for success, false otherwise */ + bool fromBottle(const yarp::os::Bottle& bt); - bool 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); - } + int getBottleTag() const override; - return true; - } + /** + * @brief Rearranges the pointcloud data so that the points are ordered from the nearest to the farthest. + * @return true for success, false otherwise + */ + bool sortDataZ(); - int getBottleTag() const override - { - return BottleTagMap(); - } + /** + * @brief Filter out points which are not included in the specified range + * @param[minZ] minimum distance. Points with z smaller than minZ are removed. + * @param[minZ] maximum distance. Points with z bugger than maxZ are removed. + * @return true for success, false otherwise + */ + bool filterDataZ(double minZ=0, double maxZ=std::numeric_limits::infinity()); private: yarp::sig::VectorOf m_storage; - void setPointType() - { - if (std::is_same::value) { - header.pointType = PCL_POINT2D_XY; - return; - } - - if (std::is_same::value) { - header.pointType = PCL_POINT_XYZ; - return; - } - - if (std::is_same::value) { - header.pointType = PCL_NORMAL; - return; - } - - if (std::is_same::value) { - header.pointType = PCL_POINT_XYZ_RGBA; - return; - } - - if (std::is_same::value) { - header.pointType = PCL_POINT_XYZ_I; - return; - } - - if (std::is_same::value) { - header.pointType = PCL_INTEREST_POINT_XYZ; - return; - } - - if (std::is_same::value) { - header.pointType = PCL_POINT_XYZ_NORMAL; - return; - } - - if (std::is_same::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; - } + void setPointType(); }; using PointCloudXY = PointCloud; @@ -452,6 +258,12 @@ using PointCloudInterestPointXYZ = PointCloud; using PointCloudXYZNormal = PointCloud; using PointCloudXYZNormalRGBA = PointCloud; +//Tests if the point of the pointcloud has the z member +template +struct has_member_z : std::false_type {}; +template +struct has_member_z().z)>> : std::true_type {}; + } // namespace yarp::sig template <> diff --git a/src/libYARP_sig/tests/PointCloudTest.cpp b/src/libYARP_sig/tests/PointCloudTest.cpp index 6aa09c9dd85..aa4db40f0d7 100644 --- a/src/libYARP_sig/tests/PointCloudTest.cpp +++ b/src/libYARP_sig/tests/PointCloudTest.cpp @@ -23,6 +23,23 @@ using namespace yarp::os; float acceptedDiff = 1e-6f; + +void fillPointCloud(PointCloud& pc, int i, double z) +{ + pc(i).x = i * 10.0; + pc(i).y = i * 10.0; + pc(i).z = z; + pc(i).r = i * 10; + pc(i).g = i * 20; + pc(i).b = i * 30; + pc(i).a = i * 40; +} + +void testPointCloud(const PointCloud& pc, int i, double z) +{ + CHECK(pc(i).z == z); +} + TEST_CASE("sig::PointCloudTest", "[yarp::sig]") { Network::setLocalMode(true); @@ -1019,5 +1036,49 @@ TEST_CASE("sig::PointCloudTest", "[yarp::sig]") CHECK(ok); // Checking data consistency } + + SECTION("Testing SortZ") + { + PointCloud testPC; + int width = 3; + int height = 2; + testPC.resize(width, height); + + fillPointCloud(testPC, 0, 2.0); + fillPointCloud(testPC, 1, 4.0); + fillPointCloud(testPC, 2, 5.0); + fillPointCloud(testPC, 3, 3.0); + fillPointCloud(testPC, 4, 0.0); + fillPointCloud(testPC, 5, 1.0); + + testPC.sortDataZ(); + + testPointCloud(testPC, 0, 0.0); + testPointCloud(testPC, 1, 1.0); + testPointCloud(testPC, 2, 2.0); + testPointCloud(testPC, 3, 3.0); + testPointCloud(testPC, 4, 4.0); + testPointCloud(testPC, 5, 5.0); + } + + SECTION("Testing filterDataZ") + { + PointCloud testPC; + testPC.push_back(DataXYZ {0, 0, 0.0}); + testPC.push_back(DataXYZ {0, 0, 3.0}); + testPC.push_back(DataXYZ {0, 0, 5.0}); + testPC.push_back(DataXYZ {0, 0, 6.0}); + testPC.push_back(DataXYZ {0, 0, 7.0}); + testPC.push_back(DataXYZ {0, 0, 9.0}); + testPC.push_back(DataXYZ {0, 0, 10.0}); + + testPC.filterDataZ(5, 7); + + CHECK(testPC.size() == 3); + CHECK(testPC(0).z == 5.0); + CHECK(testPC(1).z == 6.0); + CHECK(testPC(2).z == 7.0); + } + Network::setLocalMode(false); }