diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index a932b3ff8..80e16a450 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -2,6 +2,7 @@ // for linear algebra. Eigen itself is part of the KDE project. // // Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2009 Benoit Jacob // // Eigen is free software; you can redistribute it and/or // modify it under the terms of the GNU Lesser General Public @@ -247,7 +248,11 @@ public: template inline Transform operator*(const RotationBase& r) const; - LinearMatrixType rotation(TransformTraits traits = Affine) const; + LinearMatrixType rotation() const; + template + void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const; + template + void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const; template Transform& fromPositionOrientationScale(const MatrixBase &position, @@ -589,48 +594,61 @@ inline Transform Transform::operator*(const RotationBase return res; } -/*************************** -*** Specialial functions *** -***************************/ +/************************ +*** Special functions *** +************************/ /** \returns the rotation part of the transformation * \nonstableyet * - * \param traits allows to optimize the extraction process when the transformion - * is known to be not a general aafine transformation. The possible values are: - * - Affine which use a QR decomposition (default), - * - Isometry which simply returns the linear part ! + * \svd_module * - * \warning this function consider the scaling is positive - * - * \warning to use this method in the general case (traits==GenericAffine), you need - * to include the QR module. - * - * \sa inverse(), class QR + * \sa computeRotationScaling(), computeScalingRotation(), class SVD */ template typename Transform::LinearMatrixType -Transform::rotation(TransformTraits traits) const +Transform::rotation() const { - ei_assert(traits!=Projective && "you cannot extract a rotation from a non affine transformation"); - if (traits == Affine) - { - // FIXME maybe QR should be fixed to return a R matrix with a positive diagonal ?? - QR qr(linear()); - LinearMatrixType matQ = qr.matrixQ(); - LinearMatrixType matR = qr.matrixR(); - for (int i=0 ; i +template +void Transform::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const +{ + linear().svd().computeRotationScaling(rotation, scaling); +} + +/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * \nonstableyet + * + * \svd_module + * + * \sa computeRotationScaling(), rotation(), class SVD + */ +template +template +void Transform::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const +{ + linear().svd().computeScalingRotation(scaling, rotation); } /** Convenient method to set \c *this from a position, orientation and scale diff --git a/Eigen/src/SVD/SVD.h b/Eigen/src/SVD/SVD.h index 9d269169e..0a52acf3d 100644 --- a/Eigen/src/SVD/SVD.h +++ b/Eigen/src/SVD/SVD.h @@ -79,8 +79,14 @@ template class SVD void compute(const MatrixType& matrix); SVD& sort(); - void computeUnitaryPositive(MatrixUType *unitary, MatrixType *positive) const; - void computePositiveUnitary(MatrixType *positive, MatrixVType *unitary) const; + template + void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const; + template + void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const; + template + void computeRotationScaling(RotationType *unitary, ScalingType *positive) const; + template + void computeScalingRotation(ScalingType *positive, RotationType *unitary) const; protected: /** \internal */ @@ -542,10 +548,13 @@ bool SVD::solve(const MatrixBase &b, ResultType* resul * If either pointer is zero, the corresponding computation is skipped. * * Only for square matrices. + * + * \sa computePositiveUnitary(), computeRotationScaling() */ template -void SVD::computeUnitaryPositive(typename SVD::MatrixUType *unitary, - MatrixType *positive) const +template +void SVD::computeUnitaryPositive(UnitaryType *unitary, + PositiveType *positive) const { ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices"); if(unitary) *unitary = m_matU * m_matV.adjoint(); @@ -557,16 +566,72 @@ void SVD::computeUnitaryPositive(typename SVD::MatrixUTy * If either pointer is zero, the corresponding computation is skipped. * * Only for square matrices. + * + * \sa computeUnitaryPositive(), computeRotationScaling() */ template -void SVD::computePositiveUnitary(MatrixType *positive, - typename SVD::MatrixVType *unitary) const +template +void SVD::computePositiveUnitary(UnitaryType *positive, + PositiveType *unitary) const { ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices"); if(unitary) *unitary = m_matU * m_matV.adjoint(); if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint(); } +/** decomposes the matrix as a product rotation x scaling, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * This method requires the Geometry module. + * + * \sa computeScalingRotation(), computeUnitaryPositive() + */ +template +template +void SVD::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const +{ + ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices"); + Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1 + Matrix sv(m_sigma); + sv.coeffRef(0) *= x; + if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint()); + if(rotation) + { + MatrixType m(m_matU); + m.col(0) /= x; + rotation->lazyAssign(m * m_matV.adjoint()); + } +} + +/** decomposes the matrix as a product scaling x rotation, the scaling being + * not necessarily positive. + * + * If either pointer is zero, the corresponding computation is skipped. + * + * This method requires the Geometry module. + * + * \sa computeRotationScaling(), computeUnitaryPositive() + */ +template +template +void SVD::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const +{ + ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices"); + Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1 + Matrix sv(m_sigma); + sv.coeffRef(0) *= x; + if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint()); + if(rotation) + { + MatrixType m(m_matU); + m.col(0) /= x; + rotation->lazyAssign(m * m_matV.adjoint()); + } +} + + /** \svd_module * \returns the SVD decomposition of \c *this */ diff --git a/test/geometry.cpp b/test/geometry.cpp index df8ed421e..bea3f4660 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp @@ -25,7 +25,7 @@ #include "main.h" #include #include -#include +#include template void geometry(void) { @@ -339,10 +339,22 @@ template void geometry(void) t0.translate(v0).rotate(q1); VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse()); - // test extract rotation + // test extract rotation and scaling t0.setIdentity(); t0.translate(v0).rotate(q1).scale(v1); - VERIFY_IS_APPROX(t0.rotation(Affine) * v1, Matrix3(q1) * v1); + VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1); + + Matrix3 mat_rotation, mat_scaling; + t0.setIdentity(); + t0.translate(v0).rotate(q1).scale(v1); + t0.computeRotationScaling(&mat_rotation, &mat_scaling); + VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling); + VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); + VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); + t0.computeScalingRotation(&mat_scaling, &mat_rotation); + VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation); + VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity()); + VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); // test casting Transform t1f = t1.template cast();