38 #ifndef PCL_COMMON_IMPL_H_
39 #define PCL_COMMON_IMPL_H_
47 pcl::getAngle3D (
const Eigen::Vector4f &v1,
const Eigen::Vector4f &v2,
const bool in_degree)
50 double rad = v1.normalized ().dot (v2.normalized ());
55 return (in_degree ? std::acos (rad) * 180.0 /
M_PI : std::acos (rad));
59 pcl::getAngle3D (
const Eigen::Vector3f &v1,
const Eigen::Vector3f &v2,
const bool in_degree)
62 double rad = v1.normalized ().dot (v2.normalized ());
67 return (in_degree ? std::acos (rad) * 180.0 /
M_PI : std::acos (rad));
72 pcl::acos_SSE (
const __m128 &x)
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);
94 pcl::getAcuteAngle3DSSE (
const __m128 &x1,
const __m128 &y1,
const __m128 &z1,
const __m128 &x2,
const __m128 &y2,
const __m128 &z2)
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));
99 return acos_SSE (_mm_min_ps (_mm_set1_ps (1.0f), _mm_andnot_ps (_mm_set1_ps (-0.0f), dot_product)));
101 #endif // ifdef __SSE__
105 pcl::acos_AVX (
const __m256 &x)
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);
113 pcl::getAcuteAngle3DAVX (
const __m256 &x1,
const __m256 &y1,
const __m256 &z1,
const __m256 &x2,
const __m256 &y2,
const __m256 &z2)
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));
118 return acos_AVX (_mm256_min_ps (_mm256_set1_ps (1.0f), _mm256_andnot_ps (_mm256_set1_ps (-0.0f), dot_product)));
120 #endif // ifdef __AVX__
133 if (values.size () == 1)
135 mean = values.at (0);
140 double sum = 0, sq_sum = 0;
142 for (
const float &value : values)
145 sq_sum += value * value;
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);
153 template <
typename Po
intT>
inline void
155 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
158 indices.resize (cloud.
size ());
164 for (std::size_t i = 0; i < cloud.
size (); ++i)
167 if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
169 if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
171 indices[l++] = int (i);
177 for (std::size_t i = 0; i < cloud.
size (); ++i)
180 if (!std::isfinite (cloud[i].x) ||
181 !std::isfinite (cloud[i].y) ||
182 !std::isfinite (cloud[i].z))
185 if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
187 if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
189 indices[l++] = int (i);
196 template<
typename Po
intT>
inline void
199 float max_dist = -FLT_MAX;
202 const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
207 for (std::size_t i = 0; i < cloud.
size (); ++i)
210 dist = (pivot_pt3 - pt).norm ();
221 for (std::size_t i = 0; i < cloud.
size (); ++i)
224 if (!std::isfinite (cloud[i].x) || !std::isfinite (cloud[i].y) || !std::isfinite (cloud[i].z))
227 dist = (pivot_pt3 - pt).norm ();
237 max_pt = cloud[max_idx].getVector4fMap ();
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());
243 template<
typename Po
intT>
inline void
245 const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
247 float max_dist = -FLT_MAX;
250 const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
255 for (std::size_t i = 0; i < indices.size (); ++i)
258 dist = (pivot_pt3 - pt).norm ();
261 max_idx =
static_cast<int> (i);
269 for (std::size_t i = 0; i < indices.size (); ++i)
272 if (!std::isfinite (cloud[indices[i]].x) || !std::isfinite (cloud[indices[i]].y)
274 !std::isfinite (cloud[indices[i]].z))
278 dist = (pivot_pt3 - pt).norm ();
281 max_idx =
static_cast<int> (i);
288 max_pt = cloud[indices[max_idx]].getVector4fMap ();
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());
294 template <
typename Po
intT>
inline void
297 Eigen::Array4f min_p, max_p;
298 min_p.setConstant (FLT_MAX);
299 max_p.setConstant (-FLT_MAX);
304 for (
const auto& point: cloud.
points)
306 const auto pt = point.getArray4fMap ();
307 min_p = min_p.min (pt);
308 max_p = max_p.max (pt);
314 for (
const auto& point: cloud.
points)
317 if (!std::isfinite (point.x) ||
318 !std::isfinite (point.y) ||
319 !std::isfinite (point.z))
321 const auto pt = point.getArray4fMap ();
322 min_p = min_p.min (pt);
323 max_p = max_p.max (pt);
326 min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
327 max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
331 template <
typename Po
intT>
inline void
334 Eigen::Array4f min_p, max_p;
335 min_p.setConstant (FLT_MAX);
336 max_p.setConstant (-FLT_MAX);
341 for (
const auto& point: cloud.
points)
343 const auto pt = point.getArray4fMap ();
344 min_p = min_p.min (pt);
345 max_p = max_p.max (pt);
351 for (
const auto& point: cloud.
points)
354 if (!std::isfinite (point.x) ||
355 !std::isfinite (point.y) ||
356 !std::isfinite (point.z))
358 const auto pt = point.getArray4fMap ();
359 min_p = min_p.min (pt);
360 max_p = max_p.max (pt);
369 template <
typename Po
intT>
inline void
371 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
373 Eigen::Array4f min_p, max_p;
374 min_p.setConstant (FLT_MAX);
375 max_p.setConstant (-FLT_MAX);
380 for (
const auto &index : indices.
indices)
383 min_p = min_p.min (pt);
384 max_p = max_p.max (pt);
390 for (
const auto &index : indices.
indices)
393 if (!std::isfinite (cloud[index].x) ||
394 !std::isfinite (cloud[index].y) ||
395 !std::isfinite (cloud[index].z))
398 min_p = min_p.min (pt);
399 max_p = max_p.max (pt);
407 template <
typename Po
intT>
inline void
409 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
411 min_pt.setConstant (FLT_MAX);
412 max_pt.setConstant (-FLT_MAX);
417 for (
const auto &index : indices)
420 min_pt = min_pt.array ().min (pt);
421 max_pt = max_pt.array ().max (pt);
427 for (
const auto &index : indices)
430 if (!std::isfinite (cloud[index].x) ||
431 !std::isfinite (cloud[index].y) ||
432 !std::isfinite (cloud[index].z))
435 min_pt = min_pt.array ().min (pt);
436 max_pt = max_pt.array ().max (pt);
442 template <
typename Po
intT>
inline double
445 Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0);
446 Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0);
447 Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0);
449 double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm ();
452 double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
453 double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
455 return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
459 template <
typename Po
intT>
inline void
465 for (
int i = 0; i < len; ++i)
467 min_p = (histogram[i] > min_p) ? min_p : histogram[i];
468 max_p = (histogram[i] < max_p) ? max_p : histogram[i];
473 template <
typename Po
intT>
inline float
477 int num_points = polygon.
size ();
478 Eigen::Vector3f va,vb,res;
480 res(0) = res(1) = res(2) = 0.0f;
481 for (
int i = 0; i < num_points; ++i)
483 int j = (i + 1) % num_points;
484 va = polygon[i].getVector3fMap ();
485 vb = polygon[j].getVector3fMap ();
486 res += va.cross (vb);
492 #endif //#ifndef PCL_COMMON_IMPL_H_