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