Point Cloud Library (PCL)  1.11.1
common.hpp
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
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
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 //////////////////////////////////////////////////////////////////////////////////////////////
71 inline void
72 pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev)
73 {
74  // throw an exception when the input array is empty
75  if (values.empty ())
76  {
77  PCL_THROW_EXCEPTION (BadArgumentException, "Input array must have at least 1 element.");
78  }
79 
80  // when the array has only one element, mean is the number itself and standard dev is 0
81  if (values.size () == 1)
82  {
83  mean = values.at (0);
84  stddev = 0;
85  return;
86  }
87 
88  double sum = 0, sq_sum = 0;
89 
90  for (const float &value : values)
91  {
92  sum += value;
93  sq_sum += value * value;
94  }
95  mean = sum / static_cast<double>(values.size ());
96  double variance = (sq_sum - sum * sum / static_cast<double>(values.size ())) / (static_cast<double>(values.size ()) - 1);
97  stddev = sqrt (variance);
98 }
99 
100 //////////////////////////////////////////////////////////////////////////////////////////////
101 template <typename PointT> inline void
103  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
104  Indices &indices)
105 {
106  indices.resize (cloud.size ());
107  int l = 0;
108 
109  // If the data is dense, we don't need to check for NaN
110  if (cloud.is_dense)
111  {
112  for (std::size_t i = 0; i < cloud.size (); ++i)
113  {
114  // Check if the point is inside bounds
115  if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
116  continue;
117  if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
118  continue;
119  indices[l++] = int (i);
120  }
121  }
122  // NaN or Inf values could exist => check for them
123  else
124  {
125  for (std::size_t i = 0; i < cloud.size (); ++i)
126  {
127  // Check if the point is invalid
128  if (!std::isfinite (cloud[i].x) ||
129  !std::isfinite (cloud[i].y) ||
130  !std::isfinite (cloud[i].z))
131  continue;
132  // Check if the point is inside bounds
133  if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
134  continue;
135  if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
136  continue;
137  indices[l++] = int (i);
138  }
139  }
140  indices.resize (l);
141 }
142 
143 //////////////////////////////////////////////////////////////////////////////////////////////
144 template<typename PointT> inline void
145 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
146 {
147  float max_dist = -FLT_MAX;
148  int max_idx = -1;
149  float dist;
150  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
151 
152  // If the data is dense, we don't need to check for NaN
153  if (cloud.is_dense)
154  {
155  for (std::size_t i = 0; i < cloud.size (); ++i)
156  {
157  pcl::Vector3fMapConst pt = cloud[i].getVector3fMap ();
158  dist = (pivot_pt3 - pt).norm ();
159  if (dist > max_dist)
160  {
161  max_idx = int (i);
162  max_dist = dist;
163  }
164  }
165  }
166  // NaN or Inf values could exist => check for them
167  else
168  {
169  for (std::size_t i = 0; i < cloud.size (); ++i)
170  {
171  // Check if the point is invalid
172  if (!std::isfinite (cloud[i].x) || !std::isfinite (cloud[i].y) || !std::isfinite (cloud[i].z))
173  continue;
174  pcl::Vector3fMapConst pt = cloud[i].getVector3fMap ();
175  dist = (pivot_pt3 - pt).norm ();
176  if (dist > max_dist)
177  {
178  max_idx = int (i);
179  max_dist = dist;
180  }
181  }
182  }
183 
184  if(max_idx != -1)
185  max_pt = cloud[max_idx].getVector4fMap ();
186  else
187  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());
188 }
189 
190 //////////////////////////////////////////////////////////////////////////////////////////////
191 template<typename PointT> inline void
193  const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
194 {
195  float max_dist = -FLT_MAX;
196  int max_idx = -1;
197  float dist;
198  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
199 
200  // If the data is dense, we don't need to check for NaN
201  if (cloud.is_dense)
202  {
203  for (std::size_t i = 0; i < indices.size (); ++i)
204  {
205  pcl::Vector3fMapConst pt = cloud[indices[i]].getVector3fMap ();
206  dist = (pivot_pt3 - pt).norm ();
207  if (dist > max_dist)
208  {
209  max_idx = static_cast<int> (i);
210  max_dist = dist;
211  }
212  }
213  }
214  // NaN or Inf values could exist => check for them
215  else
216  {
217  for (std::size_t i = 0; i < indices.size (); ++i)
218  {
219  // Check if the point is invalid
220  if (!std::isfinite (cloud[indices[i]].x) || !std::isfinite (cloud[indices[i]].y)
221  ||
222  !std::isfinite (cloud[indices[i]].z))
223  continue;
224 
225  pcl::Vector3fMapConst pt = cloud[indices[i]].getVector3fMap ();
226  dist = (pivot_pt3 - pt).norm ();
227  if (dist > max_dist)
228  {
229  max_idx = static_cast<int> (i);
230  max_dist = dist;
231  }
232  }
233  }
234 
235  if(max_idx != -1)
236  max_pt = cloud[indices[max_idx]].getVector4fMap ();
237  else
238  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());
239 }
240 
241 //////////////////////////////////////////////////////////////////////////////////////////////
242 template <typename PointT> inline void
243 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt)
244 {
245  Eigen::Array4f min_p, max_p;
246  min_p.setConstant (FLT_MAX);
247  max_p.setConstant (-FLT_MAX);
248 
249  // If the data is dense, we don't need to check for NaN
250  if (cloud.is_dense)
251  {
252  for (const auto& point: cloud.points)
253  {
254  const auto pt = point.getArray4fMap ();
255  min_p = min_p.min (pt);
256  max_p = max_p.max (pt);
257  }
258  }
259  // NaN or Inf values could exist => check for them
260  else
261  {
262  for (const auto& point: cloud.points)
263  {
264  // Check if the point is invalid
265  if (!std::isfinite (point.x) ||
266  !std::isfinite (point.y) ||
267  !std::isfinite (point.z))
268  continue;
269  const auto pt = point.getArray4fMap ();
270  min_p = min_p.min (pt);
271  max_p = max_p.max (pt);
272  }
273  }
274  min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
275  max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
276 }
277 
278 //////////////////////////////////////////////////////////////////////////////////////////////
279 template <typename PointT> inline void
280 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
281 {
282  Eigen::Array4f min_p, max_p;
283  min_p.setConstant (FLT_MAX);
284  max_p.setConstant (-FLT_MAX);
285 
286  // If the data is dense, we don't need to check for NaN
287  if (cloud.is_dense)
288  {
289  for (const auto& point: cloud.points)
290  {
291  const auto pt = point.getArray4fMap ();
292  min_p = min_p.min (pt);
293  max_p = max_p.max (pt);
294  }
295  }
296  // NaN or Inf values could exist => check for them
297  else
298  {
299  for (const auto& point: cloud.points)
300  {
301  // Check if the point is invalid
302  if (!std::isfinite (point.x) ||
303  !std::isfinite (point.y) ||
304  !std::isfinite (point.z))
305  continue;
306  const auto pt = point.getArray4fMap ();
307  min_p = min_p.min (pt);
308  max_p = max_p.max (pt);
309  }
310  }
311  min_pt = min_p;
312  max_pt = max_p;
313 }
314 
315 
316 //////////////////////////////////////////////////////////////////////////////////////////////
317 template <typename PointT> inline void
319  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
320 {
321  Eigen::Array4f min_p, max_p;
322  min_p.setConstant (FLT_MAX);
323  max_p.setConstant (-FLT_MAX);
324 
325  // If the data is dense, we don't need to check for NaN
326  if (cloud.is_dense)
327  {
328  for (const int &index : indices.indices)
329  {
330  pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
331  min_p = min_p.min (pt);
332  max_p = max_p.max (pt);
333  }
334  }
335  // NaN or Inf values could exist => check for them
336  else
337  {
338  for (const int &index : indices.indices)
339  {
340  // Check if the point is invalid
341  if (!std::isfinite (cloud[index].x) ||
342  !std::isfinite (cloud[index].y) ||
343  !std::isfinite (cloud[index].z))
344  continue;
345  pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
346  min_p = min_p.min (pt);
347  max_p = max_p.max (pt);
348  }
349  }
350  min_pt = min_p;
351  max_pt = max_p;
352 }
353 
354 //////////////////////////////////////////////////////////////////////////////////////////////
355 template <typename PointT> inline void
356 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
357  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
358 {
359  min_pt.setConstant (FLT_MAX);
360  max_pt.setConstant (-FLT_MAX);
361 
362  // If the data is dense, we don't need to check for NaN
363  if (cloud.is_dense)
364  {
365  for (const int &index : indices)
366  {
367  pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
368  min_pt = min_pt.array ().min (pt);
369  max_pt = max_pt.array ().max (pt);
370  }
371  }
372  // NaN or Inf values could exist => check for them
373  else
374  {
375  for (const int &index : indices)
376  {
377  // Check if the point is invalid
378  if (!std::isfinite (cloud[index].x) ||
379  !std::isfinite (cloud[index].y) ||
380  !std::isfinite (cloud[index].z))
381  continue;
382  pcl::Array4fMapConst pt = cloud[index].getArray4fMap ();
383  min_pt = min_pt.array ().min (pt);
384  max_pt = max_pt.array ().max (pt);
385  }
386  }
387 }
388 
389 //////////////////////////////////////////////////////////////////////////////////////////////
390 template <typename PointT> inline double
391 pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc)
392 {
393  Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0);
394  Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0);
395  Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0);
396 
397  double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm ();
398  // Calculate the area of the triangle using Heron's formula
399  // (http://en.wikipedia.org/wiki/Heron's_formula)
400  double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
401  double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
402  // Compute the radius of the circumscribed circle
403  return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
404 }
405 
406 //////////////////////////////////////////////////////////////////////////////////////////////
407 template <typename PointT> inline void
408 pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p)
409 {
410  min_p = FLT_MAX;
411  max_p = -FLT_MAX;
412 
413  for (int i = 0; i < len; ++i)
414  {
415  min_p = (histogram[i] > min_p) ? min_p : histogram[i];
416  max_p = (histogram[i] < max_p) ? max_p : histogram[i];
417  }
418 }
419 
420 //////////////////////////////////////////////////////////////////////////////////////////////
421 template <typename PointT> inline float
423 {
424  float area = 0.0f;
425  int num_points = polygon.size ();
426  Eigen::Vector3f va,vb,res;
427 
428  res(0) = res(1) = res(2) = 0.0f;
429  for (int i = 0; i < num_points; ++i)
430  {
431  int j = (i + 1) % num_points;
432  va = polygon[i].getVector3fMap ();
433  vb = polygon[j].getVector3fMap ();
434  res += va.cross (vb);
435  }
436  area = res.norm ();
437  return (area*0.5);
438 }
439 
440 #endif //#ifndef PCL_COMMON_IMPL_H_
441 
point_types.h
Defines all the PCL implemented PointT point type structures.
common.h
Define standard C methods and C++ classes that are common to all methods.
pcl::getPointsInBox
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.
Definition: common.hpp:102
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:23
pcl::getMeanStd
void getMeanStd(const std::vector< float > &values, double &mean, double &stddev)
Compute both the mean and the standard deviation of an array of values.
Definition: common.hpp:72
pcl::getCircumcircleRadius
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,...
Definition: common.hpp:391
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:629
pcl::getAngle3D
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.
Definition: common.hpp:47
pcl::calculatePolygonArea
float calculatePolygonArea(const pcl::PointCloud< PointT > &polygon)
Calculate the area of a polygon given a point cloud that defines the polygon.
Definition: common.hpp:422
pcl::getMaxDistance
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.
Definition: common.hpp:145
M_PI
#define M_PI
Definition: pcl_macros.h:192
pcl::Array4fMapConst
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
Definition: point_types.hpp:179
pcl::BadArgumentException
An exception that is thrown when the arguments number or type is wrong/unhandled.
Definition: exceptions.h:257
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:419
pcl::PointIndices
Definition: PointIndices.h:14
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:141
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:459
pcl::Vector3fMapConst
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
Definition: point_types.hpp:181
pcl::getMinMax3D
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.
Definition: common.hpp:243
pcl::getMinMax
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
Definition: common.hpp:408