1 namespace Eigen { 2 3 namespace internal { 4 5 template <typename Scalar> 6 void covar( 7 Matrix< Scalar, Dynamic, Dynamic > &r, 8 const VectorXi &ipvt, 9 Scalar tol = sqrt(NumTraits<Scalar>::epsilon()) ) 10 { 11 typedef DenseIndex Index; 12 13 /* Local variables */ 14 Index i, j, k, l, ii, jj; 15 bool sing; 16 Scalar temp; 17 18 /* Function Body */ 19 const Index n = r.cols(); 20 const Scalar tolr = tol * abs(r(0,0)); 21 Matrix< Scalar, Dynamic, 1 > wa(n); 22 assert(ipvt.size()==n); 23 24 /* form the inverse of r in the full upper triangle of r. */ 25 l = -1; 26 for (k = 0; k < n; ++k) 27 if (abs(r(k,k)) > tolr) { 28 r(k,k) = 1. / r(k,k); 29 for (j = 0; j <= k-1; ++j) { 30 temp = r(k,k) * r(j,k); 31 r(j,k) = 0.; 32 r.col(k).head(j+1) -= r.col(j).head(j+1) * temp; 33 } 34 l = k; 35 } 36 37 /* form the full upper triangle of the inverse of (r transpose)*r */ 38 /* in the full upper triangle of r. */ 39 for (k = 0; k <= l; ++k) { 40 for (j = 0; j <= k-1; ++j) 41 r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k); 42 r.col(k).head(k+1) *= r(k,k); 43 } 44 45 /* form the full lower triangle of the covariance matrix */ 46 /* in the strict lower triangle of r and in wa. */ 47 for (j = 0; j < n; ++j) { 48 jj = ipvt[j]; 49 sing = j > l; 50 for (i = 0; i <= j; ++i) { 51 if (sing) 52 r(i,j) = 0.; 53 ii = ipvt[i]; 54 if (ii > jj) 55 r(ii,jj) = r(i,j); 56 if (ii < jj) 57 r(jj,ii) = r(i,j); 58 } 59 wa[jj] = r(j,j); 60 } 61 62 /* symmetrize the covariance matrix in r. */ 63 r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose(); 64 r.diagonal() = wa; 65 } 66 67 } // end namespace internal 68 69 } // end namespace Eigen 70
