mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-01-18 14:34:17 +08:00
big rewrite in Inverse.h
in particular, the API is essentially finalized and the 4x4 case is fixed to be numerically stable.
This commit is contained in:
Benoit Jacob
parent
68d48511b2
commit
ec02388a5d
@ -703,9 +703,12 @@ template<typename Derived> class MatrixBase
|
||||
const PartialLU<PlainMatrixType> partialLu() const;
|
||||
const PlainMatrixType inverse() const;
|
||||
template<typename ResultType>
|
||||
void computeInverse(ResultType *result) const;
|
||||
template<typename ResultType>
|
||||
bool computeInverseWithCheck(ResultType *result ) const;
|
||||
void computeInverseAndDetWithCheck(
|
||||
ResultType& inverse,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible,
|
||||
const RealScalar& absDeterminantThreshold = precision<Scalar>()
|
||||
) const;
|
||||
Scalar determinant() const;
|
||||
|
||||
/////////// Cholesky module ///////////
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
|
||||
// Copyright (C) 2008-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
|
||||
//
|
||||
// Eigen is free software; you can redistribute it and/or
|
||||
// modify it under the terms of the GNU Lesser General Public
|
||||
@ -29,74 +29,96 @@
|
||||
*** Part 1 : optimized implementations for fixed-size 2,3,4 cases ***
|
||||
********************************************************************/
|
||||
|
||||
template<typename XprType, typename MatrixType>
|
||||
template<typename MatrixType, typename ResultType>
|
||||
inline void ei_compute_inverse_size2_helper(
|
||||
const XprType& matrix, const typename MatrixType::Scalar& invdet,
|
||||
MatrixType* result)
|
||||
const MatrixType& matrix, const typename ResultType::Scalar& invdet,
|
||||
ResultType& result)
|
||||
{
|
||||
result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
|
||||
result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
|
||||
result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
|
||||
result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
|
||||
}
|
||||
|
||||
template<typename MatrixType>
|
||||
inline void ei_compute_inverse_size2(const MatrixType& matrix, MatrixType* result)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
const Scalar invdet = Scalar(1) / matrix.determinant();
|
||||
ei_compute_inverse_size2_helper( matrix, invdet, result );
|
||||
}
|
||||
|
||||
template<typename XprType, typename MatrixType>
|
||||
bool ei_compute_inverse_size2_with_check(const XprType& matrix, MatrixType* result)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
const Scalar det = matrix.determinant();
|
||||
if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
|
||||
const Scalar invdet = Scalar(1) / det;
|
||||
ei_compute_inverse_size2_helper( matrix, invdet, result );
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename XprType, typename MatrixType>
|
||||
void ei_compute_inverse_size3_helper(
|
||||
const XprType& matrix,
|
||||
const typename MatrixType::Scalar& invdet,
|
||||
const typename MatrixType::Scalar& det_minor00,
|
||||
const typename MatrixType::Scalar& det_minor10,
|
||||
const typename MatrixType::Scalar& det_minor20,
|
||||
MatrixType* result)
|
||||
{
|
||||
result->coeffRef(0, 0) = det_minor00 * invdet;
|
||||
result->coeffRef(0, 1) = -det_minor10 * invdet;
|
||||
result->coeffRef(0, 2) = det_minor20 * invdet;
|
||||
result->coeffRef(1, 0) = -matrix.minor(0,1).determinant() * invdet;
|
||||
result->coeffRef(1, 1) = matrix.minor(1,1).determinant() * invdet;
|
||||
result->coeffRef(1, 2) = -matrix.minor(2,1).determinant() * invdet;
|
||||
result->coeffRef(2, 0) = matrix.minor(0,2).determinant() * invdet;
|
||||
result->coeffRef(2, 1) = -matrix.minor(1,2).determinant() * invdet;
|
||||
result->coeffRef(2, 2) = matrix.minor(2,2).determinant() * invdet;
|
||||
}
|
||||
|
||||
template<bool Check, typename XprType, typename MatrixType>
|
||||
bool ei_compute_inverse_size3(const XprType& matrix, MatrixType* result)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
const Scalar det_minor00 = matrix.minor(0,0).determinant();
|
||||
const Scalar det_minor10 = matrix.minor(1,0).determinant();
|
||||
const Scalar det_minor20 = matrix.minor(2,0).determinant();
|
||||
const Scalar det = ( det_minor00 * matrix.coeff(0,0)
|
||||
- det_minor10 * matrix.coeff(1,0)
|
||||
+ det_minor20 * matrix.coeff(2,0) );
|
||||
if(Check) if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
|
||||
const Scalar invdet = Scalar(1) / det;
|
||||
ei_compute_inverse_size3_helper( matrix, invdet, det_minor00, det_minor10, det_minor20, result );
|
||||
return true;
|
||||
result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
|
||||
result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
|
||||
result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
|
||||
result.coeffRef(1,1) = matrix.coeff(0,0) * invdet;
|
||||
}
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
bool ei_compute_inverse_size4_helper(const MatrixType& matrix, ResultType* result)
|
||||
inline void ei_compute_inverse_size2(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
typedef typename ResultType::Scalar Scalar;
|
||||
const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
|
||||
ei_compute_inverse_size2_helper(matrix, invdet, result);
|
||||
}
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
inline void ei_compute_inverse_and_det_size2_with_check(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& inverse,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
typedef typename ResultType::Scalar Scalar;
|
||||
determinant = matrix.determinant();
|
||||
invertible = ei_abs(determinant) > absDeterminantThreshold;
|
||||
if(!invertible) return;
|
||||
const Scalar invdet = Scalar(1) / determinant;
|
||||
ei_compute_inverse_size2_helper(matrix, invdet, inverse);
|
||||
}
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
void ei_compute_inverse_size3_helper(
|
||||
const MatrixType& matrix,
|
||||
const typename ResultType::Scalar& invdet,
|
||||
const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
|
||||
ResultType& result)
|
||||
{
|
||||
result.row(0) = cofactors_col0 * invdet;
|
||||
result.coeffRef(1,0) = -matrix.minor(0,1).determinant() * invdet;
|
||||
result.coeffRef(1,1) = matrix.minor(1,1).determinant() * invdet;
|
||||
result.coeffRef(1,2) = -matrix.minor(2,1).determinant() * invdet;
|
||||
result.coeffRef(2,0) = matrix.minor(0,2).determinant() * invdet;
|
||||
result.coeffRef(2,1) = -matrix.minor(1,2).determinant() * invdet;
|
||||
result.coeffRef(2,2) = matrix.minor(2,2).determinant() * invdet;
|
||||
}
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
void ei_compute_inverse_size3(
|
||||
const MatrixType& matrix,
|
||||
ResultType& result)
|
||||
{
|
||||
typedef typename ResultType::Scalar Scalar;
|
||||
Matrix<Scalar,3,1> cofactors_col0;
|
||||
cofactors_col0.coeffRef(0) = matrix.minor(0,0).determinant();
|
||||
cofactors_col0.coeffRef(1) = -matrix.minor(1,0).determinant();
|
||||
cofactors_col0.coeffRef(2) = matrix.minor(2,0).determinant();
|
||||
const Scalar det = (cofactors_col0.cwise()*matrix.col(0)).sum();
|
||||
const Scalar invdet = Scalar(1) / det;
|
||||
ei_compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
|
||||
}
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
void ei_compute_inverse_and_det_size3_with_check(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& inverse,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
typedef typename ResultType::Scalar Scalar;
|
||||
Matrix<Scalar,3,1> cofactors_col0;
|
||||
cofactors_col0.coeffRef(0) = matrix.minor(0,0).determinant();
|
||||
cofactors_col0.coeffRef(1) = -matrix.minor(1,0).determinant();
|
||||
cofactors_col0.coeffRef(2) = matrix.minor(2,0).determinant();
|
||||
determinant = (cofactors_col0.cwise()*matrix.col(0)).sum();
|
||||
invertible = ei_abs(determinant) > absDeterminantThreshold;
|
||||
if(!invertible) return;
|
||||
const Scalar invdet = Scalar(1) / determinant;
|
||||
ei_compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
|
||||
}
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
void ei_compute_inverse_size4_helper(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
/* Let's split M into four 2x2 blocks:
|
||||
* (P Q)
|
||||
@ -111,113 +133,106 @@ bool ei_compute_inverse_size4_helper(const MatrixType& matrix, ResultType* resul
|
||||
* Q' = -(P_inverse*Q) * S'
|
||||
* R' = -S' * (R*P_inverse)
|
||||
*/
|
||||
typedef Block<MatrixType,2,2> XprBlock22;
|
||||
typedef Block<ResultType,2,2> XprBlock22;
|
||||
typedef typename MatrixBase<XprBlock22>::PlainMatrixType Block22;
|
||||
Block22 P_inverse;
|
||||
if(ei_compute_inverse_size2_with_check(matrix.template block<2,2>(0,0), &P_inverse))
|
||||
{
|
||||
const Block22 Q = matrix.template block<2,2>(0,2);
|
||||
const Block22 P_inverse_times_Q = P_inverse * Q;
|
||||
const XprBlock22 R = matrix.template block<2,2>(2,0);
|
||||
const Block22 R_times_P_inverse = R * P_inverse;
|
||||
const Block22 R_times_P_inverse_times_Q = R_times_P_inverse * Q;
|
||||
const XprBlock22 S = matrix.template block<2,2>(2,2);
|
||||
const Block22 X = S - R_times_P_inverse_times_Q;
|
||||
Block22 Y;
|
||||
ei_compute_inverse_size2(X, &Y);
|
||||
result->template block<2,2>(2,2) = Y;
|
||||
result->template block<2,2>(2,0) = - Y * R_times_P_inverse;
|
||||
const Block22 Z = P_inverse_times_Q * Y;
|
||||
result->template block<2,2>(0,2) = - Z;
|
||||
result->template block<2,2>(0,0) = P_inverse + Z * R_times_P_inverse;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
ei_compute_inverse_size2(matrix.template block<2,2>(0,0), P_inverse);
|
||||
const Block22 Q = matrix.template block<2,2>(0,2);
|
||||
const Block22 P_inverse_times_Q = P_inverse * Q;
|
||||
const XprBlock22 R = matrix.template block<2,2>(2,0);
|
||||
const Block22 R_times_P_inverse = R * P_inverse;
|
||||
const Block22 R_times_P_inverse_times_Q = R_times_P_inverse * Q;
|
||||
const XprBlock22 S = matrix.template block<2,2>(2,2);
|
||||
const Block22 X = S - R_times_P_inverse_times_Q;
|
||||
Block22 Y;
|
||||
ei_compute_inverse_size2(X, Y);
|
||||
result.template block<2,2>(2,2) = Y;
|
||||
result.template block<2,2>(2,0) = - Y * R_times_P_inverse;
|
||||
const Block22 Z = P_inverse_times_Q * Y;
|
||||
result.template block<2,2>(0,2) = - Z;
|
||||
result.template block<2,2>(0,0) = P_inverse + Z * R_times_P_inverse;
|
||||
}
|
||||
|
||||
template<typename XprType, typename MatrixType>
|
||||
bool ei_compute_inverse_size4_with_check(const XprType& matrix, MatrixType* result)
|
||||
template<typename MatrixType, typename ResultType>
|
||||
void ei_compute_inverse_size4(const MatrixType& _matrix, ResultType& result)
|
||||
{
|
||||
if(ei_compute_inverse_size4_helper(matrix, result))
|
||||
{
|
||||
// good ! The topleft 2x2 block was invertible, so the 2x2 blocks approach is successful.
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// rare case: the topleft 2x2 block is not invertible (but the matrix itself is assumed to be).
|
||||
// since this is a rare case, we don't need to optimize it. We just want to handle it with little
|
||||
// additional code.
|
||||
MatrixType m(matrix);
|
||||
m.row(0).swap(m.row(2));
|
||||
m.row(1).swap(m.row(3));
|
||||
if(ei_compute_inverse_size4_helper(m, result))
|
||||
typedef typename ResultType::Scalar Scalar;
|
||||
typedef typename MatrixType::RealScalar RealScalar;
|
||||
|
||||
// we will do row permutations on the matrix. This copy should have negligible cost.
|
||||
// if not, consider working in-place on the matrix (const-cast it, but then undo the permutations
|
||||
// to nevertheless honor constness)
|
||||
typename MatrixType::PlainMatrixType matrix(_matrix);
|
||||
|
||||
// let's extract from the 2 first colums a 2x2 block whose determinant is as big as possible.
|
||||
int good_row0=0, good_row1=1;
|
||||
RealScalar good_absdet(-1);
|
||||
// this double for loop shouldn't be too costly: only 6 iterations
|
||||
for(int row0=0; row0<4; ++row0) {
|
||||
for(int row1=row0+1; row1<4; ++row1)
|
||||
{
|
||||
// good, the topleft 2x2 block of m is invertible. Since m is different from matrix in that some
|
||||
// rows were permuted, the actual inverse of matrix is derived from the inverse of m by permuting
|
||||
// the corresponding columns.
|
||||
result->col(0).swap(result->col(2));
|
||||
result->col(1).swap(result->col(3));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// first, undo the swaps previously made
|
||||
m.row(0).swap(m.row(2));
|
||||
m.row(1).swap(m.row(3));
|
||||
// swap row 0 with the the row among 0 and 1 that has the biggest 2 first coeffs
|
||||
int swap0with = ei_abs(m.coeff(0,0))+ei_abs(m.coeff(0,1))>ei_abs(m.coeff(1,0))+ei_abs(m.coeff(1,1)) ? 0 : 1;
|
||||
m.row(0).swap(m.row(swap0with));
|
||||
// swap row 1 with the the row among 2 and 3 that has the biggest 2 first coeffs
|
||||
int swap1with = ei_abs(m.coeff(2,0))+ei_abs(m.coeff(2,1))>ei_abs(m.coeff(3,0))+ei_abs(m.coeff(3,1)) ? 2 : 3;
|
||||
m.row(1).swap(m.row(swap1with));
|
||||
if( ei_compute_inverse_size4_helper(m, result) )
|
||||
RealScalar absdet = ei_abs(matrix.coeff(row0,0)*matrix.coeff(row1,1)
|
||||
- matrix.coeff(row0,1)*matrix.coeff(row1,0));
|
||||
if(absdet > good_absdet)
|
||||
{
|
||||
result->col(1).swap(result->col(swap1with));
|
||||
result->col(0).swap(result->col(swap0with));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// non-invertible matrix
|
||||
return false;
|
||||
good_absdet = absdet;
|
||||
good_row0 = row0;
|
||||
good_row1 = row1;
|
||||
}
|
||||
}
|
||||
}
|
||||
// do row permutations to move this 2x2 block to the top
|
||||
matrix.row(0).swap(matrix.row(good_row0));
|
||||
matrix.row(1).swap(matrix.row(good_row1));
|
||||
// now applying our helper function is numerically stable
|
||||
ei_compute_inverse_size4_helper(matrix, result);
|
||||
// Since we did row permutations on the original matrix, we need to do column permutations
|
||||
// in the reverse order on the inverse
|
||||
result.col(1).swap(result.col(good_row1));
|
||||
result.col(0).swap(result.col(good_row0));
|
||||
}
|
||||
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
void ei_compute_inverse_and_det_size4_with_check(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& result,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
determinant = matrix.determinant();
|
||||
invertible = ei_abs(determinant) > absDeterminantThreshold;
|
||||
if(invertible) ei_compute_inverse_size4(matrix, result);
|
||||
}
|
||||
|
||||
/***********************************************
|
||||
*** Part 2 : selector and MatrixBase methods ***
|
||||
*** Part 2 : selectors and MatrixBase methods ***
|
||||
***********************************************/
|
||||
|
||||
template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
|
||||
struct ei_compute_inverse
|
||||
{
|
||||
static inline void run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
*result = matrix.partialLu().inverse();
|
||||
result = matrix.partialLu().inverse();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse<MatrixType, ResultType, 1>
|
||||
{
|
||||
static inline void run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
result->coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
|
||||
result.coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse<MatrixType, ResultType, 2>
|
||||
{
|
||||
static inline void run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
ei_compute_inverse_size2(matrix, result);
|
||||
}
|
||||
@ -226,64 +241,53 @@ struct ei_compute_inverse<MatrixType, ResultType, 2>
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse<MatrixType, ResultType, 3>
|
||||
{
|
||||
static inline void run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
ei_compute_inverse_size3<false, MatrixType, ResultType>(matrix, result);
|
||||
ei_compute_inverse_size3(matrix, result);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse<MatrixType, ResultType, 4>
|
||||
{
|
||||
static inline void run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(const MatrixType& matrix, ResultType& result)
|
||||
{
|
||||
ei_compute_inverse_size4_with_check(matrix, result);
|
||||
ei_compute_inverse_size4(matrix, result);
|
||||
}
|
||||
};
|
||||
|
||||
/** \lu_module
|
||||
*
|
||||
* Computes the matrix inverse of this matrix.
|
||||
*
|
||||
* \note This matrix must be invertible, otherwise the result is undefined. If you need an invertibility check, use
|
||||
* computeInverseWithCheck().
|
||||
*
|
||||
* \param result Pointer to the matrix in which to store the result.
|
||||
*
|
||||
* Example: \include MatrixBase_computeInverse.cpp
|
||||
* Output: \verbinclude MatrixBase_computeInverse.out
|
||||
*
|
||||
* \sa inverse(), computeInverseWithCheck()
|
||||
*/
|
||||
template<typename Derived>
|
||||
template<typename ResultType>
|
||||
inline void MatrixBase<Derived>::computeInverse(ResultType *result) const
|
||||
{
|
||||
ei_assert(rows() == cols());
|
||||
EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
|
||||
ei_compute_inverse<PlainMatrixType, ResultType>::run(eval(), result);
|
||||
}
|
||||
|
||||
/** \lu_module
|
||||
*
|
||||
* \returns the matrix inverse of this matrix.
|
||||
*
|
||||
* \note This matrix must be invertible, otherwise the result is undefined. If you need an invertibility check, use
|
||||
* computeInverseWithCheck().
|
||||
* For small fixed sizes up to 4x4, this method uses ad-hoc methods (cofactors up to 3x3, Euler's trick for 4x4).
|
||||
* In the general case, this method uses class PartialLU.
|
||||
*
|
||||
* \note This method returns a matrix by value, which can be inefficient. To avoid that overhead,
|
||||
* use computeInverse() instead.
|
||||
* \note This matrix must be invertible, otherwise the result is undefined. If you need an
|
||||
* invertibility check, do the following:
|
||||
* \li for fixed sizes up to 4x4, use computeInverseAndDetWithCheck().
|
||||
* \li for the general case, use class LU.
|
||||
*
|
||||
* Example: \include MatrixBase_inverse.cpp
|
||||
* Output: \verbinclude MatrixBase_inverse.out
|
||||
*
|
||||
* \sa computeInverse(), computeInverseWithCheck()
|
||||
* \sa computeInverseAndDetWithCheck()
|
||||
*/
|
||||
template<typename Derived>
|
||||
inline const typename MatrixBase<Derived>::PlainMatrixType MatrixBase<Derived>::inverse() const
|
||||
{
|
||||
typename MatrixBase<Derived>::PlainMatrixType result;
|
||||
computeInverse(&result);
|
||||
EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
|
||||
ei_assert(rows() == cols());
|
||||
typedef typename MatrixBase<Derived>::PlainMatrixType ResultType;
|
||||
ResultType result(rows(), cols());
|
||||
// for 2x2, it's worth giving a chance to avoid evaluating.
|
||||
// for larger sizes, evaluating has negligible cost and limits code size.
|
||||
typedef typename ei_meta_if<
|
||||
RowsAtCompileTime == 2,
|
||||
typename ei_cleantype<typename ei_nested<Derived,2>::type>::type,
|
||||
PlainMatrixType
|
||||
>::ret MatrixType;
|
||||
ei_compute_inverse<MatrixType, ResultType>::run(derived(), result);
|
||||
return result;
|
||||
}
|
||||
|
||||
@ -293,74 +297,108 @@ inline const typename MatrixBase<Derived>::PlainMatrixType MatrixBase<Derived>::
|
||||
*******************************************/
|
||||
|
||||
template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
|
||||
struct ei_compute_inverse_with_check
|
||||
struct ei_compute_inverse_and_det_with_check {};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse_and_det_with_check<MatrixType, ResultType, 1>
|
||||
{
|
||||
static inline bool run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& result,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
LU<MatrixType> lu( matrix );
|
||||
if( !lu.isInvertible() ) return false;
|
||||
*result = lu.inverse();
|
||||
return true;
|
||||
determinant = matrix.coeff(0,0);
|
||||
invertible = ei_abs(determinant) > absDeterminantThreshold;
|
||||
if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse_with_check<MatrixType, ResultType, 1>
|
||||
struct ei_compute_inverse_and_det_with_check<MatrixType, ResultType, 2>
|
||||
{
|
||||
static inline bool run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& result,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
if( matrix.coeff(0,0) == Scalar(0) ) return false;
|
||||
result->coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
|
||||
return true;
|
||||
ei_compute_inverse_and_det_size2_with_check
|
||||
(matrix, absDeterminantThreshold, result, determinant, invertible);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse_with_check<MatrixType, ResultType, 2>
|
||||
struct ei_compute_inverse_and_det_with_check<MatrixType, ResultType, 3>
|
||||
{
|
||||
static inline bool run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& result,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
return ei_compute_inverse_size2_with_check(matrix, result);
|
||||
ei_compute_inverse_and_det_size3_with_check
|
||||
(matrix, absDeterminantThreshold, result, determinant, invertible);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse_with_check<MatrixType, ResultType, 3>
|
||||
struct ei_compute_inverse_and_det_with_check<MatrixType, ResultType, 4>
|
||||
{
|
||||
static inline bool run(const MatrixType& matrix, ResultType* result)
|
||||
static inline void run(
|
||||
const MatrixType& matrix,
|
||||
const typename MatrixType::RealScalar& absDeterminantThreshold,
|
||||
ResultType& result,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible
|
||||
)
|
||||
{
|
||||
return ei_compute_inverse_size3<true, MatrixType, ResultType>(matrix, result);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename MatrixType, typename ResultType>
|
||||
struct ei_compute_inverse_with_check<MatrixType, ResultType, 4>
|
||||
{
|
||||
static inline bool run(const MatrixType& matrix, ResultType* result)
|
||||
{
|
||||
return ei_compute_inverse_size4_with_check(matrix, result);
|
||||
ei_compute_inverse_and_det_size4_with_check
|
||||
(matrix, absDeterminantThreshold, result, determinant, invertible);
|
||||
}
|
||||
};
|
||||
|
||||
/** \lu_module
|
||||
*
|
||||
* Computation of matrix inverse, with invertibility check.
|
||||
* Computation of matrix inverse and determinant, with invertibility check.
|
||||
*
|
||||
* \returns true if the matrix is invertible, false otherwise.
|
||||
* This is only for fixed-size square matrices of size up to 4x4.
|
||||
*
|
||||
* \param result Pointer to the matrix in which to store the result.
|
||||
* \param inverse Reference to the matrix in which to store the inverse.
|
||||
* \param determinant Reference to the variable in which to store the inverse.
|
||||
* \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
|
||||
* \param absDeterminantThreshold Optional parameter controlling the invertibility check.
|
||||
* The matrix will be declared invertible if the absolute value of its
|
||||
* determinant is greater than this threshold.
|
||||
*
|
||||
* \sa inverse(), computeInverse()
|
||||
* \sa inverse()
|
||||
*/
|
||||
template<typename Derived>
|
||||
template<typename ResultType>
|
||||
inline bool MatrixBase<Derived>::computeInverseWithCheck(ResultType *result) const
|
||||
inline void MatrixBase<Derived>::computeInverseAndDetWithCheck(
|
||||
ResultType& inverse,
|
||||
typename ResultType::Scalar& determinant,
|
||||
bool& invertible,
|
||||
const RealScalar& absDeterminantThreshold
|
||||
) const
|
||||
{
|
||||
// i'd love to put some static assertions there, but SFINAE means that they have no effect...
|
||||
ei_assert(rows() == cols());
|
||||
EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
|
||||
return ei_compute_inverse_with_check<PlainMatrixType, ResultType>::run(eval(), result);
|
||||
// for 2x2, it's worth giving a chance to avoid evaluating.
|
||||
// for larger sizes, evaluating has negligible cost and limits code size.
|
||||
typedef typename ei_meta_if<
|
||||
RowsAtCompileTime == 2,
|
||||
typename ei_cleantype<typename ei_nested<Derived, 2>::type>::type,
|
||||
PlainMatrixType
|
||||
>::ret MatrixType;
|
||||
ei_compute_inverse_and_det_with_check<MatrixType, ResultType>::run
|
||||
(derived(), absDeterminantThreshold, inverse, determinant, invertible);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -53,9 +53,6 @@ template<typename MatrixType> void inverse(const MatrixType& m)
|
||||
m2 = m1.inverse();
|
||||
VERIFY_IS_APPROX(m1, m2.inverse() );
|
||||
|
||||
m1.computeInverse(&m2);
|
||||
VERIFY_IS_APPROX(m1, m2.inverse() );
|
||||
|
||||
VERIFY_IS_APPROX((Scalar(2)*m2).inverse(), m2.inverse()*Scalar(0.5));
|
||||
|
||||
VERIFY_IS_APPROX(identity, m1.inverse() * m1 );
|
||||
@ -66,17 +63,23 @@ template<typename MatrixType> void inverse(const MatrixType& m)
|
||||
// since for the general case we implement separately row-major and col-major, test that
|
||||
VERIFY_IS_APPROX(m1.transpose().inverse(), m1.inverse().transpose());
|
||||
|
||||
//computeInverseWithCheck tests
|
||||
#if !defined(EIGEN_TEST_PART_5) && !defined(EIGEN_TEST_PART_6)
|
||||
//computeInverseAndDetWithCheck tests
|
||||
//First: an invertible matrix
|
||||
bool invertible = m1.computeInverseWithCheck(&m2);
|
||||
bool invertible;
|
||||
RealScalar det;
|
||||
m1.computeInverseAndDetWithCheck(m2, det, invertible);
|
||||
VERIFY(invertible);
|
||||
VERIFY_IS_APPROX(identity, m1*m2);
|
||||
VERIFY_IS_APPROX(det, m1.determinant());
|
||||
|
||||
//Second: a rank one matrix (not invertible, except for 1x1 matrices)
|
||||
VectorType v3 = VectorType::Random(rows);
|
||||
MatrixType m3 = v3*v3.transpose(), m4(rows,cols);
|
||||
invertible = m3.computeInverseWithCheck( &m4 );
|
||||
m3.computeInverseAndDetWithCheck(m4, det, invertible);
|
||||
VERIFY( rows==1 ? invertible : !invertible );
|
||||
VERIFY_IS_APPROX(det, m3.determinant());
|
||||
#endif
|
||||
}
|
||||
|
||||
void test_inverse()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user