covar.h revision 7faaa9f3f0df9d23790277834d426c3d992ac3ba 
1namespace Eigen {
2
3namespace internal {
4
5template <typename Scalar>
6void 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