Add writeBinary to ostream for PCDWriter (PointCloudLibrary#5598)

* Add writeBinary to ostream for PCDWriter

Add method writeBinary to serialize point clouds in memory uncompressed.

* Revert "Add writeBinary to ostream for PCDWriter"

This reverts commit 55f3f80.

* Add independent writeBinary to ostream function

Add method writeBinary to serialize point clouds in memory uncompressed.

* Add unit test for writeBinary to ostream

* Fix register macros to match struct definitions

Fix the sequence order in POINT_CLOUD_REGISTER macros to match structs

* Revert "Fix register macros to match struct definitions"

This reverts commit b522e74.

* Sort fields in generateHeaderBinary

Sort cloud.fields by their offset value to generate correct fake fields

Author: ptrckschcknbch

io/include/pcl/io/pcd_io.h

@@ -402,6 +402,17 @@ namespace pcl
                    const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
                    const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
 
+      /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY format
+        * \param[out] os the stream into which to write the data
+        * \param[in] cloud the point cloud data message
+        * \param[in] origin the sensor acquisition origin
+        * \param[in] orientation the sensor acquisition orientation
+        */
+      int
+      writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud,
+                   const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (),
+                   const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ());
+
       /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format
         * \param[in] file_name the output file name
         * \param[in] cloud the point cloud data message

io/src/pcd_io.cpp

@@ -930,9 +930,15 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud,
          "\nVERSION 0.7"
          "\nFIELDS";
 
+  auto fields = cloud.fields;
+  std::sort(fields.begin(), fields.end(), [](const auto& field_a, const auto& field_b)
+                                          {
+                                            return field_a.offset < field_b.offset;
+                                          });
+
   // Compute the total size of the fields
   unsigned int fsize = 0;
-  for (const auto &field : cloud.fields)
+  for (const auto &field : fields)
     fsize += field.count * getFieldSize (field.datatype);
 
   // The size of the fields cannot be larger than point_step
@@ -945,20 +951,20 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud,
   std::stringstream field_names, field_types, field_sizes, field_counts;
   // Check if the size of the fields is smaller than the size of the point step
   std::size_t toffset = 0;
-  for (std::size_t i = 0; i < cloud.fields.size (); ++i)
+  for (std::size_t i = 0; i < fields.size (); ++i)
   {
     // If field offsets do not match, then we need to create fake fields
-    if (toffset != cloud.fields[i].offset)
+    if (toffset != fields[i].offset)
     {
       // If we're at the last "valid" field
       int fake_offset = (i == 0) ?
         // Use the current_field offset
-        (cloud.fields[i].offset)
+        (fields[i].offset)
         :
         // Else, do cur_field.offset - prev_field.offset + sizeof (prev_field)
-        (cloud.fields[i].offset -
-        (cloud.fields[i-1].offset +
-         cloud.fields[i-1].count * getFieldSize (cloud.fields[i-1].datatype)));
+        (fields[i].offset -
+        (fields[i-1].offset +
+         fields[i-1].count * getFieldSize (fields[i-1].datatype)));
 
       toffset += fake_offset;
 
@@ -969,11 +975,11 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud,
     }
 
     // Add the regular dimension
-    toffset += cloud.fields[i].count * getFieldSize (cloud.fields[i].datatype);
-    field_names << " " << cloud.fields[i].name;
-    field_sizes << " " << pcl::getFieldSize (cloud.fields[i].datatype);
-    field_types << " " << pcl::getFieldType (cloud.fields[i].datatype);
-    int count = std::abs (static_cast<int> (cloud.fields[i].count));
+    toffset += fields[i].count * getFieldSize (fields[i].datatype);
+    field_names << " " << fields[i].name;
+    field_sizes << " " << pcl::getFieldSize (fields[i].datatype);
+    field_types << " " << pcl::getFieldType (fields[i].datatype);
+    int count = std::abs (static_cast<int> (fields[i].count));
     if (count == 0) count = 1;  // check for 0 counts (coming from older converter code)
     field_counts << " " << count;
   }
@@ -1173,6 +1179,29 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PCLPointClo
   return (0);
 }
 
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int
+pcl::PCDWriter::writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud,
+                             const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation)
+{
+  if (cloud.data.empty ())
+  {
+    PCL_WARN ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!\n");
+  }
+  if (cloud.fields.empty())
+  {
+    PCL_ERROR ("[pcl::PCDWriter::writeBinary] Input point cloud has no field data!\n");
+    return (-1);
+  }
+
+  os.imbue (std::locale::classic ());
+  os << generateHeaderBinary (cloud, origin, orientation) << "DATA binary\n";
+  std::copy (cloud.data.cbegin(), cloud.data.cend(), std::ostream_iterator<char> (os));
+  os.flush ();
+
+  return (os ? 0 : -1);
+}
+
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 int
 pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud,

test/io/test_io.cpp

@@ -1379,6 +1379,74 @@ TEST (PCL, LZFExtended)
   remove ("test_pcl_io_compressed.pcd");
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, WriteBinaryToOStream)
+{
+  PointCloud<PointXYZRGBNormal> cloud;
+  cloud.width  = 640;
+  cloud.height = 480;
+  cloud.resize (cloud.width * cloud.height);
+  cloud.is_dense = true;
+
+  srand (static_cast<unsigned int> (time (nullptr)));
+  const auto nr_p = cloud.size ();
+  // Randomly create a new point cloud
+  for (std::size_t i = 0; i < nr_p; ++i)
+  {
+    cloud[i].x = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+    cloud[i].y = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+    cloud[i].z = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+    cloud[i].normal_x = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+    cloud[i].normal_y = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+    cloud[i].normal_z = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+    cloud[i].rgb = static_cast<float> (1024 * rand () / (RAND_MAX + 1.0));
+  }
+
+  pcl::PCLPointCloud2 blob;
+  pcl::toPCLPointCloud2 (cloud, blob);
+
+  std::ostringstream oss;
+  PCDWriter writer;
+  int res = writer.writeBinary (oss, blob);
+  EXPECT_EQ (res, 0);
+  std::string pcd_str = oss.str ();
+
+  Eigen::Vector4f origin;
+  Eigen::Quaternionf orientation;
+  int pcd_version = -1;
+  int data_type = -1;
+  unsigned int data_idx = 0;
+  std::istringstream iss (pcd_str, std::ios::binary);
+  PCDReader reader;
+  pcl::PCLPointCloud2 blob2;
+  res = reader.readHeader (iss, blob2, origin, orientation, pcd_version, data_type, data_idx);
+  EXPECT_EQ (res, 0);
+  EXPECT_EQ (blob2.width, blob.width);
+  EXPECT_EQ (blob2.height, blob.height);
+  EXPECT_EQ (data_type, 1); // since it was written by writeBinary (), it should be uncompressed.
+
+  const auto *data = reinterpret_cast<const unsigned char *> (pcd_str.data ());
+  res = reader.readBodyBinary (data, blob2, pcd_version, data_type == 2, data_idx);
+  PointCloud<PointXYZRGBNormal> cloud2;
+  pcl::fromPCLPointCloud2 (blob2, cloud2);
+  EXPECT_EQ (res, 0);
+  EXPECT_EQ (cloud2.width, blob.width);
+  EXPECT_EQ (cloud2.height, blob.height);
+  EXPECT_EQ (cloud2.is_dense, cloud.is_dense);
+  EXPECT_EQ (cloud2.size (), cloud.size ());
+
+  for (std::size_t i = 0; i < cloud2.size (); ++i)
+  {
+    EXPECT_EQ (cloud2[i].x, cloud[i].x);
+    EXPECT_EQ (cloud2[i].y, cloud[i].y);
+    EXPECT_EQ (cloud2[i].z, cloud[i].z);
+    EXPECT_EQ (cloud2[i].normal_x, cloud[i].normal_x);
+    EXPECT_EQ (cloud2[i].normal_y, cloud[i].normal_y);
+    EXPECT_EQ (cloud2[i].normal_z, cloud[i].normal_z);
+    EXPECT_EQ (cloud2[i].rgb, cloud[i].rgb);
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, LZFInMem)
 {