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
15namespace Eigen {
16
17namespace internal {
18
19template <typename Scalar>
20void covar(
21        Matrix< Scalar, Dynamic, Dynamic > &r,
22        const VectorXi& ipvt,
23        Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) )
24{
25    using std::abs;
26    typedef DenseIndex Index;
27    /* Local variables */
28    Index i, j, k, l, ii, jj;
29    bool sing;
30    Scalar temp;
31
32    /* Function Body */
33    const Index n = r.cols();
34    const Scalar tolr = tol * abs(r(0,0));
35    Matrix< Scalar, Dynamic, 1 > wa(n);
36    eigen_assert(ipvt.size()==n);
37
38    /* form the inverse of r in the full upper triangle of r. */
39    l = -1;
40    for (k = 0; k < n; ++k)
41        if (abs(r(k,k)) > tolr) {
42            r(k,k) = 1. / r(k,k);
43            for (j = 0; j <= k-1; ++j) {
44                temp = r(k,k) * r(j,k);
45                r(j,k) = 0.;
46                r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
47            }
48            l = k;
49        }
50
51    /* form the full upper triangle of the inverse of (r transpose)*r */
52    /* in the full upper triangle of r. */
53    for (k = 0; k <= l; ++k) {
54        for (j = 0; j <= k-1; ++j)
55            r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
56        r.col(k).head(k+1) *= r(k,k);
57    }
58
59    /* form the full lower triangle of the covariance matrix */
60    /* in the strict lower triangle of r and in wa. */
61    for (j = 0; j < n; ++j) {
62        jj = ipvt[j];
63        sing = j > l;
64        for (i = 0; i <= j; ++i) {
65            if (sing)
66                r(i,j) = 0.;
67            ii = ipvt[i];
68            if (ii > jj)
69                r(ii,jj) = r(i,j);
70            if (ii < jj)
71                r(jj,ii) = r(i,j);
72        }
73        wa[jj] = r(j,j);
74    }
75
76    /* symmetrize the covariance matrix in r. */
77    r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
78    r.diagonal() = wa;
79}
80
81} // end namespace internal
82
83} // end namespace Eigen
84
85#endif // EIGEN_LMCOVAR_H
86