Merge pull request #5913 from mvieth/toPCLPointCloud2_padding

Enhance toPCLPointCloud2 to remove padding on request

Author: mvieth

common/include/pcl/conversions.h

@@ -53,6 +53,7 @@
 
 #include <algorithm>
 #include <iterator>
+#include <numeric> // for accumulate
 
 namespace pcl
 {
@@ -256,12 +257,52 @@ namespace pcl
     fromPCLPointCloud2 (msg, cloud, field_map);
   }
 
+  namespace detail {
+    /** \brief Used together with `pcl::for_each_type`, copies all point fields from `cloud_data` (respecting each field offset) to `msg_data` (tightly packed).
+      */
+    template<typename PointT>
+    struct FieldCopier {
+      FieldCopier(std::uint8_t*& msg_data, const std::uint8_t*& cloud_data) : msg_data_ (msg_data), cloud_data_ (cloud_data) {};
+
+      template<typename U> void operator() () {
+        memcpy(msg_data_, cloud_data_ + pcl::traits::offset<PointT, U>::value, sizeof(typename pcl::traits::datatype<PointT, U>::type));
+        msg_data_ += sizeof(typename pcl::traits::datatype<PointT, U>::type);
+      }
+
+      std::uint8_t*& msg_data_;
+      const std::uint8_t*& cloud_data_;
+    };
+
+    /** \brief Used together with `pcl::for_each_type`, creates list of all fields, and list of size of each field.
+      */
+    template<typename PointT>
+    struct FieldAdderAdvanced
+    {
+      FieldAdderAdvanced (std::vector<pcl::PCLPointField>& fields, std::vector<std::size_t>& field_sizes) : fields_ (fields), field_sizes_ (field_sizes) {};
+
+      template<typename U> void operator() ()
+      {
+        pcl::PCLPointField f;
+        f.name = pcl::traits::name<PointT, U>::value;
+        f.offset = pcl::traits::offset<PointT, U>::value;
+        f.datatype = pcl::traits::datatype<PointT, U>::value;
+        f.count = pcl::traits::datatype<PointT, U>::size;
+        fields_.push_back (f);
+        field_sizes_.push_back (sizeof(typename pcl::traits::datatype<PointT, U>::type)); // If field is an array, then this is the size of all array elements
+      }
+
+      std::vector<pcl::PCLPointField>& fields_;
+      std::vector<std::size_t>& field_sizes_;
+    };
+  } // namespace detail
+
   /** \brief Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
     * \param[in] cloud the input pcl::PointCloud<T>
     * \param[out] msg the resultant PCLPointCloud2 binary blob
+    * \param[in] padding Many point types have padding to ensure alignment and SIMD compatibility. Setting this to true will copy the padding to the `PCLPointCloud2` (the default in older PCL versions). Setting this to false will make the data blob in `PCLPointCloud2` smaller, while still keeping all information (useful e.g. when sending msg over network or storing it). The amount of padding depends on the point type, and can in some cases be up to 50 percent.
     */
   template<typename PointT> void
-  toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg)
+  toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg, bool padding)
   {
     // Ease the user's burden on specifying width/height for unorganized datasets
     if (cloud.width == 0 && cloud.height == 0)
@@ -275,26 +316,55 @@ namespace pcl
       msg.height = cloud.height;
       msg.width  = cloud.width;
     }
-
-    // Fill point cloud binary data (padding and all)
-    std::size_t data_size = sizeof (PointT) * cloud.size ();
-    msg.data.resize (data_size);
-    if (data_size)
-    {
-      memcpy(msg.data.data(), cloud.data(), data_size);
-    }
-
     // Fill fields metadata
     msg.fields.clear ();
-    for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields));
+    std::vector<std::size_t> field_sizes;
+    for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdderAdvanced<PointT>(msg.fields, field_sizes));
+    // Check if padding should be kept, or if the point type does not contain padding (then the single memcpy is faster)
+    if (padding || std::accumulate(field_sizes.begin(), field_sizes.end(), static_cast<std::size_t>(0)) == sizeof (PointT)) {
+      // Fill point cloud binary data (padding and all)
+      std::size_t data_size = sizeof (PointT) * cloud.size ();
+      msg.data.resize (data_size);
+      if (data_size)
+      {
+        memcpy(msg.data.data(), cloud.data(), data_size);
+      }
+
+      msg.point_step = sizeof (PointT);
+      msg.row_step   = (sizeof (PointT) * msg.width);
+    } else {
+      std::size_t point_size = 0;
+      for(std::size_t i=0; i<msg.fields.size(); ++i) {
+        msg.fields[i].offset = point_size; // Adjust offset when padding is removed
+        point_size += field_sizes[i];
+      }
+      msg.data.resize (point_size * cloud.size());
+      std::uint8_t* msg_data = &msg.data[0];
+      const std::uint8_t* cloud_data=reinterpret_cast<const std::uint8_t*>(&cloud[0]);
+      const std::uint8_t* end = cloud_data + sizeof (PointT) * cloud.size ();
+      pcl::detail::FieldCopier<PointT> copier(msg_data, cloud_data); // copier takes msg_data and cloud_data as references, so the two are shared
+      for (; cloud_data<end; cloud_data+=sizeof(PointT)) {
+        for_each_type< typename traits::fieldList<PointT>::type > (copier);
+      }
 
+      msg.point_step = point_size;
+      msg.row_step   = point_size * msg.width;
+    }
     msg.header     = cloud.header;
-    msg.point_step = sizeof (PointT);
-    msg.row_step   = (sizeof (PointT) * msg.width);
     msg.is_dense   = cloud.is_dense;
     /// @todo msg.is_bigendian = ?;
   }
 
+  /** \brief Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
+    * \param[in] cloud the input pcl::PointCloud<T>
+    * \param[out] msg the resultant PCLPointCloud2 binary blob
+    */
+  template<typename PointT> void
+  toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg)
+  {
+    toPCLPointCloud2 (cloud, msg, true); // true is the default in older PCL version
+  }
+
    /** \brief Copy the RGB fields of a PointCloud into pcl::PCLImage format
      * \param[in] cloud the point cloud message
      * \param[out] msg the resultant pcl::PCLImage

test/common/test_io.cpp

@@ -250,6 +250,52 @@ TEST (PCL, CopyPointCloudWithSameTypes)
   ASSERT_EQ (0, cloud_out.size ());
 }
 
+TEST (toPCLPointCloud2NoPadding, PointXYZI)
+{
+  pcl::PointCloud<pcl::PointXYZI> cloud;
+  cloud.resize(static_cast<pcl::uindex_t>(2), static_cast<pcl::uindex_t>(2));
+  cloud[0].x = 1.0; cloud[0].y = 2.0; cloud[0].z = 3.0; cloud[0].intensity = 123.0;
+  cloud[1].x = -1.0; cloud[1].y = -2.0; cloud[1].z = -3.0; cloud[1].intensity = -123.0;
+  cloud[2].x = 0.1; cloud[2].y = 0.2; cloud[2].z = 0.3; cloud[2].intensity = 12.3;
+  cloud[3].x = 0.0; cloud[3].y = -1.7; cloud[3].z = 100.0; cloud[3].intensity = 3.14;
+  pcl::PCLPointCloud2 msg;
+  pcl::toPCLPointCloud2(cloud, msg, false);
+  EXPECT_EQ (msg.height, cloud.height);
+  EXPECT_EQ (msg.width, cloud.width);
+  EXPECT_EQ (msg.fields.size(), 4);
+  EXPECT_EQ (msg.fields[0].name, "x");
+  EXPECT_EQ (msg.fields[0].offset, 0);
+  EXPECT_EQ (msg.fields[0].datatype, pcl::PCLPointField::FLOAT32);
+  EXPECT_EQ (msg.fields[0].count, 1);
+  EXPECT_EQ (msg.fields[1].name, "y");
+  EXPECT_EQ (msg.fields[1].offset, 4);
+  EXPECT_EQ (msg.fields[1].datatype, pcl::PCLPointField::FLOAT32);
+  EXPECT_EQ (msg.fields[1].count, 1);
+  EXPECT_EQ (msg.fields[2].name, "z");
+  EXPECT_EQ (msg.fields[2].offset, 8);
+  EXPECT_EQ (msg.fields[2].datatype, pcl::PCLPointField::FLOAT32);
+  EXPECT_EQ (msg.fields[2].count, 1);
+  EXPECT_EQ (msg.fields[3].name, "intensity");
+  EXPECT_EQ (msg.fields[3].offset, 12);
+  EXPECT_EQ (msg.fields[3].datatype, pcl::PCLPointField::FLOAT32);
+  EXPECT_EQ (msg.fields[3].count, 1);
+  EXPECT_EQ (msg.point_step, 16);
+  EXPECT_EQ (msg.row_step, 16*cloud.width);
+  EXPECT_EQ (msg.data.size(), 16*cloud.width*cloud.height);
+  EXPECT_EQ (msg.at<float>(0, 0), 1.0f);
+  EXPECT_EQ (msg.at<float>(3, 4), -1.7f);
+  EXPECT_EQ (msg.at<float>(1, 8), -3.0f);
+  EXPECT_EQ (msg.at<float>(2, 12), 12.3f);
+  pcl::PointCloud<pcl::PointXYZI> cloud2;
+  pcl::fromPCLPointCloud2(msg, cloud2);
+  for(std::size_t i=0; i<cloud2.size(); ++i) {
+    EXPECT_EQ (cloud[i].x, cloud2[i].x);
+    EXPECT_EQ (cloud[i].y, cloud2[i].y);
+    EXPECT_EQ (cloud[i].z, cloud2[i].z);
+    EXPECT_EQ (cloud[i].intensity, cloud2[i].intensity);
+  }
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)