covar.h
Go to the documentation of this file.
00001 namespace internal {
00002 
00003 template <typename Scalar>
00004 void covar(
00005         Matrix< Scalar, Dynamic, Dynamic > &r,
00006         const VectorXi &ipvt,
00007         Scalar tol = sqrt(NumTraits<Scalar>::epsilon()) )
00008 {
00009     typedef DenseIndex Index;
00010 
00011     /* Local variables */
00012     Index i, j, k, l, ii, jj;
00013     bool sing;
00014     Scalar temp;
00015 
00016     /* Function Body */
00017     const Index n = r.cols();
00018     const Scalar tolr = tol * abs(r(0,0));
00019     Matrix< Scalar, Dynamic, 1 > wa(n);
00020     assert(ipvt.size()==n);
00021 
00022     /* form the inverse of r in the full upper triangle of r. */
00023     l = -1;
00024     for (k = 0; k < n; ++k)
00025         if (abs(r(k,k)) > tolr) {
00026             r(k,k) = 1. / r(k,k);
00027             for (j = 0; j <= k-1; ++j) {
00028                 temp = r(k,k) * r(j,k);
00029                 r(j,k) = 0.;
00030                 r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
00031             }
00032             l = k;
00033         }
00034 
00035     /* form the full upper triangle of the inverse of (r transpose)*r */
00036     /* in the full upper triangle of r. */
00037     for (k = 0; k <= l; ++k) {
00038         for (j = 0; j <= k-1; ++j)
00039             r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
00040         r.col(k).head(k+1) *= r(k,k);
00041     }
00042 
00043     /* form the full lower triangle of the covariance matrix */
00044     /* in the strict lower triangle of r and in wa. */
00045     for (j = 0; j < n; ++j) {
00046         jj = ipvt[j];
00047         sing = j > l;
00048         for (i = 0; i <= j; ++i) {
00049             if (sing)
00050                 r(i,j) = 0.;
00051             ii = ipvt[i];
00052             if (ii > jj)
00053                 r(ii,jj) = r(i,j);
00054             if (ii < jj)
00055                 r(jj,ii) = r(i,j);
00056         }
00057         wa[jj] = r(j,j);
00058     }
00059 
00060     /* symmetrize the covariance matrix in r. */
00061     r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
00062     r.diagonal() = wa;
00063 }
00064 
00065 } // end namespace internal


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:30:59