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
getAngle3D(const Eigen::Vector4f & v1,const Eigen::Vector4f & v2,const bool in_degree)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
getAngle3D(const Eigen::Vector3f & v1,const Eigen::Vector3f & v2,const bool in_degree)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
acos_SSE(const __m128 & x)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
getAcuteAngle3DSSE(const __m128 & x1,const __m128 & y1,const __m128 & z1,const __m128 & x2,const __m128 & y2,const __m128 & z2)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
acos_AVX(const __m256 & x)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
getAcuteAngle3DAVX(const __m256 & x1,const __m256 & y1,const __m256 & z1,const __m256 & x2,const __m256 & y2,const __m256 & z2)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
getMeanStd(const std::vector<float> & values,double & mean,double & stddev)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
getPointsInBox(const pcl::PointCloud<PointT> & cloud,Eigen::Vector4f & min_pt,Eigen::Vector4f & max_pt,Indices & indices)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
getMaxDistance(const pcl::PointCloud<PointT> & cloud,const Eigen::Vector4f & pivot_pt,Eigen::Vector4f & max_pt)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
getMaxDistance(const pcl::PointCloud<PointT> & cloud,const Indices & indices,const Eigen::Vector4f & pivot_pt,Eigen::Vector4f & max_pt)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
getMinMax3D(const pcl::PointCloud<PointT> & cloud,PointT & min_pt,PointT & max_pt)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
getMinMax3D(const pcl::PointCloud<PointT> & cloud,Eigen::Vector4f & min_pt,Eigen::Vector4f & max_pt)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
getMinMax3D(const pcl::PointCloud<PointT> & cloud,const pcl::PointIndices & indices,Eigen::Vector4f & min_pt,Eigen::Vector4f & max_pt)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
getMinMax3D(const pcl::PointCloud<PointT> & cloud,const Indices & indices,Eigen::Vector4f & min_pt,Eigen::Vector4f & max_pt)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
getCircumcircleRadius(const PointT & pa,const PointT & pb,const PointT & pc)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
getMinMax(const PointT & histogram,int len,float & min_p,float & max_p)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
calculatePolygonArea(const pcl::PointCloud<PointT> & polygon)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
434