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