50 # pragma GCC system_header
51 #elif defined __SUNPRO_CC
55 #include <pcl/ModelCoefficients.h>
57 #include <Eigen/StdVector>
58 #include <Eigen/Geometry>
68 template <
typename Scalar,
typename Roots>
void
69 computeRoots2 (
const Scalar &b,
const Scalar &c, Roots &roots);
75 template <
typename Matrix,
typename Roots>
void
84 template <
typename Matrix,
typename Vector>
void
85 eigen22 (
const Matrix &mat,
typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
93 template <
typename Matrix,
typename Vector>
void
94 eigen22 (
const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues);
102 template <
typename Matrix,
typename Vector>
void
112 template <
typename Matrix,
typename Vector>
void
113 eigen33 (
const Matrix &mat,
typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
120 template <
typename Matrix,
typename Vector>
void
121 eigen33 (
const Matrix &mat, Vector &evals);
129 template <
typename Matrix,
typename Vector>
void
130 eigen33 (
const Matrix &mat, Matrix &evecs, Vector &evals);
139 template <
typename Matrix>
typename Matrix::Scalar
140 invert2x2 (
const Matrix &matrix, Matrix &inverse);
149 template <
typename Matrix>
typename Matrix::Scalar
158 template <
typename Matrix>
typename Matrix::Scalar
166 template <
typename Matrix>
typename Matrix::Scalar
178 const Eigen::Vector3f& y_direction,
179 Eigen::Affine3f& transformation);
188 inline Eigen::Affine3f
190 const Eigen::Vector3f& y_direction);
201 const Eigen::Vector3f& y_direction,
202 Eigen::Affine3f& transformation);
211 inline Eigen::Affine3f
213 const Eigen::Vector3f& y_direction);
224 const Eigen::Vector3f& z_axis,
225 Eigen::Affine3f& transformation);
234 inline Eigen::Affine3f
236 const Eigen::Vector3f& z_axis);
248 const Eigen::Vector3f& z_axis,
249 const Eigen::Vector3f& origin,
250 Eigen::Affine3f& transformation);
259 template <
typename Scalar>
void
260 getEulerAngles (
const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw);
265 getEulerAngles<float> (t, roll, pitch, yaw);
269 getEulerAngles (
const Eigen::Affine3d &t,
double &roll,
double &pitch,
double &yaw)
271 getEulerAngles<double> (t, roll, pitch, yaw);
284 template <
typename Scalar>
void
286 Scalar &x, Scalar &y, Scalar &z,
287 Scalar &roll, Scalar &pitch, Scalar &yaw);
291 float &x,
float &y,
float &z,
292 float &roll,
float &pitch,
float &yaw)
294 getTranslationAndEulerAngles<float> (t, x, y, z, roll, pitch, yaw);
299 double &x,
double &y,
double &z,
300 double &roll,
double &pitch,
double &yaw)
302 getTranslationAndEulerAngles<double> (t, x, y, z, roll, pitch, yaw);
315 template <
typename Scalar>
void
316 getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw,
317 Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
323 return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
330 return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
343 inline Eigen::Affine3f
347 getTransformation<float> (x, y, z, roll, pitch, yaw, t);
356 template <
typename Derived>
void
357 saveBinary (
const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
364 template <
typename Derived>
void
365 loadBinary (Eigen::MatrixBase<Derived>
const& matrix, std::istream& file);
370 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
371 : (int (a) == 1 || int (b) == 1) ? 1 \
372 : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
373 : (int (a) <= int (b)) ? int (a) : int (b))
405 template <
typename Derived,
typename OtherDerived>
406 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
407 umeyama (
const Eigen::MatrixBase<Derived>& src,
const Eigen::MatrixBase<OtherDerived>& dst,
bool with_scaling =
false);
416 template<
typename Scalar>
inline void
418 Eigen::Matrix<Scalar, 3, 1> &point_out,
419 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
421 Eigen::Matrix<Scalar, 4, 1> point;
422 point << point_in, 1.0;
423 point_out = (transformation * point).
template head<3> ();
428 Eigen::Vector3f &point_out,
429 const Eigen::Affine3f &transformation)
431 transformPoint<float> (point_in, point_out, transformation);
436 Eigen::Vector3d &point_out,
437 const Eigen::Affine3d &transformation)
439 transformPoint<double> (point_in, point_out, transformation);
449 template <
typename Scalar>
inline void
451 Eigen::Matrix<Scalar, 3, 1> &vector_out,
452 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
454 vector_out = transformation.linear () * vector_in;
459 Eigen::Vector3f &vector_out,
460 const Eigen::Affine3f &transformation)
462 transformVector<float> (vector_in, vector_out, transformation);
467 Eigen::Vector3d &vector_out,
468 const Eigen::Affine3d &transformation)
470 transformVector<double> (vector_in, vector_out, transformation);
484 template <
typename Scalar>
bool
485 transformLine (
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
486 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
487 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
491 Eigen::VectorXf &line_out,
492 const Eigen::Affine3f &transformation)
494 return (transformLine<float> (line_in, line_out, transformation));
499 Eigen::VectorXd &line_out,
500 const Eigen::Affine3d &transformation)
502 return (transformLine<double> (line_in, line_out, transformation));
514 template <
typename Scalar>
void
516 Eigen::Matrix<Scalar, 4, 1> &plane_out,
517 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
521 Eigen::Matrix<double, 4, 1> &plane_out,
522 const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
524 transformPlane<double> (plane_in, plane_out, transformation);
529 Eigen::Matrix<float, 4, 1> &plane_out,
530 const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
532 transformPlane<float> (plane_in, plane_out, transformation);
545 template<
typename Scalar>
void
548 const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
553 const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
555 transformPlane<double> (plane_in, plane_out, transformation);
561 const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
563 transformPlane<float> (plane_in, plane_out, transformation);
589 template<
typename Scalar>
bool
591 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
592 const Scalar norm_limit = 1e-3,
593 const Scalar dot_limit = 1e-3);
597 const Eigen::Matrix<double, Eigen::Dynamic, 1> &line_y,
598 const double norm_limit = 1e-3,
599 const double dot_limit = 1e-3)
601 return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
606 const Eigen::Matrix<float, Eigen::Dynamic, 1> &line_y,
607 const float norm_limit = 1e-3,
608 const float dot_limit = 1e-3)
610 return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
623 template <
typename Scalar>
inline bool
625 const Eigen::Matrix<Scalar, 3, 1> &x_direction,
626 const Eigen::Matrix<Scalar, 3, 1> &y_direction,
627 const Scalar norm_limit = 1e-3,
628 const Scalar dot_limit = 1e-3)
630 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_x;
631 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_y;
632 line_x << origin, x_direction;
633 line_y << origin, y_direction;
634 return (checkCoordinateSystem<Scalar> (line_x, line_y, norm_limit, dot_limit));
639 const Eigen::Matrix<double, 3, 1> &x_direction,
640 const Eigen::Matrix<double, 3, 1> &y_direction,
641 const double norm_limit = 1e-3,
642 const double dot_limit = 1e-3)
644 Eigen::Matrix<double, Eigen::Dynamic, 1> line_x;
645 Eigen::Matrix<double, Eigen::Dynamic, 1> line_y;
648 line_x << origin, x_direction;
649 line_y << origin, y_direction;
650 return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
655 const Eigen::Matrix<float, 3, 1> &x_direction,
656 const Eigen::Matrix<float, 3, 1> &y_direction,
657 const float norm_limit = 1e-3,
658 const float dot_limit = 1e-3)
660 Eigen::Matrix<float, Eigen::Dynamic, 1> line_x;
661 Eigen::Matrix<float, Eigen::Dynamic, 1> line_y;
664 line_x << origin, x_direction;
665 line_y << origin, y_direction;
666 return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
681 template <
typename Scalar>
bool
683 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
684 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
685 const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
686 Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
690 const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_y,
691 const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_x,
692 const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_y,
693 Eigen::Transform<double, 3, Eigen::Affine> &transformation)
695 return (transformBetween2CoordinateSystems<double> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
700 const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_y,
701 const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_x,
702 const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_y,
703 Eigen::Transform<float, 3, Eigen::Affine> &transformation)
705 return (transformBetween2CoordinateSystems<float> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
710 #include <pcl/common/impl/eigen.hpp>
712 #if defined __SUNPRO_CC