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 |
|
---|
15 | namespace Eigen {
|
---|
16 |
|
---|
17 | namespace internal {
|
---|
18 |
|
---|
19 | template <typename Scalar>
|
---|
20 | void 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
|
---|