38 #ifndef PCL_COMMON_IMPL_H_
39 #define PCL_COMMON_IMPL_H_
41 #include <pcl/point_types.h>
42 #include <pcl/common/common.h>
46 pcl::getAngle3D (
const Eigen::Vector4f &v1,
const Eigen::Vector4f &v2,
const bool in_degree)
49 double rad = v1.normalized ().dot (v2.normalized ());
54 return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
58 pcl::getAngle3D (
const Eigen::Vector3f &v1,
const Eigen::Vector3f &v2,
const bool in_degree)
61 double rad = v1.normalized ().dot (v2.normalized ());
66 return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
80 if (values.size () == 1)
87 double sum = 0, sq_sum = 0;
89 for (
const float &value : values)
92 sq_sum += value * value;
94 mean = sum /
static_cast<double>(values.size ());
95 double variance = (sq_sum - sum * sum /
static_cast<double>(values.size ())) / (
static_cast<double>(values.size ()) - 1);
96 stddev = sqrt (variance);
100 template <
typename Po
intT>
inline void
102 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
103 std::vector<int> &indices)
105 indices.resize (cloud.
points.size ());
111 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
114 if (cloud.
points[i].x < min_pt[0] || cloud.
points[i].y < min_pt[1] || cloud.
points[i].z < min_pt[2])
116 if (cloud.
points[i].x > max_pt[0] || cloud.
points[i].y > max_pt[1] || cloud.
points[i].z > max_pt[2])
118 indices[l++] = int (i);
124 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
127 if (!std::isfinite (cloud.
points[i].x) ||
128 !std::isfinite (cloud.
points[i].y) ||
129 !std::isfinite (cloud.
points[i].z))
132 if (cloud.
points[i].x < min_pt[0] || cloud.
points[i].y < min_pt[1] || cloud.
points[i].z < min_pt[2])
134 if (cloud.
points[i].x > max_pt[0] || cloud.
points[i].y > max_pt[1] || cloud.
points[i].z > max_pt[2])
136 indices[l++] = int (i);
143 template<
typename Po
intT>
inline void
146 float max_dist = -FLT_MAX;
149 const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
154 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
157 dist = (pivot_pt3 - pt).norm ();
168 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
171 if (!std::isfinite (cloud.
points[i].x) || !std::isfinite (cloud.
points[i].y) || !std::isfinite (cloud.
points[i].z))
174 dist = (pivot_pt3 - pt).norm ();
184 max_pt = cloud.
points[max_idx].getVector4fMap ();
186 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());
190 template<
typename Po
intT>
inline void
192 const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
194 float max_dist = -FLT_MAX;
197 const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
202 for (std::size_t i = 0; i < indices.size (); ++i)
205 dist = (pivot_pt3 - pt).norm ();
208 max_idx =
static_cast<int> (i);
216 for (std::size_t i = 0; i < indices.size (); ++i)
219 if (!std::isfinite (cloud.
points[indices[i]].x) || !std::isfinite (cloud.
points[indices[i]].y)
221 !std::isfinite (cloud.
points[indices[i]].z))
225 dist = (pivot_pt3 - pt).norm ();
228 max_idx =
static_cast<int> (i);
235 max_pt = cloud.
points[indices[max_idx]].getVector4fMap ();
237 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());
241 template <
typename Po
intT>
inline void
244 Eigen::Array4f min_p, max_p;
245 min_p.setConstant (FLT_MAX);
246 max_p.setConstant (-FLT_MAX);
251 for (
const auto& point: cloud.
points)
253 const auto pt = point.getArray4fMap ();
254 min_p = min_p.min (pt);
255 max_p = max_p.max (pt);
261 for (
const auto& point: cloud.
points)
264 if (!std::isfinite (point.x) ||
265 !std::isfinite (point.y) ||
266 !std::isfinite (point.z))
268 const auto pt = point.getArray4fMap ();
269 min_p = min_p.min (pt);
270 max_p = max_p.max (pt);
273 min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
274 max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
278 template <
typename Po
intT>
inline void
281 Eigen::Array4f min_p, max_p;
282 min_p.setConstant (FLT_MAX);
283 max_p.setConstant (-FLT_MAX);
288 for (
const auto& point: cloud.
points)
290 const auto pt = point.getArray4fMap ();
291 min_p = min_p.min (pt);
292 max_p = max_p.max (pt);
298 for (
const auto& point: cloud.
points)
301 if (!std::isfinite (point.x) ||
302 !std::isfinite (point.y) ||
303 !std::isfinite (point.z))
305 const auto pt = point.getArray4fMap ();
306 min_p = min_p.min (pt);
307 max_p = max_p.max (pt);
316 template <
typename Po
intT>
inline void
318 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
320 Eigen::Array4f min_p, max_p;
321 min_p.setConstant (FLT_MAX);
322 max_p.setConstant (-FLT_MAX);
327 for (
const int &index : indices.
indices)
330 min_p = min_p.min (pt);
331 max_p = max_p.max (pt);
337 for (
const int &index : indices.
indices)
340 if (!std::isfinite (cloud.
points[index].x) ||
341 !std::isfinite (cloud.
points[index].y) ||
342 !std::isfinite (cloud.
points[index].z))
345 min_p = min_p.min (pt);
346 max_p = max_p.max (pt);
354 template <
typename Po
intT>
inline void
356 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
358 min_pt.setConstant (FLT_MAX);
359 max_pt.setConstant (-FLT_MAX);
364 for (
const int &index : indices)
367 min_pt = min_pt.array ().min (pt);
368 max_pt = max_pt.array ().max (pt);
374 for (
const int &index : indices)
377 if (!std::isfinite (cloud.
points[index].x) ||
378 !std::isfinite (cloud.
points[index].y) ||
379 !std::isfinite (cloud.
points[index].z))
382 min_pt = min_pt.array ().min (pt);
383 max_pt = max_pt.array ().max (pt);
389 template <
typename Po
intT>
inline double
392 Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0);
393 Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0);
394 Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0);
396 double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm ();
399 double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
400 double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
402 return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
406 template <
typename Po
intT>
inline void
412 for (
int i = 0; i < len; ++i)
414 min_p = (histogram[i] > min_p) ? min_p : histogram[i];
415 max_p = (histogram[i] < max_p) ? max_p : histogram[i];
420 template <
typename Po
intT>
inline float
424 int num_points = polygon.
size ();
425 Eigen::Vector3f va,vb,res;
427 res(0) = res(1) = res(2) = 0.0f;
428 for (
int i = 0; i < num_points; ++i)
430 int j = (i + 1) % num_points;
431 va = polygon[i].getVector3fMap ();
432 vb = polygon[j].getVector3fMap ();
433 res += va.cross (vb);
439 #endif //#ifndef PCL_COMMON_IMPL_H_
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, std::vector< int > &indices)
Get a set of points residing in a box given its bounds.
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. ...
An exception that is thrown when the arguments number or type is wrong/unhandled. ...
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void getMeanStd(const std::vector< float > &values, double &mean, double &stddev)
Compute both the mean and the standard deviation of an array of values.
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...
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
PointCloud represents the base class in PCL for storing collections of 3D points. ...
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
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...
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...