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