39 #ifndef PCL_COMMON_EIGEN_IMPL_HPP_
40 #define PCL_COMMON_EIGEN_IMPL_HPP_
45 #include <pcl/console/print.h>
48 template <
typename Scalar,
typename Roots>
inline void
51 roots (0) = Scalar (0);
52 Scalar d = Scalar (b * b - 4.0 * c);
56 Scalar sd = std::sqrt (d);
58 roots (2) = 0.5f * (b + sd);
59 roots (1) = 0.5f * (b - sd);
63 template <
typename Matrix,
typename Roots>
inline void
66 using Scalar =
typename Matrix::Scalar;
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) -
82 Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
84 if (std::abs (c0) < Eigen::NumTraits < Scalar > ::epsilon ())
88 const Scalar s_inv3 = Scalar (1.0 / 3.0);
89 const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
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);
97 Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
99 Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
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);
113 if (roots (0) >= roots (1))
114 std::swap (roots (0), roots (1));
115 if (roots (1) >= roots (2))
117 std::swap (roots (1), roots (2));
118 if (roots (0) >= roots (1))
119 std::swap (roots (0), roots (1));
128 template <
typename Matrix,
typename Vector>
inline void
129 pcl::eigen22 (
const Matrix& mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
133 if (std::abs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
135 if (mat.coeff (0) < mat.coeff (2))
137 eigenvalue = mat.coeff (0);
138 eigenvector[0] = 1.0;
139 eigenvector[1] = 0.0;
143 eigenvalue = mat.coeff (2);
144 eigenvector[0] = 0.0;
145 eigenvector[1] = 1.0;
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);
154 typename Matrix::Scalar temp = trace * trace - determinant;
159 eigenvalue = trace - std::sqrt (temp);
161 eigenvector[0] = -mat.coeff (1);
162 eigenvector[1] = mat.coeff (0) - eigenvalue;
163 eigenvector.normalize ();
167 template <
typename Matrix,
typename Vector>
inline void
168 pcl::eigen22 (
const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
172 if (std::abs (mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
174 if (mat.coeff (0) < mat.coeff (3))
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;
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;
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);
199 typename Matrix::Scalar temp = trace * trace - determinant;
204 temp = std::sqrt (temp);
206 eigenvalues.coeffRef (0) = trace - temp;
207 eigenvalues.coeffRef (1) = trace + temp;
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);
221 template <
typename Matrix,
typename Vector>
inline void
224 using Scalar =
typename Matrix::Scalar;
228 Scalar scale = mat.cwiseAbs ().maxCoeff ();
229 if (scale <= std::numeric_limits < Scalar > ::min ())
230 scale = Scalar (1.0);
232 Matrix scaledMat = mat / scale;
234 scaledMat.diagonal ().array () -= eigenvalue / scale;
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));
240 Scalar len1 = vec1.squaredNorm ();
241 Scalar len2 = vec2.squaredNorm ();
242 Scalar len3 = vec3.squaredNorm ();
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);
249 eigenvector = vec3 / std::sqrt (len3);
254 template <
typename Vector,
typename Scalar>
270 using Scalar =
typename Matrix::Scalar;
271 using Index =
typename Matrix::Index;
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));
279 const auto len = crossProduct.rowwise ().norm ();
282 const Scalar length = len.maxCoeff (&index);
290 template <
typename Matrix,
typename Vector>
inline void
291 pcl::eigen33 (
const Matrix& mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
293 using Scalar =
typename Matrix::Scalar;
297 Scalar scale = mat.cwiseAbs ().maxCoeff ();
298 if (scale <= std::numeric_limits < Scalar > ::min ())
299 scale = Scalar (1.0);
301 Matrix scaledMat = mat / scale;
306 eigenvalue = eigenvalues (0) * scale;
308 scaledMat.diagonal ().array () -= eigenvalues (0);
310 eigenvector = detail::getLargest3x3Eigenvector<Vector> (scaledMat).vector;
314 template <
typename Matrix,
typename Vector>
inline void
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);
322 Matrix scaledMat = mat / scale;
328 template <
typename Matrix,
typename Vector>
inline void
331 using Scalar =
typename Matrix::Scalar;
335 Scalar scale = mat.cwiseAbs ().maxCoeff ();
336 if (scale <= std::numeric_limits < Scalar > ::min ())
337 scale = Scalar (1.0);
339 Matrix scaledMat = mat / scale;
344 if ( (evals (2) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
347 evecs.setIdentity ();
349 else if ( (evals (1) - evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
354 tmp.diagonal ().array () -= evals (2);
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));
360 else if ( (evals (2) - evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ())
365 tmp.diagonal ().array () -= evals (0);
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));
373 std::array<Scalar, 3> eigenVecLen;
375 for (
int i = 0; i < 3; ++i)
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;
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;
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 ();
399 template <
typename Matrix>
inline typename Matrix::Scalar
402 using Scalar =
typename Matrix::Scalar;
403 Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2);
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);
418 template <
typename Matrix>
inline typename Matrix::Scalar
421 using Scalar =
typename Matrix::Scalar;
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);
436 Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
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));
453 template <
typename Matrix>
inline typename Matrix::Scalar
456 using Scalar =
typename Matrix::Scalar;
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);
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);
486 template <
typename Matrix>
inline typename Matrix::Scalar
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)) ;
498 const Eigen::Vector3f& y_direction,
499 Eigen::Affine3f& transformation)
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();
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;
514 const Eigen::Vector3f& y_direction)
516 Eigen::Affine3f transformation;
518 return (transformation);
524 const Eigen::Vector3f& y_direction,
525 Eigen::Affine3f& transformation)
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();
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;
540 const Eigen::Vector3f& y_direction)
542 Eigen::Affine3f transformation;
544 return (transformation);
550 const Eigen::Vector3f& z_axis,
551 Eigen::Affine3f& transformation)
559 const Eigen::Vector3f& z_axis)
561 Eigen::Affine3f transformation;
563 return (transformation);
568 const Eigen::Vector3f& z_axis,
569 const Eigen::Vector3f& origin,
570 Eigen::Affine3f& transformation)
573 Eigen::Vector3f translation = transformation*origin;
574 transformation(0,3)=-translation[0]; transformation(1,3)=-translation[1]; transformation(2,3)=-translation[2];
578 template <
typename Scalar>
void
579 pcl::getEulerAngles (
const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
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));
587 template <
typename Scalar>
void
589 Scalar &x, Scalar &y, Scalar &z,
590 Scalar &roll, Scalar &pitch, Scalar &yaw)
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));
601 template <
typename Scalar>
void
603 Scalar roll, Scalar pitch, Scalar yaw,
604 Eigen::Transform<Scalar, 3, Eigen::Affine> &t)
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;
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;
616 template <
typename Derived>
void
620 file.write (
reinterpret_cast<char*
> (&rows),
sizeof (rows));
621 file.write (
reinterpret_cast<char*
> (&cols),
sizeof (cols));
625 typename Derived::Scalar tmp = matrix(i,j);
626 file.write (
reinterpret_cast<const char*
> (&tmp),
sizeof (tmp));
631 template <
typename Derived>
void
634 Eigen::MatrixBase<Derived> &matrix =
const_cast<Eigen::MatrixBase<Derived> &
> (matrix_);
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);
645 typename Derived::Scalar tmp;
646 file.read (
reinterpret_cast<char*
> (&tmp),
sizeof (tmp));
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)
656 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
657 return Eigen::umeyama (src, dst, with_scaling);
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;
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.");
668 enum { Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
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;
674 const Index m = src.rows ();
675 const Index n = src.cols ();
678 const RealScalar one_over_n = 1 /
static_cast<RealScalar
> (n);
681 const VectorType src_mean = src.rowwise ().sum () * one_over_n;
682 const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
685 const RowMajorMatrixType src_demean = src.colwise () - src_mean;
686 const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
689 const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
692 const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
694 Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
697 TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
700 VectorType S = VectorType::Ones (m);
702 if ( svd.matrixU ().determinant () * svd.matrixV ().determinant () < 0 )
706 Rt.block (0,0,m,m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
711 const Scalar c = Scalar (1)/ src_var * svd.singularValues ().dot (S);
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;
720 Rt.col (m).head (m) = dst_mean;
721 Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
729 template <
typename Scalar>
bool
731 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
732 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
734 if (line_in.innerSize () != 6 || line_out.innerSize () != 6)
736 PCL_DEBUG (
"transformLine: lines size != 6\n");
740 Eigen::Matrix<Scalar, 3, 1> point, vector;
741 point << line_in.template head<3> ();
742 vector << line_out.template tail<3> ();
746 line_out << point, vector;
751 template <
typename Scalar>
void
753 Eigen::Matrix<Scalar, 4, 1> &plane_out,
754 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
756 Eigen::Hyperplane < Scalar, 3 > plane;
757 plane.coeffs () << plane_in;
758 plane.transform (transformation);
759 plane_out << plane.coeffs ();
762 #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
763 plane_out /= plane_out.template head<3> ().norm ();
768 template <
typename Scalar>
void
771 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
773 std::vector<Scalar> values (plane_in->values.begin (), plane_in->values.end ());
774 Eigen::Matrix < Scalar, 4, 1 > v_plane_in (values.data ());
776 plane_out->values.resize (4);
777 std::copy_n(v_plane_in.data (), 4, plane_in->values.begin ());
781 template <
typename Scalar>
bool
783 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
784 const Scalar norm_limit,
785 const Scalar dot_limit)
787 if (line_x.innerSize () != 6 || line_y.innerSize () != 6)
789 PCL_DEBUG (
"checkCoordinateSystem: lines size != 6\n");
793 if (line_x.template head<3> () != line_y.template head<3> ())
795 PCL_DEBUG (
"checkCoorZdinateSystem: vector origins are different !\n");
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));
806 if (v_line_x.norm () < 1 - norm_limit || v_line_x.norm () > 1 + norm_limit)
808 PCL_DEBUG (
"checkCoordinateSystem: line_x norm %d != 1\n", v_line_x.norm ());
812 if (v_line_y.norm () < 1 - norm_limit || v_line_y.norm () > 1 + norm_limit)
814 PCL_DEBUG (
"checkCoordinateSystem: line_y norm %d != 1\n", v_line_y.norm ());
818 if (v_line_z.norm () < 1 - norm_limit || v_line_z.norm () > 1 + norm_limit)
820 PCL_DEBUG (
"checkCoordinateSystem: line_z norm %d != 1\n", v_line_z.norm ());
825 if (std::abs (v_line_x.dot (v_line_y)) > dot_limit)
827 PCL_DEBUG (
"checkCSAxis: line_x dot line_y %e = > %e\n", v_line_x.dot (v_line_y), dot_limit);
831 if (std::abs (v_line_x.dot (v_line_z)) > dot_limit)
833 PCL_DEBUG (
"checkCSAxis: line_x dot line_z = %e > %e\n", v_line_x.dot (v_line_z), dot_limit);
837 if (std::abs (v_line_y.dot (v_line_z)) > dot_limit)
839 PCL_DEBUG (
"checkCSAxis: line_y dot line_z = %e > %e\n", v_line_y.dot (v_line_z), dot_limit);
847 template <
typename Scalar>
bool
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)
854 if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
856 PCL_DEBUG (
"transformBetween2CoordinateSystems: lines size != 6\n");
863 PCL_DEBUG (
"transformBetween2CoordinateSystems: coordinate systems invalid !\n");
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>()),
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>());
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;
882 x1 = (fr1 - fr0).normalized ();
883 y1 = (fr2 - fr0).normalized ();
886 x2 = (to1 - to0).normalized ();
887 y2 = (to2 - to0).normalized ();
891 T2.linear () << x1, y1, x1.cross (y1);
894 T3.linear () << x2, y2, x2.cross (y2);
898 transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
899 transformation.linear () = T3.linear () * T2.linear ().inverse ();
900 transformation.translation () = to0 - (transformation.linear () * fr0);
904 #endif //PCL_COMMON_EIGEN_IMPL_HPP_