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>();
