85 lines
2.4 KiB
C
85 lines
2.4 KiB
C
|
// This file is part of Eigen, a lightweight C++ template library
|
||
|
|
// for linear algebra.
|
||
|
|
//
|
||
|
|
// This code initially comes from MINPACK whose original authors are:
|
||
|
|
// Copyright Jorge More - Argonne National Laboratory
|
||
|
|
// Copyright Burt Garbow - Argonne National Laboratory
|
||
|
|
// Copyright Ken Hillstrom - Argonne National Laboratory
|
||
|
|
//
|
||
|
|
// This Source Code Form is subject to the terms of the Minpack license
|
||
|
|
// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
|
||
|
|
|
||
|
|
#ifndef EIGEN_LMCOVAR_H
|
||
|
|
#define EIGEN_LMCOVAR_H
|
||
|
|
|
||
|
|
namespace Eigen {
|
||
|
|
|
||
|
|
namespace internal {
|
||
|
|
|
||
|
|
template <typename Scalar>
|
||
|
|
void covar(
|
||
|
|
Matrix< Scalar, Dynamic, Dynamic > &r,
|
||
|
|
const VectorXi& ipvt,
|
||
|
|
Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) )
|
||
|
|
{
|
||
|
|
using std::abs;
|
||
|
|
/* Local variables */
|
||
|
|
Index i, j, k, l, ii, jj;
|
||
|
|
bool sing;
|
||
|
|
Scalar temp;
|
||
|
|
|
||
|
|
/* Function Body */
|
||
|
|
const Index n = r.cols();
|
||
|
|
const Scalar tolr = tol * abs(r(0,0));
|
||
|
|
Matrix< Scalar, Dynamic, 1 > wa(n);
|
||
|
|
eigen_assert(ipvt.size()==n);
|
||
|
|
|
||
|
|
/* form the inverse of r in the full upper triangle of r. */
|
||
|
|
l = -1;
|
||
|
|
for (k = 0; k < n; ++k)
|
||
|
|
if (abs(r(k,k)) > tolr) {
|
||
|
|
r(k,k) = 1. / r(k,k);
|
||
|
|
for (j = 0; j <= k-1; ++j) {
|
||
|
|
temp = r(k,k) * r(j,k);
|
||
|
|
r(j,k) = 0.;
|
||
|
|
r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
|
||
|
|
}
|
||
|
|
l = k;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* form the full upper triangle of the inverse of (r transpose)*r */
|
||
|
|
/* in the full upper triangle of r. */
|
||
|
|
for (k = 0; k <= l; ++k) {
|
||
|
|
for (j = 0; j <= k-1; ++j)
|
||
|
|
r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
|
||
|
|
r.col(k).head(k+1) *= r(k,k);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* form the full lower triangle of the covariance matrix */
|
||
|
|
/* in the strict lower triangle of r and in wa. */
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
jj = ipvt[j];
|
||
|
|
sing = j > l;
|
||
|
|
for (i = 0; i <= j; ++i) {
|
||
|
|
if (sing)
|
||
|
|
r(i,j) = 0.;
|
||
|
|
ii = ipvt[i];
|
||
|
|
if (ii > jj)
|
||
|
|
r(ii,jj) = r(i,j);
|
||
|
|
if (ii < jj)
|
||
|
|
r(jj,ii) = r(i,j);
|
||
|
|
}
|
||
|
|
wa[jj] = r(j,j);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* symmetrize the covariance matrix in r. */
|
||
|
|
r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
|
||
|
|
r.diagonal() = wa;
|
||
|
|
}
|
||
|
|
|
||
|
|
} // end namespace internal
|
||
|
|
|
||
|
|
} // end namespace Eigen
|
||
|
|
|
||
|
|
#endif // EIGEN_LMCOVAR_H
|