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