41#include <pcl/common/eigen.h>
42#include <pcl/console/print.h>
52template <
typename Scalar,
typename Roots>
inline void
55 roots (0) = Scalar (0);
56 Scalar d = Scalar (b * b - 4.0 *
c);
60 Scalar
sd = std::sqrt (d);
67template <
typename Matrix,
typename Roots>
inline void
70 using Scalar =
typename Matrix::Scalar;
75 Scalar
c0 =
m (0, 0) *
m (1, 1) *
m (2, 2)
76 + Scalar (2) *
m (0, 1) *
m (0, 2) *
m (1, 2)
77 -
m (0, 0) *
m (1, 2) *
m (1, 2)
78 -
m (1, 1) *
m (0, 2) *
m (0, 2)
79 -
m (2, 2) *
m (0, 1) *
m (0, 1);
80 Scalar c1 =
m (0, 0) *
m (1, 1) -
86 Scalar c2 =
m (0, 0) +
m (1, 1) +
m (2, 2);
88 if (std::abs (
c0) < Eigen::NumTraits < Scalar > ::epsilon ())
92 const Scalar
s_inv3 = Scalar (1.0 / 3.0);
93 const Scalar
s_sqrt3 = std::sqrt (Scalar (3.0));
132template <
typename Matrix,
typename Vector>
inline void
133eigen22 (
const Matrix&
mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
137 if (std::abs (
mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
139 if (
mat.coeff (0) <
mat.coeff (2))
141 eigenvalue =
mat.coeff (0);
142 eigenvector[0] = 1.0;
143 eigenvector[1] = 0.0;
147 eigenvalue =
mat.coeff (2);
148 eigenvector[0] = 0.0;
149 eigenvector[1] = 1.0;
155 typename Matrix::Scalar
trace =
static_cast<typename Matrix::Scalar
> (0.5) * (
mat.coeff (0) +
mat.coeff (3));
156 typename Matrix::Scalar determinant =
mat.coeff (0) *
mat.coeff (3) -
mat.coeff (1) *
mat.coeff (1);
165 eigenvector[0] = -
mat.coeff (1);
166 eigenvector[1] =
mat.coeff (0) - eigenvalue;
167 eigenvector.normalize ();
171template <
typename Matrix,
typename Vector>
inline void
176 if (std::abs (
mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
178 if (
mat.coeff (0) <
mat.coeff (3))
200 typename Matrix::Scalar
trace =
static_cast<typename Matrix::Scalar
> (0.5) * (
mat.coeff (0) +
mat.coeff (3));
201 typename Matrix::Scalar determinant =
mat.coeff (0) *
mat.coeff (3) -
mat.coeff (1) *
mat.coeff (1);
216 typename Matrix::Scalar norm =
static_cast<typename Matrix::Scalar
> (1.0)
225template <
typename Matrix,
typename Vector>
inline void
228 using Scalar =
typename Matrix::Scalar;
232 Scalar scale =
mat.cwiseAbs ().maxCoeff ();
233 if (scale <= std::numeric_limits < Scalar > ::min ())
234 scale = Scalar (1.0);
238 scaledMat.diagonal ().array () -= eigenvalue / scale;
249 eigenvector =
vec1 / std::sqrt (
len1);
251 eigenvector =
vec2 / std::sqrt (
len2);
253 eigenvector =
vec3 / std::sqrt (
len3);
259template <
typename Vector,
typename Scalar>
275 using Scalar =
typename Matrix::Scalar;
276 using Index =
typename Matrix::Index;
284 const auto len = crossProduct.rowwise ().norm ();
287 const Scalar length =
len.maxCoeff (&index);
295template <
typename Matrix,
typename Vector>
inline void
296eigen33 (
const Matrix&
mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
298 using Scalar =
typename Matrix::Scalar;
302 Scalar scale =
mat.cwiseAbs ().maxCoeff ();
303 if (scale <= std::numeric_limits < Scalar > ::min ())
304 scale = Scalar (1.0);
315 eigenvector = detail::getLargest3x3Eigenvector<Vector> (
scaledMat).vector;
319template <
typename Matrix,
typename Vector>
inline void
322 using Scalar =
typename Matrix::Scalar;
323 Scalar scale =
mat.cwiseAbs ().maxCoeff ();
324 if (scale <= std::numeric_limits < Scalar > ::min ())
325 scale = Scalar (1.0);
333template <
typename Matrix,
typename Vector>
inline void
336 using Scalar =
typename Matrix::Scalar;
340 Scalar scale =
mat.cwiseAbs ().maxCoeff ();
341 if (scale <= std::numeric_limits < Scalar > ::min ())
342 scale = Scalar (1.0);
349 if ( (
evals (2) -
evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
352 evecs.setIdentity ();
354 else if ( (
evals (1) -
evals (0)) <= Eigen::NumTraits < Scalar > ::epsilon ())
359 tmp.diagonal ().array () -=
evals (2);
361 evecs.col (2) = detail::getLargest3x3Eigenvector<Vector> (
tmp).vector;
362 evecs.col (1) =
evecs.col (2).unitOrthogonal ();
365 else if ( (
evals (2) -
evals (1)) <= Eigen::NumTraits < Scalar > ::epsilon ())
370 tmp.diagonal ().array () -=
evals (0);
372 evecs.col (0) = detail::getLargest3x3Eigenvector<Vector> (
tmp).vector;
373 evecs.col (1) =
evecs.col (0).unitOrthogonal ();
380 for (
int i = 0; i < 3; ++i)
383 tmp.diagonal ().array () -=
evals (i);
384 const auto vec_len = detail::getLargest3x3Eigenvector<Vector> (
tmp);
404template <
typename Matrix>
inline typename Matrix::Scalar
407 using Scalar =
typename Matrix::Scalar;
413 inverse.coeffRef (0) =
matrix.coeff (3);
414 inverse.coeffRef (1) = -
matrix.coeff (1);
415 inverse.coeffRef (2) = -
matrix.coeff (2);
416 inverse.coeffRef (3) =
matrix.coeff (0);
423template <
typename Matrix>
inline typename Matrix::Scalar
426 using Scalar =
typename Matrix::Scalar;
446 inverse.coeffRef (0) =
fd_ee;
447 inverse.coeffRef (1) = inverse.coeffRef (3) =
ce_bf;
448 inverse.coeffRef (2) = inverse.coeffRef (6) =
be_cd;
450 inverse.coeffRef (5) = inverse.coeffRef (7) = (
matrix.coeff (1) *
matrix.coeff (2) -
matrix.coeff (0) *
matrix.coeff (5));
458template <
typename Matrix>
inline typename Matrix::Scalar
461 using Scalar =
typename Matrix::Scalar;
475 inverse.coeffRef (0) =
ie_hf;
476 inverse.coeffRef (1) =
hc_ib;
477 inverse.coeffRef (2) =
fb_ec;
491template <
typename Matrix>
inline typename Matrix::Scalar
504 Eigen::Affine3f& transformation)
507 Eigen::Vector3f
tmp1 = (z_axis.cross(
tmp0)).normalized();
508 Eigen::Vector3f
tmp2 = z_axis.normalized();
510 transformation(0,0)=
tmp0[0]; transformation(0,1)=
tmp0[1]; transformation(0,2)=
tmp0[2]; transformation(0,3)=0.0f;
511 transformation(1,0)=
tmp1[0]; transformation(1,1)=
tmp1[1]; transformation(1,2)=
tmp1[2]; transformation(1,3)=0.0f;
512 transformation(2,0)=
tmp2[0]; transformation(2,1)=
tmp2[1]; transformation(2,2)=
tmp2[2]; transformation(2,3)=0.0f;
513 transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
521 Eigen::Affine3f transformation;
523 return (transformation);
530 Eigen::Affine3f& transformation)
533 Eigen::Vector3f
tmp1 = (
tmp2.cross(x_axis)).normalized();
534 Eigen::Vector3f
tmp0 = x_axis.normalized();
536 transformation(0,0)=
tmp0[0]; transformation(0,1)=
tmp0[1]; transformation(0,2)=
tmp0[2]; transformation(0,3)=0.0f;
537 transformation(1,0)=
tmp1[0]; transformation(1,1)=
tmp1[1]; transformation(1,2)=
tmp1[2]; transformation(1,3)=0.0f;
538 transformation(2,0)=
tmp2[0]; transformation(2,1)=
tmp2[1]; transformation(2,2)=
tmp2[2]; transformation(2,3)=0.0f;
539 transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
547 Eigen::Affine3f transformation;
549 return (transformation);
555 const Eigen::Vector3f& z_axis,
556 Eigen::Affine3f& transformation)
564 const Eigen::Vector3f& z_axis)
566 Eigen::Affine3f transformation;
568 return (transformation);
574 const Eigen::Vector3f& z_axis,
575 const Eigen::Vector3f&
origin,
576 Eigen::Affine3f& transformation)
584template <
typename Scalar>
void
585getEulerAngles (
const Eigen::Transform<Scalar, 3, Eigen::Affine> &
t, Scalar &roll, Scalar &pitch, Scalar &yaw)
587 roll = std::atan2 (
t (2, 1),
t (2, 2));
588 pitch =
asin (-
t (2, 0));
589 yaw = std::atan2 (
t (1, 0),
t (0, 0));
593template <
typename Scalar>
void
595 Scalar &x, Scalar &y, Scalar &z,
596 Scalar &roll, Scalar &pitch, Scalar &yaw)
601 roll = std::atan2 (
t (2, 1),
t (2, 2));
602 pitch =
asin (-
t (2, 0));
603 yaw = std::atan2 (
t (1, 0),
t (0, 0));
607template <
typename Scalar>
void
609 Scalar roll, Scalar pitch, Scalar yaw,
610 Eigen::Transform<Scalar, 3, Eigen::Affine> &
t)
612 Scalar
A = std::cos (yaw),
B =
sin (yaw), C = std::cos (pitch),
D =
sin (pitch),
615 t (0, 0) =
A*C;
t (0, 1) =
A*
DF -
B*
E;
t (0, 2) =
B*
F +
A*
DE;
t (0, 3) = x;
616 t (1, 0) =
B*C;
t (1, 1) =
A*
E +
B*
DF;
t (1, 2) =
B*
DE -
A*
F;
t (1, 3) = y;
617 t (2, 0) = -
D;
t (2, 1) = C*
F;
t (2, 2) = C*
E;
t (2, 3) = z;
618 t (3, 0) = 0;
t (3, 1) = 0;
t (3, 2) = 0;
t (3, 3) = 1;
622template <
typename Derived>
void
625 std::uint32_t rows =
static_cast<std::uint32_t
> (
matrix.rows ()), cols =
static_cast<std::uint32_t
> (
matrix.cols ());
626 file.write (
reinterpret_cast<char*
> (&rows),
sizeof (rows));
627 file.write (
reinterpret_cast<char*
> (&cols),
sizeof (cols));
628 for (std::uint32_t i = 0; i < rows; ++i)
629 for (std::uint32_t j = 0; j < cols; ++j)
631 typename Derived::Scalar
tmp =
matrix(i,j);
632 file.write (
reinterpret_cast<const char*
> (&
tmp),
sizeof (
tmp));
637template <
typename Derived>
void
640 Eigen::MatrixBase<Derived> &
matrix =
const_cast<Eigen::MatrixBase<Derived> &
> (
matrix_);
642 std::uint32_t rows, cols;
643 file.read (
reinterpret_cast<char*
> (&rows),
sizeof (rows));
644 file.read (
reinterpret_cast<char*
> (&cols),
sizeof (cols));
645 if (
matrix.rows () !=
static_cast<int>(rows) ||
matrix.cols () !=
static_cast<int>(cols))
646 matrix.derived().resize(rows, cols);
648 for (std::uint32_t i = 0; i < rows; ++i)
649 for (std::uint32_t j = 0; j < cols; ++j)
651 typename Derived::Scalar
tmp;
652 file.read (
reinterpret_cast<char*
> (&
tmp),
sizeof (
tmp));
658template <
typename Derived,
typename OtherDerived>
659typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
662#if EIGEN_VERSION_AT_LEAST (3, 3, 0)
665 using TransformationMatrixType =
typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type;
666 using Scalar =
typename Eigen::internal::traits<TransformationMatrixType>::Scalar;
667 using RealScalar =
typename Eigen::NumTraits<Scalar>::Real;
668 using Index =
typename Derived::Index;
670 static_assert (!Eigen::NumTraits<Scalar>::IsComplex,
"Numeric type must be real.");
671 static_assert ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value),
672 "You mixed different numeric types. You need to use the cast method of matrixbase to cast numeric types explicitly.");
674 enum {
Dimension = PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
676 using VectorType = Eigen::Matrix<Scalar, Dimension, 1>;
677 using MatrixType = Eigen::Matrix<Scalar, Dimension, Dimension>;
678 using RowMajorMatrixType =
typename Eigen::internal::plain_matrix_type_row_major<Derived>::type;
680 const Index
m =
src.rows ();
681 const Index n =
src.cols ();
700 Eigen::JacobiSVD<MatrixType>
svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
706 VectorType S = VectorType::Ones (
m);
708 if (
svd.matrixU ().determinant () *
svd.matrixV ().determinant () < 0 )
712 Rt.block (0,0,
m,
m).noalias () =
svd.matrixU () * S.asDiagonal () *
svd.matrixV ().transpose ();
717 const Scalar
c = Scalar (1)/
src_var *
svd.singularValues ().dot (S);
722 Rt.block (0, 0,
m,
m) *=
c;
735template <
typename Scalar>
bool
737 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &
line_out,
738 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
742 PCL_DEBUG (
"transformLine: lines size != 6\n");
746 Eigen::Matrix<Scalar, 3, 1> point, vector;
757template <
typename Scalar>
void
760 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
762 Eigen::Hyperplane < Scalar, 3 >
plane;
764 plane.transform (transformation);
768 #if !EIGEN_VERSION_AT_LEAST (3, 3, 2)
774template <
typename Scalar>
void
777 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
779 std::vector<Scalar> values (
plane_in->values.begin (),
plane_in->values.end ());
780 Eigen::Matrix < Scalar, 4, 1 >
v_plane_in (values.data ());
787template <
typename Scalar>
bool
789 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &
line_y,
793 if (
line_x.innerSize () != 6 ||
line_y.innerSize () != 6)
795 PCL_DEBUG (
"checkCoordinateSystem: lines size != 6\n");
801 PCL_DEBUG (
"checkCoorZdinateSystem: vector origins are different !\n");
814 PCL_DEBUG (
"checkCoordinateSystem: line_x norm %d != 1\n",
v_line_x.norm ());
820 PCL_DEBUG (
"checkCoordinateSystem: line_y norm %d != 1\n",
v_line_y.norm ());
826 PCL_DEBUG (
"checkCoordinateSystem: line_z norm %d != 1\n",
v_line_z.norm ());
853template <
typename Scalar>
bool
855 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
from_line_y,
856 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
to_line_x,
857 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
to_line_y,
858 Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
862 PCL_DEBUG (
"transformBetween2CoordinateSystems: lines size != 6\n");
869 PCL_DEBUG (
"transformBetween2CoordinateSystems: coordinate systems invalid !\n");
884 Eigen::Transform<Scalar, 3, Eigen::Affine>
T2,
T3 = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
885 Eigen::Matrix<Scalar, 3, 1> x1,
y1, z1, x2,
y2, z2;
888 x1 = (
fr1 -
fr0).normalized ();
892 x2 = (
to1 -
to0).normalized ();
897 T2.linear () << x1,
y1, x1.cross (
y1);
900 T3.linear () << x2,
y2, x2.cross (
y2);
904 transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
905 transformation.linear () =
T3.linear () *
T2.linear ().inverse ();
906 transformation.translation () =
to0 - (transformation.linear () *
fr0);
Iterator class for point clouds with or without given indices.
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...
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 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,...
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
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)
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 eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
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 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=...
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
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...
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
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=...
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...
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.
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 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 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.
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.
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.
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 computeRoots2(const Scalar &b, const Scalar &c, Roots &roots)
Compute the roots of a quadratic polynom x^2 + b*x + c = 0.
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
shared_ptr< ::pcl::ModelCoefficients > Ptr
shared_ptr< const ::pcl::ModelCoefficients > ConstPtr