Point Cloud Library (PCL)  1.14.0-dev
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/sample_consensus/impl/sac_model_plane.hpp> // for dist4, dist8
46 #include <pcl/common/common.h> // for getAngle3D
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT, typename PointNT> void
51  const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
52 {
53  if (!normals_)
54  {
55  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
56  inliers.clear ();
57  return;
58  }
59 
60  // Check if the model is valid given the user constraints
61  if (!isModelValid (model_coefficients))
62  {
63  inliers.clear ();
64  return;
65  }
66 
67  // Obtain the plane normal
68  Eigen::Vector4f coeff = model_coefficients;
69  coeff[3] = 0.0f;
70 
71  inliers.clear ();
72  error_sqr_dists_.clear ();
73  inliers.reserve (indices_->size ());
74  error_sqr_dists_.reserve (indices_->size ());
75 
76  // Iterate through the 3d points and calculate the distances from them to the plane
77  for (std::size_t i = 0; i < indices_->size (); ++i)
78  {
79  const PointT &pt = (*input_)[(*indices_)[i]];
80  const PointNT &nt = (*normals_)[(*indices_)[i]];
81  // Calculate the distance from the point to the plane normal as the dot product
82  // D = (P-A).N/|N|
83  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
84  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
85  double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
86 
87  // Calculate the angular distance between the point normal and the plane normal
88  double d_normal = std::abs (getAngle3D (n, coeff));
89  d_normal = (std::min) (d_normal, M_PI - d_normal);
90 
91  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
92  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
93 
94  double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
95  if (distance < threshold)
96  {
97  // Returns the indices of the points whose distances are smaller than the threshold
98  inliers.push_back ((*indices_)[i]);
99  error_sqr_dists_.push_back (distance);
100  }
101  }
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
105 template <typename PointT, typename PointNT> std::size_t
107  const Eigen::VectorXf &model_coefficients, const double threshold) const
108 {
109  if (!normals_)
110  {
111  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
112  return (0);
113  }
114 
115  // Check if the model is valid given the user constraints
116  if (!isModelValid (model_coefficients))
117  return (0);
118 
119 #if defined (__AVX__) && defined (__AVX2__)
120  return countWithinDistanceAVX (model_coefficients, threshold);
121 #elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
122  return countWithinDistanceSSE (model_coefficients, threshold);
123 #else
124  return countWithinDistanceStandard (model_coefficients, threshold);
125 #endif
126 }
127 
128 //////////////////////////////////////////////////////////////////////////
129 template <typename PointT, typename PointNT> std::size_t
131  const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
132 {
133  std::size_t nr_p = 0;
134 
135  // Obtain the plane normal
136  Eigen::Vector4f coeff = model_coefficients;
137  coeff[3] = 0.0f;
138 
139  // Iterate through the 3d points and calculate the distances from them to the plane
140  for (; i < indices_->size (); ++i)
141  {
142  const PointT &pt = (*input_)[(*indices_)[i]];
143  const PointNT &nt = (*normals_)[(*indices_)[i]];
144  // Calculate the distance from the point to the plane normal as the dot product
145  // D = (P-A).N/|N|
146  const Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
147  const Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
148  const double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
149 
150  // Calculate the angular distance between the point normal and the plane normal
151  double d_normal = std::abs (getAngle3D (n, coeff));
152  d_normal = (std::min) (d_normal, M_PI - d_normal);
153 
154  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
155  const double weight = normal_distance_weight_ * (1.0 - nt.curvature);
156 
157  if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
158  {
159  nr_p++;
160  }
161  }
162  return (nr_p);
163 }
164 
165 //////////////////////////////////////////////////////////////////////////
166 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
167 template <typename PointT, typename PointNT> std::size_t
169  const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
170 {
171  std::size_t nr_p = 0;
172  const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
173  const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
174  const __m128 c_vec = _mm_set1_ps (model_coefficients[2]);
175  const __m128 d_vec = _mm_set1_ps (model_coefficients[3]);
176  const __m128 threshold_vec = _mm_set1_ps (threshold);
177  const __m128 normal_distance_weight_vec = _mm_set1_ps (normal_distance_weight_);
178  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
179  __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
180  for (; (i + 4) <= indices_->size (); i += 4)
181  {
182  const __m128 d_euclid_vec = pcl::SampleConsensusModelPlane<PointT>::dist4 (i, a_vec, b_vec, c_vec, d_vec, abs_help);
183 
184  const __m128 d_normal_vec = getAcuteAngle3DSSE (
185  _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_x,
186  (*normals_)[(*indices_)[i+1]].normal_x,
187  (*normals_)[(*indices_)[i+2]].normal_x,
188  (*normals_)[(*indices_)[i+3]].normal_x),
189  _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_y,
190  (*normals_)[(*indices_)[i+1]].normal_y,
191  (*normals_)[(*indices_)[i+2]].normal_y,
192  (*normals_)[(*indices_)[i+3]].normal_y),
193  _mm_set_ps ((*normals_)[(*indices_)[i ]].normal_z,
194  (*normals_)[(*indices_)[i+1]].normal_z,
195  (*normals_)[(*indices_)[i+2]].normal_z,
196  (*normals_)[(*indices_)[i+3]].normal_z),
197  a_vec, b_vec, c_vec);
198  const __m128 weight_vec = _mm_mul_ps (normal_distance_weight_vec, _mm_sub_ps (_mm_set1_ps (1.0f),
199  _mm_set_ps ((*normals_)[(*indices_)[i ]].curvature,
200  (*normals_)[(*indices_)[i+1]].curvature,
201  (*normals_)[(*indices_)[i+2]].curvature,
202  (*normals_)[(*indices_)[i+3]].curvature)));
203  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)));
204  const __m128 mask = _mm_cmplt_ps (dist, threshold_vec); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
205  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
206  }
207  nr_p += _mm_extract_epi32 (res, 0);
208  nr_p += _mm_extract_epi32 (res, 1);
209  nr_p += _mm_extract_epi32 (res, 2);
210  nr_p += _mm_extract_epi32 (res, 3);
211 
212  // Process the remaining points (at most 3)
213  nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
214  return (nr_p);
215 }
216 #endif
217 
218 //////////////////////////////////////////////////////////////////////////
219 #if defined (__AVX__) && defined (__AVX2__)
220 template <typename PointT, typename PointNT> std::size_t
222  const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
223 {
224  std::size_t nr_p = 0;
225  const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
226  const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
227  const __m256 c_vec = _mm256_set1_ps (model_coefficients[2]);
228  const __m256 d_vec = _mm256_set1_ps (model_coefficients[3]);
229  const __m256 threshold_vec = _mm256_set1_ps (threshold);
230  const __m256 normal_distance_weight_vec = _mm256_set1_ps (normal_distance_weight_);
231  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
232  __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
233  for (; (i + 8) <= indices_->size (); i += 8)
234  {
235  const __m256 d_euclid_vec = pcl::SampleConsensusModelPlane<PointT>::dist8 (i, a_vec, b_vec, c_vec, d_vec, abs_help);
236 
237  const __m256 d_normal_vec = getAcuteAngle3DAVX (
238  _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_x,
239  (*normals_)[(*indices_)[i+1]].normal_x,
240  (*normals_)[(*indices_)[i+2]].normal_x,
241  (*normals_)[(*indices_)[i+3]].normal_x,
242  (*normals_)[(*indices_)[i+4]].normal_x,
243  (*normals_)[(*indices_)[i+5]].normal_x,
244  (*normals_)[(*indices_)[i+6]].normal_x,
245  (*normals_)[(*indices_)[i+7]].normal_x),
246  _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_y,
247  (*normals_)[(*indices_)[i+1]].normal_y,
248  (*normals_)[(*indices_)[i+2]].normal_y,
249  (*normals_)[(*indices_)[i+3]].normal_y,
250  (*normals_)[(*indices_)[i+4]].normal_y,
251  (*normals_)[(*indices_)[i+5]].normal_y,
252  (*normals_)[(*indices_)[i+6]].normal_y,
253  (*normals_)[(*indices_)[i+7]].normal_y),
254  _mm256_set_ps ((*normals_)[(*indices_)[i ]].normal_z,
255  (*normals_)[(*indices_)[i+1]].normal_z,
256  (*normals_)[(*indices_)[i+2]].normal_z,
257  (*normals_)[(*indices_)[i+3]].normal_z,
258  (*normals_)[(*indices_)[i+4]].normal_z,
259  (*normals_)[(*indices_)[i+5]].normal_z,
260  (*normals_)[(*indices_)[i+6]].normal_z,
261  (*normals_)[(*indices_)[i+7]].normal_z),
262  a_vec, b_vec, c_vec);
263  const __m256 weight_vec = _mm256_mul_ps (normal_distance_weight_vec, _mm256_sub_ps (_mm256_set1_ps (1.0f),
264  _mm256_set_ps ((*normals_)[(*indices_)[i ]].curvature,
265  (*normals_)[(*indices_)[i+1]].curvature,
266  (*normals_)[(*indices_)[i+2]].curvature,
267  (*normals_)[(*indices_)[i+3]].curvature,
268  (*normals_)[(*indices_)[i+4]].curvature,
269  (*normals_)[(*indices_)[i+5]].curvature,
270  (*normals_)[(*indices_)[i+6]].curvature,
271  (*normals_)[(*indices_)[i+7]].curvature)));
272  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)));
273  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
274  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
275  }
276  nr_p += _mm256_extract_epi32 (res, 0);
277  nr_p += _mm256_extract_epi32 (res, 1);
278  nr_p += _mm256_extract_epi32 (res, 2);
279  nr_p += _mm256_extract_epi32 (res, 3);
280  nr_p += _mm256_extract_epi32 (res, 4);
281  nr_p += _mm256_extract_epi32 (res, 5);
282  nr_p += _mm256_extract_epi32 (res, 6);
283  nr_p += _mm256_extract_epi32 (res, 7);
284 
285  // Process the remaining points (at most 7)
286  nr_p += countWithinDistanceStandard(model_coefficients, threshold, i);
287  return (nr_p);
288 }
289 #endif
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
292 template <typename PointT, typename PointNT> void
294  const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
295 {
296  if (!normals_)
297  {
298  PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
299  return;
300  }
301 
302  // Check if the model is valid given the user constraints
303  if (!isModelValid (model_coefficients))
304  {
305  distances.clear ();
306  return;
307  }
308 
309  // Obtain the plane normal
310  Eigen::Vector4f coeff = model_coefficients;
311  coeff[3] = 0.0f;
312 
313  distances.resize (indices_->size ());
314 
315  // Iterate through the 3d points and calculate the distances from them to the plane
316  for (std::size_t i = 0; i < indices_->size (); ++i)
317  {
318  const PointT &pt = (*input_)[(*indices_)[i]];
319  const PointNT &nt = (*normals_)[(*indices_)[i]];
320  // Calculate the distance from the point to the plane normal as the dot product
321  // D = (P-A).N/|N|
322  Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
323  Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
324  double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
325 
326  // Calculate the angular distance between the point normal and the plane normal
327  double d_normal = std::abs (getAngle3D (n, coeff));
328  d_normal = (std::min) (d_normal, M_PI - d_normal);
329 
330  // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
331  double weight = normal_distance_weight_ * (1.0 - nt.curvature);
332 
333  distances[i] = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
334  }
335 }
336 
337 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
338 
339 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
340 
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
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201
A point structure representing Euclidean xyz coordinates, and the RGB color.