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