Add PCLPointCloud2::operator+=() and update concatenation operation

common/CMakeLists.txt

@@ -34,6 +34,7 @@ set(range_image_srcs
 set(srcs
   src/point_types.cpp
   src/pcl_base.cpp
+  src/PCLPointCloud2.cpp
   src/io.cpp
   src/common.cpp
   src/correspondence.cpp

common/include/pcl/PCLPointCloud2.h

@@ -4,9 +4,9 @@
    #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated
 #endif
 
-#include <string>
-#include <vector>
 #include <ostream>
+#include <vector>
+
 #include <boost/predef/other/endian.h>
 
 // Include the correct Header path here
@@ -49,6 +49,57 @@ namespace pcl
   public:
     using Ptr = boost::shared_ptr< ::pcl::PCLPointCloud2>;
     using ConstPtr = boost::shared_ptr<const ::pcl::PCLPointCloud2>;
+
+    //////////////////////////////////////////////////////////////////////////
+    /** \brief Inplace concatenate two pcl::PCLPointCloud2
+      *
+      * IFF the layout of all the fields in both the clouds is the same, this command
+      * doesn't remove any fields named "_" (aka marked as skip). For comparison of field
+      * names, "rgb" and "rgba" are considered equivalent
+      * However, if the order and/or number of non-skip fields is different, the skip fields
+      * are dropped and non-skip fields copied selectively.
+      * This function returns an error if
+      *   * the total number of non-skip fields is different
+      *   * the non-skip field names are named differently (excluding "rbg{a}") in serial order
+      *   * 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)
+      */
+    static bool
+    concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2);
+
+    /** \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)
+      */
+    static bool
+    concatenate (const PCLPointCloud2 &cloud1,
+                 const PCLPointCloud2 &cloud2,
+                 PCLPointCloud2 &cloud_out)
+    {
+      cloud_out = cloud1;
+      return concatenate(cloud_out, cloud2);
+    }
+
+    /** \brief Add a point cloud to the current cloud.
+      * \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
+      */
+    PCLPointCloud2&
+    operator += (const PCLPointCloud2& rhs);
+
+    /** \brief Add a point cloud to another cloud.
+      * \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 PCLPointCloud2
+    operator + (const PCLPointCloud2& rhs)
+    {
+      return (PCLPointCloud2 (*this) += rhs);
+    }
   }; // struct PCLPointCloud2
 
   using PCLPointCloud2Ptr = boost::shared_ptr< ::pcl::PCLPointCloud2>;

common/include/pcl/common/io.h

@@ -244,13 +244,47 @@ namespace pcl
   PCL_EXPORTS int
   interpolatePointIndex (int p, int length, InterpolationType type);
 
-  /** \brief Concatenate two pcl::PCLPointCloud2.
+  /** \brief Concatenate two pcl::PointCloud<PointT>
+    * \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 <typename PointT>
+  PCL_EXPORTS bool
+  concatenate (const pcl::PointCloud<PointT> &cloud1,
+               const pcl::PointCloud<PointT> &cloud2,
+               pcl::PointCloud<PointT> &cloud_out)
+  {
+    return pcl::PointCloud<PointT>::concatenate(cloud1, cloud2, cloud_out);
+  }
+
+  /** \brief Concatenate two pcl::PCLPointCloud2
+    *
+    * \warn This function subtly differs from the deprecated `concatenatePointloud`
+    * The difference is thatthis function will concatenate IFF the non-skip fields
+    * are in the correct order and same in number. The deprecated function skipped
+    * fields even if both clouds didn't agree on the number of output fields
+    * \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, but beware of subtle difference in behavior (see documentation)")]]
   PCL_EXPORTS bool 
   concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
                          const pcl::PCLPointCloud2 &cloud2,

common/include/pcl/pcl_macros.h

@@ -344,3 +344,22 @@ aligned_free (void* ptr)
 #define PCL_MAKE_ALIGNED_OPERATOR_NEW \
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
   using _custom_allocator_type_trait = void;
+
+/**
+ * \brief Macro to add a no-op or a fallthrough attribute based on compiler feature
+ *
+ * \ingroup common
+ */
+#if __has_cpp_attribute(fallthrough) && !(defined(__clang__) && __cplusplus < 201703L)
+  #define PCL_FALLTHROUGH [[fallthrough]];
+#elif defined(__clang__)
+  #define PCL_FALLTHROUGH [[clang::fallthrough]];
+#elif defined(__GNUC__)
+  #if __GNUC__ >= 7
+    #define PCL_FALLTHROUGH [[gnu::fallthrough]];
+  #else
+    #define PCL_FALLTHROUGH ;
+  #endif
+#else
+  #define PCL_FALLTHROUGH ;
+#endif

common/include/pcl/point_cloud.h

@@ -247,34 +247,46 @@ 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<uint32_t>(points.size ());
-        height   = 1;
-        if (rhs.is_dense && is_dense)
-          is_dense = true;
-        else
-          is_dense = false;
+        concatenate((*this), rhs);
         return (*this);
       }
 
       /** \brief Add a point cloud to another cloud.
         * \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<PointT> &cloud1,
+                  const pcl::PointCloud<PointT> &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<uint32_t>(cloud1.points.size ());
+        cloud1.height   = 1;
+        cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
+        return true;
+      }
+
+      inline static bool
+      concatenate(const pcl::PointCloud<PointT> &cloud1,
+               const pcl::PointCloud<PointT> &cloud2,
+               pcl::PointCloud<PointT> &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

common/src/PCLPointCloud2.cpp

@@ -0,0 +1,176 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#include <numeric>
+#include <vector>
+
+#include <pcl/common/io.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/exceptions.h>
+#include <pcl/PCLPointCloud2.h>
+
+bool
+pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2)
+{
+  if (cloud1.is_bigendian != cloud2.is_bigendian)
+  {
+    // In future, it might be possible to convert based on pcl::getFieldSize(fields.datatype)
+    PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Endianness of clouds does not match\n");
+    return (false);
+  }
+
+  const auto size1 = cloud1.width * cloud1.height;
+  const auto size2 = cloud2.width * cloud2.height;
+  //if one input cloud has no points, but the other input does, just select the cloud with points
+  switch ((bool (size1) << 1) + bool (size2))
+  {
+    case 1:
+      cloud1 = cloud2;
+      PCL_FALLTHROUGH;
+    case 0:
+      PCL_FALLTHROUGH;
+    case 2:
+      cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);
+      return (true);
+    default:
+      break;
+  }
+
+  // Ideally this should be in PCLPointField class since this is global behavior
+  // We're fine with the special RGB vs RGBA use case
+  auto field_eq = [](const auto& field1, const auto& field2)
+  {
+    return ((field1.name == field2.name) ||
+            (field1.name == "rgb" && field2.name == "rgba") ||
+            (field1.name == "rgba" && field2.name == "rgb"));
+  };
+
+  // A simple memcpy is possible if layout (name and order of fields) is same
+  bool memcpy_possible = std::equal(cloud1.fields.begin (),
+                                    cloud1.fields.end (),
+                                    cloud2.fields.begin (),
+                                    cloud2.fields.end (),
+                                    field_eq);
+
+  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_)
+    {}
+  };
+  std::vector<FieldDetails> valid_fields;
+  const auto max_field_size = std::max (cloud1.fields.size (), cloud2.fields.size ());
+  valid_fields.reserve (max_field_size);
+
+  // refactor to return std::optional<std::vector<FieldDetails>>
+  if (!memcpy_possible)
+  {
+    std::size_t i = 0, j = 0;
+    while (i < cloud1.fields.size () && j < cloud2.fields.size ())
+    {
+      if (cloud1.fields[i].name == "_")
+      {
+        ++i;
+        continue;
+      }
+      if (cloud2.fields[j].name == "_")
+      {
+        ++j;
+        continue;
+      }
+
+      if (field_eq(cloud1.fields[i], cloud2.fields[j]))
+      {
+        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 ())
+    {
+      PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Number of fields to copy in cloud1 (%u) != Number of fields to copy in cloud2 (%u)\n", i, j);
+      return (false);
+    }
+  }
+
+  // It's safe to modify the cloud1 inplace
+  cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);
+
+  cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
+  cloud1.height = 1;
+  cloud1.width = size1 + size2;
+
+  const auto data1_size = cloud1.data.size ();
+
+  // conservative allocation
+  cloud1.data.reserve (data1_size + cloud2.data.size ());
+  if (memcpy_possible)
+  {
+    cloud1.data.insert (cloud1.data.end (), cloud2.data.begin (), cloud2.data.end ());
+    return (true);
+  }
+  for (std::size_t cp = 0; cp < size2; ++cp)
+  {
+    for (const auto field_data: valid_fields)
+    {
+      const auto& i = field_data.idx1;
+      const auto& j = field_data.idx2;
+      const auto& size = field_data.size;
+      memcpy (reinterpret_cast<char*> (&cloud1.data[data1_size + cp * cloud1.point_step + cloud1.fields[i].offset]),
+              reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
+              cloud2.fields[j].count * size);
+    }
+  }
+  return (true);
+}
+
+pcl::PCLPointCloud2&
+pcl::PCLPointCloud2::operator += (const PCLPointCloud2& rhs)
+{
+  if (concatenate((*this), rhs))
+  {
+    return (*this);
+  }
+  PCL_THROW_EXCEPTION(ComputeFailedException, "Field or Endian mismatch. Please check log for more details");
+}