add computeRotationScaling and computeScalingRotation in SVD add convenience functions in Transform reimplement Transform::rotation() to use that add unit-test
diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index a932b3f..80e16a4 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 <g.gael@free.fr> +// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com> // // 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 @@ template<typename Derived> inline Transform operator*(const RotationBase<Derived,Dim>& r) const; - LinearMatrixType rotation(TransformTraits traits = Affine) const; + LinearMatrixType rotation() const; + template<typename RotationMatrixType, typename ScalingMatrixType> + void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const; + template<typename ScalingMatrixType, typename RotationMatrixType> + void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const; template<typename PositionDerived, typename OrientationType, typename ScaleDerived> Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position, @@ -589,48 +594,61 @@ 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 Scalar, int Dim> typename Transform<Scalar,Dim>::LinearMatrixType -Transform<Scalar,Dim>::rotation(TransformTraits traits) const +Transform<Scalar,Dim>::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<LinearMatrixType> qr(linear()); - LinearMatrixType matQ = qr.matrixQ(); - LinearMatrixType matR = qr.matrixR(); - for (int i=0 ; i<Dim; ++i) - if (matR.coeff(i,i)<0) - matQ.col(i) = -matQ.col(i); - return matQ; - } - else if (traits == Isometry) // though that's stupid let's handle it ! - return linear(); // FIXME needs to divide by determinant - else - { - ei_assert("invalid traits value in Transform::rotation()"); - return LinearMatrixType(); - } + LinearMatrixType result; + computeRotationScaling(&result, (LinearMatrixType*)0); + return result; +} + + +/** 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 computeScalingRotation(), rotation(), class SVD + */ +template<typename Scalar, int Dim> +template<typename RotationMatrixType, typename ScalingMatrixType> +void Transform<Scalar,Dim>::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<typename Scalar, int Dim> +template<typename ScalingMatrixType, typename RotationMatrixType> +void Transform<Scalar,Dim>::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 9d26916..0a52acf 100644 --- a/Eigen/src/SVD/SVD.h +++ b/Eigen/src/SVD/SVD.h
@@ -79,8 +79,14 @@ void compute(const MatrixType& matrix); SVD& sort(); - void computeUnitaryPositive(MatrixUType *unitary, MatrixType *positive) const; - void computePositiveUnitary(MatrixType *positive, MatrixVType *unitary) const; + template<typename UnitaryType, typename PositiveType> + void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const; + template<typename PositiveType, typename UnitaryType> + void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const; + template<typename RotationType, typename ScalingType> + void computeRotationScaling(RotationType *unitary, ScalingType *positive) const; + template<typename ScalingType, typename RotationType> + void computeScalingRotation(ScalingType *positive, RotationType *unitary) const; protected: /** \internal */ @@ -542,10 +548,13 @@ * If either pointer is zero, the corresponding computation is skipped. * * Only for square matrices. + * + * \sa computePositiveUnitary(), computeRotationScaling() */ template<typename MatrixType> -void SVD<MatrixType>::computeUnitaryPositive(typename SVD<MatrixType>::MatrixUType *unitary, - MatrixType *positive) const +template<typename UnitaryType, typename PositiveType> +void SVD<MatrixType>::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 @@ * If either pointer is zero, the corresponding computation is skipped. * * Only for square matrices. + * + * \sa computeUnitaryPositive(), computeRotationScaling() */ template<typename MatrixType> -void SVD<MatrixType>::computePositiveUnitary(MatrixType *positive, - typename SVD<MatrixType>::MatrixVType *unitary) const +template<typename UnitaryType, typename PositiveType> +void SVD<MatrixType>::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<typename MatrixType> +template<typename RotationType, typename ScalingType> +void SVD<MatrixType>::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<Scalar, MatrixType::RowsAtCompileTime, 1> 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<typename MatrixType> +template<typename ScalingType, typename RotationType> +void SVD<MatrixType>::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<Scalar, MatrixType::RowsAtCompileTime, 1> 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 df8ed42..bea3f46 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp
@@ -25,7 +25,7 @@ #include "main.h" #include <Eigen/Geometry> #include <Eigen/LU> -#include <Eigen/QR> +#include <Eigen/SVD> template<typename Scalar> void geometry(void) { @@ -339,10 +339,22 @@ 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<float,3> t1f = t1.template cast<float>();