covar.h
1 namespace Eigen {
2 
3 namespace internal {
4 
5 template <typename Scalar>
6 void covar(
7  Matrix< Scalar, Dynamic, Dynamic > &r,
8  const VectorXi &ipvt,
9  Scalar tol = sqrt(NumTraits<Scalar>::epsilon()) )
10 {
11  typedef DenseIndex Index;
12 
13  /* Local variables */
14  Index i, j, k, l, ii, jj;
15  bool sing;
16  Scalar temp;
17 
18  /* Function Body */
19  const Index n = r.cols();
20  const Scalar tolr = tol * abs(r(0,0));
21  Matrix< Scalar, Dynamic, 1 > wa(n);
22  assert(ipvt.size()==n);
23 
24  /* form the inverse of r in the full upper triangle of r. */
25  l = -1;
26  for (k = 0; k < n; ++k)
27  if (abs(r(k,k)) > tolr) {
28  r(k,k) = 1. / r(k,k);
29  for (j = 0; j <= k-1; ++j) {
30  temp = r(k,k) * r(j,k);
31  r(j,k) = 0.;
32  r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
33  }
34  l = k;
35  }
36 
37  /* form the full upper triangle of the inverse of (r transpose)*r */
38  /* in the full upper triangle of r. */
39  for (k = 0; k <= l; ++k) {
40  for (j = 0; j <= k-1; ++j)
41  r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
42  r.col(k).head(k+1) *= r(k,k);
43  }
44 
45  /* form the full lower triangle of the covariance matrix */
46  /* in the strict lower triangle of r and in wa. */
47  for (j = 0; j < n; ++j) {
48  jj = ipvt[j];
49  sing = j > l;
50  for (i = 0; i <= j; ++i) {
51  if (sing)
52  r(i,j) = 0.;
53  ii = ipvt[i];
54  if (ii > jj)
55  r(ii,jj) = r(i,j);
56  if (ii < jj)
57  r(jj,ii) = r(i,j);
58  }
59  wa[jj] = r(j,j);
60  }
61 
62  /* symmetrize the covariance matrix in r. */
63  r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
64  r.diagonal() = wa;
65 }
66 
67 } // end namespace internal
68 
69 } // end namespace Eigen