Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
sac_model_normal_plane.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2010, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
43
44#include <pcl/sample_consensus/sac_model_normal_plane.h>
45#include <pcl/common/common.h> // for getAngle3D
46
47//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointT, typename PointNT> void
50 const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
51{
52 if (!normals_)
53 {
54 PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
55 inliers.clear ();
56 return;
57 }
58
59 // Check if the model is valid given the user constraints
60 if (!isModelValid (model_coefficients))
61 {
62 inliers.clear ();
63 return;
64 }
65
66 // Obtain the plane normal
67 Eigen::Vector4f coeff = model_coefficients;
68 coeff[3] = 0.0f;
69
70 inliers.clear ();
71 error_sqr_dists_.clear ();
72 inliers.reserve (indices_->size ());
73 error_sqr_dists_.reserve (indices_->size ());
74
75 // Iterate through the 3d points and calculate the distances from them to the plane
76 for (std::size_t i = 0; i < indices_->size (); ++i)
77 {
78 const PointT &pt = (*input_)[(*indices_)[i]];
79 const PointNT &nt = (*normals_)[(*indices_)[i]];
80 // Calculate the distance from the point to the plane normal as the dot product
81 // D = (P-A).N/|N|
82 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
83 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
84 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
85
86 // Calculate the angular distance between the point normal and the plane normal
87 double d_normal = std::abs (getAngle3D (n, coeff));
88 d_normal = (std::min) (d_normal, M_PI - d_normal);
89
90 // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
91 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
92
93 double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
94 if (distance < threshold)
95 {
96 // Returns the indices of the points whose distances are smaller than the threshold
97 inliers.push_back ((*indices_)[i]);
98 error_sqr_dists_.push_back (distance);
99 }
100 }
101}
102
103//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104template <typename PointT, typename PointNT> std::size_t
106 const Eigen::VectorXf &model_coefficients, const double threshold) const
107{
108 if (!normals_)
109 {
110 PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
111 return (0);
112 }
113
114 // Check if the model is valid given the user constraints
115 if (!isModelValid (model_coefficients))
116 return (0);
117
118#if defined (__AVX__) && defined (__AVX2__)
119 return countWithinDistanceAVX (model_coefficients, threshold);
120#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
121 return countWithinDistanceSSE (model_coefficients, threshold);
122#else
123 return countWithinDistanceStandard (model_coefficients, threshold);
124#endif
125}
126
127//////////////////////////////////////////////////////////////////////////
128template <typename PointT, typename PointNT> std::size_t
130 const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
131{
132 std::size_t nr_p = 0;
133
134 // Obtain the plane normal
135 Eigen::Vector4f coeff = model_coefficients;
136 coeff[3] = 0.0f;
137
138 // Iterate through the 3d points and calculate the distances from them to the plane
139 for (; i < indices_->size (); ++i)
140 {
141 const PointT &pt = (*input_)[(*indices_)[i]];
142 const PointNT &nt = (*normals_)[(*indices_)[i]];
143 // Calculate the distance from the point to the plane normal as the dot product
144 // D = (P-A).N/|N|
145 const Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
146 const Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
147 const double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
148
149 // Calculate the angular distance between the point normal and the plane normal
150 double d_normal = std::abs (getAngle3D (n, coeff));
151 d_normal = (std::min) (d_normal, M_PI - d_normal);
152
153 // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
154 const double weight = normal_distance_weight_ * (1.0 - nt.curvature);
155
156 if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
157 {
158 nr_p++;
159 }
160 }
161 return (nr_p);
162}
163
164//////////////////////////////////////////////////////////////////////////
165#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
166template <typename PointT, typename PointNT> std::size_t
168 const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
169{
170 std::size_t nr_p = 0;
171 const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
172 const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
173 const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
174 const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
175 const __m128 threshold_vec = _mm_set1_ps (threshold);
176 const __m128 normal_distance_weight_vec = _mm_set1_ps (normal_distance_weight_);
177 const __m128 abs_help = _mm_set1_ps (-0.0F); // -0.0F (negative zero) means that all bits are 0, only the sign bit is 1
178 __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
179 for (; (i + 4) <= indices_->size (); i += 4)
180 {
181 const __m128 d_euclid_vec = pcl::SampleConsensusModelPlane<PointT>::dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help);
182
183 const __m128 d_normal_vec = getAcuteAngle3DSSE (
184 _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_x,
185 (*normals_)[(*indices_)[i+1]].normal_x,
186 (*normals_)[(*indices_)[i+2]].normal_x,
187 (*normals_)[(*indices_)[i+3]].normal_x),
188 _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_y,
189 (*normals_)[(*indices_)[i+1]].normal_y,
190 (*normals_)[(*indices_)[i+2]].normal_y,
191 (*normals_)[(*indices_)[i+3]].normal_y),
192 _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_z,
193 (*normals_)[(*indices_)[i+1]].normal_z,
194 (*normals_)[(*indices_)[i+2]].normal_z,
195 (*normals_)[(*indices_)[i+3]].normal_z),
196 a_vec, b_vec, c_vec);
197 const __m128 weight_vec = _mm_mul_ps (normal_distance_weight_vec, _mm_sub_ps (_mm_set1_ps (1.0f),
198 _mm_set_ps ((*normals_)[(*indices_)[i ]].curvature,
199 (*normals_)[(*indices_)[i+1]].curvature,
200 (*normals_)[(*indices_)[i+2]].curvature,
201 (*normals_)[(*indices_)[i+3]].curvature)));
202 const __m128 dist = _mm_andnot_ps (abs_help, _mm_add_ps (_mm_mul_ps (weight_vec, d_normal_vec), _mm_mul_ps (_mm_sub_ps (_mm_set1_ps (1.0f), weight_vec), d_euclid_vec)));
203 const __m128 mask = _mm_cmplt_ps (dist, threshold_vec); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
204 res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
205 }
206 nr_p += _mm_extract_epi32 (res, 0);
207 nr_p += _mm_extract_epi32 (res, 1);
208 nr_p += _mm_extract_epi32 (res, 2);
209 nr_p += _mm_extract_epi32 (res, 3);
210
211 // Process the remaining points (at most 3)
212 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
213 return (nr_p);
214}
215#endif
216
217//////////////////////////////////////////////////////////////////////////
218#if defined (__AVX__) && defined (__AVX2__)
219template <typename PointT, typename PointNT> std::size_t
221 const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
222{
223 std::size_t nr_p = 0;
224 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
225 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
226 const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
227 const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
228 const __m256 threshold_vec = _mm256_set1_ps (threshold);
229 const __m256 normal_distance_weight_vec = _mm256_set1_ps (normal_distance_weight_);
230 const __m256 abs_help = _mm256_set1_ps (-0.0F); // -0.0F (negative zero) means that all bits are 0, only the sign bit is 1
231 __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
232 for (; (i + 8) <= indices_->size (); i += 8)
233 {
234 const __m256 d_euclid_vec = pcl::SampleConsensusModelPlane<PointT>::dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help);
235
236 const __m256 d_normal_vec = getAcuteAngle3DAVX (
237 _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_x,
238 (*normals_)[(*indices_)[i+1]].normal_x,
239 (*normals_)[(*indices_)[i+2]].normal_x,
240 (*normals_)[(*indices_)[i+3]].normal_x,
241 (*normals_)[(*indices_)[i+4]].normal_x,
242 (*normals_)[(*indices_)[i+5]].normal_x,
243 (*normals_)[(*indices_)[i+6]].normal_x,
244 (*normals_)[(*indices_)[i+7]].normal_x),
245 _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_y,
246 (*normals_)[(*indices_)[i+1]].normal_y,
247 (*normals_)[(*indices_)[i+2]].normal_y,
248 (*normals_)[(*indices_)[i+3]].normal_y,
249 (*normals_)[(*indices_)[i+4]].normal_y,
250 (*normals_)[(*indices_)[i+5]].normal_y,
251 (*normals_)[(*indices_)[i+6]].normal_y,
252 (*normals_)[(*indices_)[i+7]].normal_y),
253 _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_z,
254 (*normals_)[(*indices_)[i+1]].normal_z,
255 (*normals_)[(*indices_)[i+2]].normal_z,
256 (*normals_)[(*indices_)[i+3]].normal_z,
257 (*normals_)[(*indices_)[i+4]].normal_z,
258 (*normals_)[(*indices_)[i+5]].normal_z,
259 (*normals_)[(*indices_)[i+6]].normal_z,
260 (*normals_)[(*indices_)[i+7]].normal_z),
261 a_vec, b_vec, c_vec);
262 const __m256 weight_vec = _mm256_mul_ps (normal_distance_weight_vec, _mm256_sub_ps (_mm256_set1_ps (1.0f),
263 _mm256_set_ps ((*normals_)[(*indices_)[i ]].curvature,
264 (*normals_)[(*indices_)[i+1]].curvature,
265 (*normals_)[(*indices_)[i+2]].curvature,
266 (*normals_)[(*indices_)[i+3]].curvature,
267 (*normals_)[(*indices_)[i+4]].curvature,
268 (*normals_)[(*indices_)[i+5]].curvature,
269 (*normals_)[(*indices_)[i+6]].curvature,
270 (*normals_)[(*indices_)[i+7]].curvature)));
271 const __m256 dist = _mm256_andnot_ps (abs_help, _mm256_add_ps (_mm256_mul_ps (weight_vec, d_normal_vec), _mm256_mul_ps (_mm256_sub_ps (_mm256_set1_ps (1.0f), weight_vec), d_euclid_vec)));
272 const __m256 mask = _mm256_cmp_ps (dist, threshold_vec, _CMP_LT_OQ); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
273 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
274 }
275 nr_p += _mm256_extract_epi32 (res, 0);
276 nr_p += _mm256_extract_epi32 (res, 1);
277 nr_p += _mm256_extract_epi32 (res, 2);
278 nr_p += _mm256_extract_epi32 (res, 3);
279 nr_p += _mm256_extract_epi32 (res, 4);
280 nr_p += _mm256_extract_epi32 (res, 5);
281 nr_p += _mm256_extract_epi32 (res, 6);
282 nr_p += _mm256_extract_epi32 (res, 7);
283
284 // Process the remaining points (at most 7)
285 nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
286 return (nr_p);
287}
288#endif
289
290//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
291template <typename PointT, typename PointNT> void
293 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
294{
295 if (!normals_)
296 {
297 PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
298 return;
299 }
300
301 // Check if the model is valid given the user constraints
302 if (!isModelValid (model_coefficients))
303 {
304 distances.clear ();
305 return;
306 }
307
308 // Obtain the plane normal
309 Eigen::Vector4f coeff = model_coefficients;
310 coeff[3] = 0.0f;
311
312 distances.resize (indices_->size ());
313
314 // Iterate through the 3d points and calculate the distances from them to the plane
315 for (std::size_t i = 0; i < indices_->size (); ++i)
316 {
317 const PointT &pt = (*input_)[(*indices_)[i]];
318 const PointNT &nt = (*normals_)[(*indices_)[i]];
319 // Calculate the distance from the point to the plane normal as the dot product
320 // D = (P-A).N/|N|
321 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
322 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
323 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
324
325 // Calculate the angular distance between the point normal and the plane normal
326 double d_normal = std::abs (getAngle3D (n, coeff));
327 d_normal = (std::min) (d_normal, M_PI - d_normal);
328
329 // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
330 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
331
332 distances[i] = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
333 }
334}
335
336#define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
337
338#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
339
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given plane model.
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
Define standard C methods and C++ classes that are common to all methods.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
Definition common.hpp:47
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
#define M_PI
Definition pcl_macros.h:203
A point structure representing Euclidean xyz coordinates, and the RGB color.