LMcovar.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 // This code initially comes from MINPACK whose original authors are:
5 // Copyright Jorge More - Argonne National Laboratory
6 // Copyright Burt Garbow - Argonne National Laboratory
7 // Copyright Ken Hillstrom - Argonne National Laboratory
8 //
9 // This Source Code Form is subject to the terms of the Minpack license
10 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
11 
12 #ifndef EIGEN_LMCOVAR_H
13 #define EIGEN_LMCOVAR_H
14 
15 namespace Eigen {
16 
17 namespace internal {
18 
19 template <typename Scalar>
20 void covar(
22  const VectorXi& ipvt,
24 {
25  using std::abs;
26  /* Local variables */
27  Index i, j, k, l, ii, jj;
28  bool sing;
29  Scalar temp;
30 
31  /* Function Body */
32  const Index n = r.cols();
33  const Scalar tolr = tol * abs(r(0,0));
35  eigen_assert(ipvt.size()==n);
36 
37  /* form the inverse of r in the full upper triangle of r. */
38  l = -1;
39  for (k = 0; k < n; ++k)
40  if (abs(r(k,k)) > tolr) {
41  r(k,k) = 1. / r(k,k);
42  for (j = 0; j <= k-1; ++j) {
43  temp = r(k,k) * r(j,k);
44  r(j,k) = 0.;
45  r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
46  }
47  l = k;
48  }
49 
50  /* form the full upper triangle of the inverse of (r transpose)*r */
51  /* in the full upper triangle of r. */
52  for (k = 0; k <= l; ++k) {
53  for (j = 0; j <= k-1; ++j)
54  r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
55  r.col(k).head(k+1) *= r(k,k);
56  }
57 
58  /* form the full lower triangle of the covariance matrix */
59  /* in the strict lower triangle of r and in wa. */
60  for (j = 0; j < n; ++j) {
61  jj = ipvt[j];
62  sing = j > l;
63  for (i = 0; i <= j; ++i) {
64  if (sing)
65  r(i,j) = 0.;
66  ii = ipvt[i];
67  if (ii > jj)
68  r(ii,jj) = r(i,j);
69  if (ii < jj)
70  r(jj,ii) = r(i,j);
71  }
72  wa[jj] = r(j,j);
73  }
74 
75  /* symmetrize the covariance matrix in r. */
76  r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
77  r.diagonal() = wa;
78 }
79 
80 } // end namespace internal
81 
82 } // end namespace Eigen
83 
84 #endif // EIGEN_LMCOVAR_H
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const
SCALAR Scalar
Definition: bench_gemm.cpp:33
int n
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:150
static const Line3 l(Rot3(), 1, 1)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:33
#define eigen_assert(x)
Definition: Macros.h:579
void covar(Matrix< Scalar, Dynamic, Dynamic > &r, const VectorXi &ipvt, Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
Definition: LMcovar.h:20
const G double tol
Definition: Group.h:83
#define abs(x)
Definition: datatypes.h:17
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:32