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