Skip to content

Commit c972683

Browse files
nerdneilsfieldnerdneilsfield
committed
[feat]: add downsampling benchmarks and parallel tests
1 parent d12ec4a commit c972683

File tree

6 files changed

+440
-5
lines changed

6 files changed

+440
-5
lines changed

‎benchmark/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ list(APPEND BENCHMARK_FILES
55
concurrent/parallel_benc.cpp
66
types/types_benc.cpp
77
file/memory_mapped_file_benc.cpp
8+
pcl/downsampling_benc.cpp
89
)
910

1011

‎benchmark/pcl/downsampling_benc.cpp‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
#include <cmath>
2+
#include <vector>
3+
4+
#include <catch2/benchmark/catch_benchmark.hpp>
5+
#include <catch2/catch_test_macros.hpp>
6+
7+
#include <cpp-toolbox/pcl/filters/random_downsampling.hpp>
8+
#include <cpp-toolbox/pcl/filters/voxel_grid_downsampling.hpp>
9+
#include <cpp-toolbox/types/point.hpp>
10+
#include <cpp-toolbox/utils/random.hpp>
11+
12+
using toolbox::pcl::random_downsampling_t;
13+
using toolbox::pcl::voxel_grid_downsampling_t;
14+
using toolbox::types::point_cloud_t;
15+
16+
TEST_CASE("Downsampling Filters Benchmark", "[benchmark][pcl]")
17+
{
18+
constexpr std::size_t point_count = 200000;
19+
point_cloud_t<float> cloud;
20+
cloud.points.reserve(point_count);
21+
auto& rng = toolbox::utils::random_t::instance();
22+
rng.seed(12345);
23+
for (std::size_t i = 0; i < point_count; ++i) {
24+
cloud.points.emplace_back(rng.random_float<float>(-1000.0f, 1000.0f),
25+
rng.random_float<float>(-1000.0f, 1000.0f),
26+
rng.random_float<float>(-1000.0f, 1000.0f));
27+
}
28+
29+
SECTION("Correctness Random Downsampling")
30+
{
31+
random_downsampling_t<float> filter(0.3F);
32+
filter.set_input(cloud);
33+
rng.seed(42);
34+
auto serial_result = filter.filter();
35+
rng.seed(42);
36+
filter.enable_parrallel(true);
37+
auto parallel_result = filter.filter();
38+
REQUIRE(parallel_result.points == serial_result.points);
39+
}
40+
41+
SECTION("Correctness Voxel Grid Downsampling")
42+
{
43+
voxel_grid_downsampling_t<float> filter(0.5F);
44+
filter.set_input(cloud);
45+
auto serial_result = filter.filter();
46+
filter.enable_parrallel(true);
47+
auto parallel_result = filter.filter();
48+
REQUIRE(parallel_result.size() == serial_result.size());
49+
for (const auto& p : serial_result.points) {
50+
auto it = std::find_if(parallel_result.points.begin(),
51+
parallel_result.points.end(),
52+
[&](const auto& q)
53+
{
54+
return std::fabs(p.x - q.x) < 1e-6f
55+
&& std::fabs(p.y - q.y) < 1e-6f
56+
&& std::fabs(p.z - q.z) < 1e-6f;
57+
});
58+
REQUIRE(it != parallel_result.points.end());
59+
}
60+
}
61+
62+
SECTION("Benchmark Random Downsampling")
63+
{
64+
random_downsampling_t<float> filter(0.3F);
65+
filter.set_input(cloud);
66+
BENCHMARK("Serial Random Downsampling")
67+
{
68+
rng.seed(7);
69+
filter.enable_parrallel(false);
70+
return filter.filter().size();
71+
};
72+
BENCHMARK("Parallel Random Downsampling")
73+
{
74+
rng.seed(7);
75+
filter.enable_parrallel(true);
76+
return filter.filter().size();
77+
};
78+
}
79+
80+
SECTION("Benchmark Voxel Grid Downsampling")
81+
{
82+
voxel_grid_downsampling_t<float> filter(0.5F);
83+
filter.set_input(cloud);
84+
BENCHMARK("Serial Voxel Grid Downsampling")
85+
{
86+
filter.enable_parrallel(false);
87+
return filter.filter().size();
88+
};
89+
BENCHMARK("Parallel Voxel Grid Downsampling")
90+
{
91+
filter.enable_parrallel(true);
92+
return filter.filter().size();
93+
};
94+
}
95+
}

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -19,27 +19,27 @@ class CPP_TOOLBOX_EXPORT filter_t
1919

2020
std::size_t set_input(const point_cloud& cloud)
2121
{
22-
return static_cast<const Derived*>(this)->set_input_impl(cloud);
22+
return static_cast<Derived*>(this)->set_input_impl(cloud);
2323
}
2424

2525
std::size_t set_input(const point_cloud_ptr& cloud)
2626
{
27-
return static_cast<const Derived*>(this)->set_input_impl(cloud);
27+
return static_cast<Derived*>(this)->set_input_impl(cloud);
2828
}
2929

3030
void enable_parrallel(bool enable)
3131
{
32-
return static_cast<const Derived*>(this)->enable_parallel_impl(enable);
32+
return static_cast<Derived*>(this)->enable_parallel_impl(enable);
3333
}
3434

3535
point_cloud filter()
3636
{
37-
return static_cast<const Derived*>(this)->filter_impl();
37+
return static_cast<Derived*>(this)->filter_impl();
3838
}
3939

4040
void filter(point_cloud_ptr output)
4141
{
42-
return static_cast<const Derived*>(this)->filter_impl(output);
42+
return static_cast<Derived*>(this)->filter_impl(output);
4343
}
4444

4545
protected:

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

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#pragma once
2+
3+
#include <algorithm>
4+
#include <numeric>
5+
#include <vector>
6+
7+
#include <cpp-toolbox/concurrent/parallel.hpp>
8+
9+
namespace toolbox::pcl
10+
{
11+
12+
template<typename DataType>
13+
std::size_t random_downsampling_t<DataType>::set_input_impl(
14+
const point_cloud& cloud)
15+
{
16+
m_cloud = std::make_shared<point_cloud>(cloud);
17+
return m_cloud->size();
18+
}
19+
20+
template<typename DataType>
21+
std::size_t random_downsampling_t<DataType>::set_input_impl(
22+
const point_cloud_ptr& cloud)
23+
{
24+
m_cloud = cloud;
25+
return m_cloud ? m_cloud->size() : 0U;
26+
}
27+
28+
template<typename DataType>
29+
void random_downsampling_t<DataType>::enable_parallel_impl(bool enable)
30+
{
31+
m_enable_parallel = enable;
32+
}
33+
34+
template<typename DataType>
35+
typename random_downsampling_t<DataType>::point_cloud
36+
random_downsampling_t<DataType>::filter_impl()
37+
{
38+
auto output = std::make_shared<point_cloud>();
39+
filter_impl(output);
40+
return *output;
41+
}
42+
43+
template<typename DataType>
44+
void random_downsampling_t<DataType>::filter_impl(point_cloud_ptr output)
45+
{
46+
if (!output)
47+
return;
48+
if (!m_cloud || m_cloud->empty()) {
49+
output->clear();
50+
return;
51+
}
52+
53+
const std::size_t input_size = m_cloud->size();
54+
std::size_t sample_count =
55+
static_cast<std::size_t>(std::floor(input_size * m_ration));
56+
sample_count = std::min(sample_count, input_size);
57+
if (sample_count == 0) {
58+
output->clear();
59+
return;
60+
}
61+
62+
std::vector<std::size_t> indices(input_size);
63+
std::iota(indices.begin(), indices.end(), 0);
64+
toolbox::utils::random_t::instance().shuffle(indices);
65+
indices.resize(sample_count);
66+
67+
output->points.resize(sample_count);
68+
if (!m_cloud->normals.empty()) {
69+
output->normals.resize(sample_count);
70+
}
71+
if (!m_cloud->colors.empty()) {
72+
output->colors.resize(sample_count);
73+
}
74+
output->intensity = m_cloud->intensity;
75+
76+
if (m_enable_parallel && sample_count > 1024) {
77+
toolbox::concurrent::parallel_transform(indices.cbegin(),
78+
indices.cend(),
79+
output->points.begin(),
80+
[this](std::size_t idx)
81+
{ return m_cloud->points[idx]; });
82+
if (!m_cloud->normals.empty()) {
83+
toolbox::concurrent::parallel_transform(
84+
indices.cbegin(),
85+
indices.cend(),
86+
output->normals.begin(),
87+
[this](std::size_t idx) { return m_cloud->normals[idx]; });
88+
}
89+
if (!m_cloud->colors.empty()) {
90+
toolbox::concurrent::parallel_transform(indices.cbegin(),
91+
indices.cend(),
92+
output->colors.begin(),
93+
[this](std::size_t idx)
94+
{ return m_cloud->colors[idx]; });
95+
}
96+
} else {
97+
for (std::size_t i = 0; i < sample_count; ++i) {
98+
std::size_t idx = indices[i];
99+
output->points[i] = m_cloud->points[idx];
100+
if (!m_cloud->normals.empty()) {
101+
output->normals[i] = m_cloud->normals[idx];
102+
}
103+
if (!m_cloud->colors.empty()) {
104+
output->colors[i] = m_cloud->colors[idx];
105+
}
106+
}
107+
}
108+
}
109+
110+
} // namespace toolbox::pcl

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

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
#pragma once
2+
3+
#include <cmath>
4+
#include <tuple>
5+
#include <unordered_map>
6+
#include <vector>
7+
8+
#include <cpp-toolbox/concurrent/parallel.hpp>
9+
10+
namespace toolbox::pcl
11+
{
12+
13+
template<typename DataType>
14+
std::size_t voxel_grid_downsampling_t<DataType>::set_input_impl(
15+
const point_cloud& cloud)
16+
{
17+
m_cloud = std::make_shared<point_cloud>(cloud);
18+
return m_cloud->size();
19+
}
20+
21+
template<typename DataType>
22+
std::size_t voxel_grid_downsampling_t<DataType>::set_input_impl(
23+
const point_cloud_ptr& cloud)
24+
{
25+
m_cloud = cloud;
26+
return m_cloud ? m_cloud->size() : 0U;
27+
}
28+
29+
template<typename DataType>
30+
void voxel_grid_downsampling_t<DataType>::enable_parallel_impl(bool enable)
31+
{
32+
m_enable_parallel = enable;
33+
}
34+
35+
template<typename DataType>
36+
typename voxel_grid_downsampling_t<DataType>::point_cloud
37+
voxel_grid_downsampling_t<DataType>::filter_impl()
38+
{
39+
auto output = std::make_shared<point_cloud>();
40+
filter_impl(output);
41+
return *output;
42+
}
43+
44+
template<typename DataType>
45+
void voxel_grid_downsampling_t<DataType>::filter_impl(point_cloud_ptr output)
46+
{
47+
if (!output)
48+
return;
49+
if (!m_cloud || m_cloud->empty()) {
50+
output->clear();
51+
return;
52+
}
53+
54+
struct voxel_data_t
55+
{
56+
toolbox::types::point_t<DataType> sum_point {0, 0, 0};
57+
toolbox::types::point_t<DataType> sum_normal {0, 0, 0};
58+
toolbox::types::point_t<DataType> sum_color {0, 0, 0};
59+
std::size_t count {0};
60+
};
61+
62+
using key_t = std::tuple<int, int, int>;
63+
struct key_hash
64+
{
65+
std::size_t operator()(const key_t& k) const noexcept
66+
{
67+
std::size_t h1 = std::hash<int> {}(std::get<0>(k));
68+
std::size_t h2 = std::hash<int> {}(std::get<1>(k));
69+
std::size_t h3 = std::hash<int> {}(std::get<2>(k));
70+
return h1 ^ (h2 << 1) ^ (h3 << 2);
71+
}
72+
};
73+
74+
std::unordered_map<key_t, voxel_data_t, key_hash> voxel_map;
75+
std::mutex map_mutex;
76+
auto process_point = [&](std::size_t idx)
77+
{
78+
const auto& pt = m_cloud->points[idx];
79+
int ix = static_cast<int>(std::floor(pt.x / m_voxel_size));
80+
int iy = static_cast<int>(std::floor(pt.y / m_voxel_size));
81+
int iz = static_cast<int>(std::floor(pt.z / m_voxel_size));
82+
key_t key(ix, iy, iz);
83+
std::lock_guard<std::mutex> lock(map_mutex);
84+
auto& v = voxel_map[key];
85+
v.sum_point += pt;
86+
if (!m_cloud->normals.empty()) {
87+
v.sum_normal += m_cloud->normals[idx];
88+
}
89+
if (!m_cloud->colors.empty()) {
90+
v.sum_color += m_cloud->colors[idx];
91+
}
92+
++v.count;
93+
};
94+
95+
const std::size_t total = m_cloud->size();
96+
if (m_enable_parallel && total > 1024) {
97+
std::vector<std::size_t> indices(total);
98+
std::iota(indices.begin(), indices.end(), 0);
99+
toolbox::concurrent::parallel_for_each(
100+
indices.begin(), indices.end(), process_point);
101+
} else {
102+
for (std::size_t i = 0; i < total; ++i) {
103+
process_point(i);
104+
}
105+
}
106+
107+
const bool has_normals = !m_cloud->normals.empty();
108+
const bool has_colors = !m_cloud->colors.empty();
109+
output->points.reserve(voxel_map.size());
110+
if (has_normals) {
111+
output->normals.reserve(voxel_map.size());
112+
}
113+
if (has_colors) {
114+
output->colors.reserve(voxel_map.size());
115+
}
116+
output->intensity = m_cloud->intensity;
117+
118+
for (auto& [key, v] : voxel_map) {
119+
auto centroid = v.sum_point;
120+
centroid /= static_cast<DataType>(v.count);
121+
output->points.push_back(centroid);
122+
if (has_normals) {
123+
auto n = v.sum_normal;
124+
n /= static_cast<DataType>(v.count);
125+
output->normals.push_back(n);
126+
}
127+
if (has_colors) {
128+
auto c = v.sum_color;
129+
c /= static_cast<DataType>(v.count);
130+
output->colors.push_back(c);
131+
}
132+
}
133+
}
134+
135+
} // namespace toolbox::pcl

0 commit comments

Comments
 (0)