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 = std::sqrt(NumTraits<Scalar>::epsilon()) )
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));
22     Matrix< Scalar, Dynamic, 1 > wa(n);
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
71