38 #ifndef PCL_COMMON_IMPL_H_
39 #define PCL_COMMON_IMPL_H_
41 #include <pcl/point_types.h>
42 #include <pcl/common/common.h>
49 double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ());
50 if (rad < -1.0) rad = -1.0;
51 if (rad > 1.0) rad = 1.0;
59 double sum = 0, sq_sum = 0;
61 for (
size_t i = 0; i < values.size (); ++i)
64 sq_sum += values[i] * values[i];
66 mean = sum /
static_cast<double>(values.size ());
67 double variance = (sq_sum - sum * sum /
static_cast<double>(values.size ())) / (
static_cast<double>(values.size ()) - 1);
68 stddev = sqrt (variance);
72 template <
typename Po
intT>
inline void
74 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
75 std::vector<int> &indices)
77 indices.resize (cloud.
points.size ());
83 for (
size_t i = 0; i < cloud.
points.size (); ++i)
86 if (cloud.
points[i].x < min_pt[0] || cloud.
points[i].y < min_pt[1] || cloud.
points[i].z < min_pt[2])
88 if (cloud.
points[i].x > max_pt[0] || cloud.
points[i].y > max_pt[1] || cloud.
points[i].z > max_pt[2])
90 indices[l++] = int (i);
96 for (
size_t i = 0; i < cloud.
points.size (); ++i)
99 if (!pcl_isfinite (cloud.
points[i].x) ||
100 !pcl_isfinite (cloud.
points[i].y) ||
101 !pcl_isfinite (cloud.
points[i].z))
104 if (cloud.
points[i].x < min_pt[0] || cloud.
points[i].y < min_pt[1] || cloud.
points[i].z < min_pt[2])
106 if (cloud.
points[i].x > max_pt[0] || cloud.
points[i].y > max_pt[1] || cloud.
points[i].z > max_pt[2])
108 indices[l++] = int (i);
115 template<
typename Po
intT>
inline void
118 float max_dist = -FLT_MAX;
125 for (
size_t i = 0; i < cloud.
points.size (); ++i)
128 dist = (pivot_pt - pt).norm ();
139 for (
size_t i = 0; i < cloud.
points.size (); ++i)
142 if (!pcl_isfinite (cloud.
points[i].x) || !pcl_isfinite (cloud.
points[i].y) || !pcl_isfinite (cloud.
points[i].z))
145 dist = (pivot_pt - pt).norm ();
155 max_pt = cloud.
points[max_idx].getVector4fMap ();
157 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());
161 template<
typename Po
intT>
inline void
163 const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
165 float max_dist = -FLT_MAX;
172 for (
size_t i = 0; i < indices.size (); ++i)
175 dist = (pivot_pt - pt).norm ();
178 max_idx =
static_cast<int> (i);
186 for (
size_t i = 0; i < indices.size (); ++i)
189 if (!pcl_isfinite (cloud.
points[indices[i]].x) || !pcl_isfinite (cloud.
points[indices[i]].y)
191 !pcl_isfinite (cloud.
points[indices[i]].z))
195 dist = (pivot_pt - pt).norm ();
198 max_idx =
static_cast<int> (i);
205 max_pt = cloud.
points[max_idx].getVector4fMap ();
207 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());
211 template <
typename Po
intT>
inline void
214 Eigen::Array4f min_p, max_p;
215 min_p.setConstant (FLT_MAX);
216 max_p.setConstant (-FLT_MAX);
221 for (
size_t i = 0; i < cloud.
points.size (); ++i)
224 min_p = min_p.min (pt);
225 max_p = max_p.max (pt);
231 for (
size_t i = 0; i < cloud.
points.size (); ++i)
234 if (!pcl_isfinite (cloud.
points[i].x) ||
235 !pcl_isfinite (cloud.
points[i].y) ||
236 !pcl_isfinite (cloud.
points[i].z))
239 min_p = min_p.min (pt);
240 max_p = max_p.max (pt);
243 min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
244 max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
248 template <
typename Po
intT>
inline void
251 Eigen::Array4f min_p, max_p;
252 min_p.setConstant (FLT_MAX);
253 max_p.setConstant (-FLT_MAX);
258 for (
size_t i = 0; i < cloud.
points.size (); ++i)
261 min_p = min_p.min (pt);
262 max_p = max_p.max (pt);
268 for (
size_t i = 0; i < cloud.
points.size (); ++i)
271 if (!pcl_isfinite (cloud.
points[i].x) ||
272 !pcl_isfinite (cloud.
points[i].y) ||
273 !pcl_isfinite (cloud.
points[i].z))
276 min_p = min_p.min (pt);
277 max_p = max_p.max (pt);
286 template <
typename Po
intT>
inline void
288 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
290 Eigen::Array4f min_p, max_p;
291 min_p.setConstant (FLT_MAX);
292 max_p.setConstant (-FLT_MAX);
297 for (
size_t i = 0; i < indices.
indices.size (); ++i)
300 min_p = min_p.min (pt);
301 max_p = max_p.max (pt);
307 for (
size_t i = 0; i < indices.
indices.size (); ++i)
315 min_p = min_p.min (pt);
316 max_p = max_p.max (pt);
324 template <
typename Po
intT>
inline void
326 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
328 min_pt.setConstant (FLT_MAX);
329 max_pt.setConstant (-FLT_MAX);
334 for (
size_t i = 0; i < indices.size (); ++i)
337 min_pt = min_pt.array ().min (pt);
338 max_pt = max_pt.array ().max (pt);
344 for (
size_t i = 0; i < indices.size (); ++i)
347 if (!pcl_isfinite (cloud.
points[indices[i]].x) ||
348 !pcl_isfinite (cloud.
points[indices[i]].y) ||
349 !pcl_isfinite (cloud.
points[indices[i]].z))
352 min_pt = min_pt.array ().min (pt);
353 max_pt = max_pt.array ().max (pt);
359 template <
typename Po
intT>
inline double
362 Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0);
363 Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0);
364 Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0);
366 double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm ();
369 double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
370 double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
372 return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
376 template <
typename Po
intT>
inline void
382 for (
int i = 0; i < len; ++i)
384 min_p = (histogram[i] > min_p) ? min_p : histogram[i];
385 max_p = (histogram[i] < max_p) ? max_p : histogram[i];
390 template <
typename Po
intT>
inline float
394 int num_points = polygon.
size ();
396 Eigen::Vector3f va,vb,res;
398 res(0) = res(1) = res(2) = 0.0f;
399 for (
int i = 0; i < num_points; ++i)
401 j = (i + 1) % num_points;
402 va = polygon[i].getVector3fMap ();
403 vb = polygon[j].getVector3fMap ();
404 res += va.cross (vb);
410 #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)
Compute the smallest angle between two vectors in the [ 0, PI ) interval in 3D.
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
std::vector< int > indices
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.
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
float calculatePolygonArea(const pcl::PointCloud< PointT > &polygon)
Calculate the area of a polygon given a point cloud that defines the polygon.
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
A point structure representing Euclidean xyz coordinates, and the RGB color.