Point Cloud Library (PCL)  1.8.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> bool
145  equal3 (const T a[3], const T b[3])
146  {
147  return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]);
148  }
149 
150  /** \brief a += b */
151  template <typename T> void
152  add3 (T a[3], const T b[3])
153  {
154  a[0] += b[0];
155  a[1] += b[1];
156  a[2] += b[2];
157  }
158 
159  /** \brief c = a + b */
160  template <typename T> void
161  sum3 (const T a[3], const T b[3], T c[3])
162  {
163  c[0] = a[0] + b[0];
164  c[1] = a[1] + b[1];
165  c[2] = a[2] + b[2];
166  }
167 
168  /** \brief c = a - b */
169  template <typename T> void
170  diff3 (const T a[3], const T b[3], T c[3])
171  {
172  c[0] = a[0] - b[0];
173  c[1] = a[1] - b[1];
174  c[2] = a[2] - b[2];
175  }
176 
177  template <typename T> void
178  cross3 (const T v1[3], const T v2[3], T out[3])
179  {
180  out[0] = v1[1]*v2[2] - v1[2]*v2[1];
181  out[1] = v1[2]*v2[0] - v1[0]*v2[2];
182  out[2] = v1[0]*v2[1] - v1[1]*v2[0];
183  }
184 
185  /** \brief Returns the length of v. */
186  template <typename T> T
187  length3 (const T v[3])
188  {
189  return (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
190  }
191 
192  /** \brief Returns the Euclidean distance between a and b. */
193  template <typename T> T
194  distance3 (const T a[3], const T b[3])
195  {
196  T l[3] = {a[0]-b[0], a[1]-b[1], a[2]-b[2]};
197  return (length3 (l));
198  }
199 
200  /** \brief Returns the squared Euclidean distance between a and b. */
201  template <typename T> T
202  sqrDistance3 (const T a[3], const T b[3])
203  {
204  return (aux::sqr (a[0]-b[0]) + aux::sqr (a[1]-b[1]) + aux::sqr (a[2]-b[2]));
205  }
206 
207  /** \brief Returns the dot product a*b */
208  template <typename T> T
209  dot3 (const T a[3], const T b[3])
210  {
211  return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
212  }
213 
214  /** \brief Returns the dot product (x, y, z)*(u, v, w) = x*u + y*v + z*w */
215  template <typename T> T
216  dot3 (T x, T y, T z, T u, T v, T w)
217  {
218  return (x*u + y*v + z*w);
219  }
220 
221  /** \brief v = scalar*v. */
222  template <typename T> void
223  mult3 (T* v, T scalar)
224  {
225  v[0] *= scalar;
226  v[1] *= scalar;
227  v[2] *= scalar;
228  }
229 
230  /** \brief out = scalar*v. */
231  template <typename T> void
232  mult3 (const T* v, T scalar, T* out)
233  {
234  out[0] = v[0]*scalar;
235  out[1] = v[1]*scalar;
236  out[2] = v[2]*scalar;
237  }
238 
239  /** \brief Normalize v */
240  template <typename T> void
241  normalize3 (T v[3])
242  {
243  T inv_len = (static_cast<T> (1.0))/aux::length3 (v);
244  v[0] *= inv_len;
245  v[1] *= inv_len;
246  v[2] *= inv_len;
247  }
248 
249  /** \brief Returns the square length of v. */
250  template <typename T> T
251  sqrLength3 (const T v[3])
252  {
253  return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
254  }
255 
256  /** Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'. */
257  template <typename T> void
258  projectOnPlane3 (const T x[3], const T planeNormal[3], T out[3])
259  {
260  T dot = aux::dot3 (planeNormal, x);
261  // Project 'x' on the plane normal
262  T nproj[3] = {-dot*planeNormal[0], -dot*planeNormal[1], -dot*planeNormal[2]};
263  aux::sum3 (x, nproj, out);
264  }
265 
266  /** \brief Sets 'm' to the 3x3 identity matrix. */
267  template <typename T> void
268  identity3x3 (T m[9])
269  {
270  m[0] = m[4] = m[8] = 1.0;
271  m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0;
272  }
273 
274  /** \brief out = mat*v. 'm' is an 1D array of 9 elements treated as a 3x3 matrix (row major order). */
275  template <typename T> void
276  mult3x3(const T m[9], const T v[3], T out[3])
277  {
278  out[0] = v[0]*m[0] + v[1]*m[1] + v[2]*m[2];
279  out[1] = v[0]*m[3] + v[1]*m[4] + v[2]*m[5];
280  out[2] = v[0]*m[6] + v[1]*m[7] + v[2]*m[8];
281  }
282 
283  /** Let x, y, z be the columns of the matrix a = [x|y|z]. The method computes out = a*m.
284  * Note that 'out' is a 1D array of 9 elements and the resulting matrix is stored in row
285  * major order, i.e., the first matrix row is (out[0] out[1] out[2]), the second
286  * (out[3] out[4] out[5]) and the third (out[6] out[7] out[8]). */
287  template <typename T> void
288  mult3x3 (const T x[3], const T y[3], const T z[3], const T m[3][3], T out[9])
289  {
290  out[0] = x[0]*m[0][0] + y[0]*m[1][0] + z[0]*m[2][0];
291  out[1] = x[0]*m[0][1] + y[0]*m[1][1] + z[0]*m[2][1];
292  out[2] = x[0]*m[0][2] + y[0]*m[1][2] + z[0]*m[2][2];
293 
294  out[3] = x[1]*m[0][0] + y[1]*m[1][0] + z[1]*m[2][0];
295  out[4] = x[1]*m[0][1] + y[1]*m[1][1] + z[1]*m[2][1];
296  out[5] = x[1]*m[0][2] + y[1]*m[1][2] + z[1]*m[2][2];
297 
298  out[6] = x[2]*m[0][0] + y[2]*m[1][0] + z[2]*m[2][0];
299  out[7] = x[2]*m[0][1] + y[2]*m[1][1] + z[2]*m[2][1];
300  out[8] = x[2]*m[0][2] + y[2]*m[1][2] + z[2]*m[2][2];
301  }
302 
303  /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation.
304  * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */
305  template<class T> void
306  transform(const T t[12], const T p[3], T out[3])
307  {
308  out[0] = t[0]*p[0] + t[1]*p[1] + t[2]*p[2] + t[9];
309  out[1] = t[3]*p[0] + t[4]*p[1] + t[5]*p[2] + t[10];
310  out[2] = t[6]*p[0] + t[7]*p[1] + t[8]*p[2] + t[11];
311  }
312 
313  /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation.
314  * First, (x, y, z) is multiplied by that matrix and then translated. The result is saved in 'out'. */
315  template<class T> void
316  transform(const T t[12], T x, T y, T z, T out[3])
317  {
318  out[0] = t[0]*x + t[1]*y + t[2]*z + t[9];
319  out[1] = t[3]*x + t[4]*y + t[5]*z + t[10];
320  out[2] = t[6]*x + t[7]*y + t[8]*z + t[11];
321  }
322 
323  /** \brief Compute out = (upper left 3x3 of mat)*p + last column of mat. */
324  template<class T> void
325  transform(const Eigen::Matrix<T,4,4>& mat, const pcl::PointXYZ& p, pcl::PointXYZ& out)
326  {
327  out.x = mat(0,0)*p.x + mat(0,1)*p.y + mat(0,2)*p.z + mat(0,3);
328  out.y = mat(1,0)*p.x + mat(1,1)*p.y + mat(1,2)*p.z + mat(1,3);
329  out.z = mat(2,0)*p.x + mat(2,1)*p.y + mat(2,2)*p.z + mat(2,3);
330  }
331 
332  /** \brief The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a translation.
333  * First, 'p' is multiplied by that matrix and then translated. The result is saved in 'out'. */
334  template<class T> void
335  transform(const T t[12], const pcl::PointXYZ& p, T out[3])
336  {
337  out[0] = t[0]*p.x + t[1]*p.y + t[2]*p.z + t[9];
338  out[1] = t[3]*p.x + t[4]*p.y + t[5]*p.z + t[10];
339  out[2] = t[6]*p.x + t[7]*p.y + t[8]*p.z + t[11];
340  }
341 
342  /** \brief Returns true if the points 'p1' and 'p2' are co-planar and false otherwise. The method assumes that 'n1'
343  * is a normal at 'p1' and 'n2' is a normal at 'p2'. 'max_angle' is the threshold used for the test. The bigger
344  * the value the larger the deviation between the normals can be which still leads to a positive test result. The
345  * angle has to be in radians. */
346  template<typename T> bool
347  pointsAreCoplanar (const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle)
348  {
349  // Compute the angle between 'n1' and 'n2' and compare it with 'max_angle'
350  if ( std::acos (aux::clamp (aux::dot3 (n1, n2), -1.0f, 1.0f)) > max_angle )
351  return (false);
352 
353  T cl[3] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]};
354  aux::normalize3 (cl);
355 
356  // Compute the angle between 'cl' and 'n1'
357  T tmp_angle = std::acos (aux::clamp (aux::dot3 (n1, cl), -1.0f, 1.0f));
358 
359  // 'tmp_angle' should not deviate too much from 90 degrees
360  if ( std::fabs (tmp_angle - AUX_HALF_PI) > max_angle )
361  return (false);
362 
363  // All tests passed => the points are coplanar
364  return (true);
365  }
366 
367  template<typename Scalar> void
368  array12ToMatrix4x4 (const Scalar src[12], Eigen::Matrix<Scalar, 4, 4>& dst)
369  {
370  dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2]; dst(0,3) = src[9];
371  dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5]; dst(1,3) = src[10];
372  dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8]; dst(2,3) = src[11];
373  dst(3,0) = dst(3,1) = dst(3,2) = 0.0; dst(3,3) = 1.0;
374  }
375 
376  template<typename Scalar> void
377  matrix4x4ToArray12 (const Eigen::Matrix<Scalar, 4, 4>& src, Scalar dst[12])
378  {
379  dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2); dst[9] = src(0,3);
380  dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2); dst[10] = src(1,3);
381  dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2); dst[11] = src(2,3);
382  }
383 
384  /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
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  template <typename T> void
390  eigenMatrix3x3ToArray9RowMajor (const Eigen::Matrix<T,3,3>& src, T dst[9])
391  {
392  dst[0] = src(0,0); dst[1] = src(0,1); dst[2] = src(0,2);
393  dst[3] = src(1,0); dst[4] = src(1,1); dst[5] = src(1,2);
394  dst[6] = src(2,0); dst[7] = src(2,1); dst[8] = src(2,2);
395  }
396 
397  /** \brief The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
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  template <typename T> void
403  toEigenMatrix3x3RowMajor (const T src[9], Eigen::Matrix<T,3,3>& dst)
404  {
405  dst(0,0) = src[0]; dst(0,1) = src[1]; dst(0,2) = src[2];
406  dst(1,0) = src[3]; dst(1,1) = src[4]; dst(1,2) = src[5];
407  dst(2,0) = src[6]; dst(2,1) = src[7]; dst(2,2) = src[8];
408  }
409 
410  /** brief Computes a rotation matrix from the provided input vector 'axis_angle'. The direction of 'axis_angle' is the rotation axis
411  * and its magnitude is the angle of rotation about that axis. 'rotation_matrix' is the output rotation matrix saved in row major order. */
412  template <typename T> void
413  axisAngleToRotationMatrix (const T axis_angle[3], T rotation_matrix[9])
414  {
415  // Get the angle of rotation
416  T angle = aux::length3 (axis_angle);
417  if ( angle == 0.0 )
418  {
419  // Undefined rotation -> set to identity
420  aux::identity3x3 (rotation_matrix);
421  return;
422  }
423 
424  // Normalize the input
425  T normalized_axis_angle[3];
426  aux::mult3 (axis_angle, static_cast<T> (1.0)/angle, normalized_axis_angle);
427 
428  // The eigen objects
429  Eigen::Matrix<T,3,1> mat_axis(normalized_axis_angle);
430  Eigen::AngleAxis<T> eigen_angle_axis (angle, mat_axis);
431 
432  // Save the output
433  aux::eigenMatrix3x3ToArray9RowMajor (eigen_angle_axis.toRotationMatrix (), rotation_matrix);
434  }
435 
436  /** brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis' and an 'angle'
437  * of rotation about that axis from the provided input. The output 'angle' is in the range [0, pi] and 'axis' is normalized. */
438  template <typename T> void
439  rotationMatrixToAxisAngle (const T rotation_matrix[9], T axis[3], T& angle)
440  {
441  // The eigen objects
442  Eigen::AngleAxis<T> angle_axis;
443  Eigen::Matrix<T,3,3> rot_mat;
444  // Copy the input matrix to the eigen matrix in row major order
445  aux::toEigenMatrix3x3RowMajor (rotation_matrix, rot_mat);
446 
447  // Do the computation
448  angle_axis.fromRotationMatrix (rot_mat);
449 
450  // Save the result
451  axis[0] = angle_axis.axis () (0,0);
452  axis[1] = angle_axis.axis () (1,0);
453  axis[2] = angle_axis.axis () (2,0);
454  angle = angle_axis.angle ();
455 
456  // Make sure that 'angle' is in the range [0, pi]
457  if ( angle > AUX_PI_FLOAT )
458  {
459  angle = 2.0f*AUX_PI_FLOAT - angle;
460  aux::flip3 (axis);
461  }
462  }
463  } // namespace aux
464  } // namespace recognition
465 } // namespace pcl
466 
467 #endif // AUX_H_
void expandBoundingBox(T dst[6], const T src[6])
Expands the destination bounding box 'dst' such that it contains 'src'.
Definition: auxiliary.h:84
T sqrDistance3(const T a[3], const T b[3])
Returns the squared Euclidean distance between a and b.
Definition: auxiliary.h:202
bool pointsAreCoplanar(const T p1[3], const T n1[3], const T p2[3], const T n2[3], T max_angle)
Returns true if the points 'p1' and 'p2' are co-planar and false otherwise.
Definition: auxiliary.h:347
void mult3(T *v, T scalar)
v = scalar*v.
Definition: auxiliary.h:223
bool compareOrderedPairs(const std::pair< T, T > &a, const std::pair< T, T > &b)
Definition: auxiliary.h:57
void mult3x3(const T m[9], const T v[3], T out[3])
out = mat*v.
Definition: auxiliary.h:276
void toEigenMatrix3x3RowMajor(const T src[9], Eigen::Matrix< T, 3, 3 > &dst)
The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
Definition: auxiliary.h:403
void projectOnPlane3(const T x[3], const T planeNormal[3], T out[3])
Projects 'x' on the plane through 0 and with normal 'planeNormal' and saves the result in 'out'.
Definition: auxiliary.h:258
void expandBoundingBoxToContainPoint(T bbox[6], const T p[3])
Expands the bounding box 'bbox' such that it contains the point 'p'.
Definition: auxiliary.h:97
void cross3(const T v1[3], const T v2[3], T out[3])
Definition: auxiliary.h:178
T dot3(const T a[3], const T b[3])
Returns the dot product a*b.
Definition: auxiliary.h:209
void flip3(T a[3])
a = -a
Definition: auxiliary.h:136
Defines all the PCL implemented PointT point type structures.
A point structure representing Euclidean xyz coordinates.
T clamp(T value, T min, T max)
Definition: auxiliary.h:72
T sqrLength3(const T v[3])
Returns the square length of v.
Definition: auxiliary.h:251
void rotationMatrixToAxisAngle(const T rotation_matrix[9], T axis[3], T &angle)
brief Extracts the angle-axis representation from 'rotation_matrix', i.e., computes a rotation 'axis'...
Definition: auxiliary.h:439
T distance3(const T a[3], const T b[3])
Returns the Euclidean distance between a and b.
Definition: auxiliary.h:194
void identity3x3(T m[9])
Sets 'm' to the 3x3 identity matrix.
Definition: auxiliary.h:268
void sum3(const T a[3], const T b[3], T c[3])
c = a + b
Definition: auxiliary.h:161
void axisAngleToRotationMatrix(const T axis_angle[3], T rotation_matrix[9])
brief Computes a rotation matrix from the provided input vector 'axis_angle'.
Definition: auxiliary.h:413
void set3(T v[3], T value)
v[0] = v[1] = v[2] = value
Definition: auxiliary.h:111
void normalize3(T v[3])
Normalize v.
Definition: auxiliary.h:241
void matrix4x4ToArray12(const Eigen::Matrix< Scalar, 4, 4 > &src, Scalar dst[12])
Definition: auxiliary.h:377
void array12ToMatrix4x4(const Scalar src[12], Eigen::Matrix< Scalar, 4, 4 > &dst)
Definition: auxiliary.h:368
void copy3(const T src[3], T dst[3])
dst = src
Definition: auxiliary.h:118
T length3(const T v[3])
Returns the length of v.
Definition: auxiliary.h:187
void diff3(const T a[3], const T b[3], T c[3])
c = a - b
Definition: auxiliary.h:170
void add3(T a[3], const T b[3])
a += b
Definition: auxiliary.h:152
bool equal3(const T a[3], const T b[3])
a = b
Definition: auxiliary.h:145
void eigenMatrix3x3ToArray9RowMajor(const Eigen::Matrix< T, 3, 3 > &src, T dst[9])
The method copies the input array 'src' to the eigen matrix 'dst' in row major order.
Definition: auxiliary.h:390
void transform(const T t[12], const T p[3], T out[3])
The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a transla...
Definition: auxiliary.h:306