InverseImpl.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
5 // Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
6 //
7 // This Source Code Form is subject to the terms of the Mozilla
8 // Public License v. 2.0. If a copy of the MPL was not distributed
9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10 
11 #ifndef EIGEN_INVERSE_IMPL_H
12 #define EIGEN_INVERSE_IMPL_H
13 
14 namespace Eigen {
15 
16 namespace internal {
17 
18 /**********************************
19 *** General case implementation ***
20 **********************************/
21 
22 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
24 {
26  static inline void run(const MatrixType& matrix, ResultType& result)
27  {
28  result = matrix.partialPivLu().inverse();
29  }
30 };
31 
32 template<typename MatrixType, typename ResultType, int Size = MatrixType::RowsAtCompileTime>
33 struct compute_inverse_and_det_with_check { /* nothing! general case not supported. */ };
34 
35 /****************************
36 *** Size 1 implementation ***
37 ****************************/
38 
39 template<typename MatrixType, typename ResultType>
40 struct compute_inverse<MatrixType, ResultType, 1>
41 {
43  static inline void run(const MatrixType& matrix, ResultType& result)
44  {
45  typedef typename MatrixType::Scalar Scalar;
47  result.coeffRef(0,0) = Scalar(1) / matrixEval.coeff(0,0);
48  }
49 };
50 
51 template<typename MatrixType, typename ResultType>
53 {
55  static inline void run(
56  const MatrixType& matrix,
57  const typename MatrixType::RealScalar& absDeterminantThreshold,
58  ResultType& result,
60  bool& invertible
61  )
62  {
63  using std::abs;
64  determinant = matrix.coeff(0,0);
65  invertible = abs(determinant) > absDeterminantThreshold;
66  if(invertible) result.coeffRef(0,0) = typename ResultType::Scalar(1) / determinant;
67  }
68 };
69 
70 /****************************
71 *** Size 2 implementation ***
72 ****************************/
73 
74 template<typename MatrixType, typename ResultType>
77  const MatrixType& matrix, const typename ResultType::Scalar& invdet,
78  ResultType& result)
79 {
80  typename ResultType::Scalar temp = matrix.coeff(0,0);
81  result.coeffRef(0,0) = matrix.coeff(1,1) * invdet;
82  result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
83  result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
84  result.coeffRef(1,1) = temp * invdet;
85 }
86 
87 template<typename MatrixType, typename ResultType>
88 struct compute_inverse<MatrixType, ResultType, 2>
89 {
91  static inline void run(const MatrixType& matrix, ResultType& result)
92  {
93  typedef typename ResultType::Scalar Scalar;
94  const Scalar invdet = typename MatrixType::Scalar(1) / matrix.determinant();
96  }
97 };
98 
99 template<typename MatrixType, typename ResultType>
101 {
103  static inline void run(
104  const MatrixType& matrix,
105  const typename MatrixType::RealScalar& absDeterminantThreshold,
106  ResultType& inverse,
108  bool& invertible
109  )
110  {
111  using std::abs;
112  typedef typename ResultType::Scalar Scalar;
113  determinant = matrix.determinant();
114  invertible = abs(determinant) > absDeterminantThreshold;
115  if(!invertible) return;
116  const Scalar invdet = Scalar(1) / determinant;
118  }
119 };
120 
121 /****************************
122 *** Size 3 implementation ***
123 ****************************/
124 
125 template<typename MatrixType, int i, int j>
128 {
129  enum {
130  i1 = (i+1) % 3,
131  i2 = (i+2) % 3,
132  j1 = (j+1) % 3,
133  j2 = (j+2) % 3
134  };
135  return m.coeff(i1, j1) * m.coeff(i2, j2)
136  - m.coeff(i1, j2) * m.coeff(i2, j1);
137 }
138 
139 template<typename MatrixType, typename ResultType>
142  const MatrixType& matrix,
143  const typename ResultType::Scalar& invdet,
144  const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
145  ResultType& result)
146 {
147  // Compute cofactors in a way that avoids aliasing issues.
148  typedef typename ResultType::Scalar Scalar;
149  const Scalar c01 = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
150  const Scalar c11 = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
151  const Scalar c02 = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
152  result.coeffRef(1,2) = cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
153  result.coeffRef(2,1) = cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
154  result.coeffRef(2,2) = cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
155  result.coeffRef(1,0) = c01;
156  result.coeffRef(1,1) = c11;
157  result.coeffRef(2,0) = c02;
158  result.row(0) = cofactors_col0 * invdet;
159 }
160 
161 template<typename MatrixType, typename ResultType>
162 struct compute_inverse<MatrixType, ResultType, 3>
163 {
165  static inline void run(const MatrixType& matrix, ResultType& result)
166  {
167  typedef typename ResultType::Scalar Scalar;
169  cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
170  cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
171  cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
172  const Scalar det = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
173  const Scalar invdet = Scalar(1) / det;
174  compute_inverse_size3_helper(matrix, invdet, cofactors_col0, result);
175  }
176 };
177 
178 template<typename MatrixType, typename ResultType>
180 {
182  static inline void run(
183  const MatrixType& matrix,
184  const typename MatrixType::RealScalar& absDeterminantThreshold,
185  ResultType& inverse,
187  bool& invertible
188  )
189  {
190  typedef typename ResultType::Scalar Scalar;
191  Matrix<Scalar,3,1> cofactors_col0;
192  cofactors_col0.coeffRef(0) = cofactor_3x3<MatrixType,0,0>(matrix);
193  cofactors_col0.coeffRef(1) = cofactor_3x3<MatrixType,1,0>(matrix);
194  cofactors_col0.coeffRef(2) = cofactor_3x3<MatrixType,2,0>(matrix);
195  determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
196  invertible = Eigen::numext::abs(determinant) > absDeterminantThreshold;
197  if(!invertible) return;
198  const Scalar invdet = Scalar(1) / determinant;
199  compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
200  }
201 };
202 
203 /****************************
204 *** Size 4 implementation ***
205 ****************************/
206 
207 template<typename Derived>
209 inline const typename Derived::Scalar general_det3_helper
210 (const MatrixBase<Derived>& matrix, int i1, int i2, int i3, int j1, int j2, int j3)
211 {
212  return matrix.coeff(i1,j1)
213  * (matrix.coeff(i2,j2) * matrix.coeff(i3,j3) - matrix.coeff(i2,j3) * matrix.coeff(i3,j2));
214 }
215 
216 template<typename MatrixType, int i, int j>
219 {
220  enum {
221  i1 = (i+1) % 4,
222  i2 = (i+2) % 4,
223  i3 = (i+3) % 4,
224  j1 = (j+1) % 4,
225  j2 = (j+2) % 4,
226  j3 = (j+3) % 4
227  };
228  return general_det3_helper(matrix, i1, i2, i3, j1, j2, j3)
229  + general_det3_helper(matrix, i2, i3, i1, j1, j2, j3)
230  + general_det3_helper(matrix, i3, i1, i2, j1, j2, j3);
231 }
232 
233 template<int Arch, typename Scalar, typename MatrixType, typename ResultType>
235 {
237  static void run(const MatrixType& matrix, ResultType& result)
238  {
239  result.coeffRef(0,0) = cofactor_4x4<MatrixType,0,0>(matrix);
240  result.coeffRef(1,0) = -cofactor_4x4<MatrixType,0,1>(matrix);
241  result.coeffRef(2,0) = cofactor_4x4<MatrixType,0,2>(matrix);
242  result.coeffRef(3,0) = -cofactor_4x4<MatrixType,0,3>(matrix);
243  result.coeffRef(0,2) = cofactor_4x4<MatrixType,2,0>(matrix);
244  result.coeffRef(1,2) = -cofactor_4x4<MatrixType,2,1>(matrix);
245  result.coeffRef(2,2) = cofactor_4x4<MatrixType,2,2>(matrix);
246  result.coeffRef(3,2) = -cofactor_4x4<MatrixType,2,3>(matrix);
247  result.coeffRef(0,1) = -cofactor_4x4<MatrixType,1,0>(matrix);
248  result.coeffRef(1,1) = cofactor_4x4<MatrixType,1,1>(matrix);
249  result.coeffRef(2,1) = -cofactor_4x4<MatrixType,1,2>(matrix);
250  result.coeffRef(3,1) = cofactor_4x4<MatrixType,1,3>(matrix);
251  result.coeffRef(0,3) = -cofactor_4x4<MatrixType,3,0>(matrix);
252  result.coeffRef(1,3) = cofactor_4x4<MatrixType,3,1>(matrix);
253  result.coeffRef(2,3) = -cofactor_4x4<MatrixType,3,2>(matrix);
254  result.coeffRef(3,3) = cofactor_4x4<MatrixType,3,3>(matrix);
255  result /= (matrix.col(0).cwiseProduct(result.row(0).transpose())).sum();
256  }
257 };
258 
259 template<typename MatrixType, typename ResultType>
260 struct compute_inverse<MatrixType, ResultType, 4>
261  : compute_inverse_size4<Architecture::Target, typename MatrixType::Scalar,
262  MatrixType, ResultType>
263 {
264 };
265 
266 template<typename MatrixType, typename ResultType>
268 {
270  static inline void run(
271  const MatrixType& matrix,
272  const typename MatrixType::RealScalar& absDeterminantThreshold,
273  ResultType& inverse,
275  bool& invertible
276  )
277  {
278  using std::abs;
279  determinant = matrix.determinant();
280  invertible = abs(determinant) > absDeterminantThreshold;
281  if(invertible && extract_data(matrix) != extract_data(inverse)) {
283  }
284  else if(invertible) {
285  MatrixType matrix_t = matrix;
287  }
288  }
289 };
290 
291 /*************************
292 *** MatrixBase methods ***
293 *************************/
294 
295 } // end namespace internal
296 
297 namespace internal {
298 
299 // Specialization for "dense = dense_xpr.inverse()"
300 template<typename DstXprType, typename XprType>
301 struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense>
302 {
306  {
307  Index dstRows = src.rows();
308  Index dstCols = src.cols();
309  if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
310  dst.resize(dstRows, dstCols);
311 
312  const int Size = EIGEN_PLAIN_ENUM_MIN(XprType::ColsAtCompileTime,DstXprType::ColsAtCompileTime);
314  eigen_assert(( (Size<=1) || (Size>4) || (extract_data(src.nestedExpression())!=extract_data(dst)))
315  && "Aliasing problem detected in inverse(), you need to do inverse().eval() here.");
316 
318  typedef typename internal::remove_all<ActualXprType>::type ActualXprTypeCleanded;
319 
320  ActualXprType actual_xpr(src.nestedExpression());
321 
323  }
324 };
325 
326 
327 } // end namespace internal
328 
346 template<typename Derived>
349 {
350  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
351  eigen_assert(rows() == cols());
352  return Inverse<Derived>(derived());
353 }
354 
375 template<typename Derived>
376 template<typename ResultType>
378  ResultType& inverse,
380  bool& invertible,
381  const RealScalar& absDeterminantThreshold
382  ) const
383 {
384  // i'd love to put some static assertions there, but SFINAE means that they have no effect...
385  eigen_assert(rows() == cols());
386  // for 2x2, it's worth giving a chance to avoid evaluating.
387  // for larger sizes, evaluating has negligible cost and limits code size.
388  typedef typename internal::conditional<
389  RowsAtCompileTime == 2,
394  (derived(), absDeterminantThreshold, inverse, determinant, invertible);
395 }
396 
416 template<typename Derived>
417 template<typename ResultType>
419  ResultType& inverse,
420  bool& invertible,
421  const RealScalar& absDeterminantThreshold
422  ) const
423 {
425  // i'd love to put some static assertions there, but SFINAE means that they have no effect...
426  eigen_assert(rows() == cols());
427  computeInverseAndDetWithCheck(inverse,determinant,invertible,absDeterminantThreshold);
428 }
429 
430 } // end namespace Eigen
431 
432 #endif // EIGEN_INVERSE_IMPL_H
Eigen::Inverse
Expression of the inverse of another expression.
Definition: Inverse.h:43
Eigen::MatrixBase::computeInverseWithCheck
void computeInverseWithCheck(ResultType &inverse, bool &invertible, const RealScalar &absDeterminantThreshold=NumTraits< Scalar >::dummy_precision()) const
Definition: InverseImpl.h:418
Eigen::MatrixBase::computeInverseAndDetWithCheck
void computeInverseAndDetWithCheck(ResultType &inverse, typename ResultType::Scalar &determinant, bool &invertible, const RealScalar &absDeterminantThreshold=NumTraits< Scalar >::dummy_precision()) const
Definition: InverseImpl.h:377
EIGEN_DEVICE_FUNC
#define EIGEN_DEVICE_FUNC
Definition: Macros.h:976
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::internal::Assignment< DstXprType, Inverse< XprType >, internal::assign_op< typename DstXprType::Scalar, typename XprType::Scalar >, Dense2Dense >::run
static EIGEN_DEVICE_FUNC void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op< typename DstXprType::Scalar, typename XprType::Scalar > &)
Definition: InverseImpl.h:305
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
Eigen::internal::compute_inverse
Definition: InverseImpl.h:23
Eigen::internal::compute_inverse_size3_helper
EIGEN_DEVICE_FUNC void compute_inverse_size3_helper(const MatrixType &matrix, const typename ResultType::Scalar &invdet, const Matrix< typename ResultType::Scalar, 3, 1 > &cofactors_col0, ResultType &result)
Definition: InverseImpl.h:141
Eigen::CwiseBinaryOp
Generic expression where a coefficient-wise binary operator is applied to two expressions.
Definition: CwiseBinaryOp.h:77
MatrixType
MatrixXf MatrixType
Definition: benchmark-blocking-sizes.cpp:52
Eigen::DenseBase< Solve< Decomposition, RhsType > >::Scalar
internal::traits< Solve< Decomposition, RhsType > >::Scalar Scalar
Definition: DenseBase.h:66
Eigen::internal::remove_all
Definition: Meta.h:126
Eigen::internal::compute_inverse_and_det_with_check< MatrixType, ResultType, 1 >::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, const typename MatrixType::RealScalar &absDeterminantThreshold, ResultType &result, typename ResultType::Scalar &determinant, bool &invertible)
Definition: InverseImpl.h:55
eigen_assert
#define eigen_assert(x)
Definition: Macros.h:1037
Eigen::internal::Dense2Dense
Definition: AssignEvaluator.h:814
type
Definition: pytypes.h:1525
Eigen::internal::compute_inverse_and_det_with_check< MatrixType, ResultType, 4 >::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, const typename MatrixType::RealScalar &absDeterminantThreshold, ResultType &inverse, typename ResultType::Scalar &determinant, bool &invertible)
Definition: InverseImpl.h:270
result
Values result
Definition: OdometryOptimize.cpp:8
rows
int rows
Definition: Tutorial_commainit_02.cpp:1
Eigen::Inverse::cols
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: Inverse.h:58
determinant
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
EIGEN_ONLY_USED_FOR_DEBUG
#define EIGEN_ONLY_USED_FOR_DEBUG(x)
Definition: Macros.h:1049
Eigen::internal::Assignment< DstXprType, Inverse< XprType >, internal::assign_op< typename DstXprType::Scalar, typename XprType::Scalar >, Dense2Dense >::SrcXprType
Inverse< XprType > SrcXprType
Definition: InverseImpl.h:303
Eigen::internal::compute_inverse_and_det_with_check< MatrixType, ResultType, 3 >::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, const typename MatrixType::RealScalar &absDeterminantThreshold, ResultType &inverse, typename ResultType::Scalar &determinant, bool &invertible)
Definition: InverseImpl.h:182
Eigen::internal::compute_inverse_and_det_with_check< MatrixType, ResultType, 2 >::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, const typename MatrixType::RealScalar &absDeterminantThreshold, ResultType &inverse, typename ResultType::Scalar &determinant, bool &invertible)
Definition: InverseImpl.h:103
EIGEN_PLAIN_ENUM_MIN
#define EIGEN_PLAIN_ENUM_MIN(a, b)
Definition: Macros.h:1288
Eigen::MatrixBase::inverse
const EIGEN_DEVICE_FUNC Inverse< Derived > inverse() const
Definition: InverseImpl.h:348
Eigen::internal::true_type
Definition: Meta.h:96
Eigen::DenseBase< Solve< Decomposition, RhsType > >::RealScalar
NumTraits< Scalar >::Real RealScalar
Definition: DenseBase.h:73
Eigen::internal::general_det3_helper
const EIGEN_DEVICE_FUNC Derived::Scalar general_det3_helper(const MatrixBase< Derived > &matrix, int i1, int i2, int i3, int j1, int j2, int j3)
Definition: InverseImpl.h:210
Eigen::internal::compute_inverse_size4::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, ResultType &result)
Definition: InverseImpl.h:237
Eigen::internal::cofactor_4x4
EIGEN_DEVICE_FUNC MatrixType::Scalar cofactor_4x4(const MatrixType &matrix)
Definition: InverseImpl.h:218
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
Eigen::Inverse::nestedExpression
const EIGEN_DEVICE_FUNC XprTypeNestedCleaned & nestedExpression() const
Definition: Inverse.h:60
Eigen::internal::compute_inverse_size4
Definition: InverseImpl.h:234
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
Eigen::Matrix::coeffRef
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Definition: PlainObjectBase.h:175
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
Eigen::Triplet< double >
Eigen::internal::compute_inverse< MatrixType, ResultType, 2 >::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, ResultType &result)
Definition: InverseImpl.h:91
Eigen::internal::evaluator< MatrixType >
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
Eigen::internal::assign_op
Definition: AssignmentFunctors.h:21
RealScalar
NumTraits< Scalar >::Real RealScalar
Definition: bench_gemm.cpp:47
EIGEN_STATIC_ASSERT
#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
Definition: StaticAssert.h:127
i1
double i1(double x)
Definition: i1.c:150
Eigen::internal::extract_data
EIGEN_DEVICE_FUNC const EIGEN_ALWAYS_INLINE T::Scalar * extract_data(const T &m)
Definition: BlasUtil.h:533
Eigen::Inverse::rows
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: Inverse.h:57
Eigen::internal::conditional
Definition: Meta.h:109
Eigen::internal::Assignment
Definition: AssignEvaluator.h:824
Eigen::internal::compute_inverse< MatrixType, ResultType, 3 >::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, ResultType &result)
Definition: InverseImpl.h:165
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
Eigen::numext::abs
EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE internal::enable_if< NumTraits< T >::IsSigned||NumTraits< T >::IsComplex, typename NumTraits< T >::Real >::type abs(const T &x)
Definition: Eigen/src/Core/MathFunctions.h:1511
abs
#define abs(x)
Definition: datatypes.h:17
internal
Definition: BandTriangularSolver.h:13
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Eigen::internal::compute_inverse_size2_helper
EIGEN_DEVICE_FUNC void compute_inverse_size2_helper(const MatrixType &matrix, const typename ResultType::Scalar &invdet, ResultType &result)
Definition: InverseImpl.h:76
cols
int cols
Definition: Tutorial_commainit_02.cpp:1
j1
double j1(double x)
Definition: j1.c:174
Eigen::internal::compute_inverse::run
static EIGEN_DEVICE_FUNC void run(const MatrixType &matrix, ResultType &result)
Definition: InverseImpl.h:28
Eigen::NumTraits
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:232
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
Eigen::internal::compute_inverse_and_det_with_check
Definition: InverseImpl.h:33
Eigen::internal::cofactor_3x3
EIGEN_DEVICE_FUNC MatrixType::Scalar cofactor_3x3(const MatrixType &m)
Definition: InverseImpl.h:127


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:51