diff --git a/Eigen/src/Core/VectorwiseOp.h b/Eigen/src/Core/VectorwiseOp.h index 1867961f5..c9cb8e820 100644 --- a/Eigen/src/Core/VectorwiseOp.h +++ b/Eigen/src/Core/VectorwiseOp.h @@ -440,7 +440,7 @@ template class VectorwiseOp } /** Returns the expression of the sum of the vector \a other to each subvector of \c *this */ - template + template EIGEN_STRONG_INLINE CwiseBinaryOp, ExpressionType, typename ExtendedType::Type> diff --git a/Eigen/src/Core/products/CoeffBasedProduct.h b/Eigen/src/Core/products/CoeffBasedProduct.h index c79a34de0..d2e693861 100644 --- a/Eigen/src/Core/products/CoeffBasedProduct.h +++ b/Eigen/src/Core/products/CoeffBasedProduct.h @@ -199,7 +199,7 @@ class CoeffBasedProduct } // Implicit conversion to the nested type (trigger the evaluation of the product) - operator const PlainObject& () const + EIGEN_STRONG_INLINE operator const PlainObject& () const { m_result.lazyAssign(*this); return m_result; diff --git a/Eigen/src/Core/util/Macros.h b/Eigen/src/Core/util/Macros.h index 2da9ab291..319c6c5fc 100644 --- a/Eigen/src/Core/util/Macros.h +++ b/Eigen/src/Core/util/Macros.h @@ -362,7 +362,7 @@ #define EIGEN_MAKE_CWISE_BINARY_OP(METHOD,FUNCTOR) \ template \ - inline const CwiseBinaryOp, Derived, OtherDerived> \ + EIGEN_STRONG_INLINE const CwiseBinaryOp, Derived, OtherDerived> \ METHOD(const EIGEN_CURRENT_STORAGE_BASE_CLASS &other) const \ { \ return CwiseBinaryOp, Derived, OtherDerived>(derived(), other.derived()); \ diff --git a/Eigen/src/Geometry/RotationBase.h b/Eigen/src/Geometry/RotationBase.h index 36f17584f..181e65be9 100644 --- a/Eigen/src/Geometry/RotationBase.h +++ b/Eigen/src/Geometry/RotationBase.h @@ -56,8 +56,8 @@ class RotationBase inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } /** \returns an equivalent rotation matrix - * This function is added to be conform with the Transform class' naming scheme. - */ + * This function is added to be conform with the Transform class' naming scheme. + */ inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); } /** \returns the inverse rotation */ @@ -87,6 +87,14 @@ class RotationBase inline RotationMatrixType operator*(const EigenBase& l, const Derived& r) { return l.derived() * r.toRotationMatrix(); } + /** \returns the concatenation of a scaling \a l with the rotation \a r */ + friend inline Transform operator*(const DiagonalMatrix& l, const Derived& r) + { + Transform res(r); + res.linear().applyOnTheLeft(l); + return res; + } + /** \returns the concatenation of the rotation \c *this with a transformation \a t */ template inline Transform operator*(const Transform& t) const @@ -107,6 +115,18 @@ struct ei_rotation_base_generic_product_selector +struct ei_rotation_base_generic_product_selector< RotationDerived, DiagonalMatrix, false > +{ + typedef Transform ReturnType; + inline static ReturnType run(const RotationDerived& r, const DiagonalMatrix& m) + { + ReturnType res(r); + res.linear() *= m; + return res; + } +}; + template struct ei_rotation_base_generic_product_selector { diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index 702e87663..bf55fd171 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -42,7 +42,8 @@ template< typename Other, int Dim, int HDim, int OtherRows=Other::RowsAtCompileTime, - int OtherCols=Other::ColsAtCompileTime> + int OtherCols=Other::ColsAtCompileTime, + bool IsProjective = (Mode==(int)Projective)> struct ei_transform_right_product_impl; template struct ei_transform_take_affine_part; @@ -55,9 +56,9 @@ template< typename Other, int OtherCols=Other::ColsAtCompileTime> struct ei_transform_left_product_impl; -template::value > +template< typename Lhs, + typename Rhs, + bool AnyProjective = ei_is_any_projective::value > struct ei_transform_transform_product_impl; template< typename Other, @@ -353,8 +354,8 @@ public: */ // note: this function is defined here because some compilers cannot find the respective declaration template - inline const typename ei_transform_right_product_impl::ResultType - operator * (const EigenBase &other) const + EIGEN_STRONG_INLINE const typename ei_transform_right_product_impl::ResultType + operator * (const EigenBase &other) const { 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 @@ -366,9 +367,40 @@ public: */ template friend inline const typename ei_transform_left_product_impl::ResultType - operator * (const EigenBase &a, const Transform &b) + operator * (const EigenBase &a, const Transform &b) { return ei_transform_left_product_impl::run(a.derived(),b); } + /** \returns The product expression of a transform \a a times a diagonal matrix \a b + * + * The rhs diagonal matrix is interpreted as an affine scaling transformation. The + * product results in a Transform of the same type (mode) as the lhs only if the lhs + * mode is no isometry. In that case, the returned transform is an affinity. + */ + friend inline const Transform + operator * (const Transform &a, const DiagonalMatrix &b) + { + Transform res(a); + res.linear() *= b; + return res; + } + + /** \returns The product expression of a diagonal matrix \a a times a transform \a b + * + * The lhs diagonal matrix is interpreted as an affine scaling transformation. The + * product results in a Transform of the same type (mode) as the lhs only if the lhs + * mode is no isometry. In that case, the returned transform is an affinity. + */ + friend inline const Transform + operator * (const DiagonalMatrix &a, const Transform &b) + { + Transform res; + res.linear().noalias() = a*b.linear(); + res.translation().noalias() = a*b.translation(); + if (Mode!=int(AffineCompact)) + res.matrix().row(Dim) = b.matrix().row(Dim); + return res; + } + template inline Transform& operator*=(const EigenBase& other) { return *this = *this * other; } @@ -381,8 +413,8 @@ public: /** Concatenates two different transformations */ template inline const typename ei_transform_transform_product_impl< - Transform,Transform >::ResultType - operator * (const Transform& other) const + Transform,Transform >::ResultType + operator * (const Transform& other) const { return ei_transform_transform_product_impl >::run(*this,other); } @@ -431,6 +463,8 @@ public: inline Transform& operator*=(const UniformScaling& s) { return scale(s.factor()); } inline Transform operator*(const UniformScaling& s) const; + inline Transform& operator*=(const DiagonalMatrix& s) { linear() *= s; return *this; } + template inline Transform& operator=(const RotationBase& r); template @@ -582,7 +616,7 @@ Transform& Transform::operator=(const QMatrix& m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(), 0, 0, 1; - return *this; + return *this; } /** \returns a QMatrix from \c *this assuming the dimension is 2. @@ -621,7 +655,7 @@ Transform& Transform::operator=(const QTransfo m_matrix << other.m11(), other.m21(), other.dx(), other.m12(), other.m22(), other.dy(), other.m13(), other.m23(), other.m33(); - return *this; + return *this; } /** \returns a QTransform from \c *this assuming the dimension is 2. @@ -1058,15 +1092,15 @@ struct ei_transform_construct_from_matrixmatrix() = other.template block(0,0); } }; -/********************************************************* -*** Specializations of operator* with a EigenBase *** -*********************************************************/ +/********************************************************** +*** Specializations of operator* with rhs EigenBase *** +**********************************************************/ // ei_general_product_return_type is a generalization of ProductReturnType, for all types (including e.g. DiagonalBase...), // instead of being restricted to MatrixBase. template struct ei_general_product_return_type; template struct ei_general_product_return_type, MatrixBase > - : ProductReturnType {}; + : ProductReturnType {}; template struct ei_general_product_return_type > { typedef D2 Type; }; template struct ei_general_product_return_type, Rhs > @@ -1085,145 +1119,48 @@ struct ei_transform_product_result }; }; -// Projective * set of homogeneous column vectors -template -struct ei_transform_right_product_impl +template< typename Other, int Mode, int Dim, int HDim, int OtherCols > +struct ei_transform_right_product_impl { - 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; } -}; + typedef typename Other::Scalar Scalar; + typedef typename Other::PlainObject ResultType; -// Projective * homogeneous column vector -template -struct ei_transform_right_product_impl -{ - 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(); } -}; - -// Affine * set of column vectors -// FIXME use a ReturnByValue to remove the temporary -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).colwise() + tr.translation(); } -}; - -// Affine * homogeneous column vector -// FIXME added for backward compatibility, but I'm not sure we should keep it -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() * other; } -}; -template -struct ei_transform_right_product_impl -{ - typedef Transform TransformType; - typedef Matrix ResultType; - static ResultType run(const TransformType& tr, const Other& other) + EIGEN_STRONG_INLINE static ResultType run(const Transform& T, const Other& other) { - ResultType res; - res.template head() = tr.matrix() * other; - res.coeffRef(Dim) = other.coeff(Dim); + return T.matrix() * other; } }; -// T * linear matrix => T -template -struct ei_transform_right_product_impl +template< typename Other, int Mode, int Dim, int HDim, int OtherRows, int OtherCols > +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) + typedef typename Other::Scalar Scalar; + typedef typename Other::PlainObject ResultType; + + EIGEN_STRONG_INLINE static ResultType run(const Transform& T, const Other& other) { - TransformType res; - res.matrix().col(Dim) = tr.matrix().col(Dim); - res.linearExt().noalias() = (tr.linearExt() * other); - if(Mode==int(Affine)) - res.matrix().row(Dim).template head() = tr.matrix().row(Dim).template head(); - return res; - } -}; - -// T * affine 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; - enum { Rows = Mode==int(Projective) ? HDim : Dim }; - res.matrix().template block(0,0).noalias() = (tr.linearExt() * other); - res.translationExt() += tr.translationExt(); - if(Mode!=int(Affine)) - res.makeAffine(); - return res; - } -}; - -// T * generic matrix => Projective -template -struct ei_transform_right_product_impl -{ - typedef Transform TransformType; - typedef typename TransformType::MatrixType MatrixType; - typedef Transform ResultType; - static ResultType run(const TransformType& tr, const Other& other) - { return ResultType(tr.matrix() * other); } -}; - -// 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().noalias() = tr.matrix() * other; - res.makeAffine(); + EIGEN_STATIC_ASSERT(OtherRows==Dim || OtherRows==HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); + + typedef Block TopLeftLhs; + typedef Block TopLeftRhs; + + ResultType res(other.rows(),other.cols()); + + TopLeftLhs(res, 0, 0, Dim, other.cols()) = + ( T.linear() * TopLeftRhs(other, 0, 0, Dim, other.cols()) ).colwise() + + T.translation(); + + // we need to take .rows() because OtherRows might be Dim or HDim + if (OtherRows==HDim) + res.row(other.rows()) = other.row(other.rows()); + return res; } }; +/********************************************************** +*** Specializations of operator* with lhs EigenBase *** +**********************************************************/ // generic HDim x HDim matrix * T => Projective template diff --git a/test/geo_transformations.cpp b/test/geo_transformations.cpp index d932677b5..b9ea6bb91 100644 --- a/test/geo_transformations.cpp +++ b/test/geo_transformations.cpp @@ -27,6 +27,81 @@ #include #include +template void non_projective_only(void) +{ + /* this test covers the following files: + Cross.h Quaternion.h, Transform.cpp + */ + typedef Matrix Matrix2; + typedef Matrix Matrix3; + typedef Matrix Matrix4; + typedef Matrix Vector2; + typedef Matrix Vector3; + typedef Matrix Vector4; + typedef Quaternion Quaternionx; + typedef AngleAxis AngleAxisx; + typedef Transform Transform2; + typedef Transform Transform3; + typedef Transform Isometry2; + typedef Transform Isometry3; + typedef typename Transform3::MatrixType MatrixType; + typedef DiagonalMatrix AlignedScaling2; + typedef DiagonalMatrix AlignedScaling3; + typedef Translation Translation2; + typedef Translation Translation3; + + Scalar largeEps = test_precision(); + if (ei_is_same_type::ret) + largeEps = 1e-2f; + + Vector3 v0 = Vector3::Random(), + v1 = Vector3::Random(); + + Transform3 t0, t1, t2; + + Scalar a = ei_random(-Scalar(M_PI), Scalar(M_PI)); + + Quaternionx q1, q2; + + q1 = AngleAxisx(a, v0.normalized()); + + t0 = Transform3::Identity(); + VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); + + t0.linear() = q1.toRotationMatrix(); + + v0 << 50, 2, 1; + t0.scale(v0); + + VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x()); + + t0.setIdentity(); + t1.setIdentity(); + v1 << 1, 2, 3; + t0.linear() = q1.toRotationMatrix(); + t0.pretranslate(v0); + t0.scale(v1); + t1.linear() = q1.conjugate().toRotationMatrix(); + t1.prescale(v1.cwiseInverse()); + t1.translate(-v0); + + VERIFY((t0 * t1).matrix().isIdentity(test_precision())); + + t1.fromPositionOrientationScale(v0, q1, v1); + VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); + VERIFY_IS_APPROX(t1*v1, t0*v1); + + // translation * vector + t0.setIdentity(); + t0.translate(v0); + VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1); + + // AlignedScaling * vector + t0.setIdentity(); + t0.scale(v0); + VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1); +} + template void transformations(void) { /* this test covers the following files: @@ -42,6 +117,8 @@ template void transformations(void) typedef AngleAxis AngleAxisx; typedef Transform Transform2; typedef Transform Transform3; + typedef Transform Isometry2; + typedef Transform Isometry3; typedef typename Transform3::MatrixType MatrixType; typedef DiagonalMatrix AlignedScaling2; typedef DiagonalMatrix AlignedScaling3; @@ -115,17 +192,6 @@ template void transformations(void) t0 = Transform3::Identity(); VERIFY_IS_APPROX(t0.matrix(), Transform3::MatrixType::Identity()); - t0.linear() = q1.toRotationMatrix(); - t1.setIdentity(); - t1.linear() = q1.toRotationMatrix(); - - v0 << 50, 2, 1;//= ei_random_matrix().cwiseProduct(Vector3(10,2,0.5)); - t0.scale(v0); - t1.prescale(v0); - - VERIFY_IS_APPROX( (t0 * Vector3(1,0,0)).template head<3>().norm(), v0.x()); - //VERIFY(!ei_isApprox((t1 * Vector3(1,0,0)).norm(), v0.x())); - t0.setIdentity(); t1.setIdentity(); v1 << 1, 2, 3; @@ -140,7 +206,6 @@ template void transformations(void) t1.fromPositionOrientationScale(v0, q1, v1); VERIFY_IS_APPROX(t1.matrix(), t0.matrix()); - VERIFY_IS_APPROX(t1*v1, t0*v1); t0.setIdentity(); t0.scale(v0).rotate(q1.toRotationMatrix()); t1.setIdentity(); t1.scale(v0).rotate(q1); @@ -248,20 +313,20 @@ template void transformations(void) t0.setIdentity(); t0.prerotate(q1).prescale(v0).pretranslate(v0); // translation * aligned scaling and transformation * mat - t1 = (Translation3(v0) * AlignedScaling3(v0)) * Matrix3(q1); + t1 = (Translation3(v0) * AlignedScaling3(v0)) * Transform3(q1); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); // scaling * mat and translation * mat - t1 = Translation3(v0) * (AlignedScaling3(v0) * Matrix3(q1)); + t1 = Translation3(v0) * (AlignedScaling3(v0) * Transform3(q1)); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); t0.setIdentity(); t0.scale(v0).translate(v0).rotate(q1); // translation * mat and aligned scaling * transformation - t1 = AlignedScaling3(v0) * (Translation3(v0) * Matrix3(q1)); + t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); // transformation * aligned scaling t0.scale(v0); - t1 = t1 * AlignedScaling3(v0); + t1 *= AlignedScaling3(v0); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); // transformation * translation t0.translate(v0); @@ -302,16 +367,6 @@ template void transformations(void) t1 = t1 * (q1 * AlignedScaling3(v1)); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); - // translation * vector - t0.setIdentity(); - t0.translate(v0); - VERIFY_IS_APPROX((t0 * v1).template head<3>(), Translation3(v0) * v1); - - // AlignedScaling * vector - t0.setIdentity(); - t0.scale(v0); - VERIFY_IS_APPROX((t0 * v1).template head<3>(), AlignedScaling3(v0) * v1); - // test transform inversion t0.setIdentity(); t0.translate(v0); @@ -327,11 +382,6 @@ template void transformations(void) t044.block(0,0,t0.matrix().rows(),4) = t0.matrix(); VERIFY_IS_APPROX(t0.inverse(Isometry).matrix(), t044.inverse().block(0,0,t0.matrix().rows(),4)); - // test extract rotation and aligned scaling -// t0.setIdentity(); -// t0.translate(v0).rotate(q1).scale(v1); -// VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1); - Matrix3 mat_rotation, mat_scaling; t0.setIdentity(); t0.translate(v0).rotate(q1).scale(v1); @@ -372,8 +422,12 @@ template void transformations(void) void test_geo_transformations() { for(int i = 0; i < g_repeat; i++) { - //CALL_SUBTEST_1(( transformations() )); - //CALL_SUBTEST_2(( transformations() )); + CALL_SUBTEST_1(( transformations() )); + CALL_SUBTEST_1(( non_projective_only() )); + + CALL_SUBTEST_2(( transformations() )); + CALL_SUBTEST_2(( non_projective_only() )); + CALL_SUBTEST_3(( transformations() )); } } diff --git a/test/main.h b/test/main.h index 6aee00373..8fa4ca8b5 100644 --- a/test/main.h +++ b/test/main.h @@ -118,7 +118,7 @@ namespace Eigen for (uint ai=0 ; ai