[filters] VoxelGrid<PCLPointCloud2>: average each field by its datatype

common/include/pcl/common/impl/io.hpp

@@ -45,6 +45,11 @@
 #include <pcl/common/copy_point.h>
 #include <pcl/common/io.h>
 
+#include <cmath> // for std::round
+#include <cstring> // for memcpy
+#include <limits>
+#include <type_traits>
+
 namespace pcl
 {
 
@@ -138,7 +143,11 @@ getFieldsList (const pcl::PointCloud<PointT> &)
  */
 template <typename DType, typename RType> RType reinterpret_and_cast (const std::uint8_t* p)
 {
-  return static_cast<RType>(*reinterpret_cast<const DType*>(p));
+  // memcpy instead of dereferencing p as a DType*: the fields of a PCLPointCloud2 need
+  // not be aligned (e.g. a double right after a 1 byte field)
+  DType value;
+  memcpy (&value, p, sizeof (DType));
+  return static_cast<RType>(value);
 }
 
 template <typename T> T point_field_as (const std::uint8_t* data, const std::uint8_t type)
@@ -147,6 +156,9 @@ template <typename T> T point_field_as (const std::uint8_t* data, const std::uin
   case pcl::PCLPointField::PointFieldTypes::FLOAT32:
     return reinterpret_and_cast<float, T>(data);
     break;
+  case pcl::PCLPointField::PointFieldTypes::INT8:
+    return reinterpret_and_cast<std::int8_t, T>(data);
+    break;
   case pcl::PCLPointField::PointFieldTypes::UINT8:
     return reinterpret_and_cast<std::uint8_t, T>(data);
     break;
@@ -180,6 +192,68 @@ template <typename T> T point_field_as (const std::uint8_t* data, const std::uin
   }
 }
 
+/**
+ * \brief convert a value to a type and store it at a pointer. Integer types are rounded
+ * to the nearest value and saturated to the range of the type (64 bit integers are not
+ * exact in double).
+ *
+ * \tparam DType Type the value is stored as
+ */
+template <typename DType> void storeFieldValueAs (const double value, std::uint8_t* p)
+{
+  DType result;
+  if constexpr (std::is_floating_point_v<DType>)
+    result = static_cast<DType>(value);
+  else if (!(value > static_cast<double>(std::numeric_limits<DType>::lowest())))
+    result = std::numeric_limits<DType>::lowest();
+  else if (value >= static_cast<double>(std::numeric_limits<DType>::max()))
+    result = std::numeric_limits<DType>::max();
+  else
+    result = static_cast<DType>(std::round(value));
+  memcpy (p, &result, sizeof (DType));
+}
+
+inline void storeFieldValue (const double value, std::uint8_t* data, const std::uint8_t type)
+{
+  switch (type) {
+  case pcl::PCLPointField::PointFieldTypes::FLOAT32:
+    storeFieldValueAs<float>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::INT8:
+    storeFieldValueAs<std::int8_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::UINT8:
+    storeFieldValueAs<std::uint8_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::UINT16:
+    storeFieldValueAs<std::uint16_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::UINT32:
+    storeFieldValueAs<std::uint32_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::UINT64:
+    storeFieldValueAs<std::uint64_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::BOOL:
+    storeFieldValueAs<bool>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::FLOAT64:
+    storeFieldValueAs<double>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::INT16:
+    storeFieldValueAs<std::int16_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::INT32:
+    storeFieldValueAs<std::int32_t>(value, data);
+    break;
+  case pcl::PCLPointField::PointFieldTypes::INT64:
+    storeFieldValueAs<std::int64_t>(value, data);
+    break;
+  default:
+    break;
+  }
+}
+
 
 namespace detail
 {

common/include/pcl/common/io.h

@@ -273,6 +273,18 @@ namespace pcl
    */
   template <typename T> T point_field_as (const std::uint8_t* data, const std::uint8_t type);
 
+  /**
+   * \brief   Set the value of a point field from raw data pointer and field type.
+   *
+   * Integer field types are rounded to the nearest value and saturated to their range.
+   *
+   * \param value value to store
+   * \param data  data pointer
+   * \param type  point field type (see pcl::PCLPointField::PointFieldTypes)
+   * \ingroup common
+   */
+  inline void storeFieldValue (const double value, std::uint8_t* data, const std::uint8_t type);
+
 
   enum InterpolationType
   {

filters/src/voxel_grid.cpp

@@ -38,6 +38,7 @@
  *
  */
 
+#include <cmath>
 #include <limits>
 #include <pcl/common/io.h>
 #include <pcl/filters/impl/voxel_grid.hpp>
@@ -138,10 +139,11 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
     centroid_size = static_cast<int> (input_->fields.size ());
 
     // ---[ RGB special case
-    // if the data contains "rgba" or "rgb", add an extra field for r/g/b in centroid
+    // if the data contains a 4 byte "rgba" or "rgb", add an extra field for r/g/b in centroid
     for (int d = 0; d < centroid_size; ++d)
     {
-      if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
+      if ((input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb")) &&
+          field_sizes_[d] == sizeof (pcl::RGB))
       {
         rgba_index = d;
         centroid_size += 4;
@@ -312,7 +314,7 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
     xyz_offset = Array4size_t (0, 4, 8, 12);
 
   index=0;
-  Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
+  Eigen::VectorXd temporary = Eigen::VectorXd::Zero (centroid_size);
   const std::array<index_t, 3> field_indices {x_idx_, y_idx_, z_idx_};
 
   for (const auto &cp : first_and_last_indices_vector)
@@ -350,7 +352,7 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
     else
     {
 
-      Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+      Eigen::VectorXd centroid = Eigen::VectorXd::Zero (centroid_size);
       // fill in the accumulator with leaf points
       for (index_t li = first_index; li < last_index; ++li) {
         std::size_t point_offset = index_vector[li].cloud_point_index * input_->point_step;
@@ -365,24 +367,28 @@ pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
             temporary[centroid_size-2] = rgb.b;
             temporary[centroid_size-1] = rgb.a;
         }
-        // Copy all the fields
+        // Convert all the fields
         for (std::size_t d = 0; d < input_->fields.size (); ++d)
-          memcpy (&temporary[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
+          temporary[d] = pcl::point_field_as<double> (&input_->data[point_offset + input_->fields[d].offset], input_->fields[d].datatype);
         centroid += temporary;
       }
-      centroid /= static_cast<float> (last_index - first_index);
+      centroid /= static_cast<double> (last_index - first_index);
 
       std::size_t point_offset = index * output.point_step;
-      // Copy all the fields
+      // Convert all the fields back
       for (std::size_t d = 0; d < output.fields.size (); ++d)
-        memcpy (&output.data[point_offset + output.fields[d].offset], &centroid[d], field_sizes_[d]);
+        pcl::storeFieldValue (centroid[d], &output.data[point_offset + output.fields[d].offset], output.fields[d].datatype);
 
       // ---[ RGB special case
       // full extra r/g/b centroid field
       if (rgba_index >= 0)
       {
-        float r = centroid[centroid_size-4], g = centroid[centroid_size-3], b = centroid[centroid_size-2], a = centroid[centroid_size-1];
-        int rgb = (static_cast<int> (a) << 24) | (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
+        // round each averaged channel to the nearest value (e.g. 255, 255 and 254 give 255)
+        const auto r = static_cast<std::uint32_t> (std::round (centroid[centroid_size-4]));
+        const auto g = static_cast<std::uint32_t> (std::round (centroid[centroid_size-3]));
+        const auto b = static_cast<std::uint32_t> (std::round (centroid[centroid_size-2]));
+        const auto a = static_cast<std::uint32_t> (std::round (centroid[centroid_size-1]));
+        std::uint32_t rgb = (a << 24) | (r << 16) | (g << 8) | b;
         memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float));
       }
     }

test/common/test_io.cpp

@@ -40,6 +40,10 @@
 #include <pcl/point_types.h>
 #include <pcl/common/io.h>
 
+#include <array>
+#include <cstring>
+#include <limits>
+
 using namespace pcl;
 
 using CloudXYZRGBA = PointCloud<PointXYZRGBA>;
@@ -372,6 +376,51 @@ TEST(PCL, fromPCLPointCloud2CastingSHOT352)
   }
 }
 
+TEST(PCL, pointFieldAs)
+{
+  // the fields of a PCLPointCloud2 need not be aligned
+  std::array<std::uint8_t, 1 + sizeof(double)> data{};
+  const double timestamp = 1700000000.25;
+  memcpy(&data[1], &timestamp, sizeof(timestamp));
+  EXPECT_EQ(pcl::point_field_as<double>(&data[1], pcl::PCLPointField::FLOAT64), timestamp);
+  const std::int8_t tag = -5;
+  memcpy(&data[1], &tag, sizeof(tag));
+  EXPECT_EQ(pcl::point_field_as<double>(&data[1], pcl::PCLPointField::INT8), -5.0);
+  EXPECT_EQ(pcl::point_field_as<int>(&data[1], pcl::PCLPointField::INT8), -5);
+}
+
+TEST(PCL, storeFieldValue)
+{
+  std::array<std::uint8_t, 2 + sizeof(double)> data{};
+  const auto store_and_load = [&data](double value, std::uint8_t type) {
+    pcl::storeFieldValue(value, &data[1], type);
+    return pcl::point_field_as<double>(&data[1], type);
+  };
+  // floating point types keep the value, as far as their precision allows
+  EXPECT_EQ(store_and_load(1700000000.25, pcl::PCLPointField::FLOAT64), 1700000000.25);
+  EXPECT_EQ(store_and_load(0.1, pcl::PCLPointField::FLOAT32), static_cast<double>(0.1f));
+  // integer types are rounded to the nearest value
+  EXPECT_EQ(store_and_load(2.4, pcl::PCLPointField::INT32), 2.0);
+  EXPECT_EQ(store_and_load(-4.5, pcl::PCLPointField::INT8), -5.0);
+  EXPECT_EQ(store_and_load(59999.5, pcl::PCLPointField::UINT16), 60000.0);
+  EXPECT_EQ(store_and_load(0.75, pcl::PCLPointField::BOOL), 1.0);
+  EXPECT_EQ(store_and_load(0.25, pcl::PCLPointField::BOOL), 0.0);
+  // and saturated to their range
+  EXPECT_EQ(store_and_load(300.0, pcl::PCLPointField::UINT8), 255.0);
+  EXPECT_EQ(store_and_load(-1.0, pcl::PCLPointField::UINT32), 0.0);
+  EXPECT_EQ(store_and_load(-200.0, pcl::PCLPointField::INT8), -128.0);
+  EXPECT_EQ(store_and_load(-1e30, pcl::PCLPointField::INT16), -32768.0);
+  EXPECT_EQ(store_and_load(1e30, pcl::PCLPointField::INT64),
+            static_cast<double>(std::numeric_limits<std::int64_t>::max()));
+  EXPECT_EQ(store_and_load(1e30, pcl::PCLPointField::UINT64),
+            static_cast<double>(std::numeric_limits<std::uint64_t>::max()));
+  // only the bytes of the field are written
+  data.fill(0xAA);
+  pcl::storeFieldValue(1.0, &data[1], pcl::PCLPointField::UINT16);
+  EXPECT_EQ(data[0], 0xAA);
+  EXPECT_EQ(data[3], 0xAA);
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)

test/filters/test_filters.cpp

@@ -1211,6 +1211,117 @@ TEST (VoxelGrid_RGBA, Filters)
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(VoxelGrid, DownsampleAllDataFieldTypes)
+{
+  // x/y/z, signed and unsigned integers and a double (e.g. a timestamp) as the last field
+  auto input = pcl::make_shared<PCLPointCloud2>();
+  input->fields = {{"x", 0, PCLPointField::FLOAT32, 1},
+                   {"y", 4, PCLPointField::FLOAT32, 1},
+                   {"z", 8, PCLPointField::FLOAT32, 1},
+                   {"tag", 12, PCLPointField::INT8, 1},
+                   {"ring", 14, PCLPointField::UINT16, 1},
+                   {"timestamp", 16, PCLPointField::FLOAT64, 1}};
+  input->point_step = 24;
+  input->width = 3;
+  input->height = 1;
+  input->row_step = input->point_step * input->width;
+  input->data.resize(input->row_step);
+
+  const auto set_point = [&input](pcl::uindex_t i, float xyz, std::int8_t tag, std::uint16_t ring, double timestamp) {
+    input->at<float>(i, 0) = input->at<float>(i, 4) = input->at<float>(i, 8) = xyz;
+    input->at<std::int8_t>(i, 12) = tag;
+    input->at<std::uint16_t>(i, 14) = ring;
+    input->at<double>(i, 16) = timestamp;
+  };
+  set_point(0, 0.25f, -3, 3, 1700000000.25);
+  set_point(1, 0.75f, -5, 5, 1700000000.75);
+  set_point(2, 1.5f, 100, 60000, -2.5);
+
+  VoxelGrid<PCLPointCloud2> grid;
+  grid.setInputCloud(input);
+  grid.setLeafSize(1.0f, 1.0f, 1.0f);
+  PCLPointCloud2 output;
+  grid.filter(output);
+
+  ASSERT_EQ(output.width * output.height, 2);
+  ASSERT_EQ(output.point_step, input->point_step);
+  ASSERT_EQ(output.fields.size(), input->fields.size());
+
+  EXPECT_FLOAT_EQ(output.at<float>(0, 0), 0.5f);
+  EXPECT_FLOAT_EQ(output.at<float>(0, 8), 0.5f);
+  EXPECT_EQ(output.at<std::int8_t>(0, 12), -4);
+  EXPECT_EQ(output.at<std::uint16_t>(0, 14), 4);
+  EXPECT_DOUBLE_EQ(output.at<double>(0, 16), 1700000000.5);
+
+  EXPECT_FLOAT_EQ(output.at<float>(1, 0), 1.5f);
+  EXPECT_EQ(output.at<std::int8_t>(1, 12), 100);
+  EXPECT_EQ(output.at<std::uint16_t>(1, 14), 60000);
+  EXPECT_DOUBLE_EQ(output.at<double>(1, 16), -2.5);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(VoxelGrid, DownsampleAllDataSmallRGBField)
+{
+  // A 1 byte "rgb" is not a packed color: it is averaged like any other field
+  auto input = pcl::make_shared<PCLPointCloud2>();
+  input->fields = {{"x", 0, PCLPointField::FLOAT32, 1},
+                   {"y", 4, PCLPointField::FLOAT32, 1},
+                   {"z", 8, PCLPointField::FLOAT32, 1},
+                   {"rgb", 12, PCLPointField::UINT8, 1},
+                   {"ring", 14, PCLPointField::UINT16, 1}};
+  input->point_step = 16;
+  input->width = 2;
+  input->height = 1;
+  input->row_step = input->point_step * input->width;
+  input->data.resize(input->row_step); // both points at the origin
+  input->at<std::uint8_t>(0, 12) = 10;
+  input->at<std::uint8_t>(1, 12) = 30;
+  input->at<std::uint16_t>(0, 14) = 255;
+  input->at<std::uint16_t>(1, 14) = 257;
+
+  VoxelGrid<PCLPointCloud2> grid;
+  grid.setInputCloud(input);
+  grid.setLeafSize(1.0f, 1.0f, 1.0f);
+  PCLPointCloud2 output;
+  grid.filter(output);
+
+  ASSERT_EQ(output.width * output.height, 1);
+  EXPECT_EQ(output.at<std::uint8_t>(0, 12), 20);
+  EXPECT_EQ(output.at<std::uint16_t>(0, 14), 256);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(VoxelGrid, DownsampleAllDataRGBARounded)
+{
+  // Each averaged color channel is rounded to the nearest value
+  const auto pack = [](std::uint32_t a, std::uint32_t r, std::uint32_t g, std::uint32_t b) {
+    return (a << 24) | (r << 16) | (g << 8) | b;
+  };
+  auto input = pcl::make_shared<PCLPointCloud2>();
+  input->fields = {{"x", 0, PCLPointField::FLOAT32, 1},
+                   {"y", 4, PCLPointField::FLOAT32, 1},
+                   {"z", 8, PCLPointField::FLOAT32, 1},
+                   {"rgba", 12, PCLPointField::UINT32, 1}};
+  input->point_step = 16;
+  input->width = 3;
+  input->height = 1;
+  input->row_step = input->point_step * input->width;
+  input->data.resize(input->row_step); // all points at the origin
+  input->at<std::uint32_t>(0, 12) = pack(255, 255, 0, 1);
+  input->at<std::uint32_t>(1, 12) = pack(255, 255, 0, 2);
+  input->at<std::uint32_t>(2, 12) = pack(254, 254, 1, 2);
+
+  VoxelGrid<PCLPointCloud2> grid;
+  grid.setInputCloud(input);
+  grid.setLeafSize(1.0f, 1.0f, 1.0f);
+  PCLPointCloud2 output;
+  grid.filter(output);
+
+  ASSERT_EQ(output.width * output.height, 1);
+  EXPECT_EQ(output.at<std::uint32_t>(0, 12), pack(255, 255, 0, 2));
+}
+
 #if 0
 ////////////////////////////////////////////////////////////////////////////////
 float getRandomNumber (float max = 1.0, float min = 0.0)