Point Cloud Library (PCL)  1.10.0-dev
eigen.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
7  * Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
8  * Copyright (c) 2012-, Open Perception, Inc.
9  *
10  * All rights reserved.
11  *
12  * Redistribution and use in source and binary forms, with or without
13  * modification, are permitted provided that the following conditions
14  * are met:
15  *
16  * * Redistributions of source code must retain the above copyright
17  * notice, this list of conditions and the following disclaimer.
18  * * Redistributions in binary form must reproduce the above
19  * copyright notice, this list of conditions and the following
20  * disclaimer in the documentation and/or other materials provided
21  * with the distribution.
22  * * Neither the name of the copyright holder(s) nor the names of its
23  * contributors may be used to endorse or promote products derived
24  * from this software without specific prior written permission.
25  *
26  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
27  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
28  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
29  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
30  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
31  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
32  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
33  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
34  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
35  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
36  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
37  * POSSIBILITY OF SUCH DAMAGE.
38  *
39  * $Id$
40  *
41  */
42 
43 #pragma once
44 
45 #ifndef NOMINMAX
46 #define NOMINMAX
47 #endif
48 
49 #if defined __GNUC__
50 # pragma GCC system_header
51 #elif defined __SUNPRO_CC
52 # pragma disable_warn
53 #endif
54 
55 #include <cmath>
56 #include <pcl/ModelCoefficients.h>
57 
58 #include <Eigen/StdVector>
59 #include <Eigen/Core>
60 #include <Eigen/Eigenvalues>
61 #include <Eigen/Geometry>
62 #include <Eigen/SVD>
63 #include <Eigen/LU>
64 #include <Eigen/Dense>
65 #include <Eigen/Eigenvalues>
66 
67 namespace pcl
68 {
69  /** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0
70  * \param[in] b linear parameter
71  * \param[in] c constant parameter
72  * \param[out] roots solutions of x^2 + b*x + c = 0
73  */
74  template <typename Scalar, typename Roots> void
75  computeRoots2 (const Scalar &b, const Scalar &c, Roots &roots);
76 
77  /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
78  * \param[in] m input matrix
79  * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
80  */
81  template <typename Matrix, typename Roots> void
82  computeRoots (const Matrix &m, Roots &roots);
83 
84  /** \brief determine the smallest eigenvalue and its corresponding eigenvector
85  * \param[in] mat input matrix that needs to be symmetric and positive semi definite
86  * \param[out] eigenvalue the smallest eigenvalue of the input matrix
87  * \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix
88  * \ingroup common
89  */
90  template <typename Matrix, typename Vector> void
91  eigen22 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
92 
93  /** \brief determine the smallest eigenvalue and its corresponding eigenvector
94  * \param[in] mat input matrix that needs to be symmetric and positive semi definite
95  * \param[out] eigenvectors the corresponding eigenvector to the smallest eigenvalue of the input matrix
96  * \param[out] eigenvalues the smallest eigenvalue of the input matrix
97  * \ingroup common
98  */
99  template <typename Matrix, typename Vector> void
100  eigen22 (const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues);
101 
102  /** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix
103  * \param[in] mat symmetric positive semi definite input matrix
104  * \param[in] eigenvalue the eigenvalue which corresponding eigenvector is to be computed
105  * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
106  * \ingroup common
107  */
108  template <typename Matrix, typename Vector> void
109  computeCorrespondingEigenVector (const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
110 
111  /** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix
112  * \param[in] mat symmetric positive semi definite input matrix
113  * \param[out] eigenvalue smallest eigenvalue of the input matrix
114  * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
115  * \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue.
116  * \ingroup common
117  */
118  template <typename Matrix, typename Vector> void
119  eigen33 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
120 
121  /** \brief determines the eigenvalues of the symmetric positive semi definite input matrix
122  * \param[in] mat symmetric positive semi definite input matrix
123  * \param[out] evals resulting eigenvalues in ascending order
124  * \ingroup common
125  */
126  template <typename Matrix, typename Vector> void
127  eigen33 (const Matrix &mat, Vector &evals);
128 
129  /** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix
130  * \param[in] mat symmetric positive semi definite input matrix
131  * \param[out] evecs corresponding eigenvectors in correct order according to eigenvalues
132  * \param[out] evals resulting eigenvalues in ascending order
133  * \ingroup common
134  */
135  template <typename Matrix, typename Vector> void
136  eigen33 (const Matrix &mat, Matrix &evecs, Vector &evals);
137 
138  /** \brief Calculate the inverse of a 2x2 matrix
139  * \param[in] matrix matrix to be inverted
140  * \param[out] inverse the resultant inverted matrix
141  * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
142  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
143  * \ingroup common
144  */
145  template <typename Matrix> typename Matrix::Scalar
146  invert2x2 (const Matrix &matrix, Matrix &inverse);
147 
148  /** \brief Calculate the inverse of a 3x3 symmetric matrix.
149  * \param[in] matrix matrix to be inverted
150  * \param[out] inverse the resultant inverted matrix
151  * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
152  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
153  * \ingroup common
154  */
155  template <typename Matrix> typename Matrix::Scalar
156  invert3x3SymMatrix (const Matrix &matrix, Matrix &inverse);
157 
158  /** \brief Calculate the inverse of a general 3x3 matrix.
159  * \param[in] matrix matrix to be inverted
160  * \param[out] inverse the resultant inverted matrix
161  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
162  * \ingroup common
163  */
164  template <typename Matrix> typename Matrix::Scalar
165  invert3x3Matrix (const Matrix &matrix, Matrix &inverse);
166 
167  /** \brief Calculate the determinant of a 3x3 matrix.
168  * \param[in] matrix matrix
169  * \return determinant of the matrix
170  * \ingroup common
171  */
172  template <typename Matrix> typename Matrix::Scalar
173  determinant3x3Matrix (const Matrix &matrix);
174 
175  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
176  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
177  * \param[in] z_axis the z-axis
178  * \param[in] y_direction the y direction
179  * \param[out] transformation the resultant 3D rotation
180  * \ingroup common
181  */
182  inline void
183  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
184  const Eigen::Vector3f& y_direction,
185  Eigen::Affine3f& transformation);
186 
187  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
188  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
189  * \param[in] z_axis the z-axis
190  * \param[in] y_direction the y direction
191  * \return the resultant 3D rotation
192  * \ingroup common
193  */
194  inline Eigen::Affine3f
195  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
196  const Eigen::Vector3f& y_direction);
197 
198  /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
199  * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
200  * \param[in] x_axis the x-axis
201  * \param[in] y_direction the y direction
202  * \param[out] transformation the resultant 3D rotation
203  * \ingroup common
204  */
205  inline void
206  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
207  const Eigen::Vector3f& y_direction,
208  Eigen::Affine3f& transformation);
209 
210  /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
211  * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
212  * \param[in] x_axis the x-axis
213  * \param[in] y_direction the y direction
214  * \return the resulting 3D rotation
215  * \ingroup common
216  */
217  inline Eigen::Affine3f
218  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
219  const Eigen::Vector3f& y_direction);
220 
221  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
222  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
223  * \param[in] y_direction the y direction
224  * \param[in] z_axis the z-axis
225  * \param[out] transformation the resultant 3D rotation
226  * \ingroup common
227  */
228  inline void
229  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
230  const Eigen::Vector3f& z_axis,
231  Eigen::Affine3f& transformation);
232 
233  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
234  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
235  * \param[in] y_direction the y direction
236  * \param[in] z_axis the z-axis
237  * \return transformation the resultant 3D rotation
238  * \ingroup common
239  */
240  inline Eigen::Affine3f
241  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
242  const Eigen::Vector3f& z_axis);
243 
244  /** \brief Get the transformation that will translate \a origin to (0,0,0) and rotate \a z_axis into (0,0,1)
245  * and \a y_direction into a vector with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
246  * \param[in] y_direction the y direction
247  * \param[in] z_axis the z-axis
248  * \param[in] origin the origin
249  * \param[in] transformation the resultant transformation matrix
250  * \ingroup common
251  */
252  inline void
253  getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
254  const Eigen::Vector3f& z_axis,
255  const Eigen::Vector3f& origin,
256  Eigen::Affine3f& transformation);
257 
258  /** \brief Extract the Euler angles (XYZ-convention) from the given transformation
259  * \param[in] t the input transformation matrix
260  * \param[in] roll the resulting roll angle
261  * \param[in] pitch the resulting pitch angle
262  * \param[in] yaw the resulting yaw angle
263  * \ingroup common
264  */
265  template <typename Scalar> void
266  getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw);
267 
268  inline void
269  getEulerAngles (const Eigen::Affine3f &t, float &roll, float &pitch, float &yaw)
270  {
271  getEulerAngles<float> (t, roll, pitch, yaw);
272  }
273 
274  inline void
275  getEulerAngles (const Eigen::Affine3d &t, double &roll, double &pitch, double &yaw)
276  {
277  getEulerAngles<double> (t, roll, pitch, yaw);
278  }
279 
280  /** Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation
281  * \param[in] t the input transformation matrix
282  * \param[out] x the resulting x translation
283  * \param[out] y the resulting y translation
284  * \param[out] z the resulting z translation
285  * \param[out] roll the resulting roll angle
286  * \param[out] pitch the resulting pitch angle
287  * \param[out] yaw the resulting yaw angle
288  * \ingroup common
289  */
290  template <typename Scalar> void
291  getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
292  Scalar &x, Scalar &y, Scalar &z,
293  Scalar &roll, Scalar &pitch, Scalar &yaw);
294 
295  inline void
296  getTranslationAndEulerAngles (const Eigen::Affine3f &t,
297  float &x, float &y, float &z,
298  float &roll, float &pitch, float &yaw)
299  {
300  getTranslationAndEulerAngles<float> (t, x, y, z, roll, pitch, yaw);
301  }
302 
303  inline void
304  getTranslationAndEulerAngles (const Eigen::Affine3d &t,
305  double &x, double &y, double &z,
306  double &roll, double &pitch, double &yaw)
307  {
308  getTranslationAndEulerAngles<double> (t, x, y, z, roll, pitch, yaw);
309  }
310 
311  /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
312  * \param[in] x the input x translation
313  * \param[in] y the input y translation
314  * \param[in] z the input z translation
315  * \param[in] roll the input roll angle
316  * \param[in] pitch the input pitch angle
317  * \param[in] yaw the input yaw angle
318  * \param[out] t the resulting transformation matrix
319  * \ingroup common
320  */
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);
324 
325  inline void
326  getTransformation (float x, float y, float z, float roll, float pitch, float yaw,
327  Eigen::Affine3f &t)
328  {
329  return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
330  }
331 
332  inline void
333  getTransformation (double x, double y, double z, double roll, double pitch, double yaw,
334  Eigen::Affine3d &t)
335  {
336  return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
337  }
338 
339  /** \brief Create a transformation from the given translation and Euler angles (XYZ-convention)
340  * \param[in] x the input x translation
341  * \param[in] y the input y translation
342  * \param[in] z the input z translation
343  * \param[in] roll the input roll angle
344  * \param[in] pitch the input pitch angle
345  * \param[in] yaw the input yaw angle
346  * \return the resulting transformation matrix
347  * \ingroup common
348  */
349  inline Eigen::Affine3f
350  getTransformation (float x, float y, float z, float roll, float pitch, float yaw)
351  {
352  Eigen::Affine3f t;
353  getTransformation<float> (x, y, z, roll, pitch, yaw, t);
354  return (t);
355  }
356 
357  /** \brief Write a matrix to an output stream
358  * \param[in] matrix the matrix to output
359  * \param[out] file the output stream
360  * \ingroup common
361  */
362  template <typename Derived> void
363  saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
364 
365  /** \brief Read a matrix from an input stream
366  * \param[out] matrix the resulting matrix, read from the input stream
367  * \param[in,out] file the input stream
368  * \ingroup common
369  */
370  template <typename Derived> void
371  loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);
372 
373 // PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
374 // followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
375 // finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
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))
380 
381  /** \brief Returns the transformation between two point sets.
382  * The algorithm is based on:
383  * "Least-squares estimation of transformation parameters between two point patterns",
384  * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
385  *
386  * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
387  * \f{align*}
388  * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
389  * \f}
390  * is minimized.
391  *
392  * The algorithm is based on the analysis of the covariance matrix
393  * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
394  * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where
395  * \f$d\f$ is corresponding to the dimension (which is typically small).
396  * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
397  * though the actual computational effort lies in the covariance
398  * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when
399  * the input point sets have dimension \f$d \times m\f$.
400  *
401  * \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$
402  * \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$.
403  * \param[in] with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. (default: false)
404  * \return The homogeneous transformation
405  * \f{align*}
406  * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
407  * \f}
408  * minimizing the resudiual above. This transformation is always returned as an
409  * Eigen::Matrix.
410  */
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);
414 
415 /** \brief Transform a point using an affine matrix
416  * \param[in] point_in the vector to be transformed
417  * \param[out] point_out the transformed vector
418  * \param[in] transformation the transformation matrix
419  *
420  * \note Can be used with \c point_in = \c point_out
421  */
422  template<typename Scalar> inline void
423  transformPoint (const Eigen::Matrix<Scalar, 3, 1> &point_in,
424  Eigen::Matrix<Scalar, 3, 1> &point_out,
425  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
426  {
427  Eigen::Matrix<Scalar, 4, 1> point;
428  point << point_in, 1.0;
429  point_out = (transformation * point).template head<3> ();
430  }
431 
432  inline void
433  transformPoint (const Eigen::Vector3f &point_in,
434  Eigen::Vector3f &point_out,
435  const Eigen::Affine3f &transformation)
436  {
437  transformPoint<float> (point_in, point_out, transformation);
438  }
439 
440  inline void
441  transformPoint (const Eigen::Vector3d &point_in,
442  Eigen::Vector3d &point_out,
443  const Eigen::Affine3d &transformation)
444  {
445  transformPoint<double> (point_in, point_out, transformation);
446  }
447 
448 /** \brief Transform a vector using an affine matrix
449  * \param[in] vector_in the vector to be transformed
450  * \param[out] vector_out the transformed vector
451  * \param[in] transformation the transformation matrix
452  *
453  * \note Can be used with \c vector_in = \c vector_out
454  */
455  template <typename Scalar> inline void
456  transformVector (const Eigen::Matrix<Scalar, 3, 1> &vector_in,
457  Eigen::Matrix<Scalar, 3, 1> &vector_out,
458  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
459  {
460  vector_out = transformation.linear () * vector_in;
461  }
462 
463  inline void
464  transformVector (const Eigen::Vector3f &vector_in,
465  Eigen::Vector3f &vector_out,
466  const Eigen::Affine3f &transformation)
467  {
468  transformVector<float> (vector_in, vector_out, transformation);
469  }
470 
471  inline void
472  transformVector (const Eigen::Vector3d &vector_in,
473  Eigen::Vector3d &vector_out,
474  const Eigen::Affine3d &transformation)
475  {
476  transformVector<double> (vector_in, vector_out, transformation);
477  }
478 
479 /** \brief Transform a line using an affine matrix
480  * \param[in] line_in the line to be transformed
481  * \param[out] line_out the transformed line
482  * \param[in] transformation the transformation matrix
483  *
484  * Lines must be filled in this form:\n
485  * line[0-2] = Origin coordinates of the vector\n
486  * line[3-5] = Direction vector
487  *
488  * \note Can be used with \c line_in = \c line_out
489  */
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);
494 
495  inline bool
496  transformLine (const Eigen::VectorXf &line_in,
497  Eigen::VectorXf &line_out,
498  const Eigen::Affine3f &transformation)
499  {
500  return (transformLine<float> (line_in, line_out, transformation));
501  }
502 
503  inline bool
504  transformLine (const Eigen::VectorXd &line_in,
505  Eigen::VectorXd &line_out,
506  const Eigen::Affine3d &transformation)
507  {
508  return (transformLine<double> (line_in, line_out, transformation));
509  }
510 
511 /** \brief Transform plane vectors using an affine matrix
512  * \param[in] plane_in the plane coefficients to be transformed
513  * \param[out] plane_out the transformed plane coefficients to fill
514  * \param[in] transformation the transformation matrix
515  *
516  * The plane vectors are filled in the form ax+by+cz+d=0
517  * Can be used with non Hessian form planes coefficients
518  * Can be used with \c plane_in = \c plane_out
519  */
520  template <typename Scalar> void
521  transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
522  Eigen::Matrix<Scalar, 4, 1> &plane_out,
523  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
524 
525  inline void
526  transformPlane (const Eigen::Matrix<double, 4, 1> &plane_in,
527  Eigen::Matrix<double, 4, 1> &plane_out,
528  const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
529  {
530  transformPlane<double> (plane_in, plane_out, transformation);
531  }
532 
533  inline void
534  transformPlane (const Eigen::Matrix<float, 4, 1> &plane_in,
535  Eigen::Matrix<float, 4, 1> &plane_out,
536  const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
537  {
538  transformPlane<float> (plane_in, plane_out, transformation);
539  }
540 
541 /** \brief Transform plane vectors using an affine matrix
542  * \param[in] plane_in the plane coefficients to be transformed
543  * \param[out] plane_out the transformed plane coefficients to fill
544  * \param[in] transformation the transformation matrix
545  *
546  * The plane vectors are filled in the form ax+by+cz+d=0
547  * Can be used with non Hessian form planes coefficients
548  * Can be used with \c plane_in = \c plane_out
549  * \warning ModelCoefficients stores floats only !
550  */
551  template<typename Scalar> void
553  pcl::ModelCoefficients::Ptr plane_out,
554  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
555 
556  inline void
558  pcl::ModelCoefficients::Ptr plane_out,
559  const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
560  {
561  transformPlane<double> (plane_in, plane_out, transformation);
562  }
563 
564  inline void
566  pcl::ModelCoefficients::Ptr plane_out,
567  const Eigen::Transform<float, 3, Eigen::Affine> &transformation)
568  {
569  transformPlane<float> (plane_in, plane_out, transformation);
570  }
571 
572 /** \brief Check coordinate system integrity
573  * \param[in] line_x the first axis
574  * \param[in] line_y the second axis
575  * \param[in] norm_limit the limit to ignore norm rounding errors
576  * \param[in] dot_limit the limit to ignore dot product rounding errors
577  * \return True if the coordinate system is consistent, false otherwise.
578  *
579  * Lines must be filled in this form:\n
580  * line[0-2] = Origin coordinates of the vector\n
581  * line[3-5] = Direction vector
582  *
583  * Can be used like this :\n
584  * line_x = X axis and line_y = Y axis\n
585  * line_x = Z axis and line_y = X axis\n
586  * line_x = Y axis and line_y = Z axis\n
587  * Because X^Y = Z, Z^X = Y and Y^Z = X.
588  * Do NOT invert line order !
589  *
590  * Determine whether a coordinate system is consistent or not by checking :\n
591  * Line origins: They must be the same for the 2 lines\n
592  * Norm: The 2 lines must be normalized\n
593  * Dot products: Must be 0 or perpendicular vectors
594  */
595  template<typename Scalar> bool
596  checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
597  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
598  const Scalar norm_limit = 1e-3,
599  const Scalar dot_limit = 1e-3);
600 
601  inline bool
602  checkCoordinateSystem (const Eigen::Matrix<double, Eigen::Dynamic, 1> &line_x,
603  const Eigen::Matrix<double, Eigen::Dynamic, 1> &line_y,
604  const double norm_limit = 1e-3,
605  const double dot_limit = 1e-3)
606  {
607  return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
608  }
609 
610  inline bool
611  checkCoordinateSystem (const Eigen::Matrix<float, Eigen::Dynamic, 1> &line_x,
612  const Eigen::Matrix<float, Eigen::Dynamic, 1> &line_y,
613  const float norm_limit = 1e-3,
614  const float dot_limit = 1e-3)
615  {
616  return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
617  }
618 
619 /** \brief Check coordinate system integrity
620  * \param[in] origin the origin of the coordinate system
621  * \param[in] x_direction the first axis
622  * \param[in] y_direction the second axis
623  * \param[in] norm_limit the limit to ignore norm rounding errors
624  * \param[in] dot_limit the limit to ignore dot product rounding errors
625  * \return True if the coordinate system is consistent, false otherwise.
626  *
627  * Read the other variant for more information
628  */
629  template <typename Scalar> inline bool
630  checkCoordinateSystem (const Eigen::Matrix<Scalar, 3, 1> &origin,
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)
635  {
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));
641  }
642 
643  inline bool
644  checkCoordinateSystem (const Eigen::Matrix<double, 3, 1> &origin,
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)
649  {
650  Eigen::Matrix<double, Eigen::Dynamic, 1> line_x;
651  Eigen::Matrix<double, Eigen::Dynamic, 1> line_y;
652  line_x.resize (6);
653  line_y.resize (6);
654  line_x << origin, x_direction;
655  line_y << origin, y_direction;
656  return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
657  }
658 
659  inline bool
660  checkCoordinateSystem (const Eigen::Matrix<float, 3, 1> &origin,
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)
665  {
666  Eigen::Matrix<float, Eigen::Dynamic, 1> line_x;
667  Eigen::Matrix<float, Eigen::Dynamic, 1> line_y;
668  line_x.resize (6);
669  line_y.resize (6);
670  line_x << origin, x_direction;
671  line_y << origin, y_direction;
672  return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
673  }
674 
675 /** \brief Compute the transformation between two coordinate systems
676  * \param[in] from_line_x X axis from the origin coordinate system
677  * \param[in] from_line_y Y axis from the origin coordinate system
678  * \param[in] to_line_x X axis from the destination coordinate system
679  * \param[in] to_line_y Y axis from the destination coordinate system
680  * \param[out] transformation the transformation matrix to fill
681  * \return true if transformation was filled, false otherwise.
682  *
683  * Line must be filled in this form:\n
684  * line[0-2] = Coordinate system origin coordinates \n
685  * line[3-5] = Direction vector (norm doesn't matter)
686  */
687  template <typename Scalar> bool
688  transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
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);
693 
694  inline bool
695  transformBetween2CoordinateSystems (const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_x,
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)
700  {
701  return (transformBetween2CoordinateSystems<double> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
702  }
703 
704  inline bool
705  transformBetween2CoordinateSystems (const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_x,
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)
710  {
711  return (transformBetween2CoordinateSystems<float> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
712  }
713 
714 }
715 
716 #include <pcl/common/impl/eigen.hpp>
717 
718 #if defined __SUNPRO_CC
719 # pragma enable_warn
720 #endif
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.
Definition: eigen.h:423
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
Definition: eigen.hpp:632
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=...
Definition: eigen.hpp:497
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...
Definition: eigen.hpp:222
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
Definition: eigen.hpp:400
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.
Definition: eigen.h:456
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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...
Definition: eigen.hpp:567
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.
Definition: eigen.hpp:782
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.
Definition: eigen.hpp:654
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=...
Definition: eigen.hpp:549
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues ...
Definition: eigen.hpp:64
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=...
Definition: eigen.hpp:523
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.
Definition: eigen.hpp:579
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Definition: eigen.hpp:617
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Definition: eigen.hpp:129
shared_ptr< ::pcl::ModelCoefficients > Ptr
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Definition: eigen.hpp:487
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
Definition: eigen.hpp:454
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.
Definition: eigen.hpp:848
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.
Definition: eigen.hpp:588
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...
Definition: eigen.hpp:291
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.
Definition: eigen.hpp:730
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) ...
Definition: eigen.hpp:602
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition: eigen.hpp:419
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.
Definition: eigen.hpp:752
void computeRoots2(const Scalar &b, const Scalar &c, Roots &roots)
Compute the roots of a quadratic polynom x^2 + b*x + c = 0.
Definition: eigen.hpp:49