Skip to content

Commit b6c78e7

Browse files
larshglarshg
authored
Add multithread support to boundary estimation (#6389)
* Add multithread support * Add conditionally #include <omp.h> * Mark class member functions as const accordingly. * Add unit test of boundaryEstimation using omp. Calculcate normals only once. * Apply clang-format and add to whitelist.
1 parent 8dd4100 commit b6c78e7

File tree

4 files changed

+191
-79
lines changed

4 files changed

+191
-79
lines changed

‎.dev/whitelist.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,3 +32,4 @@ tracking
3232
registration
3333
gpu/containers
3434
gpu/segmentation
35+
test/features/test_boundary_estimation.cpp

‎features/include/pcl/features/boundary.h‎

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -116,7 +116,7 @@ namespace pcl
116116
bool
117117
isBoundaryPoint (const pcl::PointCloud<PointInT> &cloud,
118118
int q_idx, const pcl::Indices &indices,
119-
const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold);
119+
const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold) const;
120120

121121
/** \brief Check whether a point is a boundary point in a planar patch of projected points given by indices.
122122
* \note A coordinate system u-v-n must be computed a-priori using \a getCoordinateSystemOnPlane
@@ -131,7 +131,7 @@ namespace pcl
131131
isBoundaryPoint (const pcl::PointCloud<PointInT> &cloud,
132132
const PointInT &q_point,
133133
const pcl::Indices &indices,
134-
const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold);
134+
const Eigen::Vector4f &u, const Eigen::Vector4f &v, const float angle_threshold) const;
135135

136136
/** \brief Set the decision boundary (angle threshold) that marks points as boundary or regular.
137137
* (default \f$\pi / 2.0\f$)
@@ -145,7 +145,7 @@ namespace pcl
145145

146146
/** \brief Get the decision boundary (angle threshold) as set by the user. */
147147
inline float
148-
getAngleThreshold ()
148+
getAngleThreshold () const
149149
{
150150
return (angle_threshold_);
151151
}
@@ -157,14 +157,27 @@ namespace pcl
157157
*/
158158
inline void
159159
getCoordinateSystemOnPlane (const PointNT &p_coeff,
160-
Eigen::Vector4f &u, Eigen::Vector4f &v)
160+
Eigen::Vector4f &u, Eigen::Vector4f &v) const
161161
{
162162
pcl::Vector4fMapConst p_coeff_v = p_coeff.getNormalVector4fMap ();
163163
v = p_coeff_v.unitOrthogonal ();
164164
u = p_coeff_v.cross3 (v);
165165
}
166166

167+
/** \brief Initialize the scheduler and set the number of threads to use.
168+
* \param nr_threads the number of hardware threads to use (0 sets the value back
169+
* to automatic)
170+
*/
171+
void
172+
setNumberOfThreads(unsigned int nr_threads = 0);
173+
167174
protected:
175+
/** \brief The number of threads the scheduler should use. */
176+
unsigned int threads_{1};
177+
178+
/** \brief Chunk size for (dynamic) scheduling. */
179+
int chunk_size_{256};
180+
168181
/** \brief Estimate whether a set of points is lying on surface boundaries using an angle criterion for all points
169182
* given in <setInputCloud (), setIndices ()> using the surface in setSearchSurface () and the spatial locator in
170183
* setSearchMethod ()

‎features/include/pcl/features/impl/boundary.hpp‎

Lines changed: 43 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -45,14 +45,39 @@
4545

4646
#include <cfloat>
4747

48+
#ifdef _OPENMP
49+
#include <omp.h>
50+
#endif
51+
52+
template <typename PointInT, typename PointNT, typename PointOutT>
53+
void
54+
pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::setNumberOfThreads(
55+
unsigned int nr_threads)
56+
{
57+
#ifdef _OPENMP
58+
if (nr_threads == 0)
59+
threads_ = omp_get_num_procs();
60+
else
61+
threads_ = nr_threads;
62+
PCL_DEBUG("[pcl::BoundaryEstimation::setNumberOfThreads] Setting number of threads "
63+
"to %u.\n",
64+
threads_);
65+
#else
66+
threads_ = 1;
67+
if (nr_threads != 1)
68+
PCL_WARN(
69+
"[pcl::BoundaryEstimation::setNumberOfThreads] Parallelization is requested, "
70+
"but OpenMP is not available! Continuing without parallelization.\n");
71+
#endif // _OPENMP
72+
}
4873

4974
//////////////////////////////////////////////////////////////////////////////////////////////
5075
template <typename PointInT, typename PointNT, typename PointOutT> bool
5176
pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
5277
const pcl::PointCloud<PointInT> &cloud, int q_idx,
5378
const pcl::Indices &indices,
5479
const Eigen::Vector4f &u, const Eigen::Vector4f &v,
55-
const float angle_threshold)
80+
const float angle_threshold) const
5681
{
5782
return (isBoundaryPoint (cloud, cloud[q_idx], indices, u, v, angle_threshold));
5883
}
@@ -63,7 +88,7 @@ pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
6388
const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point,
6489
const pcl::Indices &indices,
6590
const Eigen::Vector4f &u, const Eigen::Vector4f &v,
66-
const float angle_threshold)
91+
const float angle_threshold) const
6792
{
6893
if (indices.size () < 3)
6994
return (false);
@@ -126,8 +151,15 @@ pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointClou
126151
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
127152
if (input_->is_dense)
128153
{
154+
#pragma omp parallel for \
155+
default(none) \
156+
shared(output) \
157+
firstprivate(nn_indices, nn_dists, u, v) \
158+
num_threads(threads_) \
159+
schedule(dynamic, chunk_size_)
129160
// Iterating over the entire index vector
130-
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
161+
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t>(indices_->size());
162+
++idx)
131163
{
132164
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
133165
{
@@ -147,8 +179,15 @@ pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointClou
147179
}
148180
else
149181
{
182+
#pragma omp parallel for \
183+
default(none) \
184+
shared(output) \
185+
firstprivate(nn_indices, nn_dists, u, v) \
186+
num_threads(threads_) \
187+
schedule(dynamic, chunk_size_)
150188
// Iterating over the entire index vector
151-
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
189+
for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t>(indices_->size());
190+
++idx)
152191
{
153192
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
154193
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)

‎test/features/test_boundary_estimation.cpp‎

Lines changed: 130 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -37,122 +37,181 @@
3737
*
3838
*/
3939

40-
#include <pcl/test/gtest.h>
41-
#include <pcl/point_cloud.h>
42-
#include <pcl/features/normal_3d.h>
4340
#include <pcl/features/boundary.h>
41+
#include <pcl/features/normal_3d.h>
4442
#include <pcl/io/pcd_io.h>
4543
#include <pcl/search/kdtree.h> // for KdTree
44+
#include <pcl/test/gtest.h>
45+
#include <pcl/point_cloud.h>
4646

4747
using namespace pcl;
4848
using namespace pcl::io;
4949

5050
using KdTreePtr = search::KdTree<PointXYZ>::Ptr;
5151

5252
PointCloud<PointXYZ> cloud;
53-
pcl::Indices indices;
53+
PointCloud<Normal>::Ptr normals(new PointCloud<Normal>());
54+
pcl::IndicesPtr indicesptr(new pcl::Indices());
5455
KdTreePtr tree;
5556

56-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57-
TEST (PCL, BoundaryEstimation)
57+
TEST(PCL, BoundaryEstimationOMP)
5858
{
59-
Eigen::Vector4f u = Eigen::Vector4f::Zero ();
60-
Eigen::Vector4f v = Eigen::Vector4f::Zero ();
61-
62-
// Estimate normals first
63-
NormalEstimation<PointXYZ, Normal> n;
64-
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
59+
BoundaryEstimation<PointXYZ, Normal, Boundary> b;
60+
b.setNumberOfThreads(0);
61+
b.setInputNormals(normals);
62+
// Object
63+
PointCloud<Boundary>::Ptr bps(new PointCloud<Boundary>());
6564
// set parameters
66-
n.setInputCloud (cloud.makeShared ());
67-
pcl::IndicesPtr indicesptr (new pcl::Indices (indices));
68-
n.setIndices (indicesptr);
69-
n.setSearchMethod (tree);
70-
n.setKSearch (static_cast<int> (indices.size ()));
65+
b.setInputCloud(cloud.makeShared());
66+
b.setIndices(indicesptr);
67+
b.setSearchMethod(tree);
68+
b.setKSearch(static_cast<int>(indicesptr->size()));
7169
// estimate
72-
n.compute (*normals);
70+
b.compute(*bps);
71+
EXPECT_EQ(bps->size(), indicesptr->size());
72+
73+
bool pt = false;
74+
75+
pt = (*bps)[0].boundary_point;
76+
EXPECT_FALSE(pt);
77+
pt = (*bps)[indicesptr->size() / 3].boundary_point;
78+
EXPECT_FALSE(pt);
79+
pt = (*bps)[indicesptr->size() / 2].boundary_point;
80+
EXPECT_FALSE(pt);
81+
pt = (*bps)[indicesptr->size() - 1].boundary_point;
82+
EXPECT_TRUE(pt);
83+
}
84+
85+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86+
TEST(PCL, BoundaryEstimation)
87+
{
88+
Eigen::Vector4f u = Eigen::Vector4f::Zero();
89+
Eigen::Vector4f v = Eigen::Vector4f::Zero();
7390

7491
BoundaryEstimation<PointXYZ, Normal, Boundary> b;
75-
b.setInputNormals (normals);
76-
EXPECT_EQ (b.getInputNormals (), normals);
92+
b.setInputNormals(normals);
93+
EXPECT_EQ(b.getInputNormals(), normals);
7794

7895
// getCoordinateSystemOnPlane
79-
for (const auto &point : normals->points)
80-
{
81-
b.getCoordinateSystemOnPlane (point, u, v);
82-
Vector4fMapConst n4uv = point.getNormalVector4fMap ();
83-
EXPECT_NEAR (n4uv.dot(u), 0, 1e-4);
84-
EXPECT_NEAR (n4uv.dot(v), 0, 1e-4);
85-
EXPECT_NEAR (u.dot(v), 0, 1e-4);
96+
for (const auto& point : *normals) {
97+
b.getCoordinateSystemOnPlane(point, u, v);
98+
Vector4fMapConst n4uv = point.getNormalVector4fMap();
99+
EXPECT_NEAR(n4uv.dot(u), 0, 1e-4);
100+
EXPECT_NEAR(n4uv.dot(v), 0, 1e-4);
101+
EXPECT_NEAR(u.dot(v), 0, 1e-4);
86102
}
87103

88104
// isBoundaryPoint (indices)
89105
bool pt = false;
90-
pt = b.isBoundaryPoint (cloud, 0, indices, u, v, static_cast<float>(M_PI) / 2.0);
91-
EXPECT_FALSE (pt);
92-
pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) / 3, indices, u, v, static_cast<float>(M_PI) / 2.0);
93-
EXPECT_FALSE (pt);
94-
pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) / 2, indices, u, v, static_cast<float>(M_PI) / 2.0);
95-
EXPECT_FALSE (pt);
96-
pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) - 1, indices, u, v, static_cast<float>(M_PI) / 2.0);
97-
EXPECT_TRUE (pt);
106+
pt = b.isBoundaryPoint(cloud, 0, *indicesptr, u, v, static_cast<float>(M_PI) / 2.0);
107+
EXPECT_FALSE(pt);
108+
pt = b.isBoundaryPoint(cloud,
109+
static_cast<int>(indicesptr->size()) / 3,
110+
*indicesptr,
111+
u,
112+
v,
113+
static_cast<float>(M_PI) / 2.0);
114+
EXPECT_FALSE(pt);
115+
pt = b.isBoundaryPoint(cloud,
116+
static_cast<int>(indicesptr->size()) / 2,
117+
*indicesptr,
118+
u,
119+
v,
120+
static_cast<float>(M_PI) / 2.0);
121+
EXPECT_FALSE(pt);
122+
pt = b.isBoundaryPoint(cloud,
123+
static_cast<int>(indicesptr->size()) - 1,
124+
*indicesptr,
125+
u,
126+
v,
127+
static_cast<float>(M_PI) / 2.0);
128+
EXPECT_TRUE(pt);
98129

99130
// isBoundaryPoint (points)
100-
pt = b.isBoundaryPoint (cloud, cloud[0], indices, u, v, static_cast<float>(M_PI) / 2.0);
101-
EXPECT_FALSE (pt);
102-
pt = b.isBoundaryPoint (cloud, cloud[indices.size () / 3], indices, u, v, static_cast<float>(M_PI) / 2.0);
103-
EXPECT_FALSE (pt);
104-
pt = b.isBoundaryPoint (cloud, cloud[indices.size () / 2], indices, u, v, static_cast<float>(M_PI) / 2.0);
105-
EXPECT_FALSE (pt);
106-
pt = b.isBoundaryPoint (cloud, cloud[indices.size () - 1], indices, u, v, static_cast<float>(M_PI) / 2.0);
107-
EXPECT_TRUE (pt);
131+
pt = b.isBoundaryPoint(
132+
cloud, cloud[0], *indicesptr, u, v, static_cast<float>(M_PI) / 2.0);
133+
EXPECT_FALSE(pt);
134+
pt = b.isBoundaryPoint(cloud,
135+
cloud[indicesptr->size() / 3],
136+
*indicesptr,
137+
u,
138+
v,
139+
static_cast<float>(M_PI) / 2.0);
140+
EXPECT_FALSE(pt);
141+
pt = b.isBoundaryPoint(cloud,
142+
cloud[indicesptr->size() / 2],
143+
*indicesptr,
144+
u,
145+
v,
146+
static_cast<float>(M_PI) / 2.0);
147+
EXPECT_FALSE(pt);
148+
pt = b.isBoundaryPoint(cloud,
149+
cloud[indicesptr->size() - 1],
150+
*indicesptr,
151+
u,
152+
v,
153+
static_cast<float>(M_PI) / 2.0);
154+
EXPECT_TRUE(pt);
108155

109156
// Object
110-
PointCloud<Boundary>::Ptr bps (new PointCloud<Boundary> ());
157+
PointCloud<Boundary>::Ptr bps(new PointCloud<Boundary>());
111158

112159
// set parameters
113-
b.setInputCloud (cloud.makeShared ());
114-
b.setIndices (indicesptr);
115-
b.setSearchMethod (tree);
116-
b.setKSearch (static_cast<int> (indices.size ()));
160+
b.setInputCloud(cloud.makeShared());
161+
b.setIndices(indicesptr);
162+
b.setSearchMethod(tree);
163+
b.setKSearch(static_cast<int>(indicesptr->size()));
117164

118165
// estimate
119-
b.compute (*bps);
120-
EXPECT_EQ (bps->size (), indices.size ());
166+
b.compute(*bps);
167+
EXPECT_EQ(bps->size(), indicesptr->size());
121168

122169
pt = (*bps)[0].boundary_point;
123-
EXPECT_FALSE (pt);
124-
pt = (*bps)[indices.size () / 3].boundary_point;
125-
EXPECT_FALSE (pt);
126-
pt = (*bps)[indices.size () / 2].boundary_point;
127-
EXPECT_FALSE (pt);
128-
pt = (*bps)[indices.size () - 1].boundary_point;
129-
EXPECT_TRUE (pt);
170+
EXPECT_FALSE(pt);
171+
pt = (*bps)[indicesptr->size() / 3].boundary_point;
172+
EXPECT_FALSE(pt);
173+
pt = (*bps)[indicesptr->size() / 2].boundary_point;
174+
EXPECT_FALSE(pt);
175+
pt = (*bps)[indicesptr->size() - 1].boundary_point;
176+
EXPECT_TRUE(pt);
130177
}
131178

132179
/* ---[ */
133180
int
134-
main (int argc, char** argv)
181+
main(int argc, char** argv)
135182
{
136-
if (argc < 2)
137-
{
138-
std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
183+
if (argc < 2) {
184+
std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to "
185+
"the test."
186+
<< std::endl;
139187
return (-1);
140188
}
141189

142-
if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
143-
{
144-
std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
190+
if (loadPCDFile<PointXYZ>(argv[1], cloud) < 0) {
191+
std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its "
192+
"path to the test."
193+
<< std::endl;
145194
return (-1);
146195
}
147196

148-
indices.resize (cloud.size ());
149-
for (std::size_t i = 0; i < indices.size (); ++i)
150-
indices[i] = static_cast<int> (i);
197+
indicesptr->resize(cloud.size());
198+
for (std::size_t i = 0; i < indicesptr->size(); ++i)
199+
(*indicesptr)[i] = static_cast<int>(i);
200+
201+
tree.reset(new search::KdTree<PointXYZ>(false));
202+
tree->setInputCloud(cloud.makeShared());
151203

152-
tree.reset (new search::KdTree<PointXYZ> (false));
153-
tree->setInputCloud (cloud.makeShared ());
204+
// Estimate normals first
205+
NormalEstimation<PointXYZ, Normal> n;
206+
// set parameters
207+
n.setInputCloud(cloud.makeShared());
208+
n.setIndices(indicesptr);
209+
n.setSearchMethod(tree);
210+
n.setKSearch(static_cast<int>(indicesptr->size()));
211+
// estimate
212+
n.compute(*normals);
154213

155-
testing::InitGoogleTest (&argc, argv);
156-
return (RUN_ALL_TESTS ());
214+
testing::InitGoogleTest(&argc, argv);
215+
return (RUN_ALL_TESTS());
157216
}
158217
/* ]--- */

0 commit comments

Comments
 (0)