Point Cloud Library (PCL)  1.9.1-dev
eigen.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR a PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_COMMON_EIGEN_IMPL_HPP_
40 #define PCL_COMMON_EIGEN_IMPL_HPP_
41 
42 #include <algorithm>
43 
44 #include <pcl/console/print.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////
47 template <typename Scalar, typename Roots> inline void
48 pcl::computeRoots2 (const Scalar& b, const Scalar& c, Roots& roots)
49 {
50  roots (0) = Scalar (0);
51  Scalar d = Scalar (b * b - 4.0 * c);
52  if (d < 0.0) // no real roots ! THIS SHOULD NOT HAPPEN!
53  d = 0.0;
54 
55  Scalar sd = std::sqrt (d);
56 
57  roots (2) = 0.5f * (b + sd);
58  roots (1) = 0.5f * (b - sd);
59 }
60 
61 //////////////////////////////////////////////////////////////////////////////////////////
62 template <typename Matrix, typename Roots> inline void
63 pcl::computeRoots (const Matrix& m, Roots& roots)
64 {
65  using Scalar = typename Matrix::Scalar;
66 
67  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
68  // eigenvalues are the roots to this equation, all guaranteed to be
69  // real-valued, because the matrix is symmetric.
70  Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2)
71  + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2)
72  - m (0, 0) * m (1, 2) * m (1, 2)
73  - m (1, 1) * m (0, 2) * m (0, 2)
74  - m (2, 2) * m (0, 1) * m (0, 1);
75  Scalar c1 = m (0, 0) * m (1, 1) -
76  m (0, 1) * m (0, 1) +
77  m (0, 0) * m (2, 2) -
78  m (0, 2) * m (0, 2) +
79  m (1, 1) * m (2, 2) -
80  m (1, 2) * m (1, 2);
81  Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
82 
83  if (std::abs (c0) < Eigen::NumTraits < Scalar > ::epsilon ()) // one root is 0 -> quadratic equation
84  computeRoots2 (c2, c1, roots);
85  else
86  {
87  const Scalar s_inv3 = Scalar (1.0 / 3.0);
88  const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
89  // Construct the parameters used in classifying the roots of the equation
90  // and in solving the equation for the roots in closed form.
91  Scalar c2_over_3 = c2 * s_inv3;
92  Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
93  if (a_over_3 > Scalar (0))
94  a_over_3 = Scalar (0);
95 
96  Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
97 
98  Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
99  if (q > Scalar (0))
100  q = Scalar (0);
101 
102  // Compute the eigenvalues by solving for the roots of the polynomial.
103  Scalar rho = std::sqrt (-a_over_3);
104  Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3;
105  Scalar cos_theta = std::cos (theta);
106  Scalar sin_theta = std::sin (theta);
107  roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta;
108  roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
109  roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
110 
111  // Sort in increasing order.
112  if (roots (0) >= roots (1))
113  std::swap (roots (0), roots (1));
114  if (roots (1) >= roots (2))
115  {
116  std::swap (roots (1), roots (2));
117  if (roots (0) >= roots (1))
118  std::swap (roots (0), roots (1));
119  }
120 
121  if (roots (0) <= 0) // eigenval for symmetric positive semi-definite matrix can not be negative! Set it to 0
122  computeRoots2 (c2, c1, roots);
123  }
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////
127 template <typename Matrix, typename Vector> inline void
128 pcl::eigen22 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
129 {
130  // if diagonal matrix, the eigenvalues are the diagonal elements
131  // and the eigenvectors are not unique, thus set to Identity
132  if (std::abs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
133  {
134  if (mat.coeff (0) < mat.coeff (2))
135  {
136  eigenvalue = mat.coeff (0);
137  eigenvector[0] = 1.0;
138  eigenvector[1] = 0.0;
139  }
140  else
141  {
142  eigenvalue = mat.coeff (2);
143  eigenvector[0] = 0.0;
144  eigenvector[1] = 1.0;
145  }
146  return;
147  }
148 
149  // 0.5 to optimize further calculations
150  typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
151  typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
152 
153  typename Matrix::Scalar temp = trace * trace - determinant;
154 
155  if (temp < 0)
156  temp = 0;
157 
158  eigenvalue = trace - std::sqrt (temp);
159 
160  eigenvector[0] = -mat.coeff (1);
161  eigenvector[1] = mat.coeff (0) - eigenvalue;
162  eigenvector.normalize ();
163 }
164 
165 //////////////////////////////////////////////////////////////////////////////////////////
166 template <typename Matrix, typename Vector> inline void
167 pcl::eigen22 (const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
168 {
169  // if diagonal matrix, the eigenvalues are the diagonal elements
170  // and the eigenvectors are not unique, thus set to Identity
171  if (std::abs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
172  {
173  if (mat.coeff (0) < mat.coeff (3))
174  {
175  eigenvalues.coeffRef (0) = mat.coeff (0);
176  eigenvalues.coeffRef (1) = mat.coeff (3);
177  eigenvectors.coeffRef (0) = 1.0;
178  eigenvectors.coeffRef (1) = 0.0;
179  eigenvectors.coeffRef (2) = 0.0;
180  eigenvectors.coeffRef (3) = 1.0;
181  }
182  else
183  {
184  eigenvalues.coeffRef (0) = mat.coeff (3);
185  eigenvalues.coeffRef (1) = mat.coeff (0);
186  eigenvectors.coeffRef (0) = 0.0;
187  eigenvectors.coeffRef (1) = 1.0;
188  eigenvectors.coeffRef (2) = 1.0;
189  eigenvectors.coeffRef (3) = 0.0;
190  }
191  return;
192  }
193 
194  // 0.5 to optimize further calculations
195  typename Matrix::Scalar trace = static_cast<typename Matrix::Scalar> (0.5) * (mat.coeff (0) + mat.coeff (3));
196  typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
197 
198  typename Matrix::Scalar temp = trace * trace - determinant;
199 
200  if (temp < 0)
201  temp = 0;
202  else
203  temp = std::sqrt (temp);
204 
205  eigenvalues.coeffRef (0) = trace - temp;
206  eigenvalues.coeffRef (1) = trace + temp;
207 
208  // either this is in a row or column depending on RowMajor or ColumnMajor
209  eigenvectors.coeffRef (0) = -mat.coeff (1);
210  eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0);
211  typename Matrix::Scalar norm = static_cast<typename Matrix::Scalar> (1.0)
212  / static_cast<typename Matrix::Scalar> (std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2)));
213  eigenvectors.coeffRef (0) *= norm;
214  eigenvectors.coeffRef (2) *= norm;
215  eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2);
216  eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
217 }
218 
219 //////////////////////////////////////////////////////////////////////////////////////////
220 template <typename Matrix, typename Vector> inline void
221 pcl::computeCorrespondingEigenVector (const Matrix& mat, const typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
222 {
223  using Scalar = typename Matrix::Scalar;
224  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
225  // only when at least one matrix entry has magnitude larger than 1.
226 
227  Scalar scale = mat.cwiseAbs ().maxCoeff ();
228  if (scale <= std::numeric_limits < Scalar > ::min ())
229  scale = Scalar (1.0);
230 
231  Matrix scaledMat = mat / scale;
232 
233  scaledMat.diagonal ().array () -= eigenvalue / scale;
234 
235  Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
236  Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
237  Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
238 
239  Scalar len1 = vec1.squaredNorm ();
240  Scalar len2 = vec2.squaredNorm ();
241  Scalar len3 = vec3.squaredNorm ();
242 
243  if (len1 >= len2 && len1 >= len3)
244  eigenvector = vec1 / std::sqrt (len1);
245  else if (len2 >= len1 && len2 >= len3)
246  eigenvector = vec2 / std::sqrt (len2);
247  else
248  eigenvector = vec3 / std::sqrt (len3);
249 }
250 
251 //////////////////////////////////////////////////////////////////////////////////////////
252 template <typename Matrix, typename Vector> inline void
253 pcl::eigen33 (const Matrix& mat, typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
254 {
255  using Scalar = typename Matrix::Scalar;
256  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
257  // only when at least one matrix entry has magnitude larger than 1.
258 
259  Scalar scale = mat.cwiseAbs ().maxCoeff ();
260  if (scale <= std::numeric_limits < Scalar > ::min ())
261  scale = Scalar (1.0);
262 
263  Matrix scaledMat = mat / scale;
264 
265  Vector eigenvalues;
266  computeRoots (scaledMat, eigenvalues);
267 
268  eigenvalue = eigenvalues (0) * scale;
269 
270  scaledMat.diagonal ().array () -= eigenvalues (0);
271 
272  Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
273  Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
274  Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
275 
276  Scalar len1 = vec1.squaredNorm ();
277  Scalar len2 = vec2.squaredNorm ();
278  Scalar len3 = vec3.squaredNorm ();
279 
280  if (len1 >= len2 && len1 >= len3)
281  eigenvector = vec1 / std::sqrt (len1);
282  else if (len2 >= len1 && len2 >= len3)
283  eigenvector = vec2 / std::sqrt (len2);
284  else
285  eigenvector = vec3 / std::sqrt (len3);
286 }
287 
288 //////////////////////////////////////////////////////////////////////////////////////////
289 template <typename Matrix, typename Vector> inline void
290 pcl::eigen33 (const Matrix& mat, Vector& evals)
291 {
292  using Scalar = typename Matrix::Scalar;
293  Scalar scale = mat.cwiseAbs ().maxCoeff ();
294  if (scale <= std::numeric_limits < Scalar > ::min ())
295  scale = Scalar (1.0);
296 
297  Matrix scaledMat = mat / scale;
298  computeRoots (scaledMat, evals);
299  evals *= scale;
300 }
301 
302 //////////////////////////////////////////////////////////////////////////////////////////
303 template <typename Matrix, typename Vector> inline void
304 pcl::eigen33 (const Matrix& mat, Matrix& evecs, Vector& evals)
305 {
306  using Scalar = typename Matrix::Scalar;
307  // Scale the matrix so its entries are in [-1,1]. The scaling is applied
308  // only when at least one matrix entry has magnitude larger than 1.
309 
310  Scalar scale = mat.cwiseAbs ().maxCoeff ();
311  if (scale <= std::numeric_limits < Scalar > ::min ())
312  scale = Scalar (1.0);
313 
314  Matrix scaledMat = mat / scale;
315 
316  // Compute the eigenvalues
317  computeRoots (scaledMat, evals);
318 
319  if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
320  {
321  // all three equal
322  evecs.setIdentity ();
323  }
324  else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
325  {
326  // first and second equal
327  Matrix tmp;
328  tmp = scaledMat;
329  tmp.diagonal ().array () -= evals (2);
330 
331  Vector vec1 = tmp.row (0).cross (tmp.row (1));
332  Vector vec2 = tmp.row (0).cross (tmp.row (2));
333  Vector vec3 = tmp.row (1).cross (tmp.row (2));
334 
335  Scalar len1 = vec1.squaredNorm ();
336  Scalar len2 = vec2.squaredNorm ();
337  Scalar len3 = vec3.squaredNorm ();
338 
339  if (len1 >= len2 && len1 >= len3)
340  evecs.col (2) = vec1 / std::sqrt (len1);
341  else if (len2 >= len1 && len2 >= len3)
342  evecs.col (2) = vec2 / std::sqrt (len2);
343  else
344  evecs.col (2) = vec3 / std::sqrt (len3);
345 
346  evecs.col (1) = evecs.col (2).unitOrthogonal ();
347  evecs.col (0) = evecs.col (1).cross (evecs.col (2));
348  }
349  else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ())
350  {
351  // second and third equal
352  Matrix tmp;
353  tmp = scaledMat;
354  tmp.diagonal ().array () -= evals (0);
355 
356  Vector vec1 = tmp.row (0).cross (tmp.row (1));
357  Vector vec2 = tmp.row (0).cross (tmp.row (2));
358  Vector vec3 = tmp.row (1).cross (tmp.row (2));
359 
360  Scalar len1 = vec1.squaredNorm ();
361  Scalar len2 = vec2.squaredNorm ();
362  Scalar len3 = vec3.squaredNorm ();
363 
364  if (len1 >= len2 && len1 >= len3)
365  evecs.col (0) = vec1 / std::sqrt (len1);
366  else if (len2 >= len1 && len2 >= len3)
367  evecs.col (0) = vec2 / std::sqrt (len2);
368  else
369  evecs.col (0) = vec3 / std::sqrt (len3);
370 
371  evecs.col (1) = evecs.col (0).unitOrthogonal ();
372  evecs.col (2) = evecs.col (0).cross (evecs.col (1));
373  }
374  else
375  {
376  Matrix tmp;
377  tmp = scaledMat;
378  tmp.diagonal ().array () -= evals (2);
379 
380  Vector vec1 = tmp.row (0).cross (tmp.row (1));
381  Vector vec2 = tmp.row (0).cross (tmp.row (2));
382  Vector vec3 = tmp.row (1).cross (tmp.row (2));
383 
384  Scalar len1 = vec1.squaredNorm ();
385  Scalar len2 = vec2.squaredNorm ();
386  Scalar len3 = vec3.squaredNorm ();
387 #ifdef _WIN32
388  Scalar *mmax = new Scalar[3];
389 #else
390  Scalar mmax[3];
391 #endif
392  unsigned int min_el = 2;
393  unsigned int max_el = 2;
394  if (len1 >= len2 && len1 >= len3)
395  {
396  mmax[2] = len1;
397  evecs.col (2) = vec1 / std::sqrt (len1);
398  }
399  else if (len2 >= len1 && len2 >= len3)
400  {
401  mmax[2] = len2;
402  evecs.col (2) = vec2 / std::sqrt (len2);
403  }
404  else
405  {
406  mmax[2] = len3;
407  evecs.col (2) = vec3 / std::sqrt (len3);
408  }
409 
410  tmp = scaledMat;
411  tmp.diagonal ().array () -= evals (1);
412 
413  vec1 = tmp.row (0).cross (tmp.row (1));
414  vec2 = tmp.row (0).cross (tmp.row (2));
415  vec3 = tmp.row (1).cross (tmp.row (2));
416 
417  len1 = vec1.squaredNorm ();
418  len2 = vec2.squaredNorm ();
419  len3 = vec3.squaredNorm ();
420  if (len1 >= len2 && len1 >= len3)
421  {
422  mmax[1] = len1;
423  evecs.col (1) = vec1 / std::sqrt (len1);
424  min_el = len1 <= mmax[min_el] ? 1 : min_el;
425  max_el = len1 > mmax[max_el] ? 1 : max_el;
426  }
427  else if (len2 >= len1 && len2 >= len3)
428  {
429  mmax[1] = len2;
430  evecs.col (1) = vec2 / std::sqrt (len2);
431  min_el = len2 <= mmax[min_el] ? 1 : min_el;
432  max_el = len2 > mmax[max_el] ? 1 : max_el;
433  }
434  else
435  {
436  mmax[1] = len3;
437  evecs.col (1) = vec3 / std::sqrt (len3);
438  min_el = len3 <= mmax[min_el] ? 1 : min_el;
439  max_el = len3 > mmax[max_el] ? 1 : max_el;
440  }
441 
442  tmp = scaledMat;
443  tmp.diagonal ().array () -= evals (0);
444 
445  vec1 = tmp.row (0).cross (tmp.row (1));
446  vec2 = tmp.row (0).cross (tmp.row (2));
447  vec3 = tmp.row (1).cross (tmp.row (2));
448 
449  len1 = vec1.squaredNorm ();
450  len2 = vec2.squaredNorm ();
451  len3 = vec3.squaredNorm ();
452  if (len1 >= len2 && len1 >= len3)
453  {
454  mmax[0] = len1;
455  evecs.col (0) = vec1 / std::sqrt (len1);
456  min_el = len3 <= mmax[min_el] ? 0 : min_el;
457  max_el = len3 > mmax[max_el] ? 0 : max_el;
458  }
459  else if (len2 >= len1 && len2 >= len3)
460  {
461  mmax[0] = len2;
462  evecs.col (0) = vec2 / std::sqrt (len2);
463  min_el = len3 <= mmax[min_el] ? 0 : min_el;
464  max_el = len3 > mmax[max_el] ? 0 : max_el;
465  }
466  else
467  {
468  mmax[0] = len3;
469  evecs.col (0) = vec3 / std::sqrt (len3);
470  min_el = len3 <= mmax[min_el] ? 0 : min_el;
471  max_el = len3 > mmax[max_el] ? 0 : max_el;
472  }
473 
474  unsigned mid_el = 3 - min_el - max_el;
475  evecs.col (min_el) = evecs.col ( (min_el + 1) % 3).cross (evecs.col ( (min_el + 2) % 3)).normalized ();
476  evecs.col (mid_el) = evecs.col ( (mid_el + 1) % 3).cross (evecs.col ( (mid_el + 2) % 3)).normalized ();
477 #ifdef _WIN32
478  delete [] mmax;
479 #endif
480  }
481  // Rescale back to the original size.
482  evals *= scale;
483 }
484 
485 //////////////////////////////////////////////////////////////////////////////////////////
486 template <typename Matrix> inline typename Matrix::Scalar
487 pcl::invert2x2 (const Matrix& matrix, Matrix& inverse)
488 {
489  using Scalar = typename Matrix::Scalar;
490  Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2);
491 
492  if (det != 0)
493  {
494  //Scalar inv_det = Scalar (1.0) / det;
495  inverse.coeffRef (0) = matrix.coeff (3);
496  inverse.coeffRef (1) = -matrix.coeff (1);
497  inverse.coeffRef (2) = -matrix.coeff (2);
498  inverse.coeffRef (3) = matrix.coeff (0);
499  inverse /= det;
500  }
501  return det;
502 }
503 
504 //////////////////////////////////////////////////////////////////////////////////////////
505 template <typename Matrix> inline typename Matrix::Scalar
506 pcl::invert3x3SymMatrix (const Matrix& matrix, Matrix& inverse)
507 {
508  using Scalar = typename Matrix::Scalar;
509  // elements
510  // a b c
511  // b d e
512  // c e f
513  //| a b c |-1 | fd-ee ce-bf be-cd |
514  //| b d e | = 1/det * | ce-bf af-cc bc-ae |
515  //| c e f | | be-cd bc-ae ad-bb |
516 
517  //det = a(fd-ee) + b(ec-fb) + c(eb-dc)
518 
519  Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
520  Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
521  Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
522 
523  Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
524 
525  if (det != 0)
526  {
527  //Scalar inv_det = Scalar (1.0) / det;
528  inverse.coeffRef (0) = fd_ee;
529  inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
530  inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
531  inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
532  inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
533  inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
534  inverse /= det;
535  }
536  return det;
537 }
538 
539 //////////////////////////////////////////////////////////////////////////////////////////
540 template <typename Matrix> inline typename Matrix::Scalar
541 pcl::invert3x3Matrix (const Matrix& matrix, Matrix& inverse)
542 {
543  using Scalar = typename Matrix::Scalar;
544 
545  //| a b c |-1 | ie-hf hc-ib fb-ec |
546  //| d e f | = 1/det * | gf-id ia-gc dc-fa |
547  //| g h i | | hd-ge gb-ha ea-db |
548  //det = a(ie-hf) + d(hc-ib) + g(fb-ec)
549 
550  Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
551  Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
552  Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
553  Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec);
554 
555  if (det != 0)
556  {
557  inverse.coeffRef (0) = ie_hf;
558  inverse.coeffRef (1) = hc_ib;
559  inverse.coeffRef (2) = fb_ec;
560  inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
561  inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
562  inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
563  inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
564  inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
565  inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
566 
567  inverse /= det;
568  }
569  return det;
570 }
571 
572 //////////////////////////////////////////////////////////////////////////////////////////
573 template <typename Matrix> inline typename Matrix::Scalar
574 pcl::determinant3x3Matrix (const Matrix& matrix)
575 {
576  // result is independent of Row/Col Major storage!
577  return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
578  matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
579  matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
580 }
581 
582 //////////////////////////////////////////////////////////////////////////////////////////
583 void
584 pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
585  const Eigen::Vector3f& y_direction,
586  Eigen::Affine3f& transformation)
587 {
588  Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized();
589  Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized();
590  Eigen::Vector3f tmp2 = z_axis.normalized();
591 
592  transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
593  transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
594  transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
595  transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
596 }
597 
598 //////////////////////////////////////////////////////////////////////////////////////////
599 Eigen::Affine3f
600 pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
601  const Eigen::Vector3f& y_direction)
602 {
603  Eigen::Affine3f transformation;
604  getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
605  return (transformation);
606 }
607 
608 //////////////////////////////////////////////////////////////////////////////////////////
609 void
610 pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
611  const Eigen::Vector3f& y_direction,
612  Eigen::Affine3f& transformation)
613 {
614  Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized();
615  Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized();
616  Eigen::Vector3f tmp0 = x_axis.normalized();
617 
618  transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
619  transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
620  transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
621  transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
622 }
623 
624 //////////////////////////////////////////////////////////////////////////////////////////
625 Eigen::Affine3f
626 pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
627  const Eigen::Vector3f& y_direction)
628 {
629  Eigen::Affine3f transformation;
630  getTransFromUnitVectorsXY (x_axis, y_direction, transformation);
631  return (transformation);
632 }
633 
634 //////////////////////////////////////////////////////////////////////////////////////////
635 void
636 pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
637  const Eigen::Vector3f& z_axis,
638  Eigen::Affine3f& transformation)
639 {
640  getTransFromUnitVectorsZY (z_axis, y_direction, transformation);
641 }
642 
643 //////////////////////////////////////////////////////////////////////////////////////////
644 Eigen::Affine3f
645 pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
646  const Eigen::Vector3f& z_axis)
647 {
648  Eigen::Affine3f transformation;
649  getTransformationFromTwoUnitVectors (y_direction, z_axis, transformation);
650  return (transformation);
651 }
652 
653 void
654 pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
655  const Eigen::Vector3f& z_axis,
656  const Eigen::Vector3f& origin,
657  Eigen::Affine3f& transformation)
658 {
659  getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation);
660  Eigen::Vector3f translation = transformation*origin;
661  transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2];
662 }
663 
664 //////////////////////////////////////////////////////////////////////////////////////////
665 template <typename Scalar> void
666 pcl::getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
667 {
668  roll = std::atan2 (t (2, 1), t (2, 2));
669  pitch = asin (-t (2, 0));
670  yaw = std::atan2 (t (1, 0), t (0, 0));
671 }
672 
673 //////////////////////////////////////////////////////////////////////////////////////////
674 template <typename Scalar> void
675 pcl::getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
676  Scalar &x, Scalar &y, Scalar &z,
677  Scalar &roll, Scalar &pitch, Scalar &yaw)
678 {
679  x = t (0, 3);
680  y = t (1, 3);
681  z = t (2, 3);
682  roll = std::atan2 (t (2, 1), t (2, 2));
683  pitch = asin (-t (2, 0));
684  yaw = std::atan2 (t (1, 0), t (0, 0));
685 }
686 
687 //////////////////////////////////////////////////////////////////////////////////////////
688 template <typename Scalar> void
689 pcl::getTransformation (Scalar x, Scalar y, Scalar z,
690  Scalar roll, Scalar pitch, Scalar yaw,
691  Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
692 {
693  Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch),
694  E = std::cos (roll), F = sin (roll), DE = D*E, DF = D*F;
695 
696  t (0, 0) = A*C; t (0, 1) = A*DF - B*E; t (0, 2) = B*F + A*DE; t (0, 3) = x;
697  t (1, 0) = B*C; t (1, 1) = A*E + B*DF; t (1, 2) = B*DE - A*F; t (1, 3) = y;
698  t (2, 0) = -D; t (2, 1) = C*F; t (2, 2) = C*E; t (2, 3) = z;
699  t (3, 0) = 0; t (3, 1) = 0; t (3, 2) = 0; t (3, 3) = 1;
700 }
701 
702 //////////////////////////////////////////////////////////////////////////////////////////
703 template <typename Derived> void
704 pcl::saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file)
705 {
706  std::uint32_t rows = static_cast<std::uint32_t> (matrix.rows ()), cols = static_cast<std::uint32_t> (matrix.cols ());
707  file.write (reinterpret_cast<char*> (&rows), sizeof (rows));
708  file.write (reinterpret_cast<char*> (&cols), sizeof (cols));
709  for (std::uint32_t i = 0; i < rows; ++i)
710  for (std::uint32_t j = 0; j < cols; ++j)
711  {
712  typename Derived::Scalar tmp = matrix(i,j);
713  file.write (reinterpret_cast<const char*> (&tmp), sizeof (tmp));
714  }
715 }
716 
717 //////////////////////////////////////////////////////////////////////////////////////////
718 template <typename Derived> void
719 pcl::loadBinary (Eigen::MatrixBase<Derived> const & matrix_, std::istream& file)
720 {
721  Eigen::MatrixBase<Derived> &matrix = const_cast<Eigen::MatrixBase<Derived> &> (matrix_);
722 
723  std::uint32_t rows, cols;
724  file.read (reinterpret_cast<char*> (&rows), sizeof (rows));
725  file.read (reinterpret_cast<char*> (&cols), sizeof (cols));
726  if (matrix.rows () != static_cast<int>(rows) || matrix.cols () != static_cast<int>(cols))
727  matrix.derived().resize(rows, cols);
728 
729  for (std::uint32_t i = 0; i < rows; ++i)
730  for (std::uint32_t j = 0; j < cols; ++j)
731  {
732  typename Derived::Scalar tmp;
733  file.read (reinterpret_cast<char*> (&tmp), sizeof (tmp));
734  matrix (i, j) = tmp;
735  }
736 }
737 
738 //////////////////////////////////////////////////////////////////////////////////////////
739 template <typename Derived, typename OtherDerived>
740 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
741 pcl::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
742 {
743 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
744  return Eigen::umeyama (src, dst, with_scaling);
745 #else
746  using TransformationMatrixType = typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type;
747  using Scalar = typename Eigen::internal::traits<TransformationMatrixType>::Scalar;
748  using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
749  using Index = typename Derived::Index;
750 
751  static_assert (!Eigen::NumTraits<Scalar>::IsComplex, "Numeric type must be real.");
752  static_assert ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value),
753  "You mixed different numeric types. You need to use the cast method of matrixbase to cast numeric types explicitly.");
754 
755  enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
756 
757  using VectorType = Eigen::Matrix<Scalar, Dimension, 1>;
758  using MatrixType = Eigen::Matrix<Scalar, Dimension, Dimension>;
759  using RowMajorMatrixType = typename Eigen::internal::plain_matrix_type_row_major<Derived>::type;
760 
761  const Index m = src.rows (); // dimension
762  const Index n = src.cols (); // number of measurements
763 
764  // required for demeaning ...
765  const RealScalar one_over_n = 1 / static_cast<RealScalar> (n);
766 
767  // computation of mean
768  const VectorType src_mean = src.rowwise ().sum () * one_over_n;
769  const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
770 
771  // demeaning of src and dst points
772  const RowMajorMatrixType src_demean = src.colwise () - src_mean;
773  const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
774 
775  // Eq. (36)-(37)
776  const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
777 
778  // Eq. (38)
779  const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
780 
781  Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
782 
783  // Initialize the resulting transformation with an identity matrix...
784  TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
785 
786  // Eq. (39)
787  VectorType S = VectorType::Ones (m);
788 
789  if ( svd.matrixU ().determinant () * svd.matrixV ().determinant () < 0 )
790  S (m - 1) = -1;
791 
792  // Eq. (40) and (43)
793  Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
794 
795  if (with_scaling)
796  {
797  // Eq. (42)
798  const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S);
799 
800  // Eq. (41)
801  Rt.col (m).head (m) = dst_mean;
802  Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean;
803  Rt.block (0, 0, m, m) *= c;
804  }
805  else
806  {
807  Rt.col (m).head (m) = dst_mean;
808  Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
809  }
810 
811  return (Rt);
812 #endif
813 }
814 
815 //////////////////////////////////////////////////////////////////////////////////////////
816 template <typename Scalar> bool
817 pcl::transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
818  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
819  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
820 {
821  if (line_in.innerSize () != 6 || line_out.innerSize () != 6)
822  {
823  PCL_DEBUG ("transformLine: lines size != 6\n");
824  return (false);
825  }
826 
827  Eigen::Matrix<Scalar, 3, 1> point, vector;
828  point << line_in.template head<3> ();
829  vector << line_out.template tail<3> ();
830 
831  pcl::transformPoint (point, point, transformation);
832  pcl::transformVector (vector, vector, transformation);
833  line_out << point, vector;
834  return (true);
835 }
836 
837 //////////////////////////////////////////////////////////////////////////////////////////
838 template <typename Scalar> void
839 pcl::transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
840  Eigen::Matrix<Scalar, 4, 1> &plane_out,
841  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
842 {
843  Eigen::Hyperplane < Scalar, 3 > plane;
844  plane.coeffs () << plane_in;
845  plane.transform (transformation);
846  plane_out << plane.coeffs ();
847 
848  // Versions prior to 3.3.2 don't normalize the result
849  #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
850  plane_out /= plane_out.template head<3> ().norm ();
851  #endif
852 }
853 
854 //////////////////////////////////////////////////////////////////////////////////////////
855 template <typename Scalar> void
857  pcl::ModelCoefficients::Ptr plane_out,
858  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
859 {
860  std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
861  Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
862  pcl::transformPlane (v_plane_in, v_plane_in, transformation);
863  plane_out->values.resize (4);
864  std::copy_n(v_plane_in.data (), 4, plane_in->values.begin ());
865 }
866 
867 //////////////////////////////////////////////////////////////////////////////////////////
868 template <typename Scalar> bool
869 pcl::checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
870  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
871  const Scalar norm_limit,
872  const Scalar dot_limit)
873 {
874  if (line_x.innerSize () != 6 || line_y.innerSize () != 6)
875  {
876  PCL_DEBUG ("checkCoordinateSystem: lines size != 6\n");
877  return (false);
878  }
879 
880  if (line_x.template head<3> () != line_y.template head<3> ())
881  {
882  PCL_DEBUG ("checkCoorZdinateSystem: vector origins are different !\n");
883  return (false);
884  }
885 
886  // Make a copy of vector directions
887  // X^Y = Z | Y^Z = X | Z^X = Y
888  Eigen::Matrix<Scalar, 3, 1> v_line_x (line_x.template tail<3> ()),
889  v_line_y (line_y.template tail<3> ()),
890  v_line_z (v_line_x.cross (v_line_y));
891 
892  // Check vectors norms
893  if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit)
894  {
895  PCL_DEBUG ("checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ());
896  return (false);
897  }
898 
899  if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit)
900  {
901  PCL_DEBUG ("checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ());
902  return (false);
903  }
904 
905  if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit)
906  {
907  PCL_DEBUG ("checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ());
908  return (false);
909  }
910 
911  // Check vectors perendicularity
912  if (std::abs (v_line_x.dot (v_line_y)) > dot_limit)
913  {
914  PCL_DEBUG ("checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit);
915  return (false);
916  }
917 
918  if (std::abs (v_line_x.dot (v_line_z)) > dot_limit)
919  {
920  PCL_DEBUG ("checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit);
921  return (false);
922  }
923 
924  if (std::abs (v_line_y.dot (v_line_z)) > dot_limit)
925  {
926  PCL_DEBUG ("checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit);
927  return (false);
928  }
929 
930  return (true);
931 }
932 
933 //////////////////////////////////////////////////////////////////////////////////////////
934 template <typename Scalar> bool
935 pcl::transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
936  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
937  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
938  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
939  Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
940 {
941  if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
942  {
943  PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n");
944  return (false);
945  }
946 
947  // Check if coordinate systems are valid
948  if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y))
949  {
950  PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n");
951  return (false);
952  }
953 
954  // Convert lines into Vector3 :
955  Eigen::Matrix<Scalar, 3, 1> fr0 (from_line_x.template head<3>()),
956  fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
957  fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()),
958 
959  to0 (to_line_x.template head<3>()),
960  to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()),
961  to2 (to_line_y.template head<3>() + to_line_y.template tail<3>());
962 
963  // Code is inspired from http://stackoverflow.com/a/15277421/1816078
964  // Define matrices and points :
965  Eigen::Transform<Scalar, 3, Eigen::Affine> T2, T3 = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
966  Eigen::Matrix<Scalar, 3, 1> x1, y1, z1, x2, y2, z2;
967 
968  // Axes of the coordinate system "fr"
969  x1 = (fr1 - fr0).normalized (); // the versor (unitary vector) of the (fr1-fr0) axis vector
970  y1 = (fr2 - fr0).normalized ();
971 
972  // Axes of the coordinate system "to"
973  x2 = (to1 - to0).normalized ();
974  y2 = (to2 - to0).normalized ();
975 
976  // Transform from CS1 to CS2
977  // Note: if fr0 == (0,0,0) --> CS1 == CS2 --> T2 = Identity
978  T2.linear () << x1, y1, x1.cross (y1);
979 
980  // Transform from CS1 to CS3
981  T3.linear () << x2, y2, x2.cross (y2);
982 
983  // Identity matrix = transform to CS2 to CS3
984  // Note: if CS1 == CS2 --> transformation = T3
985  transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
986  transformation.linear () = T3.linear () * T2.linear ().inverse ();
987  transformation.translation () = to0 - (transformation.linear () * fr0);
988  return (true);
989 }
990 
991 #endif //PCL_COMMON_EIGEN_IMPL_HPP_
992 
void transformPoint(const Eigen::Matrix< Scalar, 3, 1 > &point_in, Eigen::Matrix< Scalar, 3, 1 > &point_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a point using an affine matrix.
Definition: eigen.h:423
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
Definition: eigen.hpp:719
void getTransFromUnitVectorsZY(const Eigen::Vector3f &z_axis, const Eigen::Vector3f &y_direction, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate z_axis into (0,0,1) and y_direction into a vector with x=...
Definition: eigen.hpp:584
void computeCorrespondingEigenVector(const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi defin...
Definition: eigen.hpp:221
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
Definition: eigen.hpp:487
void transformVector(const Eigen::Matrix< Scalar, 3, 1 > &vector_in, Eigen::Matrix< Scalar, 3, 1 > &vector_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a vector using an affine matrix.
Definition: eigen.h:456
void getTransformationFromTwoUnitVectorsAndOrigin(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, const Eigen::Vector3f &origin, Eigen::Affine3f &transformation)
Get the transformation that will translate origin to (0,0,0) and rotate z_axis into (0...
Definition: eigen.hpp:654
bool checkCoordinateSystem(const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_x, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_y, const Scalar norm_limit=1e-3, const Scalar dot_limit=1e-3)
Check coordinate system integrity.
Definition: eigen.hpp:869
Eigen::internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const Eigen::MatrixBase< Derived > &src, const Eigen::MatrixBase< OtherDerived > &dst, bool with_scaling=false)
Returns the transformation between two point sets.
Definition: eigen.hpp:741
void getTransformationFromTwoUnitVectors(const Eigen::Vector3f &y_direction, const Eigen::Vector3f &z_axis, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate z_axis into (0,0,1) and y_direction into a vector with x=...
Definition: eigen.hpp:636
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues ...
Definition: eigen.hpp:63
void getTransFromUnitVectorsXY(const Eigen::Vector3f &x_axis, const Eigen::Vector3f &y_direction, Eigen::Affine3f &transformation)
Get the unique 3D rotation that will rotate x_axis into (1,0,0) and y_direction into a vector with z=...
Definition: eigen.hpp:610
void getEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract the Euler angles (XYZ-convention) from the given transformation.
Definition: eigen.hpp:666
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Definition: eigen.hpp:704
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Definition: eigen.hpp:128
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Definition: eigen.hpp:574
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
Definition: eigen.hpp:541
boost::shared_ptr< ::pcl::ModelCoefficients > Ptr
bool transformBetween2CoordinateSystems(const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > from_line_x, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > from_line_y, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > to_line_x, const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > to_line_y, Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Compute the transformation between two coordinate systems.
Definition: eigen.hpp:935
void getTranslationAndEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &x, Scalar &y, Scalar &z, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation.
Definition: eigen.hpp:675
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:253
bool transformLine(const Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_in, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &line_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform a line using an affine matrix.
Definition: eigen.hpp:817
void getTransformation(Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform< Scalar, 3, Eigen::Affine > &t)
Create a transformation from the given translation and Euler angles (XYZ-convention) ...
Definition: eigen.hpp:689
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition: eigen.hpp:506
void transformPlane(const Eigen::Matrix< Scalar, 4, 1 > &plane_in, Eigen::Matrix< Scalar, 4, 1 > &plane_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transformation)
Transform plane vectors using an affine matrix.
Definition: eigen.hpp:839
void computeRoots2(const Scalar &b, const Scalar &c, Roots &roots)
Compute the roots of a quadratic polynom x^2 + b*x + c = 0.
Definition: eigen.hpp:48
Definition: norms.h:54