[feat]: implement benchmark for downsampling filters

benchmark/CMakeLists.txt

@@ -5,6 +5,7 @@ list(APPEND BENCHMARK_FILES
     concurrent/parallel_benc.cpp
     types/types_benc.cpp
     file/memory_mapped_file_benc.cpp
+    pcl/downsampling_benc.cpp
 )
 
 

benchmark/pcl/downsampling_benc.cpp

@@ -0,0 +1,95 @@
+#include <cmath>
+#include <vector>
+
+#include <catch2/benchmark/catch_benchmark.hpp>
+#include <catch2/catch_test_macros.hpp>
+
+#include <cpp-toolbox/pcl/filters/random_downsampling.hpp>
+#include <cpp-toolbox/pcl/filters/voxel_grid_downsampling.hpp>
+#include <cpp-toolbox/types/point.hpp>
+#include <cpp-toolbox/utils/random.hpp>
+
+using toolbox::pcl::random_downsampling_t;
+using toolbox::pcl::voxel_grid_downsampling_t;
+using toolbox::types::point_cloud_t;
+
+TEST_CASE("Downsampling Filters Benchmark", "[benchmark][pcl]")
+{
+  constexpr std::size_t point_count = 200000;
+  point_cloud_t<float> cloud;
+  cloud.points.reserve(point_count);
+  auto& rng = toolbox::utils::random_t::instance();
+  rng.seed(12345);
+  for (std::size_t i = 0; i < point_count; ++i) {
+    cloud.points.emplace_back(rng.random_float<float>(-1000.0f, 1000.0f),
+                              rng.random_float<float>(-1000.0f, 1000.0f),
+                              rng.random_float<float>(-1000.0f, 1000.0f));
+  }
+
+  SECTION("Correctness Random Downsampling")
+  {
+    random_downsampling_t<float> filter(0.3F);
+    filter.set_input(cloud);
+    rng.seed(42);
+    auto serial_result = filter.filter();
+    rng.seed(42);
+    filter.enable_parrallel(true);
+    auto parallel_result = filter.filter();
+    REQUIRE(parallel_result.points == serial_result.points);
+  }
+
+  SECTION("Correctness Voxel Grid Downsampling")
+  {
+    voxel_grid_downsampling_t<float> filter(0.5F);
+    filter.set_input(cloud);
+    auto serial_result = filter.filter();
+    filter.enable_parrallel(true);
+    auto parallel_result = filter.filter();
+    REQUIRE(parallel_result.size() == serial_result.size());
+    for (const auto& p : serial_result.points) {
+      auto it = std::find_if(parallel_result.points.begin(),
+                             parallel_result.points.end(),
+                             [&](const auto& q)
+                             {
+                               return std::fabs(p.x - q.x) < 1e-6f
+                                   && std::fabs(p.y - q.y) < 1e-6f
+                                   && std::fabs(p.z - q.z) < 1e-6f;
+                             });
+      REQUIRE(it != parallel_result.points.end());
+    }
+  }
+
+  SECTION("Benchmark Random Downsampling")
+  {
+    random_downsampling_t<float> filter(0.3F);
+    filter.set_input(cloud);
+    BENCHMARK("Serial Random Downsampling")
+    {
+      rng.seed(7);
+      filter.enable_parrallel(false);
+      return filter.filter().size();
+    };
+    BENCHMARK("Parallel Random Downsampling")
+    {
+      rng.seed(7);
+      filter.enable_parrallel(true);
+      return filter.filter().size();
+    };
+  }
+
+  SECTION("Benchmark Voxel Grid Downsampling")
+  {
+    voxel_grid_downsampling_t<float> filter(0.5F);
+    filter.set_input(cloud);
+    BENCHMARK("Serial Voxel Grid Downsampling")
+    {
+      filter.enable_parrallel(false);
+      return filter.filter().size();
+    };
+    BENCHMARK("Parallel Voxel Grid Downsampling")
+    {
+      filter.enable_parrallel(true);
+      return filter.filter().size();
+    };
+  }
+}

src/include/cpp-toolbox/pcl/filters/filters.hpp

@@ -19,27 +19,27 @@ class CPP_TOOLBOX_EXPORT filter_t
 
   std::size_t set_input(const point_cloud& cloud)
   {
-    return static_cast<const Derived*>(this)->set_input_impl(cloud);
+    return static_cast<Derived*>(this)->set_input_impl(cloud);
   }
 
   std::size_t set_input(const point_cloud_ptr& cloud)
   {
-    return static_cast<const Derived*>(this)->set_input_impl(cloud);
+    return static_cast<Derived*>(this)->set_input_impl(cloud);
   }
 
   void enable_parrallel(bool enable)
   {
-    return static_cast<const Derived*>(this)->enable_parallel_impl(enable);
+    return static_cast<Derived*>(this)->enable_parallel_impl(enable);
   }
 
   point_cloud filter()
   {
-    return static_cast<const Derived*>(this)->filter_impl();
+    return static_cast<Derived*>(this)->filter_impl();
   }
 
   void filter(point_cloud_ptr output)
   {
-    return static_cast<const Derived*>(this)->filter_impl(output);
+    return static_cast<Derived*>(this)->filter_impl(output);
   }
 
 protected:

src/include/cpp-toolbox/pcl/filters/impl/random_downsampling_impl.hpp

@@ -0,0 +1,110 @@
+#pragma once
+
+#include <algorithm>
+#include <numeric>
+#include <vector>
+
+#include <cpp-toolbox/concurrent/parallel.hpp>
+
+namespace toolbox::pcl
+{
+
+template<typename DataType>
+std::size_t random_downsampling_t<DataType>::set_input_impl(
+    const point_cloud& cloud)
+{
+  m_cloud = std::make_shared<point_cloud>(cloud);
+  return m_cloud->size();
+}
+
+template<typename DataType>
+std::size_t random_downsampling_t<DataType>::set_input_impl(
+    const point_cloud_ptr& cloud)
+{
+  m_cloud = cloud;
+  return m_cloud ? m_cloud->size() : 0U;
+}
+
+template<typename DataType>
+void random_downsampling_t<DataType>::enable_parallel_impl(bool enable)
+{
+  m_enable_parallel = enable;
+}
+
+template<typename DataType>
+typename random_downsampling_t<DataType>::point_cloud
+random_downsampling_t<DataType>::filter_impl()
+{
+  auto output = std::make_shared<point_cloud>();
+  filter_impl(output);
+  return *output;
+}
+
+template<typename DataType>
+void random_downsampling_t<DataType>::filter_impl(point_cloud_ptr output)
+{
+  if (!output)
+    return;
+  if (!m_cloud || m_cloud->empty()) {
+    output->clear();
+    return;
+  }
+
+  const std::size_t input_size = m_cloud->size();
+  std::size_t sample_count =
+      static_cast<std::size_t>(std::floor(input_size * m_ration));
+  sample_count = std::min(sample_count, input_size);
+  if (sample_count == 0) {
+    output->clear();
+    return;
+  }
+
+  std::vector<std::size_t> indices(input_size);
+  std::iota(indices.begin(), indices.end(), 0);
+  toolbox::utils::random_t::instance().shuffle(indices);
+  indices.resize(sample_count);
+
+  output->points.resize(sample_count);
+  if (!m_cloud->normals.empty()) {
+    output->normals.resize(sample_count);
+  }
+  if (!m_cloud->colors.empty()) {
+    output->colors.resize(sample_count);
+  }
+  output->intensity = m_cloud->intensity;
+
+  if (m_enable_parallel && sample_count > 1024) {
+    toolbox::concurrent::parallel_transform(indices.cbegin(),
+                                            indices.cend(),
+                                            output->points.begin(),
+                                            [this](std::size_t idx)
+                                            { return m_cloud->points[idx]; });
+    if (!m_cloud->normals.empty()) {
+      toolbox::concurrent::parallel_transform(
+          indices.cbegin(),
+          indices.cend(),
+          output->normals.begin(),
+          [this](std::size_t idx) { return m_cloud->normals[idx]; });
+    }
+    if (!m_cloud->colors.empty()) {
+      toolbox::concurrent::parallel_transform(indices.cbegin(),
+                                              indices.cend(),
+                                              output->colors.begin(),
+                                              [this](std::size_t idx)
+                                              { return m_cloud->colors[idx]; });
+    }
+  } else {
+    for (std::size_t i = 0; i < sample_count; ++i) {
+      std::size_t idx = indices[i];
+      output->points[i] = m_cloud->points[idx];
+      if (!m_cloud->normals.empty()) {
+        output->normals[i] = m_cloud->normals[idx];
+      }
+      if (!m_cloud->colors.empty()) {
+        output->colors[i] = m_cloud->colors[idx];
+      }
+    }
+  }
+}
+
+}  // namespace toolbox::pcl

src/include/cpp-toolbox/pcl/filters/impl/voxel_grid_downsampling_impl.hpp

@@ -0,0 +1,135 @@
+#pragma once
+
+#include <cmath>
+#include <tuple>
+#include <unordered_map>
+#include <vector>
+
+#include <cpp-toolbox/concurrent/parallel.hpp>
+
+namespace toolbox::pcl
+{
+
+template<typename DataType>
+std::size_t voxel_grid_downsampling_t<DataType>::set_input_impl(
+    const point_cloud& cloud)
+{
+  m_cloud = std::make_shared<point_cloud>(cloud);
+  return m_cloud->size();
+}
+
+template<typename DataType>
+std::size_t voxel_grid_downsampling_t<DataType>::set_input_impl(
+    const point_cloud_ptr& cloud)
+{
+  m_cloud = cloud;
+  return m_cloud ? m_cloud->size() : 0U;
+}
+
+template<typename DataType>
+void voxel_grid_downsampling_t<DataType>::enable_parallel_impl(bool enable)
+{
+  m_enable_parallel = enable;
+}
+
+template<typename DataType>
+typename voxel_grid_downsampling_t<DataType>::point_cloud
+voxel_grid_downsampling_t<DataType>::filter_impl()
+{
+  auto output = std::make_shared<point_cloud>();
+  filter_impl(output);
+  return *output;
+}
+
+template<typename DataType>
+void voxel_grid_downsampling_t<DataType>::filter_impl(point_cloud_ptr output)
+{
+  if (!output)
+    return;
+  if (!m_cloud || m_cloud->empty()) {
+    output->clear();
+    return;
+  }
+
+  struct voxel_data_t
+  {
+    toolbox::types::point_t<DataType> sum_point {0, 0, 0};
+    toolbox::types::point_t<DataType> sum_normal {0, 0, 0};
+    toolbox::types::point_t<DataType> sum_color {0, 0, 0};
+    std::size_t count {0};
+  };
+
+  using key_t = std::tuple<int, int, int>;
+  struct key_hash
+  {
+    std::size_t operator()(const key_t& k) const noexcept
+    {
+      std::size_t h1 = std::hash<int> {}(std::get<0>(k));
+      std::size_t h2 = std::hash<int> {}(std::get<1>(k));
+      std::size_t h3 = std::hash<int> {}(std::get<2>(k));
+      return h1 ^ (h2 << 1) ^ (h3 << 2);
+    }
+  };
+
+  std::unordered_map<key_t, voxel_data_t, key_hash> voxel_map;
+  std::mutex map_mutex;
+  auto process_point = [&](std::size_t idx)
+  {
+    const auto& pt = m_cloud->points[idx];
+    int ix = static_cast<int>(std::floor(pt.x / m_voxel_size));
+    int iy = static_cast<int>(std::floor(pt.y / m_voxel_size));
+    int iz = static_cast<int>(std::floor(pt.z / m_voxel_size));
+    key_t key(ix, iy, iz);
+    std::lock_guard<std::mutex> lock(map_mutex);
+    auto& v = voxel_map[key];
+    v.sum_point += pt;
+    if (!m_cloud->normals.empty()) {
+      v.sum_normal += m_cloud->normals[idx];
+    }
+    if (!m_cloud->colors.empty()) {
+      v.sum_color += m_cloud->colors[idx];
+    }
+    ++v.count;
+  };
+
+  const std::size_t total = m_cloud->size();
+  if (m_enable_parallel && total > 1024) {
+    std::vector<std::size_t> indices(total);
+    std::iota(indices.begin(), indices.end(), 0);
+    toolbox::concurrent::parallel_for_each(
+        indices.begin(), indices.end(), process_point);
+  } else {
+    for (std::size_t i = 0; i < total; ++i) {
+      process_point(i);
+    }
+  }
+
+  const bool has_normals = !m_cloud->normals.empty();
+  const bool has_colors = !m_cloud->colors.empty();
+  output->points.reserve(voxel_map.size());
+  if (has_normals) {
+    output->normals.reserve(voxel_map.size());
+  }
+  if (has_colors) {
+    output->colors.reserve(voxel_map.size());
+  }
+  output->intensity = m_cloud->intensity;
+
+  for (auto& [key, v] : voxel_map) {
+    auto centroid = v.sum_point;
+    centroid /= static_cast<DataType>(v.count);
+    output->points.push_back(centroid);
+    if (has_normals) {
+      auto n = v.sum_normal;
+      n /= static_cast<DataType>(v.count);
+      output->normals.push_back(n);
+    }
+    if (has_colors) {
+      auto c = v.sum_color;
+      c /= static_cast<DataType>(v.count);
+      output->colors.push_back(c);
+    }
+  }
+}
+
+}  // namespace toolbox::pcl