diff --git a/Eigen/src/Core/util/Constants.h b/Eigen/src/Core/util/Constants.h index 1dbba1bea..1d728a5d5 100644 --- a/Eigen/src/Core/util/Constants.h +++ b/Eigen/src/Core/util/Constants.h @@ -240,11 +240,10 @@ enum { }; enum TransformTraits { - Affine = 0x1, - Isometry = 0x2, - AffineSquare = Affine|0x10, - AffineCompact = Affine|0x20, - Projective = 0x30 + Isometry = 0x1, + Affine = 0x2, + AffineCompact = 0x10 | Affine, + Projective = 0x20 }; const int EiArch_Generic = 0x0; diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index a8953ead3..ae079a29a 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h @@ -118,7 +118,7 @@ template class Cross; template class Quaternion; template class Rotation2D; template class AngleAxis; -template class Transform; +template class Transform; template class ParametrizedLine; template class Hyperplane; template class Translation; diff --git a/Eigen/src/Geometry/Homogeneous.h b/Eigen/src/Geometry/Homogeneous.h index ef80da7ca..b38932f48 100644 --- a/Eigen/src/Geometry/Homogeneous.h +++ b/Eigen/src/Geometry/Homogeneous.h @@ -99,6 +99,26 @@ template class Homogeneous ei_assert(Direction==Vertical); return ei_homogeneous_left_product_impl(lhs.derived(),rhs.m_matrix); } + + template friend + inline const ei_homogeneous_left_product_impl::AffinePart> + operator* (const Transform& tr, const Homogeneous& rhs) + { + ei_assert(Direction==Vertical); + return ei_homogeneous_left_product_impl::AffinePart> + (tr.affine(),rhs.m_matrix); + } + + template friend + inline const ei_homogeneous_left_product_impl::MatrixType> + operator* (const Transform& tr, const Homogeneous& rhs) + { + ei_assert(Direction==Vertical); + return ei_homogeneous_left_product_impl::MatrixType> + (tr.matrix(),rhs.m_matrix); + } protected: const typename MatrixType::Nested m_matrix; diff --git a/Eigen/src/Geometry/RotationBase.h b/Eigen/src/Geometry/RotationBase.h index 92c827932..3531f0f29 100644 --- a/Eigen/src/Geometry/RotationBase.h +++ b/Eigen/src/Geometry/RotationBase.h @@ -99,8 +99,9 @@ class RotationBase inline RotationMatrixType operator*(const MatrixBase& l, const Derived& r) { return l.derived() * r.toRotationMatrix(); } - /** \returns the concatenation of the rotation \c *this with an affine transformation \a t */ - inline Transform operator*(const Transform& t) const + /** \returns the concatenation of the rotation \c *this with a transformation \a t */ + template + inline Transform operator*(const Transform& t) const { return toRotationMatrix() * t; } template diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index 3dcde2d90..8b5e9619c 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -26,34 +26,29 @@ #ifndef EIGEN_TRANSFORM_H #define EIGEN_TRANSFORM_H -// Definition of TransformTraits commented out (by andrew_coles@yahoo.co.uk) -// Is already defined (differently) in Constants.h, and gave duplicate -// definition compile-time errors - -///** Represents some traits of a transformation */ -//enum TransformTraits { -// Isometry, ///< the transformation is a concatenation of translations and rotations -// Affine, ///< the transformation is affine (linear transformation + translation) -// Projective ///< the transformation might not be affine -//}; - // Note that we have to pass Dim and HDim because it is not allowed to use a template // parameter to define a template specialization. To be more precise, in the following // specializations, it is not allowed to use Dim+1 instead of HDim. template< typename Other, + int Mode, int Dim, int HDim, int OtherRows=Other::RowsAtCompileTime, int OtherCols=Other::ColsAtCompileTime> struct ei_transform_right_product_impl; +template struct ei_transform_take_affine_part; + template< typename Other, + int Mode, int Dim, int HDim, int OtherRows=Other::RowsAtCompileTime, int OtherCols=Other::ColsAtCompileTime> struct ei_transform_left_product_impl; +template struct ei_transform_transform_product_impl; + /** \geometry_module \ingroup Geometry_Module * * \class Transform @@ -62,32 +57,116 @@ struct ei_transform_left_product_impl; * * \param _Scalar the scalar type, i.e., the type of the coefficients * \param _Dim the dimension of the space + * \param _Mode the type of the transformation. Can be: + * - Affine: the transformation is stored as a (Dim+1)^2 matrix, + * where the last row is assumed to be [0 ... 0 1]. + * This is the default. + * - AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix. + * - Projective: the transformation is stored as a (Dim+1)^2 matrix + * whithout any assumption. * - * The homography is internally represented and stored as a (Dim+1)^2 matrix which - * is available through the matrix() method. + * The homography is internally represented and stored by a matrix which + * is available through the matrix() method. To understand the behavior of + * this class you have to think a Transform object as its internal + * matrix representation. The chosen convention is right multiply: + * + * \code v' = T * v \endcode + * + * Thefore, an affine transformation matrix M is shaped like this: + * + * \f$ \left( \begin{array}{cc} + * linear & translation\\ + * 0 ... 0 & 1 + * \end{array} \right) \f$ + * + * Note that for a provective transformation the last row can be anything, + * and then the interpretation of different parts might be sighlty different. + * + * However, unlike a plain matrix, the Transform class provides many features + * simplifying both its assembly and usage. In particular, it can be composed + * with any other transformations (Transform,Trnaslation,RotationBase,Matrix) + * and can be directly used to transform implicit homogeneous vectors. All these + * operations are handled via the operator*. For the composition of transformations, + * its principle consists to first convert the right/left hand sides of the product + * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product. + * Of course, internally, operator* tries to perform the minimal number of operations + * according to the nature of each terms. Likewise, when applying the transform + * to non homogeneous vectors, the latters are automatically promoted to homogeneous + * one before doing the matrix product. The convertions to homogeneous representations + * are performed as follow: * + * \b Translation t (Dim)x(1): + * \f$ \left( \begin{array}{cc} + * I & t \\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ + * + * \b Rotation R (Dim)x(Dim): + * \f$ \left( \begin{array}{cc} + * R & 0\\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ + * + * \b Linear \b Matrix L (Dim)x(Dim): + * \f$ \left( \begin{array}{cc} + * L & 0\\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ + * + * \b Affine \b Matrix A (Dim)x(Dim+1): + * \f$ \left( \begin{array}{c} + * A\\ + * 0\,...\,0\,1 + * \end{array} \right) \f$ + * + * \b Column \b vector v (Dim)x(1): + * \f$ \left( \begin{array}{c} + * v\\ + * 1 + * \end{array} \right) \f$ + * + * \b Set \b of \b column \b vectors V1...Vn (Dim)x(n): + * \f$ \left( \begin{array}{ccc} + * v_1 & ... & v_n\\ + * 1 & ... & 1 + * \end{array} \right) \f$ + * + * The concatenation of a Tranform object with any kind of other transformation + * always returns a Transform object. + * + * A little execption to the "as pure matrix product" rule is the case of the + * transformation of non homogeneous vectors by an affine transformation. In + * that case the last matrix row can be ignored, and the product returns non + * homogeneous vectors. + * * Conversion methods from/to Qt's QMatrix and QTransform are available if the * preprocessor token EIGEN_QT_SUPPORT is defined. * * \sa class Matrix, class Quaternion */ -template +template class Transform { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1)) enum { + Mode = _Mode, Dim = _Dim, ///< space dimension in which the transformation holds - HDim = _Dim+1 ///< size of a respective homogeneous vector + HDim = _Dim+1, ///< size of a respective homogeneous vector + Rows = int(Mode)==(AffineCompact) ? Dim : HDim }; /** the scalar type of the coefficients */ typedef _Scalar Scalar; /** type of the matrix used to represent the transformation */ - typedef Matrix MatrixType; + typedef Matrix MatrixType; /** type of the matrix used to represent the linear part of the transformation */ typedef Matrix LinearMatrixType; /** type of read/write reference to the linear part of the transformation */ typedef Block LinearPart; + /** type of read/write reference to the affine part of the transformation */ + typedef typename ei_meta_if >::ret AffinePart; /** type of a vector */ typedef Matrix VectorType; /** type of a read/write reference to the translation part of the rotation */ @@ -117,38 +196,99 @@ public: inline Transform& operator=(const Transform& other) { m_matrix = other.m_matrix; return *this; } - template // MSVC 2005 will commit suicide if BigMatrix has a default value - struct construct_from_matrix - { - static inline void run(Transform *transform, const MatrixBase& other) - { - transform->matrix() = other; - } - }; - - template struct construct_from_matrix + template + struct construct_from_matrix; + + template struct construct_from_matrix { static inline void run(Transform *transform, const MatrixBase& other) { transform->linear() = other; transform->translation().setZero(); - transform->matrix()(Dim,Dim) = Scalar(1); - transform->matrix().template block<1,Dim>(Dim,0).setZero(); + transform->makeAffine(); } }; + + template struct construct_from_matrix + { + static inline void run(Transform *transform, const MatrixBase& other) + { + transform->linear() = other; + transform->translation().setZero(); + } + }; + + template struct construct_from_matrix + { + static inline void run(Transform *transform, const MatrixBase& other) + { + transform->affine() = other; + transform->makeAffine(); + } + }; + + template struct construct_from_matrix + { + static inline void run(Transform *transform, const MatrixBase& other) + { transform->affine() = other; } + }; + + template struct construct_from_matrix + { + static inline void run(Transform *transform, const MatrixBase& other) + { transform->matrix() = other; } + }; + + template struct construct_from_matrix + { + static inline void run(Transform *transform, const MatrixBase& other) + { transform->matrix() = other.template block(0,0); } + }; + + typedef ei_transform_take_affine_part take_affine_part; /** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */ template inline explicit Transform(const MatrixBase& other) { - construct_from_matrix::run(this, other); + construct_from_matrix::run(this, other); } /** Set \c *this from a Dim^2 or (Dim+1)^2 matrix. */ template inline Transform& operator=(const MatrixBase& other) { - construct_from_matrix::run(this, other); + construct_from_matrix::run(this, other); + return *this; + } + + template + inline Transform(const Transform& other) + { + ei_assert(OtherMode!=Projective && "You cannot directly assign a projective transform to an affine one."); + typedef typename Transform::MatrixType OtherMatrixType; + construct_from_matrix::run(this, other.matrix()); + } + + template + Transform(const ReturnByValue& other) + { + other.evalTo(*this); + } + + template + Transform& operator=(const ReturnByValue& other) + { + other.evalTo(*this); return *this; } @@ -173,10 +313,15 @@ public: /** \returns a writable expression of the transformation matrix */ inline MatrixType& matrix() { return m_matrix; } - /** \returns a read-only expression of the linear (linear) part of the transformation */ + /** \returns a read-only expression of the linear part of the transformation */ inline const LinearPart linear() const { return m_matrix.template block(0,0); } - /** \returns a writable expression of the linear (linear) part of the transformation */ + /** \returns a writable expression of the linear part of the transformation */ inline LinearPart linear() { return m_matrix.template block(0,0); } + + /** \returns a read-only expression of the Dim x HDim affine part of the transformation */ + inline const AffinePart affine() const { return take_affine_part::run(m_matrix); } + /** \returns a writable expression of the Dim x HDim affine part of the transformation */ + inline AffinePart affine() { return take_affine_part::run(m_matrix); } /** \returns a read-only expression of the translation vector of the transformation */ inline const TranslationPart translation() const { return m_matrix.template block(0,Dim); } @@ -188,33 +333,47 @@ public: * The right hand side \a other might be either: * \li a vector of size Dim, * \li an homogeneous vector of size Dim+1, - * \li a linear transformation matrix of size Dim x Dim + * \li a set of vectors of size Dim x Dynamic, + * \li a set of homogeneous vectors of size Dim+1 x Dynamic, + * \li a linear transformation matrix of size Dim x Dim, + * \li an affine transformation matrix of size Dim x Dim+1, * \li a transformation matrix of size Dim+1 x Dim+1. */ // note: this function is defined here because some compilers cannot find the respective declaration template - inline const typename ei_transform_right_product_impl::ResultType + inline const typename ei_transform_right_product_impl::ResultType operator * (const MatrixBase &other) const - { return ei_transform_right_product_impl::run(*this,other.derived()); } + { return ei_transform_right_product_impl::run(*this,other.derived()); } /** \returns the product expression of a transformation matrix \a a times a transform \a b * - * The right hand side \a other might be either: - * \li a linear transformation matrix of size Dim x Dim - * \li a transformation matrix of size Dim+1 x Dim+1. + * The left hand side \a other might be either: + * \li a linear transformation matrix of size Dim x Dim, + * \li an affine transformation matrix of size Dim x Dim+1, + * \li a general transformation matrix of size Dim+1 x Dim+1. */ template friend - inline const typename ei_transform_left_product_impl::ResultType + inline const typename ei_transform_left_product_impl::ResultType operator * (const MatrixBase &a, const Transform &b) - { return ei_transform_left_product_impl::run(a.derived(),b); } + { return ei_transform_left_product_impl::run(a.derived(),b); } template inline Transform& operator*=(const MatrixBase& other) { return *this = *this * other; } /** Contatenates two transformations */ - inline const Transform - operator * (const Transform& other) const - { return Transform(m_matrix * other.matrix()); } + inline const Transform operator * (const Transform& other) const + { + return ei_transform_transform_product_impl::run(*this,other); + } + + /** Contatenates two different transformations */ + template + inline const typename ei_transform_transform_product_impl< + Transform,Transform >::ResultType + operator * (const Transform& other) const + { + return ei_transform_transform_product_impl >::run(*this,other); + } /** \sa MatrixBase::setIdentity() */ void setIdentity() { m_matrix.setIdentity(); } @@ -250,14 +409,6 @@ public: inline Transform& operator=(const UniformScaling& t); inline Transform& operator*=(const UniformScaling& s) { return scale(s.factor()); } inline Transform operator*(const UniformScaling& s) const; - -// friend inline Transform operator*(const LinearMatrixType& mat, const Transform& t) -// { -// Transform res = t; -// res.matrix().row(Dim) = t.matrix().row(Dim); -// res.matrix().template block(0,0) = (mat * t.matrix().template block(0,0)).lazy(); -// return res; -// } template inline Transform& operator=(const RotationBase& r); @@ -276,7 +427,7 @@ public: Transform& fromPositionOrientationScale(const MatrixBase &position, const OrientationType& orientation, const MatrixBase &scale); - inline const MatrixType inverse(TransformTraits traits = Affine) const; + inline const MatrixType inverse(TransformTraits traits = Mode) const; /** \returns a const pointer to the column major internal matrix */ const Scalar* data() const { return m_matrix.data(); } @@ -289,12 +440,12 @@ public: * then this function smartly returns a const reference to \c *this. */ template - inline typename ei_cast_return_type >::type cast() const - { return typename ei_cast_return_type >::type(*this); } + inline typename ei_cast_return_type >::type cast() const + { return typename ei_cast_return_type >::type(*this); } /** Copy constructor with scalar type conversion */ template - inline explicit Transform(const Transform& other) + inline explicit Transform(const Transform& other) { m_matrix = other.matrix().template cast(); } /** \returns \c true if \c *this is approximately equal to \a other, within the precision @@ -304,8 +455,43 @@ public: bool isApprox(const Transform& other, typename NumTraits::Real prec = precision()) const { return m_matrix.isApprox(other.m_matrix, prec); } -protected: - + /** Sets the last row to [0 ... 0 1] + */ + void makeAffine() + { + if(int(Mode)!=int(AffineCompact)) + { + matrix().template block<1,Dim>(Dim,0).setZero(); + matrix().coeffRef(Dim,Dim) = 1; + } + } + + /** \internal + * \returns the Dim x Dim linear part if the transformation is affine, + * and the HDim x Dim part for projective transformations. + */ + inline Block linearExt() + { return m_matrix.template block(0,0); } + /** \internal + * \returns the Dim x Dim linear part if the transformation is affine, + * and the HDim x Dim part for projective transformations. + */ + inline const Block linearExt() const + { return m_matrix.template block(0,0); } + + /** \internal + * \returns the translation part if the transformation is affine, + * and the last column for projective transformations. + */ + inline Block translationExt() + { return m_matrix.template block(0,Dim); } + /** \internal + * \returns the translation part if the transformation is affine, + * and the last column for projective transformations. + */ + inline const Block translationExt() const + { return m_matrix.template block(0,Dim); } + }; /** \ingroup Geometry_Module */ @@ -317,6 +503,33 @@ typedef Transform Transform2d; /** \ingroup Geometry_Module */ typedef Transform Transform3d; +/** \ingroup Geometry_Module */ +typedef Transform Affine2f; +/** \ingroup Geometry_Module */ +typedef Transform Affine3f; +/** \ingroup Geometry_Module */ +typedef Transform Affine2d; +/** \ingroup Geometry_Module */ +typedef Transform Affine3d; + +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact2f; +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact3f; +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact2d; +/** \ingroup Geometry_Module */ +typedef Transform AffineCompact3d; + +/** \ingroup Geometry_Module */ +typedef Transform Projective2f; +/** \ingroup Geometry_Module */ +typedef Transform Projective3f; +/** \ingroup Geometry_Module */ +typedef Transform Projective2d; +/** \ingroup Geometry_Module */ +typedef Transform Projective3d; + /************************** *** Optional QT support *** **************************/ @@ -326,8 +539,8 @@ typedef Transform Transform3d; * * This function is available only if the token EIGEN_QT_SUPPORT is defined. */ -template -Transform::Transform(const QMatrix& other) +template +Transform::Transform(const QMatrix& other) { *this = other; } @@ -336,8 +549,8 @@ Transform::Transform(const QMatrix& other) * * This function is available only if the token EIGEN_QT_SUPPORT is defined. */ -template -Transform& Transform::operator=(const QMatrix& other) +template +Transform& Transform::operator=(const QMatrix& other) { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) m_matrix << other.m11(), other.m21(), other.dx(), @@ -352,8 +565,8 @@ Transform& Transform::operator=(const QMatrix& other) * * This function is available only if the token EIGEN_QT_SUPPORT is defined. */ -template -QMatrix Transform::toQMatrix(void) const +template +QMatrix Transform::toQMatrix(void) const { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) return QMatrix(other.coeffRef(0,0), other.coeffRef(1,0), @@ -365,8 +578,8 @@ QMatrix Transform::toQMatrix(void) const * * This function is available only if the token EIGEN_QT_SUPPORT is defined. */ -template -Transform::Transform(const QTransform& other) +template +Transform::Transform(const QTransform& other) { *this = other; } @@ -375,8 +588,8 @@ Transform::Transform(const QTransform& other) * * This function is available only if the token EIGEN_QT_SUPPORT is defined. */ -template -Transform& Transform::operator=(const QTransform& other) +template +Transform& Transform::operator=(const QTransform& other) { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) m_matrix << other.m11(), other.m21(), other.dx(), @@ -389,8 +602,8 @@ Transform& Transform::operator=(const QTransform& other) * * This function is available only if the token EIGEN_QT_SUPPORT is defined. */ -template -QMatrix Transform::toQTransform(void) const +template +QMatrix Transform::toQTransform(void) const { EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE) return QTransform(other.coeffRef(0,0), other.coeffRef(1,0), other.coeffRef(2,0) @@ -407,13 +620,13 @@ QMatrix Transform::toQTransform(void) const * by the vector \a other to \c *this and returns a reference to \c *this. * \sa prescale() */ -template +template template -Transform& -Transform::scale(const MatrixBase &other) +Transform& +Transform::scale(const MatrixBase &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) - linear() = (linear() * other.asDiagonal()).lazy(); + linearExt() = (linearExt() * other.asDiagonal()).lazy(); return *this; } @@ -421,10 +634,10 @@ Transform::scale(const MatrixBase &other) * and returns a reference to \c *this. * \sa prescale(Scalar) */ -template -inline Transform& Transform::scale(Scalar s) +template +inline Transform& Transform::scale(Scalar s) { - linear() *= s; + linearExt() *= s; return *this; } @@ -432,10 +645,10 @@ inline Transform& Transform::scale(Scalar s) * by the vector \a other to \c *this and returns a reference to \c *this. * \sa scale() */ -template +template template -Transform& -Transform::prescale(const MatrixBase &other) +Transform& +Transform::prescale(const MatrixBase &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) m_matrix.template block(0,0) = (other.asDiagonal() * m_matrix.template block(0,0)).lazy(); @@ -446,8 +659,8 @@ Transform::prescale(const MatrixBase &other) * and returns a reference to \c *this. * \sa scale(Scalar) */ -template -inline Transform& Transform::prescale(Scalar s) +template +inline Transform& Transform::prescale(Scalar s) { m_matrix.template corner(TopLeft) *= s; return *this; @@ -457,13 +670,13 @@ inline Transform& Transform::prescale(Scalar s) * to \c *this and returns a reference to \c *this. * \sa pretranslate() */ -template +template template -Transform& -Transform::translate(const MatrixBase &other) +Transform& +Transform::translate(const MatrixBase &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) - translation() += linear() * other; + translationExt() += linearExt() * other; return *this; } @@ -471,13 +684,16 @@ Transform::translate(const MatrixBase &other) * to \c *this and returns a reference to \c *this. * \sa translate() */ -template +template template -Transform& -Transform::pretranslate(const MatrixBase &other) +Transform& +Transform::pretranslate(const MatrixBase &other) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) - translation() += other; + if(int(Mode)==int(Projective)) + affine() += other * m_matrix.row(Dim); + else + translation() += other; return *this; } @@ -498,12 +714,12 @@ Transform::pretranslate(const MatrixBase &other) * * \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType) */ -template +template template -Transform& -Transform::rotate(const RotationType& rotation) +Transform& +Transform::rotate(const RotationType& rotation) { - linear() *= ei_toRotationMatrix(rotation); + linearExt() *= ei_toRotationMatrix(rotation); return *this; } @@ -514,10 +730,10 @@ Transform::rotate(const RotationType& rotation) * * \sa rotate() */ -template +template template -Transform& -Transform::prerotate(const RotationType& rotation) +Transform& +Transform::prerotate(const RotationType& rotation) { m_matrix.template block(0,0) = ei_toRotationMatrix(rotation) * m_matrix.template block(0,0); @@ -529,9 +745,9 @@ Transform::prerotate(const RotationType& rotation) * \warning 2D only. * \sa preshear() */ -template -Transform& -Transform::shear(Scalar sx, Scalar sy) +template +Transform& +Transform::shear(Scalar sx, Scalar sy) { EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) VectorType tmp = linear().col(0)*sy + linear().col(1); @@ -544,9 +760,9 @@ Transform::shear(Scalar sx, Scalar sy) * \warning 2D only. * \sa shear() */ -template -Transform& -Transform::preshear(Scalar sx, Scalar sy) +template +Transform& +Transform::preshear(Scalar sx, Scalar sy) { EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE) m_matrix.template block(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block(0,0); @@ -557,55 +773,53 @@ Transform::preshear(Scalar sx, Scalar sy) *** Scaling, Translation and Rotation compatibility *** ******************************************************/ -template -inline Transform& Transform::operator=(const TranslationType& t) +template +inline Transform& Transform::operator=(const TranslationType& t) { linear().setIdentity(); translation() = t.vector(); - m_matrix.template block<1,Dim>(Dim,0).setZero(); - m_matrix(Dim,Dim) = Scalar(1); + makeAffine(); return *this; } -template -inline Transform Transform::operator*(const TranslationType& t) const +template +inline Transform Transform::operator*(const TranslationType& t) const { Transform res = *this; res.translate(t.vector()); return res; } -template -inline Transform& Transform::operator=(const UniformScaling& s) +template +inline Transform& Transform::operator=(const UniformScaling& s) { m_matrix.setZero(); linear().diagonal().fill(s.factor()); - m_matrix.coeffRef(Dim,Dim) = Scalar(1); + makeAffine(); return *this; } -template -inline Transform Transform::operator*(const UniformScaling& s) const +template +inline Transform Transform::operator*(const UniformScaling& s) const { Transform res = *this; res.scale(s.factor()); return res; } -template +template template -inline Transform& Transform::operator=(const RotationBase& r) +inline Transform& Transform::operator=(const RotationBase& r) { linear() = ei_toRotationMatrix(r); translation().setZero(); - m_matrix.template block<1,Dim>(Dim,0).setZero(); - m_matrix.coeffRef(Dim,Dim) = Scalar(1); + makeAffine(); return *this; } -template +template template -inline Transform Transform::operator*(const RotationBase& r) const +inline Transform Transform::operator*(const RotationBase& r) const { Transform res = *this; res.rotate(r.derived()); @@ -623,9 +837,9 @@ inline Transform Transform::operator*(const RotationBase * * \sa computeRotationScaling(), computeScalingRotation(), class SVD */ -template -typename Transform::LinearMatrixType -Transform::rotation() const +template +typename Transform::LinearMatrixType +Transform::rotation() const { LinearMatrixType result; computeRotationScaling(&result, (LinearMatrixType*)0); @@ -644,9 +858,9 @@ Transform::rotation() const * * \sa computeScalingRotation(), rotation(), class SVD */ -template +template template -void Transform::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const +void Transform::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const { linear().svd().computeRotationScaling(rotation, scaling); } @@ -662,9 +876,9 @@ void Transform::computeRotationScaling(RotationMatrixType *rotation, * * \sa computeRotationScaling(), rotation(), class SVD */ -template +template template -void Transform::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const +void Transform::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const { linear().svd().computeScalingRotation(scaling, rotation); } @@ -672,17 +886,16 @@ void Transform::computeScalingRotation(ScalingMatrixType *scaling, R /** Convenient method to set \c *this from a position, orientation and scale * of a 3D object. */ -template +template template -Transform& -Transform::fromPositionOrientationScale(const MatrixBase &position, +Transform& +Transform::fromPositionOrientationScale(const MatrixBase &position, const OrientationType& orientation, const MatrixBase &scale) { linear() = ei_toRotationMatrix(orientation); linear() *= scale.asDiagonal(); translation() = position; - m_matrix.template block<1,Dim>(Dim,0).setZero(); - m_matrix(Dim,Dim) = Scalar(1); + makeAffine(); return *this; } @@ -705,28 +918,28 @@ Transform::fromPositionOrientationScale(const MatrixBase -inline const typename Transform::MatrixType -Transform::inverse(TransformTraits traits) const +template +const typename Transform::MatrixType +Transform::inverse(TransformTraits hint) const { - if (traits == Projective) + if (hint == Projective) { return m_matrix.inverse(); } else { MatrixType res; - if (traits == Affine) - { - res.template corner(TopLeft) = linear().inverse(); - } - else if (traits == Isometry) + if (hint == Isometry) { res.template corner(TopLeft) = linear().transpose(); } + else if(hint&Affine) + { + res.template corner(TopLeft) = linear().inverse(); + } else { - ei_assert("invalid traits value in Transform::inverse()"); + ei_assert(false && "Invalid transform traits in Transform::Inverse"); } // translation and remaining parts res.template corner(TopRight) = - res.template corner(TopLeft) * translation(); @@ -736,94 +949,260 @@ Transform::inverse(TransformTraits traits) const } } +template struct ei_transform_take_affine_part { + typedef typename TransformType::MatrixType MatrixType; + typedef typename TransformType::AffinePart AffinePart; + static inline AffinePart run(MatrixType& m) + { return m.template block(0,0); } + static inline const AffinePart run(const MatrixType& m) + { return m.template block(0,0); } +}; + +template +struct ei_transform_take_affine_part > { + typedef typename Transform::MatrixType MatrixType; + static inline MatrixType& run(MatrixType& m) { return m; } + static inline const MatrixType& run(const MatrixType& m) { return m; } +}; + /***************************************************** *** Specializations of operator* with a MatrixBase *** *****************************************************/ -// T * affine matrix +// Projective * set of homogeneous column vectors template -struct ei_transform_right_product_impl +struct ei_transform_right_product_impl { - typedef Transform TransformType; + typedef Transform TransformType; typedef typename TransformType::MatrixType MatrixType; typedef typename ProductReturnType::Type ResultType; static ResultType run(const TransformType& tr, const Other& other) { return tr.matrix() * other; } }; -// T * linear matrix +// Projective * homogeneous column vector template -struct ei_transform_right_product_impl +struct ei_transform_right_product_impl { - typedef Transform TransformType; + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef typename ProductReturnType::Type ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { return tr.matrix() * other; } +}; + +// Projective * column vector +template +struct ei_transform_right_product_impl +{ + typedef Transform TransformType; + typedef Matrix ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { return tr.matrix().template block(0,0) * other + tr.matrix().col(Dim); } +}; + +// Affine * column vector +template +struct ei_transform_right_product_impl +{ + typedef Transform TransformType; + typedef Matrix ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { return tr.linear() * other + tr.translation(); } +}; + +// T * linear matrix => T +template +struct ei_transform_right_product_impl +{ + typedef Transform TransformType; typedef typename TransformType::MatrixType MatrixType; typedef TransformType ResultType; static ResultType run(const TransformType& tr, const Other& other) { TransformType res; - res.translation() = tr.translation(); - res.matrix().row(Dim) = tr.matrix().row(Dim); - res.linear() = (tr.linear() * other).lazy(); + res.matrix().col(Dim) = tr.matrix().col(Dim); + res.linearExt() = (tr.linearExt() * other).lazy(); + if(Mode==Affine) + res.matrix().row(Dim).template start() = tr.matrix().row(Dim).template start(); return res; } }; -// T * homogeneous vector -template -struct ei_transform_right_product_impl +// T * affine matrix => T +template +struct ei_transform_right_product_impl { - typedef Transform TransformType; + typedef Transform TransformType; typedef typename TransformType::MatrixType MatrixType; - typedef typename ProductReturnType::Type ResultType; + typedef TransformType ResultType; static ResultType run(const TransformType& tr, const Other& other) - { return tr.matrix() * other; } + { + TransformType res; + const int Rows = Mode==Projective ? HDim : Dim; + res.matrix().template block(0,0) = (tr.linearExt() * other).lazy(); + res.translationExt() += tr.translationExt(); + if(Mode!=Affine) + res.makeAffine(); + return res; + } }; -// T * vector -template -struct ei_transform_right_product_impl +// T * generic matrix => Projective +template +struct ei_transform_right_product_impl { - typedef typename Other::Scalar Scalar; - typedef Transform TransformType; - typedef typename TransformType::LinearPart MatrixType; - typedef const CwiseUnaryOp< - ei_scalar_multiple_op, - NestByValue, - NestByValue,Other>::Type >, - NestByValue > > - > ResultType; - // FIXME should we offer an optimized version when the last row is known to be 0,0...,0,1 ? + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef Transform ResultType; static ResultType run(const TransformType& tr, const Other& other) - { return ((tr.linear().nestByValue() * other).nestByValue() + tr.translation().nestByValue()).nestByValue() - * (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); } + { return ResultType((tr.matrix() * other).lazy()); } +}; + +// AffineCompact * generic matrix => Projective +template +struct ei_transform_right_product_impl +{ + typedef Transform TransformType; + typedef Transform ResultType; + static ResultType run(const TransformType& tr, const Other& other) + { + ResultType res; + res.affine() = (tr.matrix() * other).lazy(); + res.makeAffine(); + return res; + } +}; + + +// generic HDim x HDim matrix * T => Projective +template +struct ei_transform_left_product_impl +{ + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef Transform ResultType; + static ResultType run(const Other& other,const TransformType& tr) + { return ResultType((other * tr.matrix()).lazy()); } +}; + +// generic HDim x HDim matrix * AffineCompact => Projective +template +struct ei_transform_left_product_impl +{ + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef Transform ResultType; + static ResultType run(const Other& other,const TransformType& tr) + { + ResultType res; + res.matrix() = (other.template block(0,0) * tr.matrix()).lazy(); + res.matrix().col(Dim) += other.col(Dim); + return res; + } }; // affine matrix * T -template -struct ei_transform_left_product_impl +template +struct ei_transform_left_product_impl { - typedef Transform TransformType; + typedef Transform TransformType; typedef typename TransformType::MatrixType MatrixType; - typedef typename ProductReturnType::Type ResultType; + typedef TransformType ResultType; static ResultType run(const Other& other,const TransformType& tr) - { return other * tr.matrix(); } + { + ResultType res; + res.affine() = (other * tr.matrix()).lazy(); + res.matrix().row(Dim) = tr.matrix().row(Dim); + return res; + } +}; + +// affine matrix * AffineCompact +template +struct ei_transform_left_product_impl +{ + typedef Transform TransformType; + typedef typename TransformType::MatrixType MatrixType; + typedef TransformType ResultType; + static ResultType run(const Other& other,const TransformType& tr) + { + ResultType res; + res.matrix() = (other.template block(0,0) * tr.matrix()).lazy(); + res.translation() += other.col(Dim); + return res; + } }; // linear matrix * T -template -struct ei_transform_left_product_impl +template +struct ei_transform_left_product_impl { - typedef Transform TransformType; + typedef Transform TransformType; typedef typename TransformType::MatrixType MatrixType; typedef TransformType ResultType; static ResultType run(const Other& other, const TransformType& tr) { TransformType res; - res.matrix().row(Dim) = tr.matrix().row(Dim); - res.matrix().template corner(TopLeft) = (other * tr.matrix().template corner(TopLeft)).lazy(); + if(Mode!=AffineCompact) + res.matrix().row(Dim) = tr.matrix().row(Dim); + res.matrix().template corner(TopLeft) + = (other * tr.matrix().template corner(TopLeft)).lazy(); return res; } }; +/********************************************************** +*** Specializations of operator* with another Transform *** +**********************************************************/ + +template +struct ei_transform_transform_product_impl,Transform > +{ + typedef Transform TransformType; + typedef TransformType ResultType; + static ResultType run(const TransformType& lhs, const TransformType& rhs) + { + return ResultType((lhs.matrix() * rhs.matrix()).lazy()); + } +}; + +template +struct ei_transform_transform_product_impl,Transform > +{ + typedef Transform TransformType; + typedef TransformType ResultType; + static ResultType run(const TransformType& lhs, const TransformType& rhs) + { + return ei_transform_right_product_impl::run(lhs,rhs.matrix()); + } +}; + +template +struct ei_transform_transform_product_impl,Transform > +{ + typedef Transform Lhs; + typedef Transform Rhs; + typedef typename ei_transform_right_product_impl::ResultType ResultType; + static ResultType run(const Lhs& lhs, const Rhs& rhs) + { + return ei_transform_right_product_impl::run(lhs,rhs.matrix()); + } +}; + +template +struct ei_transform_transform_product_impl, + Transform > +{ + typedef Transform Lhs; + typedef Transform Rhs; + typedef Transform ResultType; + static ResultType run(const Lhs& lhs, const Rhs& rhs) + { + return ResultType((lhs.matrix() * rhs.matrix()).lazy()); + } +}; + #endif // EIGEN_TRANSFORM_H diff --git a/Eigen/src/Geometry/Translation.h b/Eigen/src/Geometry/Translation.h index 4449bb29e..257ef81e1 100644 --- a/Eigen/src/Geometry/Translation.h +++ b/Eigen/src/Geometry/Translation.h @@ -53,7 +53,7 @@ public: /** corresponding linear transformation matrix type */ typedef Matrix LinearMatrixType; /** corresponding affine transformation type */ - typedef Transform TransformType; + typedef Transform AffineTransformType; protected: @@ -89,23 +89,23 @@ public: { return Translation(m_coeffs + other.m_coeffs); } /** Concatenates a translation and a uniform scaling */ - inline TransformType operator* (const UniformScaling& other) const; + inline AffineTransformType operator* (const UniformScaling& other) const; /** Concatenates a translation and a linear transformation */ template - inline TransformType operator* (const MatrixBase& linear) const; + inline AffineTransformType operator* (const MatrixBase& linear) const; /** Concatenates a translation and a rotation */ template - inline TransformType operator*(const RotationBase& r) const + inline AffineTransformType operator*(const RotationBase& r) const { return *this * r.toRotationMatrix(); } /** \returns the concatenation of a linear transformation \a l with the translation \a t */ // its a nightmare to define a templated friend function outside its declaration template friend - inline TransformType operator*(const MatrixBase& linear, const Translation& t) + inline AffineTransformType operator*(const MatrixBase& linear, const Translation& t) { - TransformType res; + AffineTransformType res; res.matrix().setZero(); res.linear() = linear.derived(); res.translation() = linear.derived() * t.m_coeffs; @@ -115,7 +115,13 @@ public: } /** Concatenates a translation and an affine transformation */ - inline TransformType operator* (const TransformType& t) const; + template + inline Transform operator* (const Transform& t) const + { + Transform res = t; + res.pretranslate(m_coeffs); + return res; + } /** Applies translation to vector */ inline VectorType operator* (const VectorType& other) const @@ -162,10 +168,10 @@ typedef Translation Translation3d; //@} template -inline typename Translation::TransformType +inline typename Translation::AffineTransformType Translation::operator* (const UniformScaling& other) const { - TransformType res; + AffineTransformType res; res.matrix().setZero(); res.linear().diagonal().fill(other.factor()); res.translation() = m_coeffs; @@ -175,10 +181,10 @@ Translation::operator* (const UniformScaling& other) const template template -inline typename Translation::TransformType +inline typename Translation::AffineTransformType Translation::operator* (const MatrixBase& linear) const { - TransformType res; + AffineTransformType res; res.matrix().setZero(); res.linear() = linear.derived(); res.translation() = m_coeffs; @@ -187,13 +193,4 @@ Translation::operator* (const MatrixBase& linear) cons return res; } -template -inline typename Translation::TransformType -Translation::operator* (const TransformType& t) const -{ - TransformType res = t; - res.pretranslate(m_coeffs); - return res; -} - #endif // EIGEN_TRANSLATION_H diff --git a/test/geo_transformations.cpp b/test/geo_transformations.cpp index 35ecdb47b..45cf3d302 100644 --- a/test/geo_transformations.cpp +++ b/test/geo_transformations.cpp @@ -27,12 +27,11 @@ #include #include -template void transformations(void) +template void transformations(void) { /* this test covers the following files: Cross.h Quaternion.h, Transform.cpp */ - typedef Matrix Matrix2; typedef Matrix Matrix3; typedef Matrix Matrix4; @@ -41,8 +40,9 @@ template void transformations(void) typedef Matrix Vector4; typedef Quaternion Quaternionx; typedef AngleAxis AngleAxisx; - typedef Transform Transform2; - typedef Transform Transform3; + typedef Transform Transform2; + typedef Transform Transform3; + typedef typename Transform3::MatrixType MatrixType; typedef DiagonalMatrix AlignedScaling2; typedef DiagonalMatrix AlignedScaling3; typedef Translation Translation2; @@ -111,7 +111,7 @@ template void transformations(void) t0.scale(v0); t1.prescale(v0); - VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).norm(), v0.x()); + VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template start<3>().norm(), v0.x()); //VERIFY(!ei_isApprox((t1 * Vector3(1,0,0)).norm(), v0.x())); t0.setIdentity(); @@ -124,7 +124,7 @@ template void transformations(void) t1.prescale(v1.cwise().inverse()); t1.translate(-v0); - VERIFY((t0.matrix() * t1.matrix()).isIdentity(test_precision())); + VERIFY((t0 * t1).matrix().isIdentity(test_precision())); t1.fromPositionOrientationScale(v0, q1, v1); VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); @@ -146,7 +146,9 @@ template void transformations(void) Matrix4 mat4; mat4 << mat3 , Vector3::Zero() , Vector4::Zero().transpose(); Transform3 tmat3(mat3), tmat4(mat4); - tmat4.matrix()(3,3) = Scalar(1); + if(Mode!=int(AffineCompact)) + tmat4.matrix()(3,3) = Scalar(1); + std::cerr << tmat3.matrix() << "\n\n" << tmat4.matrix() << "\n\n"; VERIFY_IS_APPROX(tmat3.matrix(), tmat4.matrix()); Scalar a3 = ei_random(-Scalar(M_PI), Scalar(M_PI)); @@ -157,7 +159,7 @@ template void transformations(void) t4 = aa3; VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); t4.rotate(AngleAxisx(-a3,v3)); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); + VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); t4 *= aa3; VERIFY_IS_APPROX(t3.matrix(), t4.matrix()); @@ -167,7 +169,7 @@ template void transformations(void) t4 = tv3; VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); t4.translate(-v3); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); + VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); t4 *= tv3; VERIFY_IS_APPROX(t5.matrix(), t4.matrix()); @@ -176,12 +178,12 @@ template void transformations(void) t4 = sv3; VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); t4.scale(v3.cwise().inverse()); - VERIFY_IS_APPROX(t4.matrix(), Matrix4::Identity()); + VERIFY_IS_APPROX(t4.matrix(), MatrixType::Identity()); t4 *= sv3; VERIFY_IS_APPROX(t6.matrix(), t4.matrix()); // matrix * transform - VERIFY_IS_APPROX(Transform3(t3.matrix()*t4).matrix(), Transform3(t3*t4).matrix()); + VERIFY_IS_APPROX((t3.matrix()*t4).matrix(), (t3*t4).matrix()); // chained Transform product VERIFY_IS_APPROX(((t3*t4)*t5).matrix(), (t3*(t4*t5)).matrix()); @@ -282,21 +284,24 @@ template void transformations(void) // translation * vector t0.setIdentity(); t0.translate(v0); - VERIFY_IS_APPROX(t0 * v1, Translation3(v0) * v1); + VERIFY_IS_APPROX((t0 * v1).template start<3>(), Translation3(v0) * v1); // AlignedScaling * vector t0.setIdentity(); t0.scale(v0); - VERIFY_IS_APPROX(t0 * v1, AlignedScaling3(v0) * v1); + VERIFY_IS_APPROX((t0 * v1).template start<3>(), AlignedScaling3(v0) * v1); // test transform inversion - t0.setIdentity(); - t0.translate(v0); - t0.linear().setRandom(); - VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse()); - t0.setIdentity(); - t0.translate(v0).rotate(q1); - VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse()); + if(Mode!=AffineCompact) + { + t0.setIdentity(); + t0.translate(v0); + t0.linear().setRandom(); + VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse()); + t0.setIdentity(); + t0.translate(v0).rotate(q1); + VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse()); + } // test extract rotation and aligned scaling // t0.setIdentity(); @@ -316,9 +321,9 @@ template void transformations(void) VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1)); // test casting - Transform t1f = t1.template cast(); + Transform t1f = t1.template cast(); VERIFY_IS_APPROX(t1f.template cast(),t1); - Transform t1d = t1.template cast(); + Transform t1d = t1.template cast(); VERIFY_IS_APPROX(t1d.template cast(),t1); Translation3 tr1(v0); @@ -343,12 +348,15 @@ template void transformations(void) VERIFY_IS_APPROX(r2d1f.template cast(),r2d1); Rotation2D r2d1d = r2d1.template cast(); VERIFY_IS_APPROX(r2d1d.template cast(),r2d1); + } void test_geo_transformations() { for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST( transformations() ); - CALL_SUBTEST( transformations() ); +// CALL_SUBTEST( transformations() ); + CALL_SUBTEST(( transformations() )); + CALL_SUBTEST(( transformations() )); + CALL_SUBTEST(( transformations() )); } }