Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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
61namespace 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
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:593
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:572
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
Calculate the determinant of a 3x3 matrix.
Definition eigen.hpp:491
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
Definition eigen.hpp:423
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
Definition eigen.hpp:637
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:607
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:584
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:553
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:527
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.
Definition eigen.hpp:458
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:295
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
Definition eigen.hpp:622
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
Definition eigen.hpp:404
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:501
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:787
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:757
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:659
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:853
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:735
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