[136] | 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
|
---|