42 #ifndef PCL_COMMON_EIGEN_H_ 43 #define PCL_COMMON_EIGEN_H_ 50 # pragma GCC system_header 51 #elif defined __SUNPRO_CC 56 #include <pcl/ModelCoefficients.h> 58 #include <Eigen/StdVector> 60 #include <Eigen/Eigenvalues> 61 #include <Eigen/Geometry> 64 #include <Eigen/Dense> 65 #include <Eigen/Eigenvalues> 74 template <
typename Scalar,
typename Roots>
void 75 computeRoots2 (
const Scalar &b,
const Scalar &c, Roots &roots);
81 template <
typename Matrix,
typename Roots>
void 90 template <
typename Matrix,
typename Vector>
void 91 eigen22 (
const Matrix &mat,
typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
99 template <
typename Matrix,
typename Vector>
void 100 eigen22 (
const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues);
108 template <
typename Matrix,
typename Vector>
void 118 template <
typename Matrix,
typename Vector>
void 119 eigen33 (
const Matrix &mat,
typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
126 template <
typename Matrix,
typename Vector>
void 127 eigen33 (
const Matrix &mat, Vector &evals);
135 template <
typename Matrix,
typename Vector>
void 136 eigen33 (
const Matrix &mat, Matrix &evecs, Vector &evals);
145 template <
typename Matrix>
typename Matrix::Scalar
146 invert2x2 (
const Matrix &matrix, Matrix &inverse);
155 template <
typename Matrix>
typename Matrix::Scalar
164 template <
typename Matrix>
typename Matrix::Scalar
172 template <
typename Matrix>
typename Matrix::Scalar
184 const Eigen::Vector3f& y_direction,
185 Eigen::Affine3f& transformation);
194 inline Eigen::Affine3f
196 const Eigen::Vector3f& y_direction);
207 const Eigen::Vector3f& y_direction,
208 Eigen::Affine3f& transformation);
217 inline Eigen::Affine3f
219 const Eigen::Vector3f& y_direction);
230 const Eigen::Vector3f& z_axis,
231 Eigen::Affine3f& transformation);
240 inline Eigen::Affine3f
242 const Eigen::Vector3f& z_axis);
254 const Eigen::Vector3f& z_axis,
255 const Eigen::Vector3f& origin,
256 Eigen::Affine3f& transformation);
265 template <
typename Scalar>
void 266 getEulerAngles (
const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw);
271 getEulerAngles<float> (t, roll, pitch, yaw);
275 getEulerAngles (
const Eigen::Affine3d &t,
double &roll,
double &pitch,
double &yaw)
277 getEulerAngles<double> (t, roll, pitch, yaw);
290 template <
typename Scalar>
void 292 Scalar &x, Scalar &y, Scalar &z,
293 Scalar &roll, Scalar &pitch, Scalar &yaw);
297 float &x,
float &y,
float &z,
298 float &roll,
float &pitch,
float &yaw)
300 getTranslationAndEulerAngles<float> (t, x, y, z, roll, pitch, yaw);
305 double &x,
double &y,
double &z,
306 double &roll,
double &pitch,
double &yaw)
308 getTranslationAndEulerAngles<double> (t, x, y, z, roll, pitch, yaw);
321 template <
typename Scalar>
void 322 getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw,
323 Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
329 return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
336 return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
349 inline Eigen::Affine3f
353 getTransformation<float> (x, y, z, roll, pitch, yaw, t);
362 template <
typename Derived>
void 363 saveBinary (
const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
370 template <
typename Derived>
void 371 loadBinary (Eigen::MatrixBase<Derived>
const& matrix, std::istream& file);
376 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \ 377 : (int (a) == 1 || int (b) == 1) ? 1 \ 378 : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \ 379 : (int (a) <= int (b)) ? int (a) : int (b)) 411 template <
typename Derived,
typename OtherDerived>
412 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
413 umeyama (
const Eigen::MatrixBase<Derived>& src,
const Eigen::MatrixBase<OtherDerived>& dst,
bool with_scaling =
false);
422 template<
typename Scalar>
inline void 424 Eigen::Matrix<Scalar, 3, 1> &point_out,
425 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
427 Eigen::Matrix<Scalar, 4, 1> point;
428 point << point_in, 1.0;
429 point_out = (transformation * point).
template head<3> ();
434 Eigen::Vector3f &point_out,
435 const Eigen::Affine3f &transformation)
437 transformPoint<float> (point_in, point_out, transformation);
442 Eigen::Vector3d &point_out,
443 const Eigen::Affine3d &transformation)
445 transformPoint<double> (point_in, point_out, transformation);
455 template <
typename Scalar>
inline void 457 Eigen::Matrix<Scalar, 3, 1> &vector_out,
458 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
460 vector_out = transformation.linear () * vector_in;
465 Eigen::Vector3f &vector_out,
466 const Eigen::Affine3f &transformation)
468 transformVector<float> (vector_in, vector_out, transformation);
473 Eigen::Vector3d &vector_out,
474 const Eigen::Affine3d &transformation)
476 transformVector<double> (vector_in, vector_out, transformation);
490 template <
typename Scalar>
bool 491 transformLine (
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
492 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
493 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
497 Eigen::VectorXf &line_out,
498 const Eigen::Affine3f &transformation)
500 return (transformLine<float> (line_in, line_out, transformation));
505 Eigen::VectorXd &line_out,
506 const Eigen::Affine3d &transformation)
508 return (transformLine<double> (line_in, line_out, transformation));
520 template <
typename Scalar>
void 522 Eigen::Matrix<Scalar, 4, 1> &plane_out,
523 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
527 Eigen::Matrix<double, 4, 1> &plane_out,
528 const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
530 transformPlane<double> (plane_in, plane_out, transformation);
535 Eigen::Matrix<float, 4, 1> &plane_out,
536 const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
538 transformPlane<float> (plane_in, plane_out, transformation);
551 template<
typename Scalar>
void 554 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
559 const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
561 transformPlane<double> (plane_in, plane_out, transformation);
567 const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
569 transformPlane<float> (plane_in, plane_out, transformation);
595 template<
typename Scalar>
bool 597 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
598 const Scalar norm_limit = 1e-3,
599 const Scalar dot_limit = 1e-3);
603 const Eigen::Matrix<double, Eigen::Dynamic, 1> &line_y,
604 const double norm_limit = 1e-3,
605 const double dot_limit = 1e-3)
607 return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
612 const Eigen::Matrix<float, Eigen::Dynamic, 1> &line_y,
613 const float norm_limit = 1e-3,
614 const float dot_limit = 1e-3)
616 return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
629 template <
typename Scalar>
inline bool 631 const Eigen::Matrix<Scalar, 3, 1> &x_direction,
632 const Eigen::Matrix<Scalar, 3, 1> &y_direction,
633 const Scalar norm_limit = 1e-3,
634 const Scalar dot_limit = 1e-3)
636 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_x;
637 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_y;
638 line_x << origin, x_direction;
639 line_y << origin, y_direction;
640 return (checkCoordinateSystem<Scalar> (line_x, line_y, norm_limit, dot_limit));
645 const Eigen::Matrix<double, 3, 1> &x_direction,
646 const Eigen::Matrix<double, 3, 1> &y_direction,
647 const double norm_limit = 1e-3,
648 const double dot_limit = 1e-3)
650 Eigen::Matrix<double, Eigen::Dynamic, 1> line_x;
651 Eigen::Matrix<double, Eigen::Dynamic, 1> line_y;
654 line_x << origin, x_direction;
655 line_y << origin, y_direction;
656 return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
661 const Eigen::Matrix<float, 3, 1> &x_direction,
662 const Eigen::Matrix<float, 3, 1> &y_direction,
663 const float norm_limit = 1e-3,
664 const float dot_limit = 1e-3)
666 Eigen::Matrix<float, Eigen::Dynamic, 1> line_x;
667 Eigen::Matrix<float, Eigen::Dynamic, 1> line_y;
670 line_x << origin, x_direction;
671 line_y << origin, y_direction;
672 return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
687 template <
typename Scalar>
bool 689 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
690 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
691 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
692 Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
696 const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_y,
697 const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_x,
698 const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_y,
699 Eigen::Transform<double, 3, Eigen::Affine> &transformation)
701 return (transformBetween2CoordinateSystems<double> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
706 const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_y,
707 const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_x,
708 const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_y,
709 Eigen::Transform<float, 3, Eigen::Affine> &transformation)
711 return (transformBetween2CoordinateSystems<float> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
716 #include <pcl/common/impl/eigen.hpp> 718 #if defined __SUNPRO_CC 722 #endif //PCL_COMMON_EIGEN_H_ 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 orign 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.