Point Cloud Library (PCL)  1.12.1-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 <pcl/ModelCoefficients.h>
56 
57 #include <Eigen/StdVector>
58 #include <Eigen/Geometry>
59 #include <Eigen/LU>
60 
61 namespace pcl
62 {
63  /** \brief Compute the roots of a quadratic polynom x^2 + b*x + c = 0
64  * \param[in] b linear parameter
65  * \param[in] c constant parameter
66  * \param[out] roots solutions of x^2 + b*x + c = 0
67  */
68  template <typename Scalar, typename Roots> void
69  computeRoots2 (const Scalar &b, const Scalar &c, Roots &roots);
70 
71  /** \brief computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
72  * \param[in] m input matrix
73  * \param[out] roots roots of the characteristic polynomial of the input matrix m, which are the eigenvalues
74  */
75  template <typename Matrix, typename Roots> void
76  computeRoots (const Matrix &m, Roots &roots);
77 
78  /** \brief determine the smallest eigenvalue and its corresponding eigenvector
79  * \param[in] mat input matrix that needs to be symmetric and positive semi definite
80  * \param[out] eigenvalue the smallest eigenvalue of the input matrix
81  * \param[out] eigenvector the corresponding eigenvector to the smallest eigenvalue of the input matrix
82  * \ingroup common
83  */
84  template <typename Matrix, typename Vector> void
85  eigen22 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
86 
87  /** \brief determine the smallest eigenvalue and its corresponding eigenvector
88  * \param[in] mat input matrix that needs to be symmetric and positive semi definite
89  * \param[out] eigenvectors the corresponding eigenvector to the smallest eigenvalue of the input matrix
90  * \param[out] eigenvalues the smallest eigenvalue of the input matrix
91  * \ingroup common
92  */
93  template <typename Matrix, typename Vector> void
94  eigen22 (const Matrix &mat, Matrix &eigenvectors, Vector &eigenvalues);
95 
96  /** \brief determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi definite input matrix
97  * \param[in] mat symmetric positive semi definite input matrix
98  * \param[in] eigenvalue the eigenvalue which corresponding eigenvector is to be computed
99  * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
100  * \ingroup common
101  */
102  template <typename Matrix, typename Vector> void
103  computeCorrespondingEigenVector (const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
104 
105  /** \brief determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi definite input matrix
106  * \param[in] mat symmetric positive semi definite input matrix
107  * \param[out] eigenvalue smallest eigenvalue of the input matrix
108  * \param[out] eigenvector the corresponding eigenvector for the input eigenvalue
109  * \note if the smallest eigenvalue is not unique, this function may return any eigenvector that is consistent to the eigenvalue.
110  * \ingroup common
111  */
112  template <typename Matrix, typename Vector> void
113  eigen33 (const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector);
114 
115  /** \brief determines the eigenvalues of the symmetric positive semi definite input matrix
116  * \param[in] mat symmetric positive semi definite input matrix
117  * \param[out] evals resulting eigenvalues in ascending order
118  * \ingroup common
119  */
120  template <typename Matrix, typename Vector> void
121  eigen33 (const Matrix &mat, Vector &evals);
122 
123  /** \brief determines the eigenvalues and corresponding eigenvectors of the symmetric positive semi definite input matrix
124  * \param[in] mat symmetric positive semi definite input matrix
125  * \param[out] evecs corresponding eigenvectors in correct order according to eigenvalues
126  * \param[out] evals resulting eigenvalues in ascending order
127  * \ingroup common
128  */
129  template <typename Matrix, typename Vector> void
130  eigen33 (const Matrix &mat, Matrix &evecs, Vector &evals);
131 
132  /** \brief Calculate the inverse of a 2x2 matrix
133  * \param[in] matrix matrix to be inverted
134  * \param[out] inverse the resultant inverted matrix
135  * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
136  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
137  * \ingroup common
138  */
139  template <typename Matrix> typename Matrix::Scalar
140  invert2x2 (const Matrix &matrix, Matrix &inverse);
141 
142  /** \brief Calculate the inverse of a 3x3 symmetric matrix.
143  * \param[in] matrix matrix to be inverted
144  * \param[out] inverse the resultant inverted matrix
145  * \note only the upper triangular part is taken into account => non symmetric matrices will give wrong results
146  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
147  * \ingroup common
148  */
149  template <typename Matrix> typename Matrix::Scalar
150  invert3x3SymMatrix (const Matrix &matrix, Matrix &inverse);
151 
152  /** \brief Calculate the inverse of a general 3x3 matrix.
153  * \param[in] matrix matrix to be inverted
154  * \param[out] inverse the resultant inverted matrix
155  * \return determinant of the original matrix => if 0 no inverse exists => result is invalid
156  * \ingroup common
157  */
158  template <typename Matrix> typename Matrix::Scalar
159  invert3x3Matrix (const Matrix &matrix, Matrix &inverse);
160 
161  /** \brief Calculate the determinant of a 3x3 matrix.
162  * \param[in] matrix matrix
163  * \return determinant of the matrix
164  * \ingroup common
165  */
166  template <typename Matrix> typename Matrix::Scalar
167  determinant3x3Matrix (const Matrix &matrix);
168 
169  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
170  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
171  * \param[in] z_axis the z-axis
172  * \param[in] y_direction the y direction
173  * \param[out] transformation the resultant 3D rotation
174  * \ingroup common
175  */
176  inline void
177  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
178  const Eigen::Vector3f& y_direction,
179  Eigen::Affine3f& transformation);
180 
181  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
182  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
183  * \param[in] z_axis the z-axis
184  * \param[in] y_direction the y direction
185  * \return the resultant 3D rotation
186  * \ingroup common
187  */
188  inline Eigen::Affine3f
189  getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis,
190  const Eigen::Vector3f& y_direction);
191 
192  /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
193  * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
194  * \param[in] x_axis the x-axis
195  * \param[in] y_direction the y direction
196  * \param[out] transformation the resultant 3D rotation
197  * \ingroup common
198  */
199  inline void
200  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
201  const Eigen::Vector3f& y_direction,
202  Eigen::Affine3f& transformation);
203 
204  /** \brief Get the unique 3D rotation that will rotate \a x_axis into (1,0,0) and \a y_direction into a vector
205  * with z=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
206  * \param[in] x_axis the x-axis
207  * \param[in] y_direction the y direction
208  * \return the resulting 3D rotation
209  * \ingroup common
210  */
211  inline Eigen::Affine3f
212  getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis,
213  const Eigen::Vector3f& y_direction);
214 
215  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
216  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
217  * \param[in] y_direction the y direction
218  * \param[in] z_axis the z-axis
219  * \param[out] transformation the resultant 3D rotation
220  * \ingroup common
221  */
222  inline void
223  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
224  const Eigen::Vector3f& z_axis,
225  Eigen::Affine3f& transformation);
226 
227  /** \brief Get the unique 3D rotation that will rotate \a z_axis into (0,0,1) and \a y_direction into a vector
228  * with x=0 (or into (0,1,0) should \a y_direction be orthogonal to \a z_axis)
229  * \param[in] y_direction the y direction
230  * \param[in] z_axis the z-axis
231  * \return transformation the resultant 3D rotation
232  * \ingroup common
233  */
234  inline Eigen::Affine3f
235  getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction,
236  const Eigen::Vector3f& z_axis);
237 
238  /** \brief Get the transformation that will translate \a origin to (0,0,0) and rotate \a z_axis into (0,0,1)
239  * 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)
240  * \param[in] y_direction the y direction
241  * \param[in] z_axis the z-axis
242  * \param[in] origin the origin
243  * \param[in] transformation the resultant transformation matrix
244  * \ingroup common
245  */
246  inline void
247  getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction,
248  const Eigen::Vector3f& z_axis,
249  const Eigen::Vector3f& origin,
250  Eigen::Affine3f& transformation);
251 
252  /** \brief Extract the Euler angles (intrinsic rotations, ZYX-convention) from the given transformation
253  * \param[in] t the input transformation matrix
254  * \param[in] roll the resulting roll angle
255  * \param[in] pitch the resulting pitch angle
256  * \param[in] yaw the resulting yaw angle
257  * \ingroup common
258  */
259  template <typename Scalar> void
260  getEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t, Scalar &roll, Scalar &pitch, Scalar &yaw);
261 
262  /** Extract x,y,z and the Euler angles (intrinsic rotations, ZYX-convention) from the given transformation
263  * \param[in] t the input transformation matrix
264  * \param[out] x the resulting x translation
265  * \param[out] y the resulting y translation
266  * \param[out] z the resulting z translation
267  * \param[out] roll the resulting roll angle
268  * \param[out] pitch the resulting pitch angle
269  * \param[out] yaw the resulting yaw angle
270  * \ingroup common
271  */
272  template <typename Scalar> void
273  getTranslationAndEulerAngles (const Eigen::Transform<Scalar, 3, Eigen::Affine> &t,
274  Scalar &x, Scalar &y, Scalar &z,
275  Scalar &roll, Scalar &pitch, Scalar &yaw);
276 
277  /** \brief Create a transformation from the given translation and Euler angles (intrinsic rotations, ZYX-convention)
278  * \param[in] x the input x translation
279  * \param[in] y the input y translation
280  * \param[in] z the input z translation
281  * \param[in] roll the input roll angle
282  * \param[in] pitch the input pitch angle
283  * \param[in] yaw the input yaw angle
284  * \param[out] t the resulting transformation matrix
285  * \ingroup common
286  */
287  template <typename Scalar> void
288  getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw,
289  Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
290 
291  inline void
292  getTransformation (float x, float y, float z, float roll, float pitch, float yaw,
293  Eigen::Affine3f &t)
294  {
295  return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
296  }
297 
298  inline void
299  getTransformation (double x, double y, double z, double roll, double pitch, double yaw,
300  Eigen::Affine3d &t)
301  {
302  return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
303  }
304 
305  /** \brief Create a transformation from the given translation and Euler angles (intrinsic rotations, ZYX-convention)
306  * \param[in] x the input x translation
307  * \param[in] y the input y translation
308  * \param[in] z the input z translation
309  * \param[in] roll the input roll angle
310  * \param[in] pitch the input pitch angle
311  * \param[in] yaw the input yaw angle
312  * \return the resulting transformation matrix
313  * \ingroup common
314  */
315  inline Eigen::Affine3f
316  getTransformation (float x, float y, float z, float roll, float pitch, float yaw)
317  {
318  Eigen::Affine3f t;
319  getTransformation<float> (x, y, z, roll, pitch, yaw, t);
320  return (t);
321  }
322 
323  /** \brief Write a matrix to an output stream
324  * \param[in] matrix the matrix to output
325  * \param[out] file the output stream
326  * \ingroup common
327  */
328  template <typename Derived> void
329  saveBinary (const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
330 
331  /** \brief Read a matrix from an input stream
332  * \param[out] matrix the resulting matrix, read from the input stream
333  * \param[in,out] file the input stream
334  * \ingroup common
335  */
336  template <typename Derived> void
337  loadBinary (Eigen::MatrixBase<Derived> const& matrix, std::istream& file);
338 
339 // PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC gives the min between compile-time sizes. 0 has absolute priority, followed by 1,
340 // followed by Dynamic, followed by other finite values. The reason for giving Dynamic the priority over
341 // finite values is that min(3, Dynamic) should be Dynamic, since that could be anything between 0 and 3.
342 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
343  : (int (a) == 1 || int (b) == 1) ? 1 \
344  : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
345  : (int (a) <= int (b)) ? int (a) : int (b))
346 
347  /** \brief Returns the transformation between two point sets.
348  * The algorithm is based on:
349  * "Least-squares estimation of transformation parameters between two point patterns",
350  * Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
351  *
352  * It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
353  * \f{align*}
354  * \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
355  * \f}
356  * is minimized.
357  *
358  * The algorithm is based on the analysis of the covariance matrix
359  * \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
360  * of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where
361  * \f$d\f$ is corresponding to the dimension (which is typically small).
362  * The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
363  * though the actual computational effort lies in the covariance
364  * matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when
365  * the input point sets have dimension \f$d \times m\f$.
366  *
367  * \param[in] src Source points \f$ \mathbf{x} = \left( x_1, \hdots, x_n \right) \f$
368  * \param[in] dst Destination points \f$ \mathbf{y} = \left( y_1, \hdots, y_n \right) \f$.
369  * \param[in] with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed. (default: false)
370  * \return The homogeneous transformation
371  * \f{align*}
372  * T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
373  * \f}
374  * minimizing the resudiual above. This transformation is always returned as an
375  * Eigen::Matrix.
376  */
377  template <typename Derived, typename OtherDerived>
378  typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
379  umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling = false);
380 
381 /** \brief Transform a point using an affine matrix
382  * \param[in] point_in the vector to be transformed
383  * \param[out] point_out the transformed vector
384  * \param[in] transformation the transformation matrix
385  *
386  * \note Can be used with \c point_in = \c point_out
387  */
388  template<typename Scalar> inline void
389  transformPoint (const Eigen::Matrix<Scalar, 3, 1> &point_in,
390  Eigen::Matrix<Scalar, 3, 1> &point_out,
391  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
392  {
393  Eigen::Matrix<Scalar, 4, 1> point;
394  point << point_in, 1.0;
395  point_out = (transformation * point).template head<3> ();
396  }
397 
398 /** \brief Transform a vector using an affine matrix
399  * \param[in] vector_in the vector to be transformed
400  * \param[out] vector_out the transformed vector
401  * \param[in] transformation the transformation matrix
402  *
403  * \note Can be used with \c vector_in = \c vector_out
404  */
405  template <typename Scalar> inline void
406  transformVector (const Eigen::Matrix<Scalar, 3, 1> &vector_in,
407  Eigen::Matrix<Scalar, 3, 1> &vector_out,
408  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
409  {
410  vector_out = transformation.linear () * vector_in;
411  }
412 
413 /** \brief Transform a line using an affine matrix
414  * \param[in] line_in the line to be transformed
415  * \param[out] line_out the transformed line
416  * \param[in] transformation the transformation matrix
417  *
418  * Lines must be filled in this form:\n
419  * line[0-2] = Origin coordinates of the vector\n
420  * line[3-5] = Direction vector
421  *
422  * \note Can be used with \c line_in = \c line_out
423  */
424  template <typename Scalar> bool
425  transformLine (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_in,
426  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_out,
427  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
428 
429 /** \brief Transform plane vectors using an affine matrix
430  * \param[in] plane_in the plane coefficients to be transformed
431  * \param[out] plane_out the transformed plane coefficients to fill
432  * \param[in] transformation the transformation matrix
433  *
434  * The plane vectors are filled in the form ax+by+cz+d=0
435  * Can be used with non Hessian form planes coefficients
436  * Can be used with \c plane_in = \c plane_out
437  */
438  template <typename Scalar> void
439  transformPlane (const Eigen::Matrix<Scalar, 4, 1> &plane_in,
440  Eigen::Matrix<Scalar, 4, 1> &plane_out,
441  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
442 
443 /** \brief Transform plane vectors using an affine matrix
444  * \param[in] plane_in the plane coefficients to be transformed
445  * \param[out] plane_out the transformed plane coefficients to fill
446  * \param[in] transformation the transformation matrix
447  *
448  * The plane vectors are filled in the form ax+by+cz+d=0
449  * Can be used with non Hessian form planes coefficients
450  * Can be used with \c plane_in = \c plane_out
451  * \warning ModelCoefficients stores floats only !
452  */
453  template<typename Scalar> void
455  pcl::ModelCoefficients::Ptr plane_out,
456  const Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
457 
458 /** \brief Check coordinate system integrity
459  * \param[in] line_x the first axis
460  * \param[in] line_y the second axis
461  * \param[in] norm_limit the limit to ignore norm rounding errors
462  * \param[in] dot_limit the limit to ignore dot product rounding errors
463  * \return True if the coordinate system is consistent, false otherwise.
464  *
465  * Lines must be filled in this form:\n
466  * line[0-2] = Origin coordinates of the vector\n
467  * line[3-5] = Direction vector
468  *
469  * Can be used like this :\n
470  * line_x = X axis and line_y = Y axis\n
471  * line_x = Z axis and line_y = X axis\n
472  * line_x = Y axis and line_y = Z axis\n
473  * Because X^Y = Z, Z^X = Y and Y^Z = X.
474  * Do NOT invert line order !
475  *
476  * Determine whether a coordinate system is consistent or not by checking :\n
477  * Line origins: They must be the same for the 2 lines\n
478  * Norm: The 2 lines must be normalized\n
479  * Dot products: Must be 0 or perpendicular vectors
480  */
481  template<typename Scalar> bool
482  checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_x,
483  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line_y,
484  const Scalar norm_limit = 1e-3,
485  const Scalar dot_limit = 1e-3);
486 
487 /** \brief Check coordinate system integrity
488  * \param[in] origin the origin of the coordinate system
489  * \param[in] x_direction the first axis
490  * \param[in] y_direction the second axis
491  * \param[in] norm_limit the limit to ignore norm rounding errors
492  * \param[in] dot_limit the limit to ignore dot product rounding errors
493  * \return True if the coordinate system is consistent, false otherwise.
494  *
495  * Read the other variant for more information
496  */
497  template <typename Scalar> inline bool
498  checkCoordinateSystem (const Eigen::Matrix<Scalar, 3, 1> &origin,
499  const Eigen::Matrix<Scalar, 3, 1> &x_direction,
500  const Eigen::Matrix<Scalar, 3, 1> &y_direction,
501  const Scalar norm_limit = 1e-3,
502  const Scalar dot_limit = 1e-3)
503  {
504  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_x;
505  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> line_y;
506  line_x << origin, x_direction;
507  line_y << origin, y_direction;
508  return (checkCoordinateSystem<Scalar> (line_x, line_y, norm_limit, dot_limit));
509  }
510 
511  inline bool
512  checkCoordinateSystem (const Eigen::Matrix<double, 3, 1> &origin,
513  const Eigen::Matrix<double, 3, 1> &x_direction,
514  const Eigen::Matrix<double, 3, 1> &y_direction,
515  const double norm_limit = 1e-3,
516  const double dot_limit = 1e-3)
517  {
518  Eigen::Matrix<double, Eigen::Dynamic, 1> line_x;
519  Eigen::Matrix<double, Eigen::Dynamic, 1> line_y;
520  line_x.resize (6);
521  line_y.resize (6);
522  line_x << origin, x_direction;
523  line_y << origin, y_direction;
524  return (checkCoordinateSystem<double> (line_x, line_y, norm_limit, dot_limit));
525  }
526 
527  inline bool
528  checkCoordinateSystem (const Eigen::Matrix<float, 3, 1> &origin,
529  const Eigen::Matrix<float, 3, 1> &x_direction,
530  const Eigen::Matrix<float, 3, 1> &y_direction,
531  const float norm_limit = 1e-3,
532  const float dot_limit = 1e-3)
533  {
534  Eigen::Matrix<float, Eigen::Dynamic, 1> line_x;
535  Eigen::Matrix<float, Eigen::Dynamic, 1> line_y;
536  line_x.resize (6);
537  line_y.resize (6);
538  line_x << origin, x_direction;
539  line_y << origin, y_direction;
540  return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
541  }
542 
543 /** \brief Compute the transformation between two coordinate systems
544  * \param[in] from_line_x X axis from the origin coordinate system
545  * \param[in] from_line_y Y axis from the origin coordinate system
546  * \param[in] to_line_x X axis from the destination coordinate system
547  * \param[in] to_line_y Y axis from the destination coordinate system
548  * \param[out] transformation the transformation matrix to fill
549  * \return true if transformation was filled, false otherwise.
550  *
551  * Line must be filled in this form:\n
552  * line[0-2] = Coordinate system origin coordinates \n
553  * line[3-5] = Direction vector (norm doesn't matter)
554  */
555  template <typename Scalar> bool
556  transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
557  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
558  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
559  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
560  Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation);
561 
562 }
563 
564 #include <pcl/common/impl/eigen.hpp>
565 
566 #if defined __SUNPRO_CC
567 # pragma enable_warn
568 #endif
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:226
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 (intrinsic rotations, ZYX-convention) from the given transformatio...
Definition: eigen.hpp:594
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:573
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Definition: eigen.hpp:492
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition: eigen.hpp:424
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
Definition: eigen.hpp:638
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 (intrinsic rotations,...
Definition: eigen.hpp:608
void getEulerAngles(const Eigen::Transform< Scalar, 3, Eigen::Affine > &t, Scalar &roll, Scalar &pitch, Scalar &yaw)
Extract the Euler angles (intrinsic rotations, ZYX-convention) from the given transformation.
Definition: eigen.hpp:585
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Definition: eigen.hpp:133
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:554
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:528
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
Definition: eigen.hpp:459
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:296
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Definition: eigen.hpp:623
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
Definition: eigen.hpp:405
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:502
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:788
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:758
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:389
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:406
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:660
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:854
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:736
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:53
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:68
shared_ptr< ::pcl::ModelCoefficients > Ptr
shared_ptr< const ::pcl::ModelCoefficients > ConstPtr