42 #ifndef PCL_COMMON_EIGEN_H_
43 #define PCL_COMMON_EIGEN_H_
50 # pragma GCC system_header
51 #elif defined __SUNPRO_CC
57 #include <Eigen/StdVector>
59 #include <Eigen/Eigenvalues>
60 #include <Eigen/Geometry>
63 #include <Eigen/Dense>
64 #include <Eigen/Eigenvalues>
73 template<
typename Scalar,
typename Roots>
inline void
76 roots (0) = Scalar (0);
77 Scalar d = Scalar (b * b - 4.0 * c);
81 Scalar sd = ::std::sqrt (d);
83 roots (2) = 0.5f * (b + sd);
84 roots (1) = 0.5f * (b - sd);
91 template<
typename Matrix,
typename Roots>
inline void
94 typedef typename Matrix::Scalar Scalar;
99 Scalar c0 = m (0, 0) * m (1, 1) * m (2, 2)
100 + Scalar (2) * m (0, 1) * m (0, 2) * m (1, 2)
101 - m (0, 0) * m (1, 2) * m (1, 2)
102 - m (1, 1) * m (0, 2) * m (0, 2)
103 - m (2, 2) * m (0, 1) * m (0, 1);
104 Scalar c1 = m (0, 0) * m (1, 1) -
105 m (0, 1) * m (0, 1) +
106 m (0, 0) * m (2, 2) -
107 m (0, 2) * m (0, 2) +
108 m (1, 1) * m (2, 2) -
110 Scalar c2 = m (0, 0) + m (1, 1) + m (2, 2);
113 if (fabs (c0) < Eigen::NumTraits<Scalar>::epsilon ())
117 const Scalar s_inv3 = Scalar (1.0 / 3.0);
118 const Scalar s_sqrt3 = std::sqrt (Scalar (3.0));
121 Scalar c2_over_3 = c2*s_inv3;
122 Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
123 if (a_over_3 > Scalar (0))
124 a_over_3 = Scalar (0);
126 Scalar half_b = Scalar (0.5) * (c0 + c2_over_3 * (Scalar (2) * c2_over_3 * c2_over_3 - c1));
128 Scalar q = half_b * half_b + a_over_3 * a_over_3*a_over_3;
133 Scalar rho = std::sqrt (-a_over_3);
134 Scalar theta = std::atan2 (std::sqrt (-q), half_b) * s_inv3;
135 Scalar cos_theta = std::cos (theta);
136 Scalar sin_theta = std::sin (theta);
137 roots (0) = c2_over_3 + Scalar (2) * rho * cos_theta;
138 roots (1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
139 roots (2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
142 if (roots (0) >= roots (1))
143 std::swap (roots (0), roots (1));
144 if (roots (1) >= roots (2))
146 std::swap (roots (1), roots (2));
147 if (roots (0) >= roots (1))
148 std::swap (roots (0), roots (1));
162 template <
typename Matrix,
typename Vector>
inline void
163 eigen22 (
const Matrix& mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
167 if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
169 if (mat.coeff (0) < mat.coeff (2))
171 eigenvalue = mat.coeff (0);
172 eigenvector [0] = 1.0;
173 eigenvector [1] = 0.0;
177 eigenvalue = mat.coeff (2);
178 eigenvector [0] = 0.0;
179 eigenvector [1] = 1.0;
185 typename Matrix::Scalar trace =
static_cast<typename Matrix::Scalar
> (0.5) * (mat.coeff (0) + mat.coeff (3));
186 typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
188 typename Matrix::Scalar temp = trace * trace - determinant;
193 eigenvalue = trace - ::std::sqrt (temp);
195 eigenvector [0] = - mat.coeff (1);
196 eigenvector [1] = mat.coeff (0) - eigenvalue;
197 eigenvector.normalize ();
206 template <
typename Matrix,
typename Vector>
inline void
207 eigen22 (
const Matrix& mat, Matrix& eigenvectors, Vector& eigenvalues)
211 if (fabs(mat.coeff (1)) <= std::numeric_limits<typename Matrix::Scalar>::min ())
213 if (mat.coeff (0) < mat.coeff (3))
215 eigenvalues.coeffRef (0) = mat.coeff (0);
216 eigenvalues.coeffRef (1) = mat.coeff (3);
217 eigenvectors.coeffRef (0) = 1.0;
218 eigenvectors.coeffRef (1) = 0.0;
219 eigenvectors.coeffRef (2) = 0.0;
220 eigenvectors.coeffRef (3) = 1.0;
224 eigenvalues.coeffRef (0) = mat.coeff (3);
225 eigenvalues.coeffRef (1) = mat.coeff (0);
226 eigenvectors.coeffRef (0) = 0.0;
227 eigenvectors.coeffRef (1) = 1.0;
228 eigenvectors.coeffRef (2) = 1.0;
229 eigenvectors.coeffRef (3) = 0.0;
235 typename Matrix::Scalar trace =
static_cast<typename Matrix::Scalar
> (0.5) * (mat.coeff (0) + mat.coeff (3));
236 typename Matrix::Scalar determinant = mat.coeff (0) * mat.coeff (3) - mat.coeff (1) * mat.coeff (1);
238 typename Matrix::Scalar temp = trace * trace - determinant;
243 temp = ::std::sqrt (temp);
245 eigenvalues.coeffRef (0) = trace - temp;
246 eigenvalues.coeffRef (1) = trace + temp;
249 eigenvectors.coeffRef (0) = - mat.coeff (1);
250 eigenvectors.coeffRef (2) = mat.coeff (0) - eigenvalues.coeff (0);
251 typename Matrix::Scalar norm =
static_cast<typename Matrix::Scalar
> (1.0) /
252 static_cast<typename Matrix::Scalar
> (::std::sqrt (eigenvectors.coeffRef (0) * eigenvectors.coeffRef (0) + eigenvectors.coeffRef (2) * eigenvectors.coeffRef (2)));
253 eigenvectors.coeffRef (0) *= norm;
254 eigenvectors.coeffRef (2) *= norm;
255 eigenvectors.coeffRef (1) = eigenvectors.coeffRef (2);
256 eigenvectors.coeffRef (3) = -eigenvectors.coeffRef (0);
265 template<
typename Matrix,
typename Vector>
inline void
268 typedef typename Matrix::Scalar Scalar;
272 Scalar scale = mat.cwiseAbs ().maxCoeff ();
273 if (scale <= std::numeric_limits<Scalar>::min ())
274 scale = Scalar (1.0);
276 Matrix scaledMat = mat / scale;
278 scaledMat.diagonal ().array () -= eigenvalue / scale;
280 Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
281 Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
282 Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
284 Scalar len1 = vec1.squaredNorm ();
285 Scalar len2 = vec2.squaredNorm ();
286 Scalar len3 = vec3.squaredNorm ();
288 if (len1 >= len2 && len1 >= len3)
289 eigenvector = vec1 / std::sqrt (len1);
290 else if (len2 >= len1 && len2 >= len3)
291 eigenvector = vec2 / std::sqrt (len2);
293 eigenvector = vec3 / std::sqrt (len3);
303 template<
typename Matrix,
typename Vector>
inline void
304 eigen33 (
const Matrix& mat,
typename Matrix::Scalar& eigenvalue, Vector& eigenvector)
306 typedef typename Matrix::Scalar Scalar;
310 Scalar scale = mat.cwiseAbs ().maxCoeff ();
311 if (scale <= std::numeric_limits<Scalar>::min ())
312 scale = Scalar (1.0);
314 Matrix scaledMat = mat / scale;
319 eigenvalue = eigenvalues (0) * scale;
321 scaledMat.diagonal ().array () -= eigenvalues (0);
323 Vector vec1 = scaledMat.row (0).cross (scaledMat.row (1));
324 Vector vec2 = scaledMat.row (0).cross (scaledMat.row (2));
325 Vector vec3 = scaledMat.row (1).cross (scaledMat.row (2));
327 Scalar len1 = vec1.squaredNorm ();
328 Scalar len2 = vec2.squaredNorm ();
329 Scalar len3 = vec3.squaredNorm ();
331 if (len1 >= len2 && len1 >= len3)
332 eigenvector = vec1 / std::sqrt (len1);
333 else if (len2 >= len1 && len2 >= len3)
334 eigenvector = vec2 / std::sqrt (len2);
336 eigenvector = vec3 / std::sqrt (len3);
344 template<
typename Matrix,
typename Vector>
inline void
347 typedef typename Matrix::Scalar Scalar;
348 Scalar scale = mat.cwiseAbs ().maxCoeff ();
349 if (scale <= std::numeric_limits<Scalar>::min ())
350 scale = Scalar (1.0);
352 Matrix scaledMat = mat / scale;
363 template<
typename Matrix,
typename Vector>
inline void
364 eigen33 (
const Matrix& mat, Matrix& evecs, Vector& evals)
366 typedef typename Matrix::Scalar Scalar;
370 Scalar scale = mat.cwiseAbs ().maxCoeff ();
371 if (scale <= std::numeric_limits<Scalar>::min ())
372 scale = Scalar (1.0);
374 Matrix scaledMat = mat / scale;
379 if ((evals (2) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon ())
382 evecs.setIdentity ();
384 else if ((evals (1) - evals (0)) <= Eigen::NumTraits<Scalar>::epsilon () )
389 tmp.diagonal ().array () -= evals (2);
391 Vector vec1 = tmp.row (0).cross (tmp.row (1));
392 Vector vec2 = tmp.row (0).cross (tmp.row (2));
393 Vector vec3 = tmp.row (1).cross (tmp.row (2));
395 Scalar len1 = vec1.squaredNorm ();
396 Scalar len2 = vec2.squaredNorm ();
397 Scalar len3 = vec3.squaredNorm ();
399 if (len1 >= len2 && len1 >= len3)
400 evecs.col (2) = vec1 / std::sqrt (len1);
401 else if (len2 >= len1 && len2 >= len3)
402 evecs.col (2) = vec2 / std::sqrt (len2);
404 evecs.col (2) = vec3 / std::sqrt (len3);
406 evecs.col (1) = evecs.col (2).unitOrthogonal ();
407 evecs.col (0) = evecs.col (1).cross (evecs.col (2));
409 else if ((evals (2) - evals (1)) <= Eigen::NumTraits<Scalar>::epsilon () )
414 tmp.diagonal ().array () -= evals (0);
416 Vector vec1 = tmp.row (0).cross (tmp.row (1));
417 Vector vec2 = tmp.row (0).cross (tmp.row (2));
418 Vector vec3 = tmp.row (1).cross (tmp.row (2));
420 Scalar len1 = vec1.squaredNorm ();
421 Scalar len2 = vec2.squaredNorm ();
422 Scalar len3 = vec3.squaredNorm ();
424 if (len1 >= len2 && len1 >= len3)
425 evecs.col (0) = vec1 / std::sqrt (len1);
426 else if (len2 >= len1 && len2 >= len3)
427 evecs.col (0) = vec2 / std::sqrt (len2);
429 evecs.col (0) = vec3 / std::sqrt (len3);
431 evecs.col (1) = evecs.col (0).unitOrthogonal ();
432 evecs.col (2) = evecs.col (0).cross (evecs.col (1));
438 tmp.diagonal ().array () -= evals (2);
440 Vector vec1 = tmp.row (0).cross (tmp.row (1));
441 Vector vec2 = tmp.row (0).cross (tmp.row (2));
442 Vector vec3 = tmp.row (1).cross (tmp.row (2));
444 Scalar len1 = vec1.squaredNorm ();
445 Scalar len2 = vec2.squaredNorm ();
446 Scalar len3 = vec3.squaredNorm ();
448 Scalar *mmax =
new Scalar[3];
452 unsigned int min_el = 2;
453 unsigned int max_el = 2;
454 if (len1 >= len2 && len1 >= len3)
457 evecs.col (2) = vec1 / std::sqrt (len1);
459 else if (len2 >= len1 && len2 >= len3)
462 evecs.col (2) = vec2 / std::sqrt (len2);
467 evecs.col (2) = vec3 / std::sqrt (len3);
471 tmp.diagonal ().array () -= evals (1);
473 vec1 = tmp.row (0).cross (tmp.row (1));
474 vec2 = tmp.row (0).cross (tmp.row (2));
475 vec3 = tmp.row (1).cross (tmp.row (2));
477 len1 = vec1.squaredNorm ();
478 len2 = vec2.squaredNorm ();
479 len3 = vec3.squaredNorm ();
480 if (len1 >= len2 && len1 >= len3)
483 evecs.col (1) = vec1 / std::sqrt (len1);
484 min_el = len1 <= mmax[min_el] ? 1 : min_el;
485 max_el = len1 > mmax[max_el] ? 1 : max_el;
487 else if (len2 >= len1 && len2 >= len3)
490 evecs.col (1) = vec2 / std::sqrt (len2);
491 min_el = len2 <= mmax[min_el] ? 1 : min_el;
492 max_el = len2 > mmax[max_el] ? 1 : max_el;
497 evecs.col (1) = vec3 / std::sqrt (len3);
498 min_el = len3 <= mmax[min_el] ? 1 : min_el;
499 max_el = len3 > mmax[max_el] ? 1 : max_el;
503 tmp.diagonal ().array () -= evals (0);
505 vec1 = tmp.row (0).cross (tmp.row (1));
506 vec2 = tmp.row (0).cross (tmp.row (2));
507 vec3 = tmp.row (1).cross (tmp.row (2));
509 len1 = vec1.squaredNorm ();
510 len2 = vec2.squaredNorm ();
511 len3 = vec3.squaredNorm ();
512 if (len1 >= len2 && len1 >= len3)
515 evecs.col (0) = vec1 / std::sqrt (len1);
516 min_el = len3 <= mmax[min_el] ? 0 : min_el;
517 max_el = len3 > mmax[max_el] ? 0 : max_el;
519 else if (len2 >= len1 && len2 >= len3)
522 evecs.col (0) = vec2 / std::sqrt (len2);
523 min_el = len3 <= mmax[min_el] ? 0 : min_el;
524 max_el = len3 > mmax[max_el] ? 0 : max_el;
529 evecs.col (0) = vec3 / std::sqrt (len3);
530 min_el = len3 <= mmax[min_el] ? 0 : min_el;
531 max_el = len3 > mmax[max_el] ? 0 : max_el;
534 unsigned mid_el = 3 - min_el - max_el;
535 evecs.col (min_el) = evecs.col ((min_el + 1) % 3).cross ( evecs.col ((min_el + 2) % 3) ).normalized ();
536 evecs.col (mid_el) = evecs.col ((mid_el + 1) % 3).cross ( evecs.col ((mid_el + 2) % 3) ).normalized ();
552 template<
typename Matrix>
inline typename Matrix::Scalar
555 typedef typename Matrix::Scalar Scalar;
556 Scalar det = matrix.coeff (0) * matrix.coeff (3) - matrix.coeff (1) * matrix.coeff (2) ;
561 inverse.coeffRef (0) = matrix.coeff (3);
562 inverse.coeffRef (1) = - matrix.coeff (1);
563 inverse.coeffRef (2) = - matrix.coeff (2);
564 inverse.coeffRef (3) = matrix.coeff (0);
577 template<
typename Matrix>
inline typename Matrix::Scalar
580 typedef typename Matrix::Scalar Scalar;
591 Scalar fd_ee = matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (7) * matrix.coeff (5);
592 Scalar ce_bf = matrix.coeff (2) * matrix.coeff (5) - matrix.coeff (1) * matrix.coeff (8);
593 Scalar be_cd = matrix.coeff (1) * matrix.coeff (5) - matrix.coeff (2) * matrix.coeff (4);
595 Scalar det = matrix.coeff (0) * fd_ee + matrix.coeff (1) * ce_bf + matrix.coeff (2) * be_cd;
600 inverse.coeffRef (0) = fd_ee;
601 inverse.coeffRef (1) = inverse.coeffRef (3) = ce_bf;
602 inverse.coeffRef (2) = inverse.coeffRef (6) = be_cd;
603 inverse.coeffRef (4) = (matrix.coeff (0) * matrix.coeff (8) - matrix.coeff (2) * matrix.coeff (2));
604 inverse.coeffRef (5) = inverse.coeffRef (7) = (matrix.coeff (1) * matrix.coeff (2) - matrix.coeff (0) * matrix.coeff (5));
605 inverse.coeffRef (8) = (matrix.coeff (0) * matrix.coeff (4) - matrix.coeff (1) * matrix.coeff (1));
617 template<
typename Matrix>
inline typename Matrix::Scalar
620 typedef typename Matrix::Scalar Scalar;
627 Scalar ie_hf = matrix.coeff (8) * matrix.coeff (4) - matrix.coeff (7) * matrix.coeff (5);
628 Scalar hc_ib = matrix.coeff (7) * matrix.coeff (2) - matrix.coeff (8) * matrix.coeff (1);
629 Scalar fb_ec = matrix.coeff (5) * matrix.coeff (1) - matrix.coeff (4) * matrix.coeff (2);
630 Scalar det = matrix.coeff (0) * (ie_hf) + matrix.coeff (3) * (hc_ib) + matrix.coeff (6) * (fb_ec) ;
634 inverse.coeffRef (0) = ie_hf;
635 inverse.coeffRef (1) = hc_ib;
636 inverse.coeffRef (2) = fb_ec;
637 inverse.coeffRef (3) = matrix.coeff (6) * matrix.coeff (5) - matrix.coeff (8) * matrix.coeff (3);
638 inverse.coeffRef (4) = matrix.coeff (8) * matrix.coeff (0) - matrix.coeff (6) * matrix.coeff (2);
639 inverse.coeffRef (5) = matrix.coeff (3) * matrix.coeff (2) - matrix.coeff (5) * matrix.coeff (0);
640 inverse.coeffRef (6) = matrix.coeff (7) * matrix.coeff (3) - matrix.coeff (6) * matrix.coeff (4);
641 inverse.coeffRef (7) = matrix.coeff (6) * matrix.coeff (1) - matrix.coeff (7) * matrix.coeff (0);
642 inverse.coeffRef (8) = matrix.coeff (4) * matrix.coeff (0) - matrix.coeff (3) * matrix.coeff (1);
649 template<
typename Matrix>
inline typename Matrix::Scalar
653 return matrix.coeff (0) * (matrix.coeff (4) * matrix.coeff (8) - matrix.coeff (5) * matrix.coeff (7)) +
654 matrix.coeff (1) * (matrix.coeff (5) * matrix.coeff (6) - matrix.coeff (3) * matrix.coeff (8)) +
655 matrix.coeff (2) * (matrix.coeff (3) * matrix.coeff (7) - matrix.coeff (4) * matrix.coeff (6)) ;
667 const Eigen::Vector3f& y_direction,
668 Eigen::Affine3f& transformation);
677 inline Eigen::Affine3f
679 const Eigen::Vector3f& y_direction);
690 const Eigen::Vector3f& y_direction,
691 Eigen::Affine3f& transformation);
700 inline Eigen::Affine3f
702 const Eigen::Vector3f& y_direction);
713 const Eigen::Vector3f& z_axis,
714 Eigen::Affine3f& transformation);
723 inline Eigen::Affine3f
725 const Eigen::Vector3f& z_axis);
737 const Eigen::Vector3f& z_axis,
738 const Eigen::Vector3f& origin,
739 Eigen::Affine3f& transformation);
749 getEulerAngles (
const Eigen::Affine3f& t,
float& roll,
float& pitch,
float& yaw);
763 float& x,
float& y,
float& z,
764 float& roll,
float& pitch,
float& yaw);
776 template <
typename Scalar>
inline void
777 getTransformation (Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw,
778 Eigen::Transform<Scalar, 3, Eigen::Affine> &t);
784 return (getTransformation<float> (x, y, z, roll, pitch, yaw, t));
791 return (getTransformation<double> (x, y, z, roll, pitch, yaw, t));
804 inline Eigen::Affine3f
805 getTransformation (
float x,
float y,
float z,
float roll,
float pitch,
float yaw);
812 template <
typename Derived>
void
813 saveBinary (
const Eigen::MatrixBase<Derived>& matrix, std::ostream& file);
820 template <
typename Derived>
void
821 loadBinary (Eigen::MatrixBase<Derived>
const& matrix, std::istream& file);
826 #define PCL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
827 : (int (a) == 1 || int (b) == 1) ? 1 \
828 : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
829 : (int (a) <= int (b)) ? int (a) : int (b))
861 template <
typename Derived,
typename OtherDerived>
862 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
863 umeyama (
const Eigen::MatrixBase<Derived>& src,
const Eigen::MatrixBase<OtherDerived>& dst,
bool with_scaling =
false);
866 #include <pcl/common/impl/eigen.hpp>
868 #if defined __SUNPRO_CC
872 #endif //PCL_COMMON_EIGEN_H_
void computeRoots(const Matrix &m, Roots &roots)
computes the roots of the characteristic polynomial of the input matrix m, which are the eigenvalues ...
Matrix::Scalar invert2x2(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 2x2 matrix.
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...
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...
Matrix::Scalar determinant3x3Matrix(const Matrix &matrix)
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.
void saveBinary(const Eigen::MatrixBase< Derived > &matrix, std::ostream &file)
Write a matrix to an output stream.
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=...
void computeRoots2(const Scalar &b, const Scalar &c, Roots &roots)
Compute the roots of a quadratic polynom x^2 + b*x + c = 0.
void getTranslationAndEulerAngles(const Eigen::Affine3f &t, float &x, float &y, float &z, float &roll, float &pitch, float &yaw)
Extract x,y,z and the Euler angles (XYZ-convention) from the given transformation.
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=...
void loadBinary(Eigen::MatrixBase< Derived > const &matrix, std::istream &file)
Read a matrix from an input stream.
void getEulerAngles(const Eigen::Affine3f &t, float &roll, float &pitch, float &yaw)
Extract the Euler angles (XYZ-convention) from the given transformation.
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 orign to (0,0,0) and rotate z_axis into (0...
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) ...
Matrix::Scalar invert3x3SymMatrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a 3x3 symmetric matrix.
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=...
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Matrix::Scalar invert3x3Matrix(const Matrix &matrix, Matrix &inverse)
Calculate the inverse of a general 3x3 matrix.