diff --git a/common/include/pcl/PCLPointCloud2.h b/common/include/pcl/PCLPointCloud2.h index ed8805bc5f3..3825f1090ab 100644 --- a/common/include/pcl/PCLPointCloud2.h +++ b/common/include/pcl/PCLPointCloud2.h @@ -51,7 +51,22 @@ namespace pcl using ConstPtr = boost::shared_ptr; ////////////////////////////////////////////////////////////////////////// + /** \brief Removes fields named "_" + * \return true if any fields were removed, false otherwise + */ + bool + squeeze (); + /** \brief Inplace concatenate two pcl::PCLPointCloud2 + * + * This command doesn't remove any fields named "_" (aka marked as skip) + * IFF the layout of all the fields in both the clouds is the same. However, if + * the order of skip fields is different, relevant fields are selctively copied. + * It considers it an error + * * if the total number of fields to copy after discounting the skip fields + * is different + * * if the non-skip field names in serial order are named differently + * * if the endian-ness of both clouds is different * \param[in,out] cloud1 the first input and output point cloud dataset * \param[in] cloud2 the second input point cloud dataset * \return true if successful, false if failed (e.g., name/number of fields differs) diff --git a/common/include/pcl/common/io.h b/common/include/pcl/common/io.h index 8aac9bc4fcc..e96a5e81cc4 100644 --- a/common/include/pcl/common/io.h +++ b/common/include/pcl/common/io.h @@ -244,13 +244,39 @@ namespace pcl PCL_EXPORTS int interpolatePointIndex (int p, int length, InterpolationType type); - /** \brief Concatenate two pcl::PCLPointCloud2. + /** \brief Concatenate two pcl::PointCloud + * \param[in] cloud1 the first input point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \return true if successful, false if failed + * \ingroup common + */ + template + PCL_EXPORTS bool + concatenate (const pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2, + pcl::PointCloud &cloud_out); + + /** \brief Concatenate two pcl::PCLPointCloud2 + * \param[in] cloud1 the first input point cloud dataset + * \param[in] cloud2 the second input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * \return true if successful, false if failed + * \ingroup common + */ + PCL_EXPORTS bool + concatenate (const pcl::PCLPointCloud2 &cloud1, + const pcl::PCLPointCloud2 &cloud2, + pcl::PCLPointCloud2 &cloud_out); + + /** \brief Concatenate two pcl::PCLPointCloud2 * \param[in] cloud1 the first input point cloud dataset * \param[in] cloud2 the second input point cloud dataset * \param[out] cloud_out the resultant output point cloud dataset * \return true if successful, false if failed (e.g., name/number of fields differs) * \ingroup common */ + [[deprecated("use pcl::concatenate() instead")]] PCL_EXPORTS bool concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, diff --git a/common/include/pcl/point_cloud.h b/common/include/pcl/point_cloud.h index 4be62f32dc7..ae64d089d20 100644 --- a/common/include/pcl/point_cloud.h +++ b/common/include/pcl/point_cloud.h @@ -247,21 +247,7 @@ namespace pcl inline PointCloud& operator += (const PointCloud& rhs) { - // Make the resultant point cloud take the newest stamp - if (rhs.header.stamp > header.stamp) - header.stamp = rhs.header.stamp; - - size_t nr_points = points.size (); - points.resize (nr_points + rhs.points.size ()); - for (size_t i = nr_points; i < points.size (); ++i) - points[i] = rhs.points[i - nr_points]; - - width = static_cast(points.size ()); - height = 1; - if (rhs.is_dense && is_dense) - is_dense = true; - else - is_dense = false; + concatenate((*this), rhs); return (*this); } @@ -269,12 +255,38 @@ namespace pcl * \param[in] 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 + inline PointCloud operator + (const PointCloud& rhs) { return (PointCloud (*this) += rhs); } + inline static bool + concatenate(pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2) + { + // Make the resultant point cloud take the newest stamp + cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp); + + size_t nr_points = cloud1.points.size (); + cloud1.points.reserve (nr_points + cloud2.points.size ()); + cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ()); + + cloud1.width = static_cast(cloud1.points.size ()); + cloud1.height = 1; + cloud1.is_dense = cloud1.is_dense && cloud2.is_dense; + return true; + } + + inline static bool + concatenate(const pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2, + pcl::PointCloud &cloud_out) + { + cloud_out = cloud1; + return concatenate(cloud_out, cloud2); + } + /** \brief Obtain the point given by the (column, row) coordinates. Only works on organized * datasets (those that have height != 1). * \param[in] column the column coordinate diff --git a/common/src/PCLPointCloud2.cpp b/common/src/PCLPointCloud2.cpp index 0d1c390a867..152cee6f0e1 100644 --- a/common/src/PCLPointCloud2.cpp +++ b/common/src/PCLPointCloud2.cpp @@ -46,8 +46,7 @@ #include bool -pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, - const pcl::PCLPointCloud2 &cloud2) +pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2) { if (cloud1.is_bigendian != cloud2.is_bigendian) { @@ -67,61 +66,78 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, case 0: PCL_FALLTHROUGH; case 2: - cloud1.header.stamp = std::max(cloud1.header.stamp, cloud2.header.stamp); + cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp); return (true); default: break; } + bool strip = true; + if (cloud1.fields.size () == cloud2.fields.size ()) + { + // potentially we can do a simple memcpy + if (std::equal(cloud1.fields.begin (), + cloud1.fields.end (), + cloud2.fields.begin (), + [](const auto& f1, const auto&f2) + { + return f1.name == f2.name; + })) + { + strip = false; + } + } + struct FieldDetails { std::size_t idx1, idx2; std::uint16_t size; - FieldDetails(std::size_t idx1_, std::size_t idx2_, std::uint16_t size_): idx1(idx1_), idx2(idx2_), size(size_) + FieldDetails (std::size_t idx1_, std::size_t idx2_, std::uint16_t size_): idx1 (idx1_), idx2 (idx2_), size (size_) {} }; std::vector valid_fields; - const auto max_field_size = std::max(cloud1.fields.size (), cloud2.fields.size ()); - valid_fields.reserve(max_field_size); + const auto max_field_size = std::max (cloud1.fields.size (), cloud2.fields.size ()); + valid_fields.reserve (max_field_size); - std::size_t i = 0, j = 0; - bool strip = false; - while (i < cloud1.fields.size () && j < cloud2.fields.size ()) + // refactor to return std::optional> + if (strip) { - if (cloud1.fields[i].name == "_") + std::size_t i = 0, j = 0; + while (i < cloud1.fields.size () && j < cloud2.fields.size ()) { - strip = true; - ++i; - continue; - } - if (cloud2.fields[j].name == "_") - { - strip = true; - ++j; - continue; - } + if (cloud1.fields[i].name == "_") + { + ++i; + continue; + } + if (cloud2.fields[j].name == "_") + { + ++j; + continue; + } - // We're fine with the special RGB vs RGBA use case - if ((cloud1.fields[i].name == "rgb" && cloud2.fields[j].name == "rgba") || - (cloud1.fields[i].name == "rgba" && cloud2.fields[j].name == "rgb") || - (cloud1.fields[i].name == cloud2.fields[j].name)) + // We're fine with the special RGB vs RGBA use case + if ((cloud1.fields[i].name == "rgb" && cloud2.fields[j].name == "rgba") || + (cloud1.fields[i].name == "rgba" && cloud2.fields[j].name == "rgb") || + (cloud1.fields[i].name == cloud2.fields[j].name)) + { + valid_fields.emplace_back(i, j, pcl::getFieldSize (cloud2.fields[j].datatype)); + ++i; + ++j; + continue; + } + PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ()); + return (false); + } + if (i != cloud1.fields.size () && j != cloud2.fields.size ()) { - valid_fields.emplace_back(i, j, pcl::getFieldSize (cloud2.fields[j].datatype)); - ++i; - ++j; - continue; + PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ()); + return (false); } - PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ()); - return (false); - } - if (i != cloud1.fields.size () && j != cloud2.fields.size ()) - { - PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ()); - return (false); } // It's safe to modify the cloud1 inplace - cloud1.header.stamp = std::max(cloud1.header.stamp, cloud2.header.stamp); + cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp); cloud1.is_dense = cloud1.is_dense && cloud2.is_dense; cloud1.height = 1; @@ -151,6 +167,62 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, return (true); } +bool +pcl::PCLPointCloud2::squeeze () +{ + const auto size = width * height; + struct offset + { + std::size_t from = 0, to = 0, size = 0; + explicit offset(std::size_t f = 0, std::size_t t = 0, std::size_t s = 0): from(f), to(t), size(s) {} + }; + std::vector memcpy_offsets; + // worst case: every second field needs to be moved + memcpy_offsets.reserve (fields.size ()/2 + 1); + + offset running_offset; + bool skipping = false; + for (const auto& field: fields) + { + const auto& size = field.count * pcl::getFieldSize (field.datatype); + + if (field.name != "_") + { + skipping = false; + running_offset.size += size; + continue; + } + if (skipping) + { + running_offset.from += size; + continue; + } + skipping = true; + memcpy_offsets.emplace_back(running_offset); + running_offset.from += running_offset.size; + running_offset.to += running_offset.size; + running_offset.size = 0; + } + if (!skipping) + { + memcpy_offsets.emplace_back(running_offset); + } + + for (std::size_t cp = 0; cp < size; ++cp) + { + for (const auto& memloc: memcpy_offsets) + { + if (memloc.to == memloc.from) + { + continue; + } + memcpy (reinterpret_cast (&data[cp * point_step + memloc.to]), + reinterpret_cast (&data[cp * point_step + memloc.from]), + memloc.size); + } + } +} + pcl::PCLPointCloud2& pcl::PCLPointCloud2::operator += (const PCLPointCloud2& rhs) { diff --git a/common/src/io.cpp b/common/src/io.cpp index 964af8e55b3..8d5a83eb772 100644 --- a/common/src/io.cpp +++ b/common/src/io.cpp @@ -215,13 +215,29 @@ pcl::concatenateFields (const pcl::PCLPointCloud2 &cloud1, } ////////////////////////////////////////////////////////////////////////// +template +bool +pcl::concatenate (const pcl::PointCloud &cloud1, + const pcl::PointCloud &cloud2, + pcl::PointCloud &cloud_out) +{ + return pcl::PointCloud::concatenate(cloud1, cloud2, cloud_out); +} + +bool +pcl::concatenate (const pcl::PCLPointCloud2 &cloud1, + const pcl::PCLPointCloud2 &cloud2, + pcl::PCLPointCloud2 &cloud_out) +{ + return pcl::PCLPointCloud2::concatenate(cloud1, cloud2, cloud_out); +} + bool pcl::concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out) { - // replace the following with - return pcl::PCLPointCloud2::concatenate(cloud1, cloud2, cloud_out); + return pcl::concatenate(cloud1, cloud2, cloud_out); } //////////////////////////////////////////////////////////////////////////