Point Cloud Library (PCL)  1.7.1
auxiliary.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_RECOGNITION_RANSAC_BASED_AUX_H_
39 #define PCL_RECOGNITION_RANSAC_BASED_AUX_H_
40 
41 #include <cmath>
42 #include <cstdlib>
43 #include <pcl/common/eigen.h>
44 #include <pcl/point_types.h>
45 
46 #define AUX_PI_FLOAT 3.14159265358979323846f
47 #define AUX_HALF_PI 1.57079632679489661923f
48 #define AUX_DEG_TO_RADIANS (3.14159265358979323846f/180.0f)
49 
50 namespace pcl
51 {
52  namespace recognition
53  {
54  namespace aux
55  {
56  template<typename T> bool
57  compareOrderedPairs (const std::pair<T,T>& a, const std::pair<T,T>& b)
58  {
59  if ( a.first == b.first )
60  return static_cast<bool> (a.second < b.second);
61 
62  return static_cast<bool> (a.first < b.first);
63  }
64 
65  template<typename T> T
66  sqr (T a)
67  {
68  return (a*a);
69  }
70 
71  template<typename T> T
72  clamp (T value, T min, T max)
73  {
74  if ( value < min )
75  return min;
76  else if ( value > max )
77  return max;
78 
79  return value;
80  }
81 
82  /** \brief Expands the destination bounding box 'dst' such that it contains 'src'. */
83  template<typename T> void
84  expandBoundingBox (T dst[6], const T src[6])
85  {
86  if ( src[0] < dst[0] ) dst[0] = src[0];
87  if ( src[2] < dst[2] ) dst[2] = src[2];
88  if ( src[4] < dst[4] ) dst[4] = src[4];
89 
90  if ( src[1] > dst[1] ) dst[1] = src[1];
91  if ( src[3] > dst[3] ) dst[3] = src[3];
92  if ( src[5] > dst[5] ) dst[5] = src[5];
93  }
94 
95  /** \brief Expands the bounding box 'bbox' such that it contains the point 'p'. */
96  template<typename T> void
97  expandBoundingBoxToContainPoint (T bbox[6], const T p[3])
98  {
99  if ( p[0] < bbox[0] ) bbox[0] = p[0];
100  else if ( p[0] > bbox[1] ) bbox[1] = p[0];
101 
102  if ( p[1] < bbox[2] ) bbox[2] = p[1];
103  else if ( p[1] > bbox[3] ) bbox[3] = p[1];
104 
105  if ( p[2] < bbox[4] ) bbox[4] = p[2];
106  else if ( p[2] > bbox[5] ) bbox[5] = p[2];
107  }
108 
109  /** \brief v[0] = v[1] = v[2] = value */
110  template <typename T> void
111  set3 (T v[3], T value)
112  {
113  v[0] = v[1] = v[2] = value;
114  }
115 
116  /** \brief dst = src */
117  template <typename T> void
118  copy3 (const T src[3], T dst[3])
119  {
120  dst[0] = src[0];
121  dst[1] = src[1];
122  dst[2] = src[2];
123  }
124 
125  /** \brief dst = src */
126  template <typename T> void
127  copy3 (const T src[3], pcl::PointXYZ& dst)
128  {
129  dst.x = src[0];
130  dst.y = src[1];
131  dst.z = src[2];
132  }
133 
134  /** \brief a = -a */
135  template <typename T> void
136  flip3 (T a[3])
137  {
138  a[0] = -a[0];
139  a[1] = -a[1];
140  a[2] = -a[2];
141  }
142 
143  /** \brief a += b */
144  template <typename T> void
145  add3 (T a[3], const T b[3])
146  {
147  a[0] += b[0];
148  a[1] += b[1];
149  a[2] += b[2];
150  }
151 
152  /** \brief c = a + b */
153  template <typename T> void
154  sum3 (const T a[3], const T b[3], T c[3])
155  {
156  c[0] = a[0] + b[0];
157  c[1] = a[1] + b[1];
158  c[2] = a[2] + b[2];
159  }
160 
161  /** \brief c = a - b */
162  template <typename T> void
163  diff3 (const T a[3], const T b[3], T c[3])
164  {
165  c[0] = a[0] - b[0];
166  c[1] = a[1] - b[1];
167  c[2] = a[2] - b[2];
168  }
169 
170  template <typename T> void
171  cross3 (const T v1[3], const T v2[3], T out[3])
172  {
173  out[0] = v1[1]*v2[2] - v1[2]*v2[1];
174  out[1] = v1[2]*v2[0] - v1[0]*v2[2];
175  out[2] = v1[0]*v2[1] - v1[1]*v2[0];
176  }
177 
178  /** \brief Returns the length of v. */
179  template <typename T> T
180  length3 (const T v[3])
181  {
182  return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
183  }
184 
185  /** \brief Returns the Euclidean distance between a and b. */
186  template <typename T> T
187  distance3 (const T a[3], const T b[3])
188  {
189  T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
190  return (length3 (l));
191  }
192 
193  /** \brief Returns the squared Euclidean distance between a and b. */
194  template <typename T> T
195  sqrDistance3 (const T a[3], const T b[3])
196  {
197  return (aux::sqr (a[0]-b[0]) + aux::sqr (a[1]-b[1]) + aux::sqr (a[2]-b[2]));
198  }
199 
200  /** \brief Returns the dot product a*b */
201  template <typename T> T
202  dot3 (const T a[3], const T b[3])
203  {
204  return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
205  }
206 
207  /** \brief Returns the dot product (x, y, z)*(u, v, w) = x*u + y*v + z*w */
208  template <typename T> T
209  dot3 (T x, T y, T z, T u, T v, T w)
210  {
211  return (x*u + y*v + z*w);
212  }
213 
214  /** \brief v = scalar*v. */
215  template <typename T> void
216  mult3 (T* v, T scalar)
217  {
218  v[0] *= scalar;
219  v[1] *= scalar;
220  v[2] *= scalar;
221  }
222 
223  /** \brief out = scalar*v. */
224  template <typename T> void
225  mult3 (const T* v, T scalar, T* out)
226  {
227  out[0] = v[0]*scalar;
228  out[1] = v[1]*scalar;
229  out[2] = v[2]*scalar;
230  }
231 
232  /** \brief Normalize v */
233  template <typename T> void
234  normalize3 (T v[3])
235  {
236  T inv_len = (static_cast<T> (1.0))/aux::length3 (v);
237  v[0] *= inv_len;
238  v[1] *= inv_len;
239  v[2] *= inv_len;
240  }
241 
242  /** \brief Returns the square length of v. */
243  template <typename T> T
244  sqrLength3 (const T v[3])
245  {
246  return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
247  }
248 
249  /** Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'. */
250  template <typename T> void
251  projectOnPlane3 (const T x[3], const T planeNormal[3], T out[3])
252  {
253  T dot = aux::dot3 (planeNormal, x);
254  // Project 'x' on the plane normal
255  T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]};
256  aux::sum3 (x, nproj, out);
257  }
258 
259  /** \brief Sets 'm' to the 3x3 identity matrix. */
260  template <typename T> void
261  identity3x3 (T m[9])
262  {
263  m[0] = m[4] = m[8] = 1.0;
264  m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0;
265  }
266 
267  /** \brief out = mat*v. 'm' is an 1D array of 9 elements treated as a 3x3 matrix (row major order). */
268  template <typename T> void
269  mult3x3(const T m[9], const T v[3], T out[3])
270  {
271  out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2];
272  out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5];
273  out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8];
274  }
275 
276  /** Let x, y, z be the columns of the matrix a = [x|y|z]. The method computes out = a*m.
277  * Note that 'out' is a 1D array of 9 elements and the resulting matrix is stored in row
278  * major order, i.e., the first matrix row is (out[0] out[1] out[2]), the second
279  * (out[3] out[4] out[5]) and the third (out[6] out[7] out[8]). */
280  template <typename T> void
281  mult3x3 (const T x[3], const T y[3], const T z[3], const T m[3][3], T out[9])
282  {
283  out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0];
284  out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1];
285  out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2];
286 
287  out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0];
288  out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1];
289  out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2];
290 
291  out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0];
292  out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1];
293  out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2];
294  }
295 
296  /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation.
297  * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */
298  template<class T> void
299  transform(const T t[12], const T p[3], T out[3])
300  {
301  out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9];
302  out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10];
303  out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11];
304  }
305 
306  /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation.
307  * First, (x, y, z) is multiplied by that matrix and then translated. The result is saved in 'out'. */
308  template<class T> void
309  transform(const T t[12], T x, T y, T z, T out[3])
310  {
311  out[0] = t[0]*x + t[1]*y + t[2]*z + t[9];
312  out[1] = t[3]*x + t[4]*y + t[5]*z + t[10];
313  out[2] = t[6]*x + t[7]*y + t[8]*z + t[11];
314  }
315 
316  /** \brief Compute out = (upper left 3x3 of mat)*p + last column of mat. */
317  template<class T> void
318  transform(const Eigen::Matrix<T,4,4>& mat, const pcl::PointXYZ& p, pcl::PointXYZ& out)
319  {
320  out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3);
321  out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3);
322  out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3);
323  }
324 
325  /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation.
326  * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */
327  template<class T> void
328  transform(const T t[12], const pcl::PointXYZ& p, T out[3])
329  {
330  out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9];
331  out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10];
332  out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11];
333  }
334 
335  /** \brief Returns true if the points 'p1' and 'p2' are co-planar and false otherwise. The method assumes that 'n1'
336  * is a normal at 'p1' and 'n2' is a normal at 'p2'. 'max_angle' is the threshold used for the test. The bigger
337  * the value the larger the deviation between the normals can be which still leads to a positive test result. The
338  * angle has to be in radians. */
339  template<typename T> bool
340  pointsAreCoplanar (const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle)
341  {
342  // Compute the angle between 'n1' and 'n2' and compare it with 'max_angle'
343  if ( std::acos (aux::clamp (aux::dot3 (n1, n2), -1.0f, 1.0f)) > max_angle )
344  return (false);
345 
346  T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
347  aux::normalize3 (cl);
348 
349  // Compute the angle between 'cl' and 'n1'
350  T tmp_angle = std::acos (aux::clamp (aux::dot3 (n1, cl), -1.0f, 1.0f));
351 
352  // 'tmp_angle' should not deviate too much from 90 degrees
353  if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle )
354  return (false);
355 
356  // All tests passed => the points are coplanar
357  return (true);
358  }
359 
360  template<typename Scalar> void
361  array12ToMatrix4x4 (const Scalar src[12], Eigen::Matrix<Scalar, 4, 4>& dst)
362  {
363  dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9];
364  dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10];
365  dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11];
366  dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0;
367  }
368 
369  template<typename Scalar> void
370  matrix4x4ToArray12 (const Eigen::Matrix<Scalar, 4, 4>& src, Scalar dst[12])
371  {
372  dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3);
373  dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3);
374  dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3);
375  }
376 
377  /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
378  * dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2);
379  * dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2);
380  * dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2);
381  * */
382  template <typename T> void
383  eigenMatrix3x3ToArray9RowMajor (const Eigen::Matrix<T,3,3>& src, T dst[9])
384  {
385  dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2);
386  dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2);
387  dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2);
388  }
389 
390  /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
391  * dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2];
392  * dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5];
393  * dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8];
394  * */
395  template <typename T> void
396  toEigenMatrix3x3RowMajor (const T src[9], Eigen::Matrix<T,3,3>& dst)
397  {
398  dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2];
399  dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5];
400  dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8];
401  }
402 
403  /** brief Computes a rotation matrix from the provided input vector 'axis_angle'. The direction of 'axis_angle' is the rotation axis
404  * and its magnitude is the angle of rotation about that axis. 'rotation_matrix' is the output rotation matrix saved in row major order. */
405  template <typename T> void
406  axisAngleToRotationMatrix (const T axis_angle[3], T rotation_matrix[9])
407  {
408  // Get the angle of rotation
409  T angle = aux::length3 (axis_angle);
410  if ( angle == 0.0 )
411  {
412  // Undefined rotation -> set to identity
413  aux::identity3x3 (rotation_matrix);
414  return;
415  }
416 
417  // Normalize the input
418  T normalized_axis_angle[3];
419  aux::mult3 (axis_angle, static_cast<T> (1.0)/angle, normalized_axis_angle);
420 
421  // The eigen objects
422  Eigen::Matrix<T,3,1> mat_axis(normalized_axis_angle);
423  Eigen::AngleAxis<T> eigen_angle_axis (angle, mat_axis);
424 
425  // Save the output
426  aux::eigenMatrix3x3ToArray9RowMajor (eigen_angle_axis.toRotationMatrix (), rotation_matrix);
427  }
428 
429  /** brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis' and an 'angle'
430  * of rotation about that axis from the provided input. The output 'angle' is in the range [0, pi] and 'axis' is normalized. */
431  template <typename T> void
432  rotationMatrixToAxisAngle (const T rotation_matrix[9], T axis[3], T& angle)
433  {
434  // The eigen objects
435  Eigen::AngleAxis<T> angle_axis;
436  Eigen::Matrix<T,3,3> rot_mat;
437  // Copy the input matrix to the eigen matrix in row major order
438  aux::toEigenMatrix3x3RowMajor (rotation_matrix, rot_mat);
439 
440  // Do the computation
441  angle_axis.fromRotationMatrix (rot_mat);
442 
443  // Save the result
444  axis[0] = angle_axis.axis () (0,0);
445  axis[1] = angle_axis.axis () (1,0);
446  axis[2] = angle_axis.axis () (2,0);
447  angle = angle_axis.angle ();
448 
449  // Make sure that 'angle' is in the range [0, pi]
450  if ( angle > AUX_PI_FLOAT )
451  {
452  angle = 2.0f*AUX_PI_FLOAT - angle;
453  aux::flip3 (axis);
454  }
455  }
456  } // namespace aux
457  } // namespace recognition
458 } // namespace pcl
459 
460 #endif // AUX_H_