From 95dd09bea62010ba3aba36d4833a3cd1594a2372 Mon Sep 17 00:00:00 2001 From: Gael Guennebaud Date: Mon, 18 Aug 2008 22:17:42 +0000 Subject: [PATCH] * revert the previous interface change in solveTriangular (pointer vs reference) * remove the cast operators in the Geometry module: they are replaced by constructors and new operator= in Matrix * extended the operations supported by Rotation2D * rewrite in solveTriangular: - merge the Upper and Lower specializations - big optimization of the path for row-major triangular matrices --- .../src/Cholesky/CholeskyWithoutSquareRoot.h | 2 +- Eigen/src/Core/Matrix.h | 7 + Eigen/src/Core/MatrixBase.h | 2 +- Eigen/src/Core/SolveTriangular.h | 202 +++++++++--------- Eigen/src/Core/util/StaticAssert.h | 6 + Eigen/src/Geometry/AngleAxis.h | 40 +++- Eigen/src/Geometry/Quaternion.h | 29 ++- Eigen/src/Geometry/Rotation.h | 25 ++- Eigen/src/LU/LU.h | 6 +- 9 files changed, 202 insertions(+), 117 deletions(-) diff --git a/Eigen/src/Cholesky/CholeskyWithoutSquareRoot.h b/Eigen/src/Cholesky/CholeskyWithoutSquareRoot.h index 6cf5d84c2..b00dc0a11 100644 --- a/Eigen/src/Cholesky/CholeskyWithoutSquareRoot.h +++ b/Eigen/src/Cholesky/CholeskyWithoutSquareRoot.h @@ -149,7 +149,7 @@ typename Derived::Eval CholeskyWithoutSquareRoot::solve(const Matrix return m_matrix.adjoint().template part() .solveTriangular( - ( m_matrix.cwise().inverse().diagonal().asDiagonal() + ( m_matrix.cwise().inverse().template part() * matrixL().solveTriangular(b)) ); } diff --git a/Eigen/src/Core/Matrix.h b/Eigen/src/Core/Matrix.h index 5b96eedac..268261b77 100644 --- a/Eigen/src/Core/Matrix.h +++ b/Eigen/src/Core/Matrix.h @@ -363,6 +363,13 @@ class Matrix : public MatrixBaseBase::swap(other); } + +/////////// Geometry module /////////// + + explicit Matrix(const Quaternion& q); + Matrix& operator=(const Quaternion& q); + explicit Matrix(const AngleAxis& aa); + Matrix& operator=(const AngleAxis& aa); }; /** \defgroup matrixtypedefs Global matrix typedefs diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index b6bb83d13..52b64283e 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h @@ -325,7 +325,7 @@ template class MatrixBase typename OtherDerived::Eval solveTriangular(const MatrixBase& other) const; template - void solveTriangularInPlace(MatrixBase* p_other) const; + void solveTriangularInPlace(MatrixBase& other) const; template diff --git a/Eigen/src/Core/SolveTriangular.h b/Eigen/src/Core/SolveTriangular.h index 5f6cf7c28..ccb13991c 100755 --- a/Eigen/src/Core/SolveTriangular.h +++ b/Eigen/src/Core/SolveTriangular.h @@ -29,19 +29,19 @@ template struct ei_is_part { enum {value=false}; }; template struct ei_is_part > { enum {value=true}; }; template::value ? -1 // this is to solve ambiguous specializations - : (int(Lhs::Flags) & LowerTriangularBit) + int TriangularPart = (int(Lhs::Flags) & LowerTriangularBit) ? Lower : (int(Lhs::Flags) & UpperTriangularBit) ? Upper : -1, - int StorageOrder = int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor + int StorageOrder = ei_is_part::value ? -1 // this is to solve ambiguous specializations + : int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor > struct ei_solve_triangular_selector; // transform a Part xpr to a Flagged xpr -template -struct ei_solve_triangular_selector,Rhs,TriangularPart,StorageOrder> +template +struct ei_solve_triangular_selector,Rhs,UpLo,StorageOrder> { static void run(const Part& lhs, Rhs& other) { @@ -50,88 +50,129 @@ struct ei_solve_triangular_selector,Rhs,TriangularPart,Storage }; // forward substitution, row-major -template -struct ei_solve_triangular_selector -{ - typedef typename Rhs::Scalar Scalar; - static void run(const Lhs& lhs, Rhs& other) - { - for(int c=0 ; c -struct ei_solve_triangular_selector +template +struct ei_solve_triangular_selector { typedef typename Rhs::Scalar Scalar; static void run(const Lhs& lhs, Rhs& other) { + const bool IsLower = (UpLo==Lower); const int size = lhs.cols(); + /* We perform the inverse product per block of 4 rows such that we perfectly match + * our optimized matrix * vector product. blockyStart represents the number of rows + * we have process first using the non-block version. + */ + int blockyStart = (std::max(size-5,0)/4)*4; + if (IsLower) + blockyStart = size - blockyStart; + else + blockyStart -= 1; for(int c=0 ; c=0 ; --i) + if (IsLower) + other.coeffRef(0,c) = other.coeff(0,c)/lhs.coeff(0, 0); + else + other.coeffRef(size-1,c) = other.coeff(size-1, c)/lhs.coeff(size-1, size-1); + for(int i=(IsLower ? 1 : size-2); IsLower ? iblockyStart; i += (IsLower ? 1 : -1) ) { Scalar tmp = other.coeff(i,c) - - ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0); + - (IsLower ? ((lhs.row(i).start(i)) * other.col(c).start(i)).coeff(0,0) + : ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0)); if (Lhs::Flags & UnitDiagBit) other.coeffRef(i,c) = tmp; else other.coeffRef(i,c) = tmp/lhs.coeff(i,i); } + + // now let process the remaining rows 4 at once + for(int i=blockyStart; IsLower ? i0; ) + { + int startBlock = i; + int endBlock = startBlock + (IsLower ? 4 : -4); + + /* Process the i cols times 4 rows block, and keep the result in a temporary vector */ + Matrix btmp; + if (IsLower) + btmp = lhs.block(startBlock,0,4,i) * other.col(c).start(i); + else + btmp = lhs.block(i-3,i+1,4,size-1-i) * other.col(c).end(size-1-i); + + /* Let's process the 4x4 sub-matrix as usual. + * btmp stores the diagonal coefficients used to update the remaining part of the result. + */ + { + Scalar tmp = other.coeff(startBlock,c)-btmp.coeff(IsLower?0:3); + if (Lhs::Flags & UnitDiagBit) + other.coeffRef(i,c) = tmp; + else + other.coeffRef(i,c) = tmp/lhs.coeff(i,i); + } + + i += IsLower ? 1 : -1; + for (;IsLower ? iendBlock; i += IsLower ? 1 : -1) + { + int remainingSize = IsLower ? i-startBlock : startBlock-i; + Scalar tmp = other.coeff(i,c) + - btmp.coeff(IsLower ? remainingSize : 3-remainingSize) + - ( lhs.row(i).block(IsLower ? startBlock : i+1, remainingSize) + * other.col(c).block(IsLower ? startBlock : i+1, remainingSize)).coeff(0,0); + + if (Lhs::Flags & UnitDiagBit) + other.coeffRef(i,c) = tmp; + else + other.coeffRef(i,c) = tmp/lhs.coeff(i,i); + } + } } } }; -// forward substitution, col-major -// FIXME the Lower and Upper specialization could be merged using a small helper class -// performing reflexions on the coordinates... -template -struct ei_solve_triangular_selector +// Implements the following configurations: +// - inv(Lower, ColMajor) * Column vector +// - inv(Lower,UnitDiag,ColMajor) * Column vector +// - inv(Upper, ColMajor) * Column vector +// - inv(Upper,UnitDiag,ColMajor) * Column vector +template +struct ei_solve_triangular_selector { typedef typename Rhs::Scalar Scalar; typedef typename ei_packet_traits::type Packet; - enum {PacketSize = ei_packet_traits::size}; + enum { PacketSize = ei_packet_traits::size }; static void run(const Lhs& lhs, Rhs& other) { + static const bool IsLower = (UpLo==Lower); const int size = lhs.cols(); for(int c=0 ; cblockyEnd;) { /* Let's process the 4x4 sub-matrix as usual. * btmp stores the diagonal coefficients used to update the remaining part of the result. */ int startBlock = i; - int endBlock = startBlock+4; + int endBlock = startBlock + (IsLower ? 4 : -4); Matrix btmp; - for (;iendBlock; + i += IsLower ? 1 : -1) { if(!(Lhs::Flags & UnitDiagBit)) other.coeffRef(i,c) /= lhs.coeff(i,i); - int remainingSize = endBlock-i-1; + int remainingSize = IsLower ? endBlock-i-1 : i-endBlock-1; if (remainingSize>0) - other.col(c).block(i+1,remainingSize) -= other.coeffRef(i,c) * Block(lhs, i+1, i, remainingSize, 1); - btmp.coeffRef(i-startBlock) = -other.coeffRef(i,c); + other.col(c).block((IsLower ? i : endBlock) + 1, remainingSize) -= + other.coeffRef(i,c) + * Block(lhs, (IsLower ? i : endBlock) + 1, i, remainingSize, 1); + btmp.coeffRef(IsLower ? i-startBlock : remainingSize) = -other.coeffRef(i,c); } /* Now we can efficiently update the remaining part of the result as a matrix * vector product. @@ -143,13 +184,15 @@ struct ei_solve_triangular_selector // FIXME this is cool but what about conjugate/adjoint expressions ? do we want to evaluate them ? // this is a more general problem though. ei_cache_friendly_product_colmajor_times_vector( - size-endBlock, &(lhs.const_cast_derived().coeffRef(endBlock,startBlock)), lhs.stride(), - btmp, &(other.coeffRef(endBlock,c))); + IsLower ? size-endBlock : endBlock+1, + &(lhs.const_cast_derived().coeffRef(IsLower ? endBlock : 0, IsLower ? startBlock : endBlock+1)), + lhs.stride(), + btmp, &(other.coeffRef(IsLower ? endBlock : 0, c))); } /* Now we have to process the remaining part as usual */ int i; - for(i=blockyEnd; i0; i += (IsLower ? 1 : -1) ) { if(!(Lhs::Flags & UnitDiagBit)) other.coeffRef(i,c) /= lhs.coeff(i,i); @@ -157,7 +200,10 @@ struct ei_solve_triangular_selector /* NOTE we cannot use lhs.col(i).end(size-i-1) because Part::coeffRef gets called by .col() to * get the address of the start of the row */ - other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block(lhs, i+1,i, size-i-1,1); + if(IsLower) + other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block(lhs, i+1,i, size-i-1,1); + else + other.col(c).start(i) -= other.coeffRef(i,c) * Block(lhs, 0,i, i, 1); } if(!(Lhs::Flags & UnitDiagBit)) other.coeffRef(i,c) /= lhs.coeff(i,i); @@ -165,68 +211,20 @@ struct ei_solve_triangular_selector } }; -// backward substitution, col-major -// see the previous specialization for details on the algorithm -template -struct ei_solve_triangular_selector -{ - typedef typename Rhs::Scalar Scalar; - static void run(const Lhs& lhs, Rhs& other) - { - const int size = lhs.cols(); - for(int c=0 ; cblockyEnd;) - { - int startBlock = i; - int endBlock = startBlock-4; - Matrix btmp; - /* Let's process the 4x4 sub-matrix as usual. - * btmp stores the diagonal coefficients used to update the remaining part of the result. - */ - for (; i>endBlock; --i) - { - if(!(Lhs::Flags & UnitDiagBit)) - other.coeffRef(i,c) /= lhs.coeff(i,i); - int remainingSize = i-endBlock-1; - if (remainingSize>0) - other.col(c).block(endBlock+1,remainingSize) -= other.coeffRef(i,c) * Block(lhs, endBlock+1, i, remainingSize, 1); - btmp.coeffRef(remainingSize) = -other.coeffRef(i,c); - } - - ei_cache_friendly_product_colmajor_times_vector( - endBlock+1, &(lhs.const_cast_derived().coeffRef(0,endBlock+1)), lhs.stride(), - btmp, &(other.coeffRef(0,c))); - } - - for(int i=blockyEnd; i>0; --i) - { - if(!(Lhs::Flags & UnitDiagBit)) - other.coeffRef(i,c) /= lhs.coeff(i,i); - other.col(c).start(i) -= other.coeffRef(i,c) * Block(lhs, 0,i, i, 1); - } - if(!(Lhs::Flags & UnitDiagBit)) - other.coeffRef(0,c) /= lhs.coeff(0,0); - } - } -}; - /** "in-place" version of MatrixBase::solveTriangular() where the result is written in \a other * * See MatrixBase:solveTriangular() for the details. */ template template -void MatrixBase::solveTriangularInPlace(MatrixBase* p_other) const +void MatrixBase::solveTriangularInPlace(MatrixBase& other) const { - ei_assert(p_other!=0); ei_assert(derived().cols() == derived().rows()); - ei_assert(derived().cols() == p_other->rows()); + ei_assert(derived().cols() == other.rows()); ei_assert(!(Flags & ZeroDiagBit)); ei_assert(Flags & (UpperTriangularBit|LowerTriangularBit)); - ei_solve_triangular_selector::run(derived(), p_other->derived()); + ei_solve_triangular_selector::run(derived(), other.derived()); } /** \returns the product of the inverse of \c *this with \a other, \a *this being triangular. @@ -265,7 +263,7 @@ template typename OtherDerived::Eval MatrixBase::solveTriangular(const MatrixBase& other) const { typename OtherDerived::Eval res(other); - solveTriangularInPlace(&res); + solveTriangularInPlace(res); return res; } diff --git a/Eigen/src/Core/util/StaticAssert.h b/Eigen/src/Core/util/StaticAssert.h index 3584926af..cf5e7239a 100644 --- a/Eigen/src/Core/util/StaticAssert.h +++ b/Eigen/src/Core/util/StaticAssert.h @@ -59,6 +59,7 @@ you_mixed_vectors_of_different_sizes, you_mixed_matrices_of_different_sizes, this_method_is_only_for_vectors_of_a_specific_size, + this_method_is_only_for_matrices_of_a_specific_size, you_did_a_programming_error, you_called_a_fixed_size_method_on_a_dynamic_size_matrix_or_vector, unaligned_load_and_store_operations_unimplemented_on_AltiVec, @@ -96,6 +97,11 @@ EIGEN_STATIC_ASSERT(TYPE::IsVectorAtCompileTime && TYPE::SizeAtCompileTime==SIZE, \ this_method_is_only_for_vectors_of_a_specific_size) +// static assertion failing if the type \a TYPE is not a vector type of the given size +#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(TYPE, ROWS, COLS) \ + EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime==ROWS && TYPE::ColsAtCompileTime==COLS, \ + this_method_is_only_for_matrices_of_a_specific_size) + // static assertion failing if the two vector expression types are not compatible (same fixed-size or dynamic size) #define EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(TYPE0,TYPE1) \ EIGEN_STATIC_ASSERT( \ diff --git a/Eigen/src/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h index ba3e9f46e..563735376 100644 --- a/Eigen/src/Geometry/AngleAxis.h +++ b/Eigen/src/Geometry/AngleAxis.h @@ -82,27 +82,32 @@ public: const Vector3& axis() const { return m_axis; } Vector3& axis() { return m_axis; } - /** Automatic conversion to a 3x3 rotation matrix. - * \sa toRotationMatrix() */ - operator Matrix3 () const { return toRotationMatrix(); } - + /** Concatenates two rotations */ inline QuaternionType operator* (const AngleAxis& other) const { return QuaternionType(*this) * QuaternionType(other); } - + + /** Concatenates two rotations */ inline QuaternionType operator* (const QuaternionType& other) const { return QuaternionType(*this) * other; } + /** Concatenates two rotations */ friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) { return a * QuaternionType(b); } + /** Concatenates two rotations */ inline typename ProductReturnType::Type operator* (const Matrix3& other) const { return toRotationMatrix() * other; } + /** Concatenates two rotations */ inline friend typename ProductReturnType::Type operator* (const Matrix3& a, const AngleAxis& b) { return a * b.toRotationMatrix(); } + /** \Returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ + AngleAxis inverse() const + { return AngleAxis(-m_angle, m_axis); } + AngleAxis& operator=(const QuaternionType& q); template AngleAxis& operator=(const MatrixBase& m); @@ -179,4 +184,29 @@ AngleAxis::toRotationMatrix(void) const return res; } +/** \geometry_module + * + * Constructs a 3x3 rotation matrix from the angle-axis \a aa + * + * \sa Matrix(const Quaternion&) + */ +template +Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::Matrix(const AngleAxis& aa) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3); + *this = aa.toRotationMatrix(); +} + +/** \geometry_module + * + * Set a 3x3 rotation matrix from the angle-axis \a aa + */ +template +Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>& +Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::operator=(const AngleAxis& aa) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3); + return *this = aa.toRotationMatrix(); +} + #endif // EIGEN_ANGLEAXIS_H diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index b2065fdcc..ba753fa43 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -118,6 +118,7 @@ public: /** Constructs and initializes a quaternion from the angle-axis \a aa */ explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } + /** Constructs and initializes a quaternion from either: * - a rotation matrix expression, * - a 4D vector expression representing quaternion coefficients. @@ -131,9 +132,6 @@ public: template Quaternion& operator=(const MatrixBase& m); - /** Automatic conversion to a rotation matrix. */ - operator Matrix3 () const { return toRotationMatrix(); } - /** \returns a quaternion representing an identity rotation * \sa MatrixBase::Identity() */ @@ -426,4 +424,29 @@ struct ei_quaternion_assign_impl } }; +/** \geometry_module + * + * Constructs a 3x3 rotation matrix from the quaternion \a q + * + * \sa Matrix(const AngleAxis&) + */ +template +Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::Matrix(const Quaternion& q) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3); + *this = q.toRotationMatrix(); +} + +/** \geometry_module + * + * Set a 3x3 rotation matrix from the quaternion \a q + */ +template +Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>& +Matrix<_Scalar, _Rows, _Cols, _MaxRows, _MaxCols, _Flags>::operator=(const Quaternion& q) +{ + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3); + return *this = q.toRotationMatrix(); +} + #endif // EIGEN_QUATERNION_H diff --git a/Eigen/src/Geometry/Rotation.h b/Eigen/src/Geometry/Rotation.h index a98ed061a..f6c4fd4a1 100644 --- a/Eigen/src/Geometry/Rotation.h +++ b/Eigen/src/Geometry/Rotation.h @@ -126,6 +126,7 @@ public: enum { Dim = 2 }; /** the scalar type of the coefficients */ typedef _Scalar Scalar; + typedef Matrix Vector2; typedef Matrix Matrix2; protected: @@ -136,14 +137,34 @@ public: /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ inline Rotation2D(Scalar a) : m_angle(a) {} - inline operator Scalar& () { return m_angle; } - inline operator Scalar () const { return m_angle; } + + /** \Returns the rotation angle */ + inline Scalar angle() const { return m_angle; } + + /** \Returns a read-write reference to the rotation angle */ + inline Scalar& angle() { return m_angle; } /** Automatic convertion to a 2D rotation matrix. * \sa toRotationMatrix() */ inline operator Matrix2() const { return toRotationMatrix(); } + /** \Returns the inverse rotation */ + inline Rotation2D inverse() const { return -m_angle; } + + /** Concatenates two rotations */ + inline Rotation2D operator*(const Rotation2D& other) const + { return m_angle + other.m_angle; } + + /** Concatenates two rotations */ + inline Rotation2D& operator*=(const Rotation2D& other) + { return m_angle += other.m_angle; } + + /** Applies the rotation to a 2D vector */ + template + Vector2 operator* (const MatrixBase& vec) const + { return toRotationMatrix() * vec; } + template Rotation2D& fromRotationMatrix(const MatrixBase& m); Matrix2 toRotationMatrix(void) const; diff --git a/Eigen/src/LU/LU.h b/Eigen/src/LU/LU.h index 0f68f563e..1267ec386 100644 --- a/Eigen/src/LU/LU.h +++ b/Eigen/src/LU/LU.h @@ -399,7 +399,7 @@ void LU::computeKernel(Matrix() - .solveTriangularInPlace(&y); + .solveTriangularInPlace(y); for(int i = 0; i < m_rank; i++) result->row(m_q.coeff(i)) = y.row(i); @@ -451,7 +451,7 @@ bool LU::solve( l.setZero(); l.corner(Eigen::TopLeft,rows,smalldim) = m_lu.corner(Eigen::TopLeft,rows,smalldim); - l.template marked().solveTriangularInPlace(&c); + l.template marked().solveTriangularInPlace(c); // Step 3 if(!isSurjective()) @@ -468,7 +468,7 @@ bool LU::solve( d(c.corner(TopLeft, m_rank, c.cols())); m_lu.corner(TopLeft, m_rank, m_rank) .template marked() - .solveTriangularInPlace(&d); + .solveTriangularInPlace(d); // Step 4 result->resize(m_lu.cols(), b.cols());