Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 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 |