Skip to content

Commit 4d7ab41

Browse files
LimHyungTaeLimHyungTae
committed
perf(patchwork): TBB parallel_for over patches in classic Patchwork
The classic Patchwork main loop in cpp/patchwork/src/patchwork.cpp is now parallelised with a single tbb::parallel_for over all (zone, ring, sector) patches, mirroring the upstream ~/git/patchwork pattern. Per-patch work (sort + plane fit + GLE) runs in worker threads; a serial reduction then accumulates ground / nonground in deterministic order. median per-frame time on KITTI seq 00 (i7-12700, 24 logical cores): single-thread (taskset -c 0) 8.31 ms (120.4 Hz) parallel (default TBB scheduler) 4.81 ms (207.8 Hz) → 1.73x speedup Patchwork++ (cpp/patchworkpp/src/patchworkpp.cpp) was also benchmarked under the same TBB pattern at 1 / 2 / 4 / 8 / 16 / 24 threads. Every multi-thread configuration was SLOWER than single-thread on KITTI: 1 thread → 111 Hz (baseline) 2 threads → 93 Hz 4 threads → 91 Hz 8 threads → 91 Hz 16 threads → 85 Hz 24 threads → 69 Hz The per-patch work in Patchwork++ is small (~14 µs avg) and dominated by short-lived std::vector / Eigen::Matrix allocations inside R-VPF and R-GPF. Concurrent malloc serialises on the heap allocator and TBB scheduler overhead exceeds the parallelisation benefit at every thread count. Single-threaded Patchwork++ already runs at ~2x the paper's reported 55 Hz on i7-7700K, so there is no real-time motivation to parallelise. Patchwork++ remains single-threaded; the estimateGround loop has a long-form comment explaining why. Numerical equivalence verified on KITTI 00-10 full sweep (23,201 frames), both methods, Patchwork++ paper protocol: patchwork x pp protocol pre: 96.0172 post: 96.0172 (byte-identical) patchwork++ x pp protocol pre: 96.2918 post: 96.2919 (Δ +0.0001) Both within the ±0.05 budget set in the refactor plan. Build: - Adds find_package(TBB CONFIG/MODULE REQUIRED) to cpp/CMakeLists.txt with a helpful error message listing the install command for Ubuntu / macOS / Windows. - cpp/patchwork/CMakeLists.txt links TBB::tbb; cpp/patchworkpp/ does not (since it does not use TBB). Also adds: - python/examples/bench_hz.py — small per-frame timing harness that reports median / mean / p95 / p99 ms and Hz from getTimeTaken(). - A `const` qualifier on PatchWork::extract_initial_seeds and PatchWork::perform_regionwise_segmentation since neither writes to *this any more — needed so the TBB worker can call them.
1 parent a1f410c commit 4d7ab41

7 files changed

Lines changed: 210 additions & 78 deletions

File tree

‎cpp/CMakeLists.txt‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,20 @@ endfunction()
4040

4141
find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/cmake/eigen.cmake")
4242

43+
# Intel oneTBB / TBB for thread-parallel per-patch ground segmentation.
44+
# Required by both `cpp/patchwork/` (classic Patchwork) and `cpp/patchworkpp/`.
45+
find_package(TBB CONFIG QUIET)
46+
if (NOT TBB_FOUND)
47+
find_package(TBB MODULE QUIET)
48+
endif()
49+
if (NOT TBB_FOUND)
50+
message(FATAL_ERROR
51+
"TBB not found. Install Intel oneTBB:\n"
52+
" Ubuntu/Debian: apt install libtbb-dev\n"
53+
" macOS: brew install tbb\n"
54+
" Windows: vcpkg install tbb (or use oneAPI Base Toolkit)")
55+
endif()
56+
4357
# Parameters in `patchworkpp` subdirectory.
4458
# Thus, link should be `patchworkpp::ground_seg_cores`
4559
set(PARENT_PROJECT_NAME ${PROJECT_NAME})

‎cpp/patchwork/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ target_include_directories(${CLASSIC_TARGET} PUBLIC
1111
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
1212
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
1313
)
14-
target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_common ground_seg_cores)
14+
target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_common ground_seg_cores TBB::tbb)
1515
add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET})
1616

1717
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/

‎cpp/patchwork/include/patchwork/patchwork.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -72,14 +72,14 @@ class PatchWork {
7272
void pc2regionwise_patches(const std::vector<PointXYZ>& src);
7373
void extract_initial_seeds(int zone_idx,
7474
const std::vector<PointXYZ>& sorted,
75-
std::vector<PointXYZ>& seeds);
75+
std::vector<PointXYZ>& seeds) const;
7676
PatchStatus determine_gle_status(int zone_idx, int ring_idx, const PCAFeature& feature) const;
7777
void perform_regionwise_segmentation(int zone_idx,
7878
int ring_idx,
7979
const std::vector<PointXYZ>& patch,
8080
std::vector<PointXYZ>& patch_ground,
8181
std::vector<PointXYZ>& patch_nonground,
82-
PatchStatus& status_out);
82+
PatchStatus& status_out) const;
8383
void estimate_sensor_height(std::vector<PointXYZ>& cloud);
8484
double consensus_set_based_height_estimation(const std::vector<double>& candidate_heights);
8585
void materialize() const;

‎cpp/patchwork/src/patchwork.cpp‎

Lines changed: 52 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,9 @@
66
#include <iostream>
77
#include <limits>
88

9+
#include <tbb/blocked_range.h>
10+
#include <tbb/parallel_for.h>
11+
912
#include "patchwork/plane_fit.h"
1013

1114
namespace {
@@ -78,7 +81,7 @@ void PatchWork::pc2regionwise_patches(const std::vector<PointXYZ>& src) {
7881

7982
void PatchWork::extract_initial_seeds(int zone_idx,
8083
const std::vector<PointXYZ>& sorted,
81-
std::vector<PointXYZ>& seeds) {
84+
std::vector<PointXYZ>& seeds) const {
8285
seeds.clear();
8386
if (sorted.empty()) return;
8487

@@ -150,7 +153,7 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx,
150153
const std::vector<PointXYZ>& patch,
151154
std::vector<PointXYZ>& patch_ground,
152155
std::vector<PointXYZ>& patch_nonground,
153-
PatchStatus& status_out) {
156+
PatchStatus& status_out) const {
154157
patch_ground.clear();
155158
patch_nonground.clear();
156159

@@ -376,29 +379,59 @@ void PatchWork::estimateGround(const Eigen::MatrixXf& cloud) {
376379
flush();
377380
pc2regionwise_patches(kept);
378381

379-
// 5) Per-patch segmentation (sequential — was tbb::parallel_for upstream)
380-
ground_pts_.clear();
381-
nonground_pts_.clear();
382+
// 5) Per-patch segmentation, parallelised over all (zone, ring, sector)
383+
// patches. Each patch is independent: `perform_regionwise_segmentation`
384+
// is const, takes the patch by const ref, and writes to caller-owned
385+
// output buffers. We collect per-patch results into an indexed buffer
386+
// and then accumulate into the final ground/nonground point lists in
387+
// a serial reduction so the accumulation order is deterministic
388+
// (mirrors the original upstream patchwork's two-phase pattern).
389+
struct PatchOutcome {
390+
std::vector<PointXYZ> patch_ground;
391+
std::vector<PointXYZ> patch_nonground;
392+
PatchStatus status = PatchStatus::NotAssigned;
393+
const std::vector<PointXYZ>* patch_ref = nullptr; // for "reject whole patch" case
394+
};
395+
396+
std::vector<std::tuple<int, int, int>> patch_indices;
397+
patch_indices.reserve(params_.num_zones * 8 * 32);
382398
for (int z = 0; z < params_.num_zones; ++z) {
383399
for (int r = 0; r < params_.num_rings_each_zone[z]; ++r) {
384400
for (int s = 0; s < params_.num_sectors_each_zone[z]; ++s) {
385-
const auto& patch = regionwise_patches_[z][r][s];
386-
std::vector<PointXYZ> pg, png;
387-
PatchStatus status;
388-
perform_regionwise_segmentation(z, r, patch, pg, png, status);
389-
switch (status) {
390-
case PatchStatus::UprightEnough:
391-
case PatchStatus::FlatEnough:
392-
ground_pts_.insert(ground_pts_.end(), pg.begin(), pg.end());
393-
nonground_pts_.insert(nonground_pts_.end(), png.begin(), png.end());
394-
break;
395-
default:
396-
// Reject the whole patch as nonground
397-
nonground_pts_.insert(nonground_pts_.end(), patch.begin(), patch.end());
398-
}
401+
patch_indices.emplace_back(z, r, s);
399402
}
400403
}
401404
}
405+
const int num_patches = static_cast<int>(patch_indices.size());
406+
std::vector<PatchOutcome> outcomes(num_patches);
407+
408+
tbb::parallel_for(tbb::blocked_range<int>(0, num_patches),
409+
[&](const tbb::blocked_range<int>& range) {
410+
for (int k = range.begin(); k < range.end(); ++k) {
411+
const auto& [z, r, s] = patch_indices[k];
412+
const auto& patch = regionwise_patches_[z][r][s];
413+
auto& out = outcomes[k];
414+
out.patch_ref = &patch;
415+
perform_regionwise_segmentation(
416+
z, r, patch, out.patch_ground, out.patch_nonground, out.status);
417+
}
418+
});
419+
420+
ground_pts_.clear();
421+
nonground_pts_.clear();
422+
for (const auto& out : outcomes) {
423+
switch (out.status) {
424+
case PatchStatus::UprightEnough:
425+
case PatchStatus::FlatEnough:
426+
ground_pts_.insert(ground_pts_.end(), out.patch_ground.begin(), out.patch_ground.end());
427+
nonground_pts_.insert(
428+
nonground_pts_.end(), out.patch_nonground.begin(), out.patch_nonground.end());
429+
break;
430+
default:
431+
// Reject the whole patch as nonground
432+
nonground_pts_.insert(nonground_pts_.end(), out.patch_ref->begin(), out.patch_ref->end());
433+
}
434+
}
402435

403436
// 6) Mark outputs dirty (actual matrix materialization is lazy)
404437
outputs_dirty_ = true;

‎cpp/patchworkpp/include/patchwork/patchworkpp.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -231,12 +231,12 @@ class PatchWorkpp {
231231

232232
void extract_initial_seeds(const int zone_idx,
233233
const vector<PointXYZ> &p_sorted,
234-
vector<PointXYZ> &init_seeds);
234+
vector<PointXYZ> &init_seeds) const;
235235

236236
void extract_initial_seeds(const int zone_idx,
237237
const vector<PointXYZ> &p_sorted,
238238
vector<PointXYZ> &init_seeds,
239-
double th_seed);
239+
double th_seed) const;
240240
};
241241

242242
}; // namespace patchwork

‎cpp/patchworkpp/src/patchworkpp.cpp‎

Lines changed: 35 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,8 @@
11
#include "patchwork/patchworkpp.h"
22

3+
#include <algorithm>
4+
#include <vector>
5+
36
#include "patchwork/plane_fit.h" // xy2theta, xy2radius, point_z_cmp
47

58
using namespace std;
@@ -79,7 +82,7 @@ void PatchWorkpp::estimate_plane(const vector<PointXYZ> &ground) {
7982
void PatchWorkpp::extract_initial_seeds(const int zone_idx,
8083
const vector<PointXYZ> &p_sorted,
8184
vector<PointXYZ> &init_seeds,
82-
double th_seed) {
85+
double th_seed) const {
8386
init_seeds.clear();
8487

8588
// LPR is the mean of low point representative
@@ -115,7 +118,7 @@ void PatchWorkpp::extract_initial_seeds(const int zone_idx,
115118

116119
void PatchWorkpp::extract_initial_seeds(const int zone_idx,
117120
const vector<PointXYZ> &p_sorted,
118-
vector<PointXYZ> &init_seeds) {
121+
vector<PointXYZ> &init_seeds) const {
119122
init_seeds.clear();
120123

121124
// LPR is the mean of low point representative
@@ -185,37 +188,40 @@ void PatchWorkpp::estimateGround(Eigen::MatrixXf cloud_in) {
185188
std::vector<patchwork::RevertCandidate> candidates;
186189
std::vector<double> ringwise_flatness;
187190

191+
// NOTE: TBB parallelisation was evaluated for this main loop and
192+
// measurably HURT throughput on KITTI (24-core / 8-core / 4-core all
193+
// 30-50% slower than single-thread). The per-patch work is small
194+
// (~14 µs avg) and dominated by short-lived `std::vector` /
195+
// `Eigen::Matrix` allocations inside R-VPF + R-GPF, so concurrent
196+
// mallocs serialise on the heap and TBB scheduler overhead exceeds
197+
// the parallelisation benefit. Single-threaded Patchwork++ already
198+
// runs ~110 Hz on KITTI HDL-64E (2× the paper's reported 55 Hz on
199+
// i7-7700K), so there is no real-time motivation to parallelise.
200+
// The classic Patchwork (see cpp/patchwork/src/patchwork.cpp) does
201+
// benefit from TBB because it has no R-VPF and fewer allocations
202+
// per patch.
188203
for (int zone_idx = 0; zone_idx < params_.num_zones; ++zone_idx) {
189204
auto zone = ConcentricZoneModel_[zone_idx];
190205

191206
for (int ring_idx = 0; ring_idx < params_.num_rings_each_zone[zone_idx]; ++ring_idx) {
192-
for (int sector_idx = 0; sector_idx < params_.num_sectors_each_zone[zone_idx]; ++sector_idx) {
207+
const int num_sectors = params_.num_sectors_each_zone[zone_idx];
208+
209+
clock_t t_bef_gle = clock();
210+
for (int sector_idx = 0; sector_idx < num_sectors; ++sector_idx) {
193211
if (zone[ring_idx][sector_idx].size() < params_.num_min_pts) {
194212
addCloud(cloud_nonground_, zone[ring_idx][sector_idx]);
195213
continue;
196214
}
197215

198-
// --------- region-wise sorting (faster than global sorting method) ---------------- //
199-
clock_t t_bef_sort = clock();
200-
sort(zone[ring_idx][sector_idx].begin(), zone[ring_idx][sector_idx].end(), point_z_cmp);
201-
clock_t t_aft_sort = clock();
202-
203-
t_sort += t_aft_sort - t_bef_sort;
204-
// ---------------------------------------------------------------------------------- //
216+
std::sort(
217+
zone[ring_idx][sector_idx].begin(), zone[ring_idx][sector_idx].end(), point_z_cmp);
205218

206-
clock_t t_bef_pca = clock();
207219
extract_piecewiseground(
208220
zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_);
209-
clock_t t_aft_pca = clock();
210-
211-
t_pca += t_aft_pca - t_bef_pca;
212221

213222
centers_.push_back(PointXYZ(pc_mean_(0), pc_mean_(1), pc_mean_(2)));
214223
normals_.push_back(PointXYZ(normal_(0), normal_(1), normal_(2)));
215224

216-
clock_t t_bef_gle = clock();
217-
// Status of each patch
218-
// used in checking uprightness, elevation, and flatness, respectively
219225
const double ground_uprightness = normal_(2);
220226
const double ground_elevation = pc_mean_(2);
221227
const double ground_flatness = singular_values_.minCoeff();
@@ -224,46 +230,25 @@ void PatchWorkpp::estimateGround(Eigen::MatrixXf cloud_in) {
224230
: std::numeric_limits<double>::max();
225231

226232
double heading = 0.0;
227-
for (int i = 0; i < 3; i++) heading += pc_mean_(i) * normal_(i);
228-
229-
/*
230-
About 'is_heading_outside' condition, heading should be smaller than 0 theoretically.
231-
( Imagine the geometric relationship between the surface normal vector on the ground
232-
plane and the vector connecting the sensor origin and the mean point of the ground plane
233-
)
233+
for (int i = 0; i < 3; ++i) heading += pc_mean_(i) * normal_(i);
234234

235-
However, when the patch is far awaw from the sensor origin,
236-
heading could be larger than 0 even if it's ground due to lack of amount of ground plane
237-
points.
238-
239-
Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near
240-
condition )
241-
*/
242-
bool is_upright = ground_uprightness > params_.uprightness_thr;
243-
bool is_near_zone = concentric_idx < params_.num_rings_of_interest;
244-
bool is_heading_outside = heading < 0.0;
235+
const bool is_upright = ground_uprightness > params_.uprightness_thr;
236+
const bool is_near_zone = concentric_idx < params_.num_rings_of_interest;
237+
const bool is_heading_outside = heading < 0.0;
245238

246239
bool is_not_elevated = false;
247240
bool is_flat = false;
248-
249241
if (concentric_idx < params_.num_rings_of_interest) {
250242
is_not_elevated = ground_elevation < params_.elevation_thr[concentric_idx];
251243
is_flat = ground_flatness < params_.flatness_thr[concentric_idx];
252244
}
253245

254-
/*
255-
Store the elevation & flatness variables
256-
for A-GLE (Adaptive Ground Likelihood Estimation)
257-
and TGR (Temporal Ground Revert). More information in the paper Patchwork++.
258-
*/
259246
if (is_upright && is_not_elevated && is_near_zone) {
260247
update_elevation_[concentric_idx].push_back(ground_elevation);
261248
update_flatness_[concentric_idx].push_back(ground_flatness);
262-
263249
ringwise_flatness.push_back(ground_flatness);
264250
}
265251

266-
// Ground estimation based on conditions
267252
if (!is_upright) {
268253
addCloud(cloud_nonground_, regionwise_ground_);
269254
} else if (!is_near_zone) {
@@ -273,21 +258,17 @@ void PatchWorkpp::estimateGround(Eigen::MatrixXf cloud_in) {
273258
} else if (is_not_elevated || is_flat) {
274259
addCloud(cloud_ground_, regionwise_ground_);
275260
} else {
276-
patchwork::RevertCandidate candidate(concentric_idx,
277-
sector_idx,
278-
ground_flatness,
279-
line_variable,
280-
pc_mean_,
281-
regionwise_ground_);
282-
candidates.push_back(candidate);
261+
candidates.emplace_back(concentric_idx,
262+
sector_idx,
263+
ground_flatness,
264+
line_variable,
265+
pc_mean_,
266+
regionwise_ground_);
283267
}
284-
// Every regionwise_nonground is considered nonground.
285268
addCloud(cloud_nonground_, regionwise_nonground_);
286-
287-
clock_t t_aft_gle = clock();
288-
289-
t_gle += t_aft_gle - t_bef_gle;
290269
}
270+
clock_t t_aft_gle = clock();
271+
t_gle += t_aft_gle - t_bef_gle;
291272

292273
clock_t t_bef_revert = clock();
293274
if (!candidates.empty()) {

0 commit comments

Comments
 (0)