src/LevenbergMarquardt/LMcovar.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // This code initially comes from MINPACK whose original authors are:
0005 // Copyright Jorge More - Argonne National Laboratory
0006 // Copyright Burt Garbow - Argonne National Laboratory
0007 // Copyright Ken Hillstrom - Argonne National Laboratory
0008 //
0009 // This Source Code Form is subject to the terms of the Minpack license
0010 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
0011
0012 #ifndef EIGEN_LMCOVAR_H
0013 #define EIGEN_LMCOVAR_H
0014
0015 namespace Eigen {
0016
0017 namespace internal {
0018
0019 template <typename Scalar>
0020 void covar(
0021         Matrix< Scalar, Dynamic, Dynamic > &r,
0022         const VectorXi& ipvt,
0023         Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) )
0024 {
0025     using std::abs;
0026     /* Local variables */
0027     Index i, j, k, l, ii, jj;
0028     bool sing;
0029     Scalar temp;
0030
0031     /* Function Body */
0032     const Index n = r.cols();
0033     const Scalar tolr = tol * abs(r(0,0));
0034     Matrix< Scalar, Dynamic, 1 > wa(n);
0035     eigen_assert(ipvt.size()==n);
0036
0037     /* form the inverse of r in the full upper triangle of r. */
0038     l = -1;
0039     for (k = 0; k < n; ++k)
0040         if (abs(r(k,k)) > tolr) {
0041             r(k,k) = 1. / r(k,k);
0042             for (j = 0; j <= k-1; ++j) {
0043                 temp = r(k,k) * r(j,k);
0044                 r(j,k) = 0.;
0045                 r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
0046             }
0047             l = k;
0048         }
0049
0050     /* form the full upper triangle of the inverse of (r transpose)*r */
0051     /* in the full upper triangle of r. */
0052     for (k = 0; k <= l; ++k) {
0053         for (j = 0; j <= k-1; ++j)
0054             r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
0055         r.col(k).head(k+1) *= r(k,k);
0056     }
0057
0058     /* form the full lower triangle of the covariance matrix */
0059     /* in the strict lower triangle of r and in wa. */
0060     for (j = 0; j < n; ++j) {
0061         jj = ipvt[j];
0062         sing = j > l;
0063         for (i = 0; i <= j; ++i) {
0064             if (sing)
0065                 r(i,j) = 0.;
0066             ii = ipvt[i];
0067             if (ii > jj)
0068                 r(ii,jj) = r(i,j);
0069             if (ii < jj)
0070                 r(jj,ii) = r(i,j);
0071         }
0072         wa[jj] = r(j,j);
0073     }
0074
0075     /* symmetrize the covariance matrix in r. */
0076     r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
0077     r.diagonal() = wa;
0078 }
0079
0080 } // end namespace internal
0081
0082 } // end namespace Eigen
0083
0084 #endif // EIGEN_LMCOVAR_H