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