covar.h
Go to the documentation of this file.
1 namespace Eigen {
2 
3 namespace internal {
4 
5 template <typename Scalar>
6 void covar(
8  const VectorXi &ipvt,
10 {
11  using std::abs;
12  typedef DenseIndex Index;
13 
14  /* Local variables */
15  Index i, j, k, l, ii, jj;
16  bool sing;
17  Scalar temp;
18 
19  /* Function Body */
20  const Index n = r.cols();
21  const Scalar tolr = tol * abs(r(0,0));
23  eigen_assert(ipvt.size()==n);
24 
25  /* form the inverse of r in the full upper triangle of r. */
26  l = -1;
27  for (k = 0; k < n; ++k)
28  if (abs(r(k,k)) > tolr) {
29  r(k,k) = 1. / r(k,k);
30  for (j = 0; j <= k-1; ++j) {
31  temp = r(k,k) * r(j,k);
32  r(j,k) = 0.;
33  r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
34  }
35  l = k;
36  }
37 
38  /* form the full upper triangle of the inverse of (r transpose)*r */
39  /* in the full upper triangle of r. */
40  for (k = 0; k <= l; ++k) {
41  for (j = 0; j <= k-1; ++j)
42  r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
43  r.col(k).head(k+1) *= r(k,k);
44  }
45 
46  /* form the full lower triangle of the covariance matrix */
47  /* in the strict lower triangle of r and in wa. */
48  for (j = 0; j < n; ++j) {
49  jj = ipvt[j];
50  sing = j > l;
51  for (i = 0; i <= j; ++i) {
52  if (sing)
53  r(i,j) = 0.;
54  ii = ipvt[i];
55  if (ii > jj)
56  r(ii,jj) = r(i,j);
57  if (ii < jj)
58  r(jj,ii) = r(i,j);
59  }
60  wa[jj] = r(j,j);
61  }
62 
63  /* symmetrize the covariance matrix in r. */
64  r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
65  r.diagonal() = wa;
66 }
67 
68 } // end namespace internal
69 
70 } // end namespace Eigen
SCALAR Scalar
Definition: bench_gemm.cpp:46
int n
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
static double epsilon
Definition: testRot3.cpp:37
static const Line3 l(Rot3(), 1, 1)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
#define eigen_assert(x)
Definition: Macros.h:1037
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: Meta.h:66
void covar(Matrix< Scalar, Dynamic, Dynamic > &r, const VectorXi &ipvt, Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
Definition: LMcovar.h:20
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
#define abs(x)
Definition: datatypes.h:17
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:06