Point Cloud Library (PCL)  1.8.1-dev
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 
44 //////////////////////////////////////////////////////////////////////////////////////////////
45 inline double
46 pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree)
47 {
48  // Compute the actual angle
49  double rad = v1.normalized ().dot (v2.normalized ());
50  if (rad < -1.0)
51  rad = -1.0;
52  else if (rad > 1.0)
53  rad = 1.0;
54  return (in_degree ? acos (rad) * 180.0 / M_PI : acos (rad));
55 }
56 
57 inline double
58 pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree)
59 {
60  // Compute the actual angle
61  double rad = v1.normalized ().dot (v2.normalized ());
62  if (rad < -1.0)
63  rad = -1.0;
64  else if (rad > 1.0)
65  rad = 1.0;
66  return (in_degree ? acos (rad) * 180.0 / M_PI : acos (rad));
67 }
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 inline void
71 pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev)
72 {
73  // throw an exception when the input array is empty
74  if (values.empty ())
75  {
76  PCL_THROW_EXCEPTION (BadArgumentException, "Input array must have at least 1 element.");
77  }
78 
79  // when the array has only one element, mean is the number itself and standard dev is 0
80  if (values.size () == 1)
81  {
82  mean = values.at (0);
83  stddev = 0;
84  return;
85  }
86 
87  double sum = 0, sq_sum = 0;
88 
89  for (size_t i = 0; i < values.size (); ++i)
90  {
91  sum += values[i];
92  sq_sum += values[i] * values[i];
93  }
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);
97 }
98 
99 //////////////////////////////////////////////////////////////////////////////////////////////
100 template <typename PointT> inline void
102  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
103  std::vector<int> &indices)
104 {
105  indices.resize (cloud.points.size ());
106  int l = 0;
107 
108  // If the data is dense, we don't need to check for NaN
109  if (cloud.is_dense)
110  {
111  for (size_t i = 0; i < cloud.points.size (); ++i)
112  {
113  // Check if the point is inside bounds
114  if (cloud.points[i].x < min_pt[0] || cloud.points[i].y < min_pt[1] || cloud.points[i].z < min_pt[2])
115  continue;
116  if (cloud.points[i].x > max_pt[0] || cloud.points[i].y > max_pt[1] || cloud.points[i].z > max_pt[2])
117  continue;
118  indices[l++] = int (i);
119  }
120  }
121  // NaN or Inf values could exist => check for them
122  else
123  {
124  for (size_t i = 0; i < cloud.points.size (); ++i)
125  {
126  // Check if the point is invalid
127  if (!pcl_isfinite (cloud.points[i].x) ||
128  !pcl_isfinite (cloud.points[i].y) ||
129  !pcl_isfinite (cloud.points[i].z))
130  continue;
131  // Check if the point is inside bounds
132  if (cloud.points[i].x < min_pt[0] || cloud.points[i].y < min_pt[1] || cloud.points[i].z < min_pt[2])
133  continue;
134  if (cloud.points[i].x > max_pt[0] || cloud.points[i].y > max_pt[1] || cloud.points[i].z > max_pt[2])
135  continue;
136  indices[l++] = int (i);
137  }
138  }
139  indices.resize (l);
140 }
141 
142 //////////////////////////////////////////////////////////////////////////////////////////////
143 template<typename PointT> inline void
144 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
145 {
146  float max_dist = -FLT_MAX;
147  int max_idx = -1;
148  float dist;
149  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
150 
151  // If the data is dense, we don't need to check for NaN
152  if (cloud.is_dense)
153  {
154  for (size_t i = 0; i < cloud.points.size (); ++i)
155  {
156  pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
157  dist = (pivot_pt3 - pt).norm ();
158  if (dist > max_dist)
159  {
160  max_idx = int (i);
161  max_dist = dist;
162  }
163  }
164  }
165  // NaN or Inf values could exist => check for them
166  else
167  {
168  for (size_t i = 0; i < cloud.points.size (); ++i)
169  {
170  // Check if the point is invalid
171  if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z))
172  continue;
173  pcl::Vector3fMapConst pt = cloud.points[i].getVector3fMap ();
174  dist = (pivot_pt3 - pt).norm ();
175  if (dist > max_dist)
176  {
177  max_idx = int (i);
178  max_dist = dist;
179  }
180  }
181  }
182 
183  if(max_idx != -1)
184  max_pt = cloud.points[max_idx].getVector4fMap ();
185  else
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());
187 }
188 
189 //////////////////////////////////////////////////////////////////////////////////////////////
190 template<typename PointT> inline void
191 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
192  const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
193 {
194  float max_dist = -FLT_MAX;
195  int max_idx = -1;
196  float dist;
197  const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();
198 
199  // If the data is dense, we don't need to check for NaN
200  if (cloud.is_dense)
201  {
202  for (size_t i = 0; i < indices.size (); ++i)
203  {
204  pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap ();
205  dist = (pivot_pt3 - pt).norm ();
206  if (dist > max_dist)
207  {
208  max_idx = static_cast<int> (i);
209  max_dist = dist;
210  }
211  }
212  }
213  // NaN or Inf values could exist => check for them
214  else
215  {
216  for (size_t i = 0; i < indices.size (); ++i)
217  {
218  // Check if the point is invalid
219  if (!pcl_isfinite (cloud.points[indices[i]].x) || !pcl_isfinite (cloud.points[indices[i]].y)
220  ||
221  !pcl_isfinite (cloud.points[indices[i]].z))
222  continue;
223 
224  pcl::Vector3fMapConst pt = cloud.points[indices[i]].getVector3fMap ();
225  dist = (pivot_pt3 - pt).norm ();
226  if (dist > max_dist)
227  {
228  max_idx = static_cast<int> (i);
229  max_dist = dist;
230  }
231  }
232  }
233 
234  if(max_idx != -1)
235  max_pt = cloud.points[indices[max_idx]].getVector4fMap ();
236  else
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());
238 }
239 
240 //////////////////////////////////////////////////////////////////////////////////////////////
241 template <typename PointT> inline void
242 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt)
243 {
244  Eigen::Array4f min_p, max_p;
245  min_p.setConstant (FLT_MAX);
246  max_p.setConstant (-FLT_MAX);
247 
248  // If the data is dense, we don't need to check for NaN
249  if (cloud.is_dense)
250  {
251  for (size_t i = 0; i < cloud.points.size (); ++i)
252  {
253  pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap ();
254  min_p = min_p.min (pt);
255  max_p = max_p.max (pt);
256  }
257  }
258  // NaN or Inf values could exist => check for them
259  else
260  {
261  for (size_t i = 0; i < cloud.points.size (); ++i)
262  {
263  // Check if the point is invalid
264  if (!pcl_isfinite (cloud.points[i].x) ||
265  !pcl_isfinite (cloud.points[i].y) ||
266  !pcl_isfinite (cloud.points[i].z))
267  continue;
268  pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap ();
269  min_p = min_p.min (pt);
270  max_p = max_p.max (pt);
271  }
272  }
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];
275 }
276 
277 //////////////////////////////////////////////////////////////////////////////////////////////
278 template <typename PointT> inline void
279 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
280 {
281  Eigen::Array4f min_p, max_p;
282  min_p.setConstant (FLT_MAX);
283  max_p.setConstant (-FLT_MAX);
284 
285  // If the data is dense, we don't need to check for NaN
286  if (cloud.is_dense)
287  {
288  for (size_t i = 0; i < cloud.points.size (); ++i)
289  {
290  pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap ();
291  min_p = min_p.min (pt);
292  max_p = max_p.max (pt);
293  }
294  }
295  // NaN or Inf values could exist => check for them
296  else
297  {
298  for (size_t i = 0; i < cloud.points.size (); ++i)
299  {
300  // Check if the point is invalid
301  if (!pcl_isfinite (cloud.points[i].x) ||
302  !pcl_isfinite (cloud.points[i].y) ||
303  !pcl_isfinite (cloud.points[i].z))
304  continue;
305  pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap ();
306  min_p = min_p.min (pt);
307  max_p = max_p.max (pt);
308  }
309  }
310  min_pt = min_p;
311  max_pt = max_p;
312 }
313 
314 
315 //////////////////////////////////////////////////////////////////////////////////////////////
316 template <typename PointT> inline void
318  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
319 {
320  Eigen::Array4f min_p, max_p;
321  min_p.setConstant (FLT_MAX);
322  max_p.setConstant (-FLT_MAX);
323 
324  // If the data is dense, we don't need to check for NaN
325  if (cloud.is_dense)
326  {
327  for (size_t i = 0; i < indices.indices.size (); ++i)
328  {
329  pcl::Array4fMapConst pt = cloud.points[indices.indices[i]].getArray4fMap ();
330  min_p = min_p.min (pt);
331  max_p = max_p.max (pt);
332  }
333  }
334  // NaN or Inf values could exist => check for them
335  else
336  {
337  for (size_t i = 0; i < indices.indices.size (); ++i)
338  {
339  // Check if the point is invalid
340  if (!pcl_isfinite (cloud.points[indices.indices[i]].x) ||
341  !pcl_isfinite (cloud.points[indices.indices[i]].y) ||
342  !pcl_isfinite (cloud.points[indices.indices[i]].z))
343  continue;
344  pcl::Array4fMapConst pt = cloud.points[indices.indices[i]].getArray4fMap ();
345  min_p = min_p.min (pt);
346  max_p = max_p.max (pt);
347  }
348  }
349  min_pt = min_p;
350  max_pt = max_p;
351 }
352 
353 //////////////////////////////////////////////////////////////////////////////////////////////
354 template <typename PointT> inline void
355 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
356  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
357 {
358  min_pt.setConstant (FLT_MAX);
359  max_pt.setConstant (-FLT_MAX);
360 
361  // If the data is dense, we don't need to check for NaN
362  if (cloud.is_dense)
363  {
364  for (size_t i = 0; i < indices.size (); ++i)
365  {
366  pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
367  min_pt = min_pt.array ().min (pt);
368  max_pt = max_pt.array ().max (pt);
369  }
370  }
371  // NaN or Inf values could exist => check for them
372  else
373  {
374  for (size_t i = 0; i < indices.size (); ++i)
375  {
376  // Check if the point is invalid
377  if (!pcl_isfinite (cloud.points[indices[i]].x) ||
378  !pcl_isfinite (cloud.points[indices[i]].y) ||
379  !pcl_isfinite (cloud.points[indices[i]].z))
380  continue;
381  pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
382  min_pt = min_pt.array ().min (pt);
383  max_pt = max_pt.array ().max (pt);
384  }
385  }
386 }
387 
388 //////////////////////////////////////////////////////////////////////////////////////////////
389 template <typename PointT> inline double
390 pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc)
391 {
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);
395 
396  double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm ();
397  // Calculate the area of the triangle using Heron's formula
398  // (http://en.wikipedia.org/wiki/Heron's_formula)
399  double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
400  double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3));
401  // Compute the radius of the circumscribed circle
402  return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
403 }
404 
405 //////////////////////////////////////////////////////////////////////////////////////////////
406 template <typename PointT> inline void
407 pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p)
408 {
409  min_p = FLT_MAX;
410  max_p = -FLT_MAX;
411 
412  for (int i = 0; i < len; ++i)
413  {
414  min_p = (histogram[i] > min_p) ? min_p : histogram[i];
415  max_p = (histogram[i] < max_p) ? max_p : histogram[i];
416  }
417 }
418 
419 //////////////////////////////////////////////////////////////////////////////////////////////
420 template <typename PointT> inline float
422 {
423  float area = 0.0f;
424  int num_points = polygon.size ();
425  int j = 0;
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  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 
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.
Definition: common.hpp:101
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
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:46
An exception that is thrown when the arguments number or type is wrong/unhandled. ...
Definition: exceptions.h:256
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:407
std::vector< int > indices
Definition: PointIndices.h:19
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:144
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
float calculatePolygonArea(const pcl::PointCloud< PointT > &polygon)
Calculate the area of a polygon given a point cloud that defines the polygon.
Definition: common.hpp:421
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:71
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
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:242
size_t size() const
Definition: point_cloud.h:448
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:390
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)...
Definition: point_cloud.h:418