38#ifndef PCL_COMMON_IMPL_H_
39#define PCL_COMMON_IMPL_H_
50 double rad =
v1.normalized ().dot (
v2.normalized ());
62 double rad =
v1.normalized ().dot (
v2.normalized ());
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);
94pcl::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)));
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);
113pcl::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)));
133 if (values.size () == 1)
135 mean = values.at (0);
142 for (
const float &value : values)
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);
153template <
typename Po
intT>
inline void
158 indices.resize (cloud.
size ());
164 for (std::size_t i = 0; i < cloud.
size (); ++i)
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))
189 indices[l++] =
int (i);
196template<
typename Po
intT>
inline void
207 for (std::size_t i = 0; i < cloud.
size (); ++i)
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))
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());
243template<
typename Po
intT>
inline void
255 for (std::size_t i = 0; i < indices.size (); ++i)
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))
281 max_idx =
static_cast<int> (i);
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());
294template <
typename Po
intT>
inline void
304template <
typename Po
intT>
inline void
313 for (
const auto& point: cloud.points)
323 for (
const auto& point: cloud.points)
326 if (!std::isfinite (point.x) ||
327 !std::isfinite (point.y) ||
328 !std::isfinite (point.z))
339template <
typename Po
intT>
inline void
347template <
typename Po
intT>
inline void
357 for (
const auto &index : indices)
367 for (
const auto &index : indices)
370 if (!std::isfinite (cloud[index].x) ||
371 !std::isfinite (cloud[index].y) ||
372 !std::isfinite (cloud[index].z))
382template <
typename Po
intT>
inline double
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);
389 double p2p1 = (p2 - p1).norm (),
p3p2 = (p3 - p2).norm (),
p1p3 = (p1 - p3).norm ();
399template <
typename Po
intT>
inline void
405 for (
int i = 0; i <
len; ++i)
413template <
typename Po
intT>
inline float
418 Eigen::Vector3f
va,
vb,res;
420 res(0) = res(1) = res(2) = 0.0f;
426 res +=
va.cross (
vb);
An exception that is thrown when the arguments number or type is wrong/unhandled.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
Define standard C methods and C++ classes that are common to all methods.
Defines all the PCL implemented PointT point type structures.
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.
float calculatePolygonArea(const pcl::PointCloud< PointT > &polygon)
Calculate the area of a polygon given a point cloud that defines the polygon.
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.
void getMeanStd(const std::vector< float > &values, double &mean, double &stddev)
Compute both the mean and the standard deviation of an array of values.
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.
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.
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
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,...
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
A point structure representing Euclidean xyz coordinates, and the RGB color.