39 #ifndef PCL_COMMON_EIGEN_IMPL_HPP_
40 #define PCL_COMMON_EIGEN_IMPL_HPP_
42 #include <pcl/console/print.h>
45 template <
typename Scalar,
typename Roots>
inline void
48 roots (0) = Scalar (0);
49 Scalar d = Scalar (b * b - 4.0 * c);
53 Scalar sd = ::std::sqrt (d);
55 roots (2) = 0.5f * (b + sd);
56 roots (1) = 0.5f * (b - sd);
60 template <
typename Matrix,
typename Roots>
inline void
63 typedef typename Matrix::Scalar Scalar;
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) -
79 Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
81 if (fabs (c0) < Eigen::NumTraits < Scalar > ::epsilon ())
85 const Scalar s_inv3 = Scalar (1.0 / 3.0);
86 const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
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);
94 Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
96 Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
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);
110 if (roots (0) >= roots (1))
111 std::swap (roots (0), roots (1));
112 if (roots (1) >= roots (2))
114 std::swap (roots (1), roots (2));
115 if (roots (0) >= roots (1))
116 std::swap (roots (0), roots (1));
125 template <
typename Matrix,
typename Vector>
inline void
126 pcl::eigen22 (
const Matrix& mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
130 if (fabs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
132 if (mat.coeff (0) < mat.coeff (2))
134 eigenvalue = mat.coeff (0);
135 eigenvector[0] = 1.0;
136 eigenvector[1] = 0.0;
140 eigenvalue = mat.coeff (2);
141 eigenvector[0] = 0.0;
142 eigenvector[1] = 1.0;
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);
151 typename Matrix::Scalar temp = trace * trace - determinant;
156 eigenvalue = trace - ::std::sqrt (temp);
158 eigenvector[0] = -mat.coeff (1);
159 eigenvector[1] = mat.coeff (0) - eigenvalue;
160 eigenvector.normalize ();
164 template <
typename Matrix,
typename Vector>
inline void
165 pcl::eigen22 (
const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
169 if (fabs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
171 if (mat.coeff (0) < mat.coeff (3))
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;
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;
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);
196 typename Matrix::Scalar temp = trace * trace - determinant;
201 temp = ::std::sqrt (temp);
203 eigenvalues.coeffRef (0) = trace - temp;
204 eigenvalues.coeffRef (1) = trace + temp;
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);
218 template <
typename Matrix,
typename Vector>
inline void
221 typedef typename Matrix::Scalar Scalar;
225 Scalar scale = mat.cwiseAbs ().maxCoeff ();
226 if (scale <= std::numeric_limits < Scalar > ::min ())
227 scale = Scalar (1.0);
229 Matrix scaledMat = mat / scale;
231 scaledMat.diagonal ().array () -= eigenvalue / scale;
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));
237 Scalar len1 = vec1.squaredNorm ();
238 Scalar len2 = vec2.squaredNorm ();
239 Scalar len3 = vec3.squaredNorm ();
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);
246 eigenvector = vec3 / std::sqrt (len3);
250 template <
typename Matrix,
typename Vector>
inline void
251 pcl::eigen33 (
const Matrix& mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
253 typedef typename Matrix::Scalar Scalar;
257 Scalar scale = mat.cwiseAbs ().maxCoeff ();
258 if (scale <= std::numeric_limits < Scalar > ::min ())
259 scale = Scalar (1.0);
261 Matrix scaledMat = mat / scale;
266 eigenvalue = eigenvalues (0) * scale;
268 scaledMat.diagonal ().array () -= eigenvalues (0);
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));
274 Scalar len1 = vec1.squaredNorm ();
275 Scalar len2 = vec2.squaredNorm ();
276 Scalar len3 = vec3.squaredNorm ();
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);
283 eigenvector = vec3 / std::sqrt (len3);
287 template <
typename Matrix,
typename Vector>
inline void
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);
295 Matrix scaledMat = mat / scale;
301 template <
typename Matrix,
typename Vector>
inline void
304 typedef typename Matrix::Scalar Scalar;
308 Scalar scale = mat.cwiseAbs ().maxCoeff ();
309 if (scale <= std::numeric_limits < Scalar > ::min ())
310 scale = Scalar (1.0);
312 Matrix scaledMat = mat / scale;
317 if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
320 evecs.setIdentity ();
322 else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
327 tmp.diagonal ().array () -= evals (2);
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));
333 Scalar len1 = vec1.squaredNorm ();
334 Scalar len2 = vec2.squaredNorm ();
335 Scalar len3 = vec3.squaredNorm ();
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);
342 evecs.col (2) = vec3 / std::sqrt (len3);
344 evecs.col (1) = evecs.col (2).unitOrthogonal ();
345 evecs.col (0) = evecs.col (1).cross (evecs.col (2));
347 else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ())
352 tmp.diagonal ().array () -= evals (0);
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));
358 Scalar len1 = vec1.squaredNorm ();
359 Scalar len2 = vec2.squaredNorm ();
360 Scalar len3 = vec3.squaredNorm ();
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);
367 evecs.col (0) = vec3 / std::sqrt (len3);
369 evecs.col (1) = evecs.col (0).unitOrthogonal ();
370 evecs.col (2) = evecs.col (0).cross (evecs.col (1));
376 tmp.diagonal ().array () -= evals (2);
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));
382 Scalar len1 = vec1.squaredNorm ();
383 Scalar len2 = vec2.squaredNorm ();
384 Scalar len3 = vec3.squaredNorm ();
386 Scalar *mmax =
new Scalar[3];
390 unsigned int min_el = 2;
391 unsigned int max_el = 2;
392 if (len1 >= len2 && len1 >= len3)
395 evecs.col (2) = vec1 / std::sqrt (len1);
397 else if (len2 >= len1 && len2 >= len3)
400 evecs.col (2) = vec2 / std::sqrt (len2);
405 evecs.col (2) = vec3 / std::sqrt (len3);
409 tmp.diagonal ().array () -= evals (1);
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));
415 len1 = vec1.squaredNorm ();
416 len2 = vec2.squaredNorm ();
417 len3 = vec3.squaredNorm ();
418 if (len1 >= len2 && len1 >= len3)
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;
425 else if (len2 >= len1 && len2 >= len3)
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;
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;
441 tmp.diagonal ().array () -= evals (0);
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));
447 len1 = vec1.squaredNorm ();
448 len2 = vec2.squaredNorm ();
449 len3 = vec3.squaredNorm ();
450 if (len1 >= len2 && len1 >= len3)
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;
457 else if (len2 >= len1 && len2 >= len3)
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;
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;
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 ();
484 template <
typename Matrix>
inline typename Matrix::Scalar
487 typedef typename Matrix::Scalar Scalar;
488 Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2);
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);
503 template <
typename Matrix>
inline typename Matrix::Scalar
506 typedef typename Matrix::Scalar Scalar;
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);
521 Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
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));
538 template <
typename Matrix>
inline typename Matrix::Scalar
541 typedef typename Matrix::Scalar Scalar;
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);
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);
571 template <
typename Matrix>
inline typename Matrix::Scalar
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)) ;
583 const Eigen::Vector3f& y_direction,
584 Eigen::Affine3f& transformation)
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();
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;
599 const Eigen::Vector3f& y_direction)
601 Eigen::Affine3f transformation;
603 return (transformation);
609 const Eigen::Vector3f& y_direction,
610 Eigen::Affine3f& transformation)
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();
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;
625 const Eigen::Vector3f& y_direction)
627 Eigen::Affine3f transformation;
629 return (transformation);
635 const Eigen::Vector3f& z_axis,
636 Eigen::Affine3f& transformation)
644 const Eigen::Vector3f& z_axis)
646 Eigen::Affine3f transformation;
648 return (transformation);
653 const Eigen::Vector3f& z_axis,
654 const Eigen::Vector3f& origin,
655 Eigen::Affine3f& transformation)
658 Eigen::Vector3f translation = transformation*origin;
659 transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2];
663 template <
typename Scalar>
void
664 pcl::getEulerAngles (
const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
666 roll = atan2 (t (2, 1), t (2, 2));
667 pitch = asin (-t (2, 0));
668 yaw = atan2 (t (1, 0), t (0, 0));
672 template <
typename Scalar>
void
674 Scalar &x, Scalar &y, Scalar &z,
675 Scalar &roll, Scalar &pitch, Scalar &yaw)
680 roll = atan2 (t (2, 1), t (2, 2));
681 pitch = asin (-t (2, 0));
682 yaw = atan2 (t (1, 0), t (0, 0));
686 template <
typename Scalar>
void
688 Scalar roll, Scalar pitch, Scalar yaw,
689 Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
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;
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;
701 template <
typename Derived>
void
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)
710 typename Derived::Scalar tmp = matrix(i,j);
711 file.write (reinterpret_cast<const char*> (&tmp),
sizeof (tmp));
716 template <
typename Derived>
void
719 Eigen::MatrixBase<Derived> &matrix =
const_cast<Eigen::MatrixBase<Derived> &
> (matrix_);
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);
727 for (uint32_t i = 0; i < rows; ++i)
728 for (uint32_t j = 0; j < cols; ++j)
730 typename Derived::Scalar tmp;
731 file.read (reinterpret_cast<char*> (&tmp),
sizeof (tmp));
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)
741 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
742 return Eigen::umeyama (src, dst, with_scaling);
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;
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)
753 enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
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;
759 const Index m = src.rows ();
760 const Index n = src.cols ();
763 const RealScalar one_over_n = 1 /
static_cast<RealScalar
> (n);
766 const VectorType src_mean = src.rowwise ().sum () * one_over_n;
767 const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
770 const RowMajorMatrixType src_demean = src.colwise () - src_mean;
771 const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
774 const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
777 const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
779 Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
782 TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
785 VectorType S = VectorType::Ones (m);
787 if ( svd.matrixU ().determinant () * svd.matrixV ().determinant () < 0 )
791 Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
796 const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S);
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;
805 Rt.col (m).head (m) = dst_mean;
806 Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
814 template <
typename Scalar>
bool
816 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
817 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
819 if (line_in.innerSize () != 6 || line_out.innerSize () != 6)
821 PCL_DEBUG (
"transformLine: lines size != 6\n");
825 Eigen::Matrix<Scalar, 3, 1> point, vector;
826 point << line_in.template head<3> ();
827 vector << line_out.template tail<3> ();
831 line_out << point, vector;
836 template <
typename Scalar>
void
838 Eigen::Matrix<Scalar, 4, 1> &plane_out,
839 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
841 Eigen::Hyperplane < Scalar, 3 > plane;
842 plane.coeffs () << plane_in;
843 plane.transform (transformation);
844 plane_out << plane.coeffs ();
847 #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
848 plane_out /= plane_out.template head<3> ().norm ();
853 template <
typename Scalar>
void
856 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
858 std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
859 Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
861 plane_out->values.resize (4);
862 for (
int i = 0; i < 4; i++)
863 plane_in->values[i] = v_plane_in[i];
867 template <
typename Scalar>
bool
869 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
870 const Scalar norm_limit,
871 const Scalar dot_limit)
873 if (line_x.innerSize () != 6 || line_y.innerSize () != 6)
875 PCL_DEBUG (
"checkCoordinateSystem: lines size != 6\n");
879 if (line_x.template head<3> () != line_y.template head<3> ())
881 PCL_DEBUG (
"checkCoorZdinateSystem: vector origins are different !\n");
887 Eigen::Matrix<Scalar, 3, 1> v_line_x (line_x.template tail<3> ()),
888 v_line_y (line_y.template tail<3> ()),
889 v_line_z (v_line_x.cross (v_line_y));
892 if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit)
894 PCL_DEBUG (
"checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ());
898 if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit)
900 PCL_DEBUG (
"checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ());
904 if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit)
906 PCL_DEBUG (
"checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ());
911 if (std::abs (v_line_x.dot (v_line_y)) > dot_limit)
913 PCL_DEBUG (
"checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit);
917 if (std::abs (v_line_x.dot (v_line_z)) > dot_limit)
919 PCL_DEBUG (
"checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit);
923 if (std::abs (v_line_y.dot (v_line_z)) > dot_limit)
925 PCL_DEBUG (
"checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit);
933 template <
typename Scalar>
bool
935 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
936 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
937 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
938 Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
940 if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
942 PCL_DEBUG (
"transformBetween2CoordinateSystems: lines size != 6\n");
949 PCL_DEBUG (
"transformBetween2CoordinateSystems: coordinate systems invalid !\n");
954 Eigen::Matrix<Scalar, 3, 1> fr0 (from_line_x.template head<3>()),
955 fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
956 fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()),
958 to0 (to_line_x.template head<3>()),
959 to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()),
960 to2 (to_line_y.template head<3>() + to_line_y.template tail<3>());
964 Eigen::Transform<Scalar, 3, Eigen::Affine> T2, T3 = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
965 Eigen::Matrix<Scalar, 3, 1> x1, y1, z1, x2, y2, z2;
968 x1 = (fr1 - fr0).normalized ();
969 y1 = (fr2 - fr0).normalized ();
972 x2 = (to1 - to0).normalized ();
973 y2 = (to2 - to0).normalized ();
977 T2.linear () << x1, y1, x1.cross (y1);
980 T3.linear () << x2, y2, x2.cross (y2);
984 transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
985 transformation.linear () = T3.linear () * T2.linear ().inverse ();
986 transformation.translation () = to0 - (transformation.linear () * fr0);
990 #endif //PCL_COMMON_EIGEN_IMPL_HPP_
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.
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
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=...
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...
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
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.
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...
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.
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.
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=...
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues ...
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=...
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.
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
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.
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.
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...
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.
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) ...
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
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.
void computeRoots2(const Scalar &b, const Scalar &c, Roots &roots)
Compute the roots of a quadratic polynom x^2 + b*x + c = 0.