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