Point Cloud Library (PCL)  1.11.1-dev
common.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_COMMON_IMPL_H_
39 #define PCL_COMMON_IMPL_H_
40 
41 #include <pcl/point_types.h>
42 #include <pcl/common/common.h>
43 #include <cfloat> // for FLT_MAX
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////
46 inline double
47 pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree)
48 {
49  // Compute the actual angle
50  double rad = v1.normalized ().dot (v2.normalized ());
51  if (rad < -1.0)
52  rad = -1.0;
53  else if (rad > 1.0)
54  rad = 1.0;
55  return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
56 }
57 
58 inline double
59 pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree)
60 {
61  // Compute the actual angle
62  double rad = v1.normalized ().dot (v2.normalized ());
63  if (rad < -1.0)
64  rad = -1.0;
65  else if (rad > 1.0)
66  rad = 1.0;
67  return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
68 }
69 
70 #ifdef __SSE__
71 inline __m128
72 pcl::acos_SSE (const __m128 &x)
73 {
74  /*
75  This python code generates the coefficients:
76  import math, numpy, scipy.optimize
77  def get_error(S):
78  err_sum=0.0
79  for x in numpy.arange(0.0, 1.0, 0.0025):
80  if (S[3]+S[4]*x)<0.0:
81  err_sum+=10.0
82  else:
83  err_sum+=((S[0]+x*(S[1]+x*S[2]))*numpy.sqrt(S[3]+S[4]*x)+S[5]+x*(S[6]+x*S[7])-math.acos(x))**2.0
84  return err_sum/400.0
85 
86  print(scipy.optimize.minimize(fun=get_error, x0=[1.57, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0], method='Nelder-Mead', options={'maxiter':42000, 'maxfev':42000, 'disp':True, 'xatol':1e-6, 'fatol':1e-6}))
87  */
88  const __m128 mul_term = _mm_add_ps (_mm_set1_ps (1.59121552f), _mm_mul_ps (x, _mm_add_ps (_mm_set1_ps (-0.15461442f), _mm_mul_ps (x, _mm_set1_ps (0.05354897f)))));
89  const __m128 add_term = _mm_add_ps (_mm_set1_ps (0.06681017f), _mm_mul_ps (x, _mm_add_ps (_mm_set1_ps (-0.09402311f), _mm_mul_ps (x, _mm_set1_ps (0.02708663f)))));
90  return _mm_add_ps (_mm_mul_ps (mul_term, _mm_sqrt_ps (_mm_add_ps (_mm_set1_ps (0.89286965f), _mm_mul_ps (_mm_set1_ps (-0.89282669f), x)))), add_term);
91 }
92 
93 inline __m128
94 pcl::getAcuteAngle3DSSE (const __m128 &x1, const __m128 &y1, const __m128 &z1, const __m128 &x2, const __m128 &y2, const __m128 &z2)
95 {
96  const __m128 dot_product = _mm_add_ps (_mm_add_ps (_mm_mul_ps (x1, x2), _mm_mul_ps (y1, y2)), _mm_mul_ps (z1, z2));
97  // The andnot-function realizes an abs-operation: the sign bit is removed
98  // -0.0f (negative zero) means that all bits are 0, only the sign bit is 1
99  return acos_SSE (_mm_min_ps (_mm_set1_ps (1.0f), _mm_andnot_ps (_mm_set1_ps (-0.0f), dot_product)));
100 }
101 #endif // ifdef __SSE__
102 
103 #ifdef __AVX__
104 inline __m256
105 pcl::acos_AVX (const __m256 &x)
106 {
107  const __m256 mul_term = _mm256_add_ps (_mm256_set1_ps (1.59121552f), _mm256_mul_ps (x, _mm256_add_ps (_mm256_set1_ps (-0.15461442f), _mm256_mul_ps (x, _mm256_set1_ps (0.05354897f)))));
108  const __m256 add_term = _mm256_add_ps (_mm256_set1_ps (0.06681017f), _mm256_mul_ps (x, _mm256_add_ps (_mm256_set1_ps (-0.09402311f), _mm256_mul_ps (x, _mm256_set1_ps (0.02708663f)))));
109  return _mm256_add_ps (_mm256_mul_ps (mul_term, _mm256_sqrt_ps (_mm256_add_ps (_mm256_set1_ps (0.89286965f), _mm256_mul_ps (_mm256_set1_ps (-0.89282669f), x)))), add_term);
110 }
111 
112 inline __m256
113 pcl::getAcuteAngle3DAVX (const __m256 &x1, const __m256 &y1, const __m256 &z1, const __m256 &x2, const __m256 &y2, const __m256 &z2)
114 {
115  const __m256 dot_product = _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (x1, x2), _mm256_mul_ps (y1, y2)), _mm256_mul_ps (z1, z2));
116  // The andnot-function realizes an abs-operation: the sign bit is removed
117  // -0.0f (negative zero) means that all bits are 0, only the sign bit is 1
118  return acos_AVX (_mm256_min_ps (_mm256_set1_ps (1.0f), _mm256_andnot_ps (_mm256_set1_ps (-0.0f), dot_product)));
119 }
120 #endif // ifdef __AVX__
121 
122 //////////////////////////////////////////////////////////////////////////////////////////////
123 inline void
124 pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev)
125 {
126  // throw an exception when the input array is empty
127  if (values.empty ())
128  {
129  PCL_THROW_EXCEPTION (BadArgumentException, "Input array must have at least 1 element.");
130  }
131 
132  // when the array has only one element, mean is the number itself and standard dev is 0
133  if (values.size () == 1)
134  {
135  mean = values.at (0);
136  stddev = 0;
137  return;
138  }
139 
140  double sum = 0, sq_sum = 0;
141 
142  for (const float &value : values)
143  {
144  sum += value;
145  sq_sum += value * value;
146  }
147  mean = sum / static_cast<double>(values.size ());
148  double variance = (sq_sum - sum * sum / static_cast<double>(values.size ())) / (static_cast<double>(values.size ()) - 1);
149  stddev = sqrt (variance);
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////////
153 template <typename PointT> inline void
154 pcl::getPointsInBox (const pcl::PointCloud<PointT> &cloud,
155  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
156  Indices &indices)
157 {
158  indices.resize (cloud.size ());
159  int l = 0;
160 
161  // If the data is dense, we don't need to check for NaN
162  if (cloud.is_dense)
163  {
164  for (std::size_t i = 0; i < cloud.size (); ++i)
165  {
166  // Check if the point is inside bounds
167  if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
168  continue;
169  if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
170  continue;
171  indices[l++] = int (i);
172  }
173  }
174  // NaN or Inf values could exist => check for them
175  else
176  {
177  for (std::size_t i = 0; i < cloud.size (); ++i)
178  {
179  // Check if the point is invalid
180  if (!std::isfinite (cloud[i].x) ||
181  !std::isfinite (cloud[i].y) ||
182  !std::isfinite (cloud[i].z))
183  continue;
184  // Check if the point is inside bounds
185  if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
186  continue;
187  if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
188  continue;
189  indices[l++] = int (i);
190  }
191  }
192  indices.resize (l);
193 }
194 
195 //////////////////////////////////////////////////////////////////////////////////////////////
196 template<typename PointT> inline void
197 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
198 {
199  float max_dist = -FLT_MAX;
200  int max_idx = -1;
201  float dist;
202  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
203 
204  // If the data is dense, we don't need to check for NaN
205  if (cloud.is_dense)
206  {
207  for (std::size_t i = 0; i < cloud.size (); ++i)
208  {
209  pcl::Vector3fMapConst pt = cloud[i].getVector3fMap ();
210  dist = (pivot_pt3 - pt).norm ();
211  if (dist > max_dist)
212  {
213  max_idx = int (i);
214  max_dist = dist;
215  }
216  }
217  }
218  // NaN or Inf values could exist => check for them
219  else
220  {
221  for (std::size_t i = 0; i < cloud.size (); ++i)
222  {
223  // Check if the point is invalid
224  if (!std::isfinite (cloud[i].x) || !std::isfinite (cloud[i].y) || !std::isfinite (cloud[i].z))
225  continue;
226  pcl::Vector3fMapConst pt = cloud[i].getVector3fMap ();
227  dist = (pivot_pt3 - pt).norm ();
228  if (dist > max_dist)
229  {
230  max_idx = int (i);
231  max_dist = dist;
232  }
233  }
234  }
235 
236  if(max_idx != -1)
237  max_pt = cloud[max_idx].getVector4fMap ();
238  else
239  max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
240 }
241 
242 //////////////////////////////////////////////////////////////////////////////////////////////
243 template<typename PointT> inline void
244 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
245  const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
246 {
247  float max_dist = -FLT_MAX;
248  int max_idx = -1;
249  float dist;
250  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
251 
252  // If the data is dense, we don't need to check for NaN
253  if (cloud.is_dense)
254  {
255  for (std::size_t i = 0; i < indices.size (); ++i)
256  {
257  pcl::Vector3fMapConst pt = cloud[indices[i]].getVector3fMap ();
258  dist = (pivot_pt3 - pt).norm ();
259  if (dist > max_dist)
260  {
261  max_idx = static_cast<int> (i);
262  max_dist = dist;
263  }
264  }
265  }
266  // NaN or Inf values could exist => check for them
267  else
268  {
269  for (std::size_t i = 0; i < indices.size (); ++i)
270  {
271  // Check if the point is invalid
272  if (!std::isfinite (cloud[indices[i]].x) || !std::isfinite (cloud[indices[i]].y)
273  ||
274  !std::isfinite (cloud[indices[i]].z))
275  continue;
276 
277  pcl::Vector3fMapConst pt = cloud[indices[i]].getVector3fMap ();
278  dist = (pivot_pt3 - pt).norm ();
279  if (dist > max_dist)
280  {
281  max_idx = static_cast<int> (i);
282  max_dist = dist;
283  }
284  }
285  }
286 
287  if(max_idx != -1)
288  max_pt = cloud[indices[max_idx]].getVector4fMap ();
289  else
290  max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
291 }
292 
293 //////////////////////////////////////////////////////////////////////////////////////////////
294 template <typename PointT> inline void
295 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt)
296 {
297  Eigen::Vector4f min_p, max_p;
298  pcl::getMinMax3D (cloud, min_p, max_p);
299  min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
300  max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
301 }
302 
303 //////////////////////////////////////////////////////////////////////////////////////////////
304 template <typename PointT> inline void
305 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
306 {
307  min_pt.setConstant (FLT_MAX);
308  max_pt.setConstant (-FLT_MAX);
309 
310  // If the data is dense, we don't need to check for NaN
311  if (cloud.is_dense)
312  {
313  for (const auto& point: cloud.points)
314  {
315  const pcl::Vector4fMapConst pt = point.getVector4fMap ();
316  min_pt = min_pt.cwiseMin (pt);
317  max_pt = max_pt.cwiseMax (pt);
318  }
319  }
320  // NaN or Inf values could exist => check for them
321  else
322  {
323  for (const auto& point: cloud.points)
324  {
325  // Check if the point is invalid
326  if (!std::isfinite (point.x) ||
327  !std::isfinite (point.y) ||
328  !std::isfinite (point.z))
329  continue;
330  const pcl::Vector4fMapConst pt = point.getVector4fMap ();
331  min_pt = min_pt.cwiseMin (pt);
332  max_pt = max_pt.cwiseMax (pt);
333  }
334  }
335 }
336 
337 
338 //////////////////////////////////////////////////////////////////////////////////////////////
339 template <typename PointT> inline void
340 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices,
341  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
342 {
343  pcl::getMinMax3D (cloud, indices.indices, min_pt, max_pt);
344 }
345 
346 //////////////////////////////////////////////////////////////////////////////////////////////
347 template <typename PointT> inline void
348 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
349  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
350 {
351  min_pt.setConstant (FLT_MAX);
352  max_pt.setConstant (-FLT_MAX);
353 
354  // If the data is dense, we don't need to check for NaN
355  if (cloud.is_dense)
356  {
357  for (const auto &index : indices)
358  {
359  const pcl::Vector4fMapConst pt = cloud[index].getVector4fMap ();
360  min_pt = min_pt.cwiseMin (pt);
361  max_pt = max_pt.cwiseMax (pt);
362  }
363  }
364  // NaN or Inf values could exist => check for them
365  else
366  {
367  for (const auto &index : indices)
368  {
369  // Check if the point is invalid
370  if (!std::isfinite (cloud[index].x) ||
371  !std::isfinite (cloud[index].y) ||
372  !std::isfinite (cloud[index].z))
373  continue;
374  const pcl::Vector4fMapConst pt = cloud[index].getVector4fMap ();
375  min_pt = min_pt.cwiseMin (pt);
376  max_pt = max_pt.cwiseMax (pt);
377  }
378  }
379 }
380 
381 //////////////////////////////////////////////////////////////////////////////////////////////
382 template <typename PointT> inline double
383 pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc)
384 {
385  Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0);
386  Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0);
387  Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0);
388 
389  double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm ();
390  // Calculate the area of the triangle using Heron's formula
391  // (http://en.wikipedia.org/wiki/Heron's_formula)
392  double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
393  double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
394  // Compute the radius of the circumscribed circle
395  return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
396 }
397 
398 //////////////////////////////////////////////////////////////////////////////////////////////
399 template <typename PointT> inline void
400 pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p)
401 {
402  min_p = FLT_MAX;
403  max_p = -FLT_MAX;
404 
405  for (int i = 0; i < len; ++i)
406  {
407  min_p = (histogram[i] > min_p) ? min_p : histogram[i];
408  max_p = (histogram[i] < max_p) ? max_p : histogram[i];
409  }
410 }
411 
412 //////////////////////////////////////////////////////////////////////////////////////////////
413 template <typename PointT> inline float
414 pcl::calculatePolygonArea (const pcl::PointCloud<PointT> &polygon)
415 {
416  float area = 0.0f;
417  int num_points = polygon.size ();
418  Eigen::Vector3f va,vb,res;
419 
420  res(0) = res(1) = res(2) = 0.0f;
421  for (int i = 0; i < num_points; ++i)
422  {
423  int j = (i + 1) % num_points;
424  va = polygon[i].getVector3fMap ();
425  vb = polygon[j].getVector3fMap ();
426  res += va.cross (vb);
427  }
428  area = res.norm ();
429  return (area*0.5);
430 }
431 
432 #endif //#ifndef PCL_COMMON_IMPL_H_
433 
point_types.h
common.h
pcl::getPointsInBox
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, Indices &indices)
Get a set of points residing in a box given its bounds.
Definition: common.hpp:154
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:389
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::getMeanStd
void getMeanStd(const std::vector< float > &values, double &mean, double &stddev)
Compute both the mean and the standard deviation of an array of values.
Definition: common.hpp:124
pcl::getCircumcircleRadius
double getCircumcircleRadius(const PointT &pa, const PointT &pb, const PointT &pc)
Compute the radius of a circumscribed circle for a triangle formed of three points pa,...
Definition: common.hpp:383
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:630
pcl::getAngle3D
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
pcl::calculatePolygonArea
float calculatePolygonArea(const pcl::PointCloud< PointT > &polygon)
Calculate the area of a polygon given a point cloud that defines the polygon.
Definition: common.hpp:414
pcl::getMaxDistance
void getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
Definition: common.hpp:197
M_PI
#define M_PI
Definition: pcl_macros.h:201
pcl::BadArgumentException
An exception that is thrown when the arguments number or type is wrong/unhandled.
Definition: exceptions.h:255
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:397
pcl::PointIndices
Definition: PointIndices.h:11
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::Vector3fMapConst
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
Definition: point_types.hpp:183
pcl::getMinMax3D
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition: common.hpp:295
pcl::getMinMax
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
Definition: common.hpp:400
pcl::Vector4fMapConst
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
Definition: point_types.hpp:185