diff options
author | Gael Guennebaud <g.gael@free.fr> | 2008-06-15 08:33:44 +0000 |
---|---|---|
committer | Gael Guennebaud <g.gael@free.fr> | 2008-06-15 08:33:44 +0000 |
commit | fbbd8afe3086cc1a1f455cc5264f0b6fc063f8b6 (patch) | |
tree | 48a7af7b7060033a3555c74be3a73fc7c638fcf2 /Eigen | |
parent | 4af7089ab82583d39cc6fa1679a1406fb1185da5 (diff) |
Started a Transform class in the Geometry module to represent
homography.
Fix indentation in Quaternion.h
Diffstat (limited to 'Eigen')
-rw-r--r-- | Eigen/Geometry | 1 | ||||
-rw-r--r-- | Eigen/src/Core/Product.h | 6 | ||||
-rw-r--r-- | Eigen/src/Core/util/StaticAssert.h | 3 | ||||
-rw-r--r-- | Eigen/src/Geometry/Quaternion.h | 176 | ||||
-rw-r--r-- | Eigen/src/Geometry/Transform.h | 227 |
5 files changed, 323 insertions, 90 deletions
diff --git a/Eigen/Geometry b/Eigen/Geometry index 2b39336a2..fe241b5c9 100644 --- a/Eigen/Geometry +++ b/Eigen/Geometry @@ -7,6 +7,7 @@ namespace Eigen { #include "src/Geometry/Cross.h" #include "src/Geometry/Quaternion.h" +#include "src/Geometry/Transform.h" // the Geometry module use cwiseCos and cwiseSin which are defined in the Array module #include "src/Array/CwiseOperators.h" diff --git a/Eigen/src/Core/Product.h b/Eigen/src/Core/Product.h index 39bda7e6e..6c653a558 100644 --- a/Eigen/src/Core/Product.h +++ b/Eigen/src/Core/Product.h @@ -269,6 +269,10 @@ template<typename Lhs, typename Rhs, int EvalMode> class Product : ei_no_assignm { return m_lhs.coeff(row, col) * m_rhs.coeff(col, col); } + else if ((Lhs::Flags&Diagonal)==Diagonal) + { + return m_lhs.coeff(row, row) * m_rhs.coeff(row, col); + } else { Scalar res; @@ -286,7 +290,7 @@ template<typename Lhs, typename Rhs, int EvalMode> class Product : ei_no_assignm { if ((Rhs::Flags&Diagonal)==Diagonal) { - assert((_LhsNested::Flags&RowMajorBit)==0); + ei_assert((_LhsNested::Flags&RowMajorBit)==0); return ei_pmul(m_lhs.template packetCoeff<LoadMode>(row, col), ei_pset1(m_rhs.coeff(col, col))); } else diff --git a/Eigen/src/Core/util/StaticAssert.h b/Eigen/src/Core/util/StaticAssert.h index a3316aaef..37ae1ed82 100644 --- a/Eigen/src/Core/util/StaticAssert.h +++ b/Eigen/src/Core/util/StaticAssert.h @@ -57,7 +57,8 @@ enum { you_tried_calling_a_vector_method_on_a_matrix, you_mixed_vectors_of_different_sizes, - you_mixed_matrices_of_different_sizes + you_mixed_matrices_of_different_sizes, + you_did_a_programming_error }; }; diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 2a575691a..966eca415 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -43,122 +43,122 @@ template<typename _Scalar> class Quaternion { - typedef Matrix<_Scalar, 4, 1> Coefficients; - Coefficients m_coeffs; + typedef Matrix<_Scalar, 4, 1> Coefficients; + Coefficients m_coeffs; - public: +public: - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; - typedef Matrix<Scalar,3,1> Vector3; - typedef Matrix<Scalar,3,3> Matrix3; + typedef Matrix<Scalar,3,1> Vector3; + typedef Matrix<Scalar,3,3> Matrix3; - inline Scalar x() const { return m_coeffs.coeff(0); } - inline Scalar y() const { return m_coeffs.coeff(1); } - inline Scalar z() const { return m_coeffs.coeff(2); } - inline Scalar w() const { return m_coeffs.coeff(3); } + inline Scalar x() const { return m_coeffs.coeff(0); } + inline Scalar y() const { return m_coeffs.coeff(1); } + inline Scalar z() const { return m_coeffs.coeff(2); } + inline Scalar w() const { return m_coeffs.coeff(3); } - inline Scalar& x() { return m_coeffs.coeffRef(0); } - inline Scalar& y() { return m_coeffs.coeffRef(1); } - inline Scalar& z() { return m_coeffs.coeffRef(2); } - inline Scalar& w() { return m_coeffs.coeffRef(3); } + inline Scalar& x() { return m_coeffs.coeffRef(0); } + inline Scalar& y() { return m_coeffs.coeffRef(1); } + inline Scalar& z() { return m_coeffs.coeffRef(2); } + inline Scalar& w() { return m_coeffs.coeffRef(3); } - /** \returns a read-only vector expression of the imaginary part (x,y,z) */ - inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); } + /** \returns a read-only vector expression of the imaginary part (x,y,z) */ + inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); } - /** \returns a vector expression of the imaginary part (x,y,z) */ - inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); } + /** \returns a vector expression of the imaginary part (x,y,z) */ + inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); } - /** \returns a read-only vector expression of the coefficients */ - inline const Coefficients& _coeffs() const { return m_coeffs; } + /** \returns a read-only vector expression of the coefficients */ + inline const Coefficients& _coeffs() const { return m_coeffs; } - /** \returns a vector expression of the coefficients */ - inline Coefficients& _coeffs() { return m_coeffs; } + /** \returns a vector expression of the coefficients */ + inline Coefficients& _coeffs() { return m_coeffs; } - // FIXME what is the prefered order: w x,y,z or x,y,z,w ? - inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0) - { - m_coeffs.coeffRef(0) = x; - m_coeffs.coeffRef(1) = y; - m_coeffs.coeffRef(2) = z; - m_coeffs.coeffRef(3) = w; - } + // FIXME what is the prefered order: w x,y,z or x,y,z,w ? + inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0) + { + m_coeffs.coeffRef(0) = x; + m_coeffs.coeffRef(1) = y; + m_coeffs.coeffRef(2) = z; + m_coeffs.coeffRef(3) = w; + } - /** Copy constructor */ - inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; } + /** Copy constructor */ + inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; } - /** This is a special case of the templated operator=. Its purpose is to - * prevent a default operator= from hiding the templated operator=. - */ - inline Quaternion& operator=(const Quaternion& other) - { - m_coeffs = other.m_coeffs; - return *this; - } + /** This is a special case of the templated operator=. Its purpose is to + * prevent a default operator= from hiding the templated operator=. + */ + inline Quaternion& operator=(const Quaternion& other) + { + m_coeffs = other.m_coeffs; + return *this; + } - /** \returns a quaternion representing an identity rotation - * \sa MatrixBase::identity() - */ - inline static Quaternion identity() { return Quaternion(1, 0, 0, 0); } + /** \returns a quaternion representing an identity rotation + * \sa MatrixBase::identity() + */ + inline static Quaternion identity() { return Quaternion(1, 0, 0, 0); } - /** \sa Quaternion::identity(), MatrixBase::setIdentity() - */ - inline Quaternion& setIdentity() { m_coeffs << 1, 0, 0, 0; return *this; } + /** \sa Quaternion::identity(), MatrixBase::setIdentity() + */ + inline Quaternion& setIdentity() { m_coeffs << 1, 0, 0, 0; return *this; } - /** \returns the squared norm of the quaternion's coefficients - * \sa Quaternion::norm(), MatrixBase::norm2() - */ - inline Scalar norm2() const { return m_coeffs.norm2(); } + /** \returns the squared norm of the quaternion's coefficients + * \sa Quaternion::norm(), MatrixBase::norm2() + */ + inline Scalar norm2() const { return m_coeffs.norm2(); } - /** \returns the norm of the quaternion's coefficients - * \sa Quaternion::norm2(), MatrixBase::norm() - */ - inline Scalar norm() const { return m_coeffs.norm(); } + /** \returns the norm of the quaternion's coefficients + * \sa Quaternion::norm2(), MatrixBase::norm() + */ + inline Scalar norm() const { return m_coeffs.norm(); } - template<typename Derived> - Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m); - Matrix3 toRotationMatrix(void) const; + template<typename Derived> + Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m); + Matrix3 toRotationMatrix(void) const; - template<typename Derived> - Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis); - void toAngleAxis(Scalar& angle, Vector3& axis) const; + template<typename Derived> + Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis); + void toAngleAxis(Scalar& angle, Vector3& axis) const; - Quaternion& fromEulerAngles(Vector3 eulerAngles); + Quaternion& fromEulerAngles(Vector3 eulerAngles); - Vector3 toEulerAngles(void) const; + Vector3 toEulerAngles(void) const; - template<typename Derived1, typename Derived2> - Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); + template<typename Derived1, typename Derived2> + Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); - inline Quaternion operator* (const Quaternion& q) const; - inline Quaternion& operator*= (const Quaternion& q); + inline Quaternion operator* (const Quaternion& q) const; + inline Quaternion& operator*= (const Quaternion& q); - Quaternion inverse(void) const; - Quaternion conjugate(void) const; + Quaternion inverse(void) const; + Quaternion conjugate(void) const; - Quaternion slerp(Scalar t, const Quaternion& other) const; + Quaternion slerp(Scalar t, const Quaternion& other) const; - template<typename Derived> - Vector3 operator* (const MatrixBase<Derived>& vec) const; + template<typename Derived> + Vector3 operator* (const MatrixBase<Derived>& vec) const; protected: - /** Constructor copying the value of the expression \a other */ - template<typename OtherDerived> - inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other) - { - m_coeffs = other; - } - - /** Copies the value of the expression \a other into \c *this. - */ - template<typename OtherDerived> - inline Quaternion& operator=(const MatrixBase<OtherDerived>& other) - { - m_coeffs = other.derived(); - return *this; - } + /** Constructor copying the value of the expression \a other */ + template<typename OtherDerived> + inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other) + { + m_coeffs = other; + } + + /** Copies the value of the expression \a other into \c *this. + */ + template<typename OtherDerived> + inline Quaternion& operator=(const MatrixBase<OtherDerived>& other) + { + m_coeffs = other.derived(); + return *this; + } }; diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h new file mode 100644 index 000000000..80f02e71c --- /dev/null +++ b/Eigen/src/Geometry/Transform.h @@ -0,0 +1,227 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. Eigen itself is part of the KDE project. +// +// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see <http://www.gnu.org/licenses/>. + +#ifndef EIGEN_TRANSFORM_H +#define EIGEN_TRANSFORM_H + +/** \class Transform + * + * \brief Represents an homogeneous transformation in a N dimensional space + * + * \param _Scalar the scalar type, i.e., the type of the coefficients + * \param _Dim the dimension of the space + * + * + */ +template<typename _Scalar, int _Dim> +class Transform +{ +public: + + enum { Dim = _Dim, HDim = _Dim+1 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix<Scalar,HDim,HDim> MatrixType; + typedef Matrix<Scalar,Dim,Dim> AffineMatrixType; + typedef Block<MatrixType,Dim,Dim> AffineMatrixRef; + typedef Matrix<Scalar,Dim,1> VectorType; + typedef Block<MatrixType,Dim,1> VectorRef; + +protected: + + MatrixType m_matrix; + + template<typename Other, + int OtherRows=Other::RowsAtCompileTime, + int OtherCols=Other::ColsAtCompileTime> + struct ei_transform_product_impl; + +public: + + inline const MatrixType matrix() const { return m_matrix; } + inline MatrixType matrix() { return m_matrix; } + + inline const AffineMatrixRef affine() const { return m_matrix.template block<Dim,Dim>(0,0); } + inline AffineMatrixRef affine() { return m_matrix.template block<Dim,Dim>(0,0); } + + inline const VectorRef translation() const { return m_matrix.template block<Dim,1>(0,Dim); } + inline VectorRef translation() { return m_matrix.template block<Dim,1>(0,Dim); } + + template<typename OtherDerived> + struct ProductReturnType + { + typedef typename ei_transform_product_impl<OtherDerived>::ResultType Type; + }; + + template<typename OtherDerived> + const typename ProductReturnType<OtherDerived>::Type + operator * (const MatrixBase<OtherDerived> &other) const; + + void setIdentity() { m_matrix.setIdentity(); } + + template<typename OtherDerived> + Transform& scale(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> + Transform& prescale(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> + Transform& translate(const MatrixBase<OtherDerived> &other); + + template<typename OtherDerived> + Transform& pretranslate(const MatrixBase<OtherDerived> &other); + + AffineMatrixType extractRotation() const; + AffineMatrixType extractRotationNoShear() const; + +protected: + +}; + +template<typename Scalar, int Dim> +template<typename OtherDerived> +const typename Transform<Scalar,Dim>::template ProductReturnType<OtherDerived>::Type +Transform<Scalar,Dim>::operator*(const MatrixBase<OtherDerived> &other) const +{ + return ei_transform_product_impl<OtherDerived>::run(*this,other.derived()); +} + +/** Applies on the right the non uniform scale transformation represented + * by the vector \a other to \c *this and returns a reference to \c *this. + * \sa prescale() + */ +template<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::scale(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime) + && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error); + affine() = (affine() * other.asDiagonal()).lazy(); + return *this; +} + +/** Applies on the left the non uniform scale transformation represented + * by the vector \a other to \c *this and returns a reference to \c *this. + * \sa scale() + */ +template<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime) + && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error); + m_matrix.template block<3,4>(0,0) = (other.asDiagonal().eval() * m_matrix.template block<3,4>(0,0)).lazy(); + return *this; +} + +/** Applies on the right translation matrix represented by the vector \a other + * to \c *this and returns a reference to \c *this. + * \sa pretranslate() + */ +template<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime) + && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error); + translation() += affine() * other; + return *this; +} + +/** Applies on the left translation matrix represented by the vector \a other + * to \c *this and returns a reference to \c *this. + * \sa translate() + */ +template<typename Scalar, int Dim> +template<typename OtherDerived> +Transform<Scalar,Dim>& +Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other) +{ + EIGEN_STATIC_ASSERT(int(OtherDerived::IsVectorAtCompileTime) + && int(OtherDerived::SizeAtCompileTime)==int(Dim), you_did_a_programming_error); + translation() += other; + return *this; +} + +/** \returns the rotation part of the transformation using a QR decomposition. + * \sa extractRotationNoShear() + */ +template<typename Scalar, int Dim> +typename Transform<Scalar,Dim>::AffineMatrixType +Transform<Scalar,Dim>::extractRotation() const +{ + return affine().qr().matrixQ(); +} + +/** \returns the rotation part of the transformation assuming no shear in + * the affine part. + * \sa extractRotation() + */ +template<typename Scalar, int Dim> +typename Transform<Scalar,Dim>::AffineMatrixType +Transform<Scalar,Dim>::extractRotationNoShear() const +{ + return affine().cwiseAbs2() + .verticalRedux(ei_scalar_sum_op<Scalar>()).cwiseSqrt(); +} + +//---------- + +template<typename Scalar, int Dim> +template<typename Other> +struct Transform<Scalar,Dim>::ei_transform_product_impl<Other,Dim+1,Dim+1> +{ + typedef typename Transform<Scalar,Dim>::MatrixType MatrixType; + typedef Product<MatrixType,Other> ResultType; + static ResultType run(const Transform<Scalar,Dim>& tr, const Other& other) + { return tr.matrix() * other; } +}; + +template<typename Scalar, int Dim> +template<typename Other> +struct Transform<Scalar,Dim>::ei_transform_product_impl<Other,Dim+1,1> +{ + typedef typename Transform<Scalar,Dim>::MatrixType MatrixType; + typedef Product<MatrixType,Other> ResultType; + static ResultType run(const Transform<Scalar,Dim>& tr, const Other& other) + { return tr.matrix() * other; } +}; + +template<typename Scalar, int Dim> +template<typename Other> +struct Transform<Scalar,Dim>::ei_transform_product_impl<Other,Dim,1> +{ + typedef typename Transform<Scalar,Dim>::AffineMatrixRef MatrixType; + typedef const CwiseBinaryOp< + ei_scalar_sum_op<Scalar>, + NestByValue<Product<NestByValue<MatrixType>,Other> >, + NestByValue<typename Transform<Scalar,Dim>::VectorRef> > ResultType; + static ResultType run(const Transform<Scalar,Dim>& tr, const Other& other) + { return (tr.affine().nestByValue() * other).nestByValue() + tr.translation().nestByValue(); } +}; + +#endif // EIGEN_TRANSFORM_H |