mirror of
https://gitlab.com/libeigen/eigen.git
synced 2026-04-10 11:34:33 +08:00
Apply clang-format
This commit is contained in:
Tobias Wood
File diff suppressed because it is too large
Load Diff
@@ -1,3 +1,4 @@
|
||||
#ifndef EIGEN_NONLINEAROPTIMIZATION_MODULE_H
|
||||
#error "Please include unsupported/Eigen/NonLinearOptimization instead of including headers inside the src directory directly."
|
||||
#error \
|
||||
"Please include unsupported/Eigen/NonLinearOptimization instead of including headers inside the src directory directly."
|
||||
#endif
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -4,66 +4,54 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename Scalar>
|
||||
void chkder(
|
||||
const Matrix< Scalar, Dynamic, 1 > &x,
|
||||
const Matrix< Scalar, Dynamic, 1 > &fvec,
|
||||
const Matrix< Scalar, Dynamic, Dynamic > &fjac,
|
||||
Matrix< Scalar, Dynamic, 1 > &xp,
|
||||
const Matrix< Scalar, Dynamic, 1 > &fvecp,
|
||||
int mode,
|
||||
Matrix< Scalar, Dynamic, 1 > &err
|
||||
)
|
||||
{
|
||||
using std::sqrt;
|
||||
using std::abs;
|
||||
using std::log;
|
||||
|
||||
typedef DenseIndex Index;
|
||||
template <typename Scalar>
|
||||
void chkder(const Matrix<Scalar, Dynamic, 1> &x, const Matrix<Scalar, Dynamic, 1> &fvec,
|
||||
const Matrix<Scalar, Dynamic, Dynamic> &fjac, Matrix<Scalar, Dynamic, 1> &xp,
|
||||
const Matrix<Scalar, Dynamic, 1> &fvecp, int mode, Matrix<Scalar, Dynamic, 1> &err) {
|
||||
using std::abs;
|
||||
using std::log;
|
||||
using std::sqrt;
|
||||
|
||||
const Scalar eps = sqrt(NumTraits<Scalar>::epsilon());
|
||||
const Scalar epsf = chkder_factor * NumTraits<Scalar>::epsilon();
|
||||
const Scalar epslog = chkder_log10e * log(eps);
|
||||
Scalar temp;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
const Index m = fvec.size(), n = x.size();
|
||||
const Scalar eps = sqrt(NumTraits<Scalar>::epsilon());
|
||||
const Scalar epsf = chkder_factor * NumTraits<Scalar>::epsilon();
|
||||
const Scalar epslog = chkder_log10e * log(eps);
|
||||
Scalar temp;
|
||||
|
||||
if (mode != 2) {
|
||||
/* mode = 1. */
|
||||
xp.resize(n);
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
temp = eps * abs(x[j]);
|
||||
if (temp == 0.)
|
||||
temp = eps;
|
||||
xp[j] = x[j] + temp;
|
||||
}
|
||||
const Index m = fvec.size(), n = x.size();
|
||||
|
||||
if (mode != 2) {
|
||||
/* mode = 1. */
|
||||
xp.resize(n);
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
temp = eps * abs(x[j]);
|
||||
if (temp == 0.) temp = eps;
|
||||
xp[j] = x[j] + temp;
|
||||
}
|
||||
else {
|
||||
/* mode = 2. */
|
||||
err.setZero(m);
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
temp = abs(x[j]);
|
||||
if (temp == 0.)
|
||||
temp = 1.;
|
||||
err += temp * fjac.col(j);
|
||||
}
|
||||
for (Index i = 0; i < m; ++i) {
|
||||
temp = 1.;
|
||||
if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i]))
|
||||
temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i]));
|
||||
err[i] = 1.;
|
||||
if (temp > NumTraits<Scalar>::epsilon() && temp < eps)
|
||||
err[i] = (chkder_log10e * log(temp) - epslog) / epslog;
|
||||
if (temp >= eps)
|
||||
err[i] = 0.;
|
||||
}
|
||||
} else {
|
||||
/* mode = 2. */
|
||||
err.setZero(m);
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
temp = abs(x[j]);
|
||||
if (temp == 0.) temp = 1.;
|
||||
err += temp * fjac.col(j);
|
||||
}
|
||||
for (Index i = 0; i < m; ++i) {
|
||||
temp = 1.;
|
||||
if (fvec[i] != 0. && fvecp[i] != 0. && abs(fvecp[i] - fvec[i]) >= epsf * abs(fvec[i]))
|
||||
temp = eps * abs((fvecp[i] - fvec[i]) / eps - err[i]) / (abs(fvec[i]) + abs(fvecp[i]));
|
||||
err[i] = 1.;
|
||||
if (temp > NumTraits<Scalar>::epsilon() && temp < eps) err[i] = (chkder_log10e * log(temp) - epslog) / epslog;
|
||||
if (temp >= eps) err[i] = 0.;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,73 +1,66 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
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;
|
||||
typedef DenseIndex Index;
|
||||
void covar(Matrix<Scalar, Dynamic, Dynamic> &r, const VectorXi &ipvt,
|
||||
Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())) {
|
||||
using std::abs;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* Local variables */
|
||||
Index i, j, k, l, ii, jj;
|
||||
bool sing;
|
||||
Scalar temp;
|
||||
/* 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);
|
||||
/* 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 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 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);
|
||||
}
|
||||
/* 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);
|
||||
}
|
||||
|
||||
/* symmetrize the covariance matrix in r. */
|
||||
r.topLeftCorner(n,n).template triangularView<StrictlyUpper>() = r.topLeftCorner(n,n).transpose();
|
||||
r.diagonal() = wa;
|
||||
/* 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 internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,110 +1,103 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template <typename Scalar>
|
||||
void dogleg(
|
||||
const Matrix< Scalar, Dynamic, Dynamic > &qrfac,
|
||||
const Matrix< Scalar, Dynamic, 1 > &diag,
|
||||
const Matrix< Scalar, Dynamic, 1 > &qtb,
|
||||
Scalar delta,
|
||||
Matrix< Scalar, Dynamic, 1 > &x)
|
||||
{
|
||||
using std::abs;
|
||||
using std::sqrt;
|
||||
|
||||
typedef DenseIndex Index;
|
||||
void dogleg(const Matrix<Scalar, Dynamic, Dynamic> &qrfac, const Matrix<Scalar, Dynamic, 1> &diag,
|
||||
const Matrix<Scalar, Dynamic, 1> &qtb, Scalar delta, Matrix<Scalar, Dynamic, 1> &x) {
|
||||
using std::abs;
|
||||
using std::sqrt;
|
||||
|
||||
/* Local variables */
|
||||
Index i, j;
|
||||
Scalar sum, temp, alpha, bnorm;
|
||||
Scalar gnorm, qnorm;
|
||||
Scalar sgnorm;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* Function Body */
|
||||
const Scalar epsmch = NumTraits<Scalar>::epsilon();
|
||||
const Index n = qrfac.cols();
|
||||
eigen_assert(n==qtb.size());
|
||||
eigen_assert(n==x.size());
|
||||
eigen_assert(n==diag.size());
|
||||
Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
|
||||
/* Local variables */
|
||||
Index i, j;
|
||||
Scalar sum, temp, alpha, bnorm;
|
||||
Scalar gnorm, qnorm;
|
||||
Scalar sgnorm;
|
||||
|
||||
/* first, calculate the gauss-newton direction. */
|
||||
for (j = n-1; j >=0; --j) {
|
||||
temp = qrfac(j,j);
|
||||
if (temp == 0.) {
|
||||
temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
|
||||
if (temp == 0.)
|
||||
temp = epsmch;
|
||||
}
|
||||
if (j==n-1)
|
||||
x[j] = qtb[j] / temp;
|
||||
else
|
||||
x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
|
||||
/* Function Body */
|
||||
const Scalar epsmch = NumTraits<Scalar>::epsilon();
|
||||
const Index n = qrfac.cols();
|
||||
eigen_assert(n == qtb.size());
|
||||
eigen_assert(n == x.size());
|
||||
eigen_assert(n == diag.size());
|
||||
Matrix<Scalar, Dynamic, 1> wa1(n), wa2(n);
|
||||
|
||||
/* first, calculate the gauss-newton direction. */
|
||||
for (j = n - 1; j >= 0; --j) {
|
||||
temp = qrfac(j, j);
|
||||
if (temp == 0.) {
|
||||
temp = epsmch * qrfac.col(j).head(j + 1).maxCoeff();
|
||||
if (temp == 0.) temp = epsmch;
|
||||
}
|
||||
if (j == n - 1)
|
||||
x[j] = qtb[j] / temp;
|
||||
else
|
||||
x[j] = (qtb[j] - qrfac.row(j).tail(n - j - 1).dot(x.tail(n - j - 1))) / temp;
|
||||
}
|
||||
|
||||
/* test whether the gauss-newton direction is acceptable. */
|
||||
qnorm = diag.cwiseProduct(x).stableNorm();
|
||||
if (qnorm <= delta)
|
||||
return;
|
||||
/* test whether the gauss-newton direction is acceptable. */
|
||||
qnorm = diag.cwiseProduct(x).stableNorm();
|
||||
if (qnorm <= delta) return;
|
||||
|
||||
// TODO : this path is not tested by Eigen unit tests
|
||||
// TODO : this path is not tested by Eigen unit tests
|
||||
|
||||
/* the gauss-newton direction is not acceptable. */
|
||||
/* next, calculate the scaled gradient direction. */
|
||||
/* the gauss-newton direction is not acceptable. */
|
||||
/* next, calculate the scaled gradient direction. */
|
||||
|
||||
wa1.fill(0.);
|
||||
for (j = 0; j < n; ++j) {
|
||||
wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
|
||||
wa1[j] /= diag[j];
|
||||
wa1.fill(0.);
|
||||
for (j = 0; j < n; ++j) {
|
||||
wa1.tail(n - j) += qrfac.row(j).tail(n - j) * qtb[j];
|
||||
wa1[j] /= diag[j];
|
||||
}
|
||||
|
||||
/* calculate the norm of the scaled gradient and test for */
|
||||
/* the special case in which the scaled gradient is zero. */
|
||||
gnorm = wa1.stableNorm();
|
||||
sgnorm = 0.;
|
||||
alpha = delta / qnorm;
|
||||
if (gnorm == 0.) goto algo_end;
|
||||
|
||||
/* calculate the point along the scaled gradient */
|
||||
/* at which the quadratic is minimized. */
|
||||
wa1.array() /= (diag * gnorm).array();
|
||||
// TODO : once unit tests cover this part,:
|
||||
// wa2 = qrfac.template triangularView<Upper>() * wa1;
|
||||
for (j = 0; j < n; ++j) {
|
||||
sum = 0.;
|
||||
for (i = j; i < n; ++i) {
|
||||
sum += qrfac(j, i) * wa1[i];
|
||||
}
|
||||
wa2[j] = sum;
|
||||
}
|
||||
temp = wa2.stableNorm();
|
||||
sgnorm = gnorm / temp / temp;
|
||||
|
||||
/* calculate the norm of the scaled gradient and test for */
|
||||
/* the special case in which the scaled gradient is zero. */
|
||||
gnorm = wa1.stableNorm();
|
||||
sgnorm = 0.;
|
||||
alpha = delta / qnorm;
|
||||
if (gnorm == 0.)
|
||||
goto algo_end;
|
||||
/* test whether the scaled gradient direction is acceptable. */
|
||||
alpha = 0.;
|
||||
if (sgnorm >= delta) goto algo_end;
|
||||
|
||||
/* calculate the point along the scaled gradient */
|
||||
/* at which the quadratic is minimized. */
|
||||
wa1.array() /= (diag*gnorm).array();
|
||||
// TODO : once unit tests cover this part,:
|
||||
// wa2 = qrfac.template triangularView<Upper>() * wa1;
|
||||
for (j = 0; j < n; ++j) {
|
||||
sum = 0.;
|
||||
for (i = j; i < n; ++i) {
|
||||
sum += qrfac(j,i) * wa1[i];
|
||||
}
|
||||
wa2[j] = sum;
|
||||
}
|
||||
temp = wa2.stableNorm();
|
||||
sgnorm = gnorm / temp / temp;
|
||||
|
||||
/* test whether the scaled gradient direction is acceptable. */
|
||||
alpha = 0.;
|
||||
if (sgnorm >= delta)
|
||||
goto algo_end;
|
||||
|
||||
/* the scaled gradient direction is not acceptable. */
|
||||
/* finally, calculate the point along the dogleg */
|
||||
/* at which the quadratic is minimized. */
|
||||
bnorm = qtb.stableNorm();
|
||||
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
|
||||
temp = temp - delta / qnorm * numext::abs2(sgnorm / delta) + sqrt(numext::abs2(temp - delta / qnorm) + (1.-numext::abs2(delta / qnorm)) * (1.-numext::abs2(sgnorm / delta)));
|
||||
alpha = delta / qnorm * (1. - numext::abs2(sgnorm / delta)) / temp;
|
||||
/* the scaled gradient direction is not acceptable. */
|
||||
/* finally, calculate the point along the dogleg */
|
||||
/* at which the quadratic is minimized. */
|
||||
bnorm = qtb.stableNorm();
|
||||
temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
|
||||
temp = temp - delta / qnorm * numext::abs2(sgnorm / delta) +
|
||||
sqrt(numext::abs2(temp - delta / qnorm) +
|
||||
(1. - numext::abs2(delta / qnorm)) * (1. - numext::abs2(sgnorm / delta)));
|
||||
alpha = delta / qnorm * (1. - numext::abs2(sgnorm / delta)) / temp;
|
||||
algo_end:
|
||||
|
||||
/* form appropriate convex combination of the gauss-newton */
|
||||
/* direction and the scaled gradient direction. */
|
||||
temp = (1.-alpha) * (std::min)(sgnorm,delta);
|
||||
x = temp * wa1 + alpha * x;
|
||||
/* form appropriate convex combination of the gauss-newton */
|
||||
/* direction and the scaled gradient direction. */
|
||||
temp = (1. - alpha) * (std::min)(sgnorm, delta);
|
||||
x = temp * wa1 + alpha * x;
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,82 +1,73 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template<typename FunctorType, typename Scalar>
|
||||
DenseIndex fdjac1(
|
||||
const FunctorType &Functor,
|
||||
Matrix< Scalar, Dynamic, 1 > &x,
|
||||
Matrix< Scalar, Dynamic, 1 > &fvec,
|
||||
Matrix< Scalar, Dynamic, Dynamic > &fjac,
|
||||
DenseIndex ml, DenseIndex mu,
|
||||
Scalar epsfcn)
|
||||
{
|
||||
using std::sqrt;
|
||||
using std::abs;
|
||||
|
||||
typedef DenseIndex Index;
|
||||
template <typename FunctorType, typename Scalar>
|
||||
DenseIndex fdjac1(const FunctorType &Functor, Matrix<Scalar, Dynamic, 1> &x, Matrix<Scalar, Dynamic, 1> &fvec,
|
||||
Matrix<Scalar, Dynamic, Dynamic> &fjac, DenseIndex ml, DenseIndex mu, Scalar epsfcn) {
|
||||
using std::abs;
|
||||
using std::sqrt;
|
||||
|
||||
/* Local variables */
|
||||
Scalar h;
|
||||
Index j, k;
|
||||
Scalar eps, temp;
|
||||
Index msum;
|
||||
int iflag;
|
||||
Index start, length;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* Function Body */
|
||||
const Scalar epsmch = NumTraits<Scalar>::epsilon();
|
||||
const Index n = x.size();
|
||||
eigen_assert(fvec.size()==n);
|
||||
Matrix< Scalar, Dynamic, 1 > wa1(n);
|
||||
Matrix< Scalar, Dynamic, 1 > wa2(n);
|
||||
/* Local variables */
|
||||
Scalar h;
|
||||
Index j, k;
|
||||
Scalar eps, temp;
|
||||
Index msum;
|
||||
int iflag;
|
||||
Index start, length;
|
||||
|
||||
eps = sqrt((std::max)(epsfcn,epsmch));
|
||||
msum = ml + mu + 1;
|
||||
if (msum >= n) {
|
||||
/* computation of dense approximate jacobian. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
temp = x[j];
|
||||
h = eps * abs(temp);
|
||||
if (h == 0.)
|
||||
h = eps;
|
||||
x[j] = temp + h;
|
||||
iflag = Functor(x, wa1);
|
||||
if (iflag < 0)
|
||||
return iflag;
|
||||
x[j] = temp;
|
||||
fjac.col(j) = (wa1-fvec)/h;
|
||||
}
|
||||
/* Function Body */
|
||||
const Scalar epsmch = NumTraits<Scalar>::epsilon();
|
||||
const Index n = x.size();
|
||||
eigen_assert(fvec.size() == n);
|
||||
Matrix<Scalar, Dynamic, 1> wa1(n);
|
||||
Matrix<Scalar, Dynamic, 1> wa2(n);
|
||||
|
||||
}else {
|
||||
/* computation of banded approximate jacobian. */
|
||||
for (k = 0; k < msum; ++k) {
|
||||
for (j = k; (msum<0) ? (j>n): (j<n); j += msum) {
|
||||
wa2[j] = x[j];
|
||||
h = eps * abs(wa2[j]);
|
||||
if (h == 0.) h = eps;
|
||||
x[j] = wa2[j] + h;
|
||||
}
|
||||
iflag = Functor(x, wa1);
|
||||
if (iflag < 0)
|
||||
return iflag;
|
||||
for (j = k; (msum<0) ? (j>n): (j<n); j += msum) {
|
||||
x[j] = wa2[j];
|
||||
h = eps * abs(wa2[j]);
|
||||
if (h == 0.) h = eps;
|
||||
fjac.col(j).setZero();
|
||||
start = std::max<Index>(0,j-mu);
|
||||
length = (std::min)(n-1, j+ml) - start + 1;
|
||||
fjac.col(j).segment(start, length) = ( wa1.segment(start, length)-fvec.segment(start, length))/h;
|
||||
}
|
||||
}
|
||||
eps = sqrt((std::max)(epsfcn, epsmch));
|
||||
msum = ml + mu + 1;
|
||||
if (msum >= n) {
|
||||
/* computation of dense approximate jacobian. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
temp = x[j];
|
||||
h = eps * abs(temp);
|
||||
if (h == 0.) h = eps;
|
||||
x[j] = temp + h;
|
||||
iflag = Functor(x, wa1);
|
||||
if (iflag < 0) return iflag;
|
||||
x[j] = temp;
|
||||
fjac.col(j) = (wa1 - fvec) / h;
|
||||
}
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
/* computation of banded approximate jacobian. */
|
||||
for (k = 0; k < msum; ++k) {
|
||||
for (j = k; (msum < 0) ? (j > n) : (j < n); j += msum) {
|
||||
wa2[j] = x[j];
|
||||
h = eps * abs(wa2[j]);
|
||||
if (h == 0.) h = eps;
|
||||
x[j] = wa2[j] + h;
|
||||
}
|
||||
iflag = Functor(x, wa1);
|
||||
if (iflag < 0) return iflag;
|
||||
for (j = k; (msum < 0) ? (j > n) : (j < n); j += msum) {
|
||||
x[j] = wa2[j];
|
||||
h = eps * abs(wa2[j]);
|
||||
if (h == 0.) h = eps;
|
||||
fjac.col(j).setZero();
|
||||
start = std::max<Index>(0, j - mu);
|
||||
length = (std::min)(n - 1, j + ml) - start + 1;
|
||||
fjac.col(j).segment(start, length) = (wa1.segment(start, length) - fvec.segment(start, length)) / h;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,301 +1,265 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template <typename Scalar>
|
||||
void lmpar(
|
||||
Matrix< Scalar, Dynamic, Dynamic > &r,
|
||||
const VectorXi &ipvt,
|
||||
const Matrix< Scalar, Dynamic, 1 > &diag,
|
||||
const Matrix< Scalar, Dynamic, 1 > &qtb,
|
||||
Scalar delta,
|
||||
Scalar &par,
|
||||
Matrix< Scalar, Dynamic, 1 > &x)
|
||||
{
|
||||
using std::abs;
|
||||
using std::sqrt;
|
||||
typedef DenseIndex Index;
|
||||
void lmpar(Matrix<Scalar, Dynamic, Dynamic> &r, const VectorXi &ipvt, const Matrix<Scalar, Dynamic, 1> &diag,
|
||||
const Matrix<Scalar, Dynamic, 1> &qtb, Scalar delta, Scalar &par, Matrix<Scalar, Dynamic, 1> &x) {
|
||||
using std::abs;
|
||||
using std::sqrt;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* Local variables */
|
||||
Index i, j, l;
|
||||
Scalar fp;
|
||||
Scalar parc, parl;
|
||||
Index iter;
|
||||
Scalar temp, paru;
|
||||
Scalar gnorm;
|
||||
Scalar dxnorm;
|
||||
/* Local variables */
|
||||
Index i, j, l;
|
||||
Scalar fp;
|
||||
Scalar parc, parl;
|
||||
Index iter;
|
||||
Scalar temp, paru;
|
||||
Scalar gnorm;
|
||||
Scalar dxnorm;
|
||||
|
||||
/* Function Body */
|
||||
const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
|
||||
const Index n = r.cols();
|
||||
eigen_assert(n == diag.size());
|
||||
eigen_assert(n == qtb.size());
|
||||
eigen_assert(n == x.size());
|
||||
|
||||
/* Function Body */
|
||||
const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
|
||||
const Index n = r.cols();
|
||||
eigen_assert(n==diag.size());
|
||||
eigen_assert(n==qtb.size());
|
||||
eigen_assert(n==x.size());
|
||||
Matrix<Scalar, Dynamic, 1> wa1, wa2;
|
||||
|
||||
Matrix< Scalar, Dynamic, 1 > wa1, wa2;
|
||||
/* compute and store in x the gauss-newton direction. if the */
|
||||
/* jacobian is rank-deficient, obtain a least squares solution. */
|
||||
Index nsing = n - 1;
|
||||
wa1 = qtb;
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (r(j, j) == 0. && nsing == n - 1) nsing = j - 1;
|
||||
if (nsing < n - 1) wa1[j] = 0.;
|
||||
}
|
||||
for (j = nsing; j >= 0; --j) {
|
||||
wa1[j] /= r(j, j);
|
||||
temp = wa1[j];
|
||||
for (i = 0; i < j; ++i) wa1[i] -= r(i, j) * temp;
|
||||
}
|
||||
|
||||
/* compute and store in x the gauss-newton direction. if the */
|
||||
/* jacobian is rank-deficient, obtain a least squares solution. */
|
||||
Index nsing = n-1;
|
||||
wa1 = qtb;
|
||||
for (j = 0; j < n; ++j) x[ipvt[j]] = wa1[j];
|
||||
|
||||
/* initialize the iteration counter. */
|
||||
/* evaluate the function at the origin, and test */
|
||||
/* for acceptance of the gauss-newton direction. */
|
||||
iter = 0;
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
fp = dxnorm - delta;
|
||||
if (fp <= Scalar(0.1) * delta) {
|
||||
par = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* if the jacobian is not rank deficient, the newton */
|
||||
/* step provides a lower bound, parl, for the zero of */
|
||||
/* the function. otherwise set this bound to zero. */
|
||||
parl = 0.;
|
||||
if (nsing >= n - 1) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (r(j,j) == 0. && nsing == n-1)
|
||||
nsing = j - 1;
|
||||
if (nsing < n-1)
|
||||
wa1[j] = 0.;
|
||||
l = ipvt[j];
|
||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||
}
|
||||
for (j = nsing; j>=0; --j) {
|
||||
wa1[j] /= r(j,j);
|
||||
temp = wa1[j];
|
||||
for (i = 0; i < j ; ++i)
|
||||
wa1[i] -= r(i,j) * temp;
|
||||
// it's actually a triangularView.solveInplace(), though in a weird
|
||||
// way:
|
||||
for (j = 0; j < n; ++j) {
|
||||
Scalar sum = 0.;
|
||||
for (i = 0; i < j; ++i) sum += r(i, j) * wa1[i];
|
||||
wa1[j] = (wa1[j] - sum) / r(j, j);
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parl = fp / delta / temp / temp;
|
||||
}
|
||||
|
||||
for (j = 0; j < n; ++j)
|
||||
x[ipvt[j]] = wa1[j];
|
||||
/* calculate an upper bound, paru, for the zero of the function. */
|
||||
for (j = 0; j < n; ++j) wa1[j] = r.col(j).head(j + 1).dot(qtb.head(j + 1)) / diag[ipvt[j]];
|
||||
|
||||
gnorm = wa1.stableNorm();
|
||||
paru = gnorm / delta;
|
||||
if (paru == 0.) paru = dwarf / (std::min)(delta, Scalar(0.1));
|
||||
|
||||
/* if the input par lies outside of the interval (parl,paru), */
|
||||
/* set par to the closer endpoint. */
|
||||
par = (std::max)(par, parl);
|
||||
par = (std::min)(par, paru);
|
||||
if (par == 0.) par = gnorm / dxnorm;
|
||||
|
||||
/* beginning of an iteration. */
|
||||
while (true) {
|
||||
++iter;
|
||||
|
||||
/* evaluate the function at the current value of par. */
|
||||
if (par == 0.) par = (std::max)(dwarf, Scalar(.001) * paru); /* Computing MAX */
|
||||
wa1 = sqrt(par) * diag;
|
||||
|
||||
Matrix<Scalar, Dynamic, 1> sdiag(n);
|
||||
qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
|
||||
|
||||
/* initialize the iteration counter. */
|
||||
/* evaluate the function at the origin, and test */
|
||||
/* for acceptance of the gauss-newton direction. */
|
||||
iter = 0;
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
temp = fp;
|
||||
fp = dxnorm - delta;
|
||||
if (fp <= Scalar(0.1) * delta) {
|
||||
par = 0;
|
||||
return;
|
||||
|
||||
/* if the function is small enough, accept the current value */
|
||||
/* of par. also test for the exceptional cases where parl */
|
||||
/* is zero or the number of iterations has reached 10. */
|
||||
if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) break;
|
||||
|
||||
/* compute the newton correction. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
l = ipvt[j];
|
||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||
}
|
||||
|
||||
/* if the jacobian is not rank deficient, the newton */
|
||||
/* step provides a lower bound, parl, for the zero of */
|
||||
/* the function. otherwise set this bound to zero. */
|
||||
parl = 0.;
|
||||
if (nsing >= n-1) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
l = ipvt[j];
|
||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||
}
|
||||
// it's actually a triangularView.solveInplace(), though in a weird
|
||||
// way:
|
||||
for (j = 0; j < n; ++j) {
|
||||
Scalar sum = 0.;
|
||||
for (i = 0; i < j; ++i)
|
||||
sum += r(i,j) * wa1[i];
|
||||
wa1[j] = (wa1[j] - sum) / r(j,j);
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parl = fp / delta / temp / temp;
|
||||
for (j = 0; j < n; ++j) {
|
||||
wa1[j] /= sdiag[j];
|
||||
temp = wa1[j];
|
||||
for (i = j + 1; i < n; ++i) wa1[i] -= r(i, j) * temp;
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parc = fp / delta / temp / temp;
|
||||
|
||||
/* calculate an upper bound, paru, for the zero of the function. */
|
||||
for (j = 0; j < n; ++j)
|
||||
wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]];
|
||||
/* depending on the sign of the function, update parl or paru. */
|
||||
if (fp > 0.) parl = (std::max)(parl, par);
|
||||
if (fp < 0.) paru = (std::min)(paru, par);
|
||||
|
||||
gnorm = wa1.stableNorm();
|
||||
paru = gnorm / delta;
|
||||
if (paru == 0.)
|
||||
paru = dwarf / (std::min)(delta,Scalar(0.1));
|
||||
/* compute an improved estimate for par. */
|
||||
/* Computing MAX */
|
||||
par = (std::max)(parl, par + parc);
|
||||
|
||||
/* if the input par lies outside of the interval (parl,paru), */
|
||||
/* set par to the closer endpoint. */
|
||||
par = (std::max)(par,parl);
|
||||
par = (std::min)(par,paru);
|
||||
if (par == 0.)
|
||||
par = gnorm / dxnorm;
|
||||
/* end of an iteration. */
|
||||
}
|
||||
|
||||
/* beginning of an iteration. */
|
||||
while (true) {
|
||||
++iter;
|
||||
|
||||
/* evaluate the function at the current value of par. */
|
||||
if (par == 0.)
|
||||
par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
|
||||
wa1 = sqrt(par)* diag;
|
||||
|
||||
Matrix< Scalar, Dynamic, 1 > sdiag(n);
|
||||
qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
|
||||
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
temp = fp;
|
||||
fp = dxnorm - delta;
|
||||
|
||||
/* if the function is small enough, accept the current value */
|
||||
/* of par. also test for the exceptional cases where parl */
|
||||
/* is zero or the number of iterations has reached 10. */
|
||||
if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
|
||||
break;
|
||||
|
||||
/* compute the newton correction. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
l = ipvt[j];
|
||||
wa1[j] = diag[l] * (wa2[l] / dxnorm);
|
||||
}
|
||||
for (j = 0; j < n; ++j) {
|
||||
wa1[j] /= sdiag[j];
|
||||
temp = wa1[j];
|
||||
for (i = j+1; i < n; ++i)
|
||||
wa1[i] -= r(i,j) * temp;
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parc = fp / delta / temp / temp;
|
||||
|
||||
/* depending on the sign of the function, update parl or paru. */
|
||||
if (fp > 0.)
|
||||
parl = (std::max)(parl,par);
|
||||
if (fp < 0.)
|
||||
paru = (std::min)(paru,par);
|
||||
|
||||
/* compute an improved estimate for par. */
|
||||
/* Computing MAX */
|
||||
par = (std::max)(parl,par+parc);
|
||||
|
||||
/* end of an iteration. */
|
||||
}
|
||||
|
||||
/* termination. */
|
||||
if (iter == 0)
|
||||
par = 0.;
|
||||
return;
|
||||
/* termination. */
|
||||
if (iter == 0) par = 0.;
|
||||
return;
|
||||
}
|
||||
|
||||
template <typename Scalar>
|
||||
void lmpar2(
|
||||
const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr,
|
||||
const Matrix< Scalar, Dynamic, 1 > &diag,
|
||||
const Matrix< Scalar, Dynamic, 1 > &qtb,
|
||||
Scalar delta,
|
||||
Scalar &par,
|
||||
Matrix< Scalar, Dynamic, 1 > &x)
|
||||
void lmpar2(const ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic> > &qr, const Matrix<Scalar, Dynamic, 1> &diag,
|
||||
const Matrix<Scalar, Dynamic, 1> &qtb, Scalar delta, Scalar &par, Matrix<Scalar, Dynamic, 1> &x)
|
||||
|
||||
{
|
||||
using std::sqrt;
|
||||
using std::abs;
|
||||
typedef DenseIndex Index;
|
||||
using std::abs;
|
||||
using std::sqrt;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* Local variables */
|
||||
Index j;
|
||||
Scalar fp;
|
||||
Scalar parc, parl;
|
||||
Index iter;
|
||||
Scalar temp, paru;
|
||||
Scalar gnorm;
|
||||
Scalar dxnorm;
|
||||
/* Local variables */
|
||||
Index j;
|
||||
Scalar fp;
|
||||
Scalar parc, parl;
|
||||
Index iter;
|
||||
Scalar temp, paru;
|
||||
Scalar gnorm;
|
||||
Scalar dxnorm;
|
||||
|
||||
/* Function Body */
|
||||
const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
|
||||
const Index n = qr.matrixQR().cols();
|
||||
eigen_assert(n == diag.size());
|
||||
eigen_assert(n == qtb.size());
|
||||
|
||||
/* Function Body */
|
||||
const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
|
||||
const Index n = qr.matrixQR().cols();
|
||||
eigen_assert(n==diag.size());
|
||||
eigen_assert(n==qtb.size());
|
||||
Matrix<Scalar, Dynamic, 1> wa1, wa2;
|
||||
|
||||
Matrix< Scalar, Dynamic, 1 > wa1, wa2;
|
||||
/* compute and store in x the gauss-newton direction. if the */
|
||||
/* jacobian is rank-deficient, obtain a least squares solution. */
|
||||
|
||||
/* compute and store in x the gauss-newton direction. if the */
|
||||
/* jacobian is rank-deficient, obtain a least squares solution. */
|
||||
// const Index rank = qr.nonzeroPivots(); // exactly double(0.)
|
||||
const Index rank = qr.rank(); // use a threshold
|
||||
wa1 = qtb;
|
||||
wa1.tail(n - rank).setZero();
|
||||
qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
|
||||
|
||||
// const Index rank = qr.nonzeroPivots(); // exactly double(0.)
|
||||
const Index rank = qr.rank(); // use a threshold
|
||||
wa1 = qtb;
|
||||
wa1.tail(n-rank).setZero();
|
||||
qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
|
||||
x = qr.colsPermutation() * wa1;
|
||||
|
||||
x = qr.colsPermutation()*wa1;
|
||||
/* initialize the iteration counter. */
|
||||
/* evaluate the function at the origin, and test */
|
||||
/* for acceptance of the gauss-newton direction. */
|
||||
iter = 0;
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
fp = dxnorm - delta;
|
||||
if (fp <= Scalar(0.1) * delta) {
|
||||
par = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* if the jacobian is not rank deficient, the newton */
|
||||
/* step provides a lower bound, parl, for the zero of */
|
||||
/* the function. otherwise set this bound to zero. */
|
||||
parl = 0.;
|
||||
if (rank == n) {
|
||||
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2) / dxnorm;
|
||||
qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
|
||||
temp = wa1.blueNorm();
|
||||
parl = fp / delta / temp / temp;
|
||||
}
|
||||
|
||||
/* calculate an upper bound, paru, for the zero of the function. */
|
||||
for (j = 0; j < n; ++j)
|
||||
wa1[j] = qr.matrixQR().col(j).head(j + 1).dot(qtb.head(j + 1)) / diag[qr.colsPermutation().indices()(j)];
|
||||
|
||||
gnorm = wa1.stableNorm();
|
||||
paru = gnorm / delta;
|
||||
if (paru == 0.) paru = dwarf / (std::min)(delta, Scalar(0.1));
|
||||
|
||||
/* if the input par lies outside of the interval (parl,paru), */
|
||||
/* set par to the closer endpoint. */
|
||||
par = (std::max)(par, parl);
|
||||
par = (std::min)(par, paru);
|
||||
if (par == 0.) par = gnorm / dxnorm;
|
||||
|
||||
/* beginning of an iteration. */
|
||||
Matrix<Scalar, Dynamic, Dynamic> s = qr.matrixQR();
|
||||
while (true) {
|
||||
++iter;
|
||||
|
||||
/* evaluate the function at the current value of par. */
|
||||
if (par == 0.) par = (std::max)(dwarf, Scalar(.001) * paru); /* Computing MAX */
|
||||
wa1 = sqrt(par) * diag;
|
||||
|
||||
Matrix<Scalar, Dynamic, 1> sdiag(n);
|
||||
qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
|
||||
|
||||
/* initialize the iteration counter. */
|
||||
/* evaluate the function at the origin, and test */
|
||||
/* for acceptance of the gauss-newton direction. */
|
||||
iter = 0;
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
temp = fp;
|
||||
fp = dxnorm - delta;
|
||||
if (fp <= Scalar(0.1) * delta) {
|
||||
par = 0;
|
||||
return;
|
||||
|
||||
/* if the function is small enough, accept the current value */
|
||||
/* of par. also test for the exceptional cases where parl */
|
||||
/* is zero or the number of iterations has reached 10. */
|
||||
if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10) break;
|
||||
|
||||
/* compute the newton correction. */
|
||||
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2 / dxnorm);
|
||||
// we could almost use this here, but the diagonal is outside qr, in sdiag[]
|
||||
// qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
|
||||
for (j = 0; j < n; ++j) {
|
||||
wa1[j] /= sdiag[j];
|
||||
temp = wa1[j];
|
||||
for (Index i = j + 1; i < n; ++i) wa1[i] -= s(i, j) * temp;
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parc = fp / delta / temp / temp;
|
||||
|
||||
/* if the jacobian is not rank deficient, the newton */
|
||||
/* step provides a lower bound, parl, for the zero of */
|
||||
/* the function. otherwise set this bound to zero. */
|
||||
parl = 0.;
|
||||
if (rank==n) {
|
||||
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2)/dxnorm;
|
||||
qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
|
||||
temp = wa1.blueNorm();
|
||||
parl = fp / delta / temp / temp;
|
||||
}
|
||||
/* depending on the sign of the function, update parl or paru. */
|
||||
if (fp > 0.) parl = (std::max)(parl, par);
|
||||
if (fp < 0.) paru = (std::min)(paru, par);
|
||||
|
||||
/* calculate an upper bound, paru, for the zero of the function. */
|
||||
for (j = 0; j < n; ++j)
|
||||
wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
|
||||
|
||||
gnorm = wa1.stableNorm();
|
||||
paru = gnorm / delta;
|
||||
if (paru == 0.)
|
||||
paru = dwarf / (std::min)(delta,Scalar(0.1));
|
||||
|
||||
/* if the input par lies outside of the interval (parl,paru), */
|
||||
/* set par to the closer endpoint. */
|
||||
par = (std::max)(par,parl);
|
||||
par = (std::min)(par,paru);
|
||||
if (par == 0.)
|
||||
par = gnorm / dxnorm;
|
||||
|
||||
/* beginning of an iteration. */
|
||||
Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR();
|
||||
while (true) {
|
||||
++iter;
|
||||
|
||||
/* evaluate the function at the current value of par. */
|
||||
if (par == 0.)
|
||||
par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
|
||||
wa1 = sqrt(par)* diag;
|
||||
|
||||
Matrix< Scalar, Dynamic, 1 > sdiag(n);
|
||||
qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
|
||||
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
temp = fp;
|
||||
fp = dxnorm - delta;
|
||||
|
||||
/* if the function is small enough, accept the current value */
|
||||
/* of par. also test for the exceptional cases where parl */
|
||||
/* is zero or the number of iterations has reached 10. */
|
||||
if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
|
||||
break;
|
||||
|
||||
/* compute the newton correction. */
|
||||
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
|
||||
// we could almost use this here, but the diagonal is outside qr, in sdiag[]
|
||||
// qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
|
||||
for (j = 0; j < n; ++j) {
|
||||
wa1[j] /= sdiag[j];
|
||||
temp = wa1[j];
|
||||
for (Index i = j+1; i < n; ++i)
|
||||
wa1[i] -= s(i,j) * temp;
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parc = fp / delta / temp / temp;
|
||||
|
||||
/* depending on the sign of the function, update parl or paru. */
|
||||
if (fp > 0.)
|
||||
parl = (std::max)(parl,par);
|
||||
if (fp < 0.)
|
||||
paru = (std::min)(paru,par);
|
||||
|
||||
/* compute an improved estimate for par. */
|
||||
par = (std::max)(parl,par+parc);
|
||||
}
|
||||
if (iter == 0)
|
||||
par = 0.;
|
||||
return;
|
||||
/* compute an improved estimate for par. */
|
||||
par = (std::max)(parl, par + parc);
|
||||
}
|
||||
if (iter == 0) par = 0.;
|
||||
return;
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,94 +1,89 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
|
||||
template <typename Scalar>
|
||||
void qrsolv(
|
||||
Matrix< Scalar, Dynamic, Dynamic > &s,
|
||||
// TODO : use a PermutationMatrix once lmpar is no more:
|
||||
const VectorXi &ipvt,
|
||||
const Matrix< Scalar, Dynamic, 1 > &diag,
|
||||
const Matrix< Scalar, Dynamic, 1 > &qtb,
|
||||
Matrix< Scalar, Dynamic, 1 > &x,
|
||||
Matrix< Scalar, Dynamic, 1 > &sdiag)
|
||||
void qrsolv(Matrix<Scalar, Dynamic, Dynamic> &s,
|
||||
// TODO : use a PermutationMatrix once lmpar is no more:
|
||||
const VectorXi &ipvt, const Matrix<Scalar, Dynamic, 1> &diag, const Matrix<Scalar, Dynamic, 1> &qtb,
|
||||
Matrix<Scalar, Dynamic, 1> &x, Matrix<Scalar, Dynamic, 1> &sdiag)
|
||||
|
||||
{
|
||||
typedef DenseIndex Index;
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* Local variables */
|
||||
Index i, j, k, l;
|
||||
Scalar temp;
|
||||
Index n = s.cols();
|
||||
Matrix< Scalar, Dynamic, 1 > wa(n);
|
||||
JacobiRotation<Scalar> givens;
|
||||
/* Local variables */
|
||||
Index i, j, k, l;
|
||||
Scalar temp;
|
||||
Index n = s.cols();
|
||||
Matrix<Scalar, Dynamic, 1> wa(n);
|
||||
JacobiRotation<Scalar> givens;
|
||||
|
||||
/* Function Body */
|
||||
// the following will only change the lower triangular part of s, including
|
||||
// the diagonal, though the diagonal is restored afterward
|
||||
/* Function Body */
|
||||
// the following will only change the lower triangular part of s, including
|
||||
// the diagonal, though the diagonal is restored afterward
|
||||
|
||||
/* copy r and (q transpose)*b to preserve input and initialize s. */
|
||||
/* in particular, save the diagonal elements of r in x. */
|
||||
x = s.diagonal();
|
||||
wa = qtb;
|
||||
/* copy r and (q transpose)*b to preserve input and initialize s. */
|
||||
/* in particular, save the diagonal elements of r in x. */
|
||||
x = s.diagonal();
|
||||
wa = qtb;
|
||||
|
||||
s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
|
||||
s.topLeftCorner(n, n).template triangularView<StrictlyLower>() = s.topLeftCorner(n, n).transpose();
|
||||
|
||||
/* eliminate the diagonal matrix d using a givens rotation. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
/* eliminate the diagonal matrix d using a givens rotation. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
/* prepare the row of d to be eliminated, locating the */
|
||||
/* diagonal element using p from the qr factorization. */
|
||||
l = ipvt[j];
|
||||
if (diag[l] == 0.) break;
|
||||
sdiag.tail(n - j).setZero();
|
||||
sdiag[j] = diag[l];
|
||||
|
||||
/* prepare the row of d to be eliminated, locating the */
|
||||
/* diagonal element using p from the qr factorization. */
|
||||
l = ipvt[j];
|
||||
if (diag[l] == 0.)
|
||||
break;
|
||||
sdiag.tail(n-j).setZero();
|
||||
sdiag[j] = diag[l];
|
||||
/* the transformations to eliminate the row of d */
|
||||
/* modify only a single element of (q transpose)*b */
|
||||
/* beyond the first n, which is initially zero. */
|
||||
Scalar qtbpj = 0.;
|
||||
for (k = j; k < n; ++k) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* appropriate element in the current row of d. */
|
||||
givens.makeGivens(-s(k, k), sdiag[k]);
|
||||
|
||||
/* the transformations to eliminate the row of d */
|
||||
/* modify only a single element of (q transpose)*b */
|
||||
/* beyond the first n, which is initially zero. */
|
||||
Scalar qtbpj = 0.;
|
||||
for (k = j; k < n; ++k) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* appropriate element in the current row of d. */
|
||||
givens.makeGivens(-s(k,k), sdiag[k]);
|
||||
/* compute the modified diagonal element of r and */
|
||||
/* the modified element of ((q transpose)*b,0). */
|
||||
s(k, k) = givens.c() * s(k, k) + givens.s() * sdiag[k];
|
||||
temp = givens.c() * wa[k] + givens.s() * qtbpj;
|
||||
qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
|
||||
wa[k] = temp;
|
||||
|
||||
/* compute the modified diagonal element of r and */
|
||||
/* the modified element of ((q transpose)*b,0). */
|
||||
s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
|
||||
temp = givens.c() * wa[k] + givens.s() * qtbpj;
|
||||
qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
|
||||
wa[k] = temp;
|
||||
|
||||
/* accumulate the transformation in the row of s. */
|
||||
for (i = k+1; i<n; ++i) {
|
||||
temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
|
||||
sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
|
||||
s(i,k) = temp;
|
||||
}
|
||||
}
|
||||
/* accumulate the transformation in the row of s. */
|
||||
for (i = k + 1; i < n; ++i) {
|
||||
temp = givens.c() * s(i, k) + givens.s() * sdiag[i];
|
||||
sdiag[i] = -givens.s() * s(i, k) + givens.c() * sdiag[i];
|
||||
s(i, k) = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* solve the triangular system for z. if the system is */
|
||||
/* singular, then obtain a least squares solution. */
|
||||
Index nsing;
|
||||
for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
|
||||
/* solve the triangular system for z. if the system is */
|
||||
/* singular, then obtain a least squares solution. */
|
||||
Index nsing;
|
||||
for (nsing = 0; nsing < n && sdiag[nsing] != 0; nsing++) {
|
||||
}
|
||||
|
||||
wa.tail(n-nsing).setZero();
|
||||
s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
|
||||
wa.tail(n - nsing).setZero();
|
||||
s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
|
||||
|
||||
// restore
|
||||
sdiag = s.diagonal();
|
||||
s.diagonal() = x;
|
||||
// restore
|
||||
sdiag = s.diagonal();
|
||||
s.diagonal() = x;
|
||||
|
||||
/* permute the components of z back to components of x. */
|
||||
for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
|
||||
/* permute the components of z back to components of x. */
|
||||
for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,33 +1,33 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
// TODO : move this to GivensQR once there's such a thing in Eigen
|
||||
|
||||
template <typename Scalar>
|
||||
void r1mpyq(DenseIndex m, DenseIndex n, Scalar *a, const std::vector<JacobiRotation<Scalar> > &v_givens, const std::vector<JacobiRotation<Scalar> > &w_givens)
|
||||
{
|
||||
typedef DenseIndex Index;
|
||||
void r1mpyq(DenseIndex m, DenseIndex n, Scalar *a, const std::vector<JacobiRotation<Scalar> > &v_givens,
|
||||
const std::vector<JacobiRotation<Scalar> > &w_givens) {
|
||||
typedef DenseIndex Index;
|
||||
|
||||
/* apply the first set of givens rotations to a. */
|
||||
for (Index j = n-2; j>=0; --j)
|
||||
for (Index i = 0; i<m; ++i) {
|
||||
Scalar temp = v_givens[j].c() * a[i+m*j] - v_givens[j].s() * a[i+m*(n-1)];
|
||||
a[i+m*(n-1)] = v_givens[j].s() * a[i+m*j] + v_givens[j].c() * a[i+m*(n-1)];
|
||||
a[i+m*j] = temp;
|
||||
}
|
||||
/* apply the second set of givens rotations to a. */
|
||||
for (Index j = 0; j<n-1; ++j)
|
||||
for (Index i = 0; i<m; ++i) {
|
||||
Scalar temp = w_givens[j].c() * a[i+m*j] + w_givens[j].s() * a[i+m*(n-1)];
|
||||
a[i+m*(n-1)] = -w_givens[j].s() * a[i+m*j] + w_givens[j].c() * a[i+m*(n-1)];
|
||||
a[i+m*j] = temp;
|
||||
}
|
||||
/* apply the first set of givens rotations to a. */
|
||||
for (Index j = n - 2; j >= 0; --j)
|
||||
for (Index i = 0; i < m; ++i) {
|
||||
Scalar temp = v_givens[j].c() * a[i + m * j] - v_givens[j].s() * a[i + m * (n - 1)];
|
||||
a[i + m * (n - 1)] = v_givens[j].s() * a[i + m * j] + v_givens[j].c() * a[i + m * (n - 1)];
|
||||
a[i + m * j] = temp;
|
||||
}
|
||||
/* apply the second set of givens rotations to a. */
|
||||
for (Index j = 0; j < n - 1; ++j)
|
||||
for (Index i = 0; i < m; ++i) {
|
||||
Scalar temp = w_givens[j].c() * a[i + m * j] + w_givens[j].s() * a[i + m * (n - 1)];
|
||||
a[i + m * (n - 1)] = -w_givens[j].s() * a[i + m * j] + w_givens[j].c() * a[i + m * (n - 1)];
|
||||
a[i + m * j] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,102 +1,96 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template <typename Scalar>
|
||||
void r1updt(
|
||||
Matrix< Scalar, Dynamic, Dynamic > &s,
|
||||
const Matrix< Scalar, Dynamic, 1> &u,
|
||||
std::vector<JacobiRotation<Scalar> > &v_givens,
|
||||
std::vector<JacobiRotation<Scalar> > &w_givens,
|
||||
Matrix< Scalar, Dynamic, 1> &v,
|
||||
Matrix< Scalar, Dynamic, 1> &w,
|
||||
bool *sing)
|
||||
{
|
||||
typedef DenseIndex Index;
|
||||
const JacobiRotation<Scalar> IdentityRotation = JacobiRotation<Scalar>(1,0);
|
||||
void r1updt(Matrix<Scalar, Dynamic, Dynamic> &s, const Matrix<Scalar, Dynamic, 1> &u,
|
||||
std::vector<JacobiRotation<Scalar> > &v_givens, std::vector<JacobiRotation<Scalar> > &w_givens,
|
||||
Matrix<Scalar, Dynamic, 1> &v, Matrix<Scalar, Dynamic, 1> &w, bool *sing) {
|
||||
typedef DenseIndex Index;
|
||||
const JacobiRotation<Scalar> IdentityRotation = JacobiRotation<Scalar>(1, 0);
|
||||
|
||||
/* Local variables */
|
||||
const Index m = s.rows();
|
||||
const Index n = s.cols();
|
||||
Index i, j=1;
|
||||
Scalar temp;
|
||||
JacobiRotation<Scalar> givens;
|
||||
/* Local variables */
|
||||
const Index m = s.rows();
|
||||
const Index n = s.cols();
|
||||
Index i, j = 1;
|
||||
Scalar temp;
|
||||
JacobiRotation<Scalar> givens;
|
||||
|
||||
// r1updt had a broader usecase, but we don't use it here. And, more
|
||||
// importantly, we can not test it.
|
||||
eigen_assert(m==n);
|
||||
eigen_assert(u.size()==m);
|
||||
eigen_assert(v.size()==n);
|
||||
eigen_assert(w.size()==n);
|
||||
// r1updt had a broader usecase, but we don't use it here. And, more
|
||||
// importantly, we can not test it.
|
||||
eigen_assert(m == n);
|
||||
eigen_assert(u.size() == m);
|
||||
eigen_assert(v.size() == n);
|
||||
eigen_assert(w.size() == n);
|
||||
|
||||
/* move the nontrivial part of the last column of s into w. */
|
||||
w[n-1] = s(n-1,n-1);
|
||||
/* move the nontrivial part of the last column of s into w. */
|
||||
w[n - 1] = s(n - 1, n - 1);
|
||||
|
||||
/* rotate the vector v into a multiple of the n-th unit vector */
|
||||
/* in such a way that a spike is introduced into w. */
|
||||
for (j=n-2; j>=0; --j) {
|
||||
w[j] = 0.;
|
||||
if (v[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of v. */
|
||||
givens.makeGivens(-v[n-1], v[j]);
|
||||
/* rotate the vector v into a multiple of the n-th unit vector */
|
||||
/* in such a way that a spike is introduced into w. */
|
||||
for (j = n - 2; j >= 0; --j) {
|
||||
w[j] = 0.;
|
||||
if (v[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of v. */
|
||||
givens.makeGivens(-v[n - 1], v[j]);
|
||||
|
||||
/* apply the transformation to v and store the information */
|
||||
/* necessary to recover the givens rotation. */
|
||||
v[n-1] = givens.s() * v[j] + givens.c() * v[n-1];
|
||||
v_givens[j] = givens;
|
||||
/* apply the transformation to v and store the information */
|
||||
/* necessary to recover the givens rotation. */
|
||||
v[n - 1] = givens.s() * v[j] + givens.c() * v[n - 1];
|
||||
v_givens[j] = givens;
|
||||
|
||||
/* apply the transformation to s and extend the spike in w. */
|
||||
for (i = j; i < m; ++i) {
|
||||
temp = givens.c() * s(j,i) - givens.s() * w[i];
|
||||
w[i] = givens.s() * s(j,i) + givens.c() * w[i];
|
||||
s(j,i) = temp;
|
||||
}
|
||||
} else
|
||||
v_givens[j] = IdentityRotation;
|
||||
/* apply the transformation to s and extend the spike in w. */
|
||||
for (i = j; i < m; ++i) {
|
||||
temp = givens.c() * s(j, i) - givens.s() * w[i];
|
||||
w[i] = givens.s() * s(j, i) + givens.c() * w[i];
|
||||
s(j, i) = temp;
|
||||
}
|
||||
} else
|
||||
v_givens[j] = IdentityRotation;
|
||||
}
|
||||
|
||||
/* add the spike from the rank 1 update to w. */
|
||||
w += v[n - 1] * u;
|
||||
|
||||
/* eliminate the spike. */
|
||||
*sing = false;
|
||||
for (j = 0; j < n - 1; ++j) {
|
||||
if (w[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of the spike. */
|
||||
givens.makeGivens(-s(j, j), w[j]);
|
||||
|
||||
/* apply the transformation to s and reduce the spike in w. */
|
||||
for (i = j; i < m; ++i) {
|
||||
temp = givens.c() * s(j, i) + givens.s() * w[i];
|
||||
w[i] = -givens.s() * s(j, i) + givens.c() * w[i];
|
||||
s(j, i) = temp;
|
||||
}
|
||||
|
||||
/* store the information necessary to recover the */
|
||||
/* givens rotation. */
|
||||
w_givens[j] = givens;
|
||||
} else
|
||||
v_givens[j] = IdentityRotation;
|
||||
|
||||
/* test for zero diagonal elements in the output s. */
|
||||
if (s(j, j) == 0.) {
|
||||
*sing = true;
|
||||
}
|
||||
}
|
||||
/* move w back into the last column of the output s. */
|
||||
s(n - 1, n - 1) = w[n - 1];
|
||||
|
||||
/* add the spike from the rank 1 update to w. */
|
||||
w += v[n-1] * u;
|
||||
|
||||
/* eliminate the spike. */
|
||||
*sing = false;
|
||||
for (j = 0; j < n-1; ++j) {
|
||||
if (w[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of the spike. */
|
||||
givens.makeGivens(-s(j,j), w[j]);
|
||||
|
||||
/* apply the transformation to s and reduce the spike in w. */
|
||||
for (i = j; i < m; ++i) {
|
||||
temp = givens.c() * s(j,i) + givens.s() * w[i];
|
||||
w[i] = -givens.s() * s(j,i) + givens.c() * w[i];
|
||||
s(j,i) = temp;
|
||||
}
|
||||
|
||||
/* store the information necessary to recover the */
|
||||
/* givens rotation. */
|
||||
w_givens[j] = givens;
|
||||
} else
|
||||
v_givens[j] = IdentityRotation;
|
||||
|
||||
/* test for zero diagonal elements in the output s. */
|
||||
if (s(j,j) == 0.) {
|
||||
*sing = true;
|
||||
}
|
||||
}
|
||||
/* move w back into the last column of the output s. */
|
||||
s(n-1,n-1) = w[n-1];
|
||||
|
||||
if (s(j,j) == 0.) {
|
||||
*sing = true;
|
||||
}
|
||||
return;
|
||||
if (s(j, j) == 0.) {
|
||||
*sing = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
@@ -1,52 +1,47 @@
|
||||
// IWYU pragma: private
|
||||
#include "./InternalHeaderCheck.h"
|
||||
|
||||
namespace Eigen {
|
||||
namespace Eigen {
|
||||
|
||||
namespace internal {
|
||||
|
||||
template <typename Scalar>
|
||||
void rwupdt(
|
||||
Matrix< Scalar, Dynamic, Dynamic > &r,
|
||||
const Matrix< Scalar, Dynamic, 1> &w,
|
||||
Matrix< Scalar, Dynamic, 1> &b,
|
||||
Scalar alpha)
|
||||
{
|
||||
typedef DenseIndex Index;
|
||||
void rwupdt(Matrix<Scalar, Dynamic, Dynamic> &r, const Matrix<Scalar, Dynamic, 1> &w, Matrix<Scalar, Dynamic, 1> &b,
|
||||
Scalar alpha) {
|
||||
typedef DenseIndex Index;
|
||||
|
||||
const Index n = r.cols();
|
||||
eigen_assert(r.rows()>=n);
|
||||
std::vector<JacobiRotation<Scalar> > givens(n);
|
||||
const Index n = r.cols();
|
||||
eigen_assert(r.rows() >= n);
|
||||
std::vector<JacobiRotation<Scalar> > givens(n);
|
||||
|
||||
/* Local variables */
|
||||
Scalar temp, rowj;
|
||||
/* Local variables */
|
||||
Scalar temp, rowj;
|
||||
|
||||
/* Function Body */
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
rowj = w[j];
|
||||
/* Function Body */
|
||||
for (Index j = 0; j < n; ++j) {
|
||||
rowj = w[j];
|
||||
|
||||
/* apply the previous transformations to */
|
||||
/* r(i,j), i=0,1,...,j-1, and to w(j). */
|
||||
for (Index i = 0; i < j; ++i) {
|
||||
temp = givens[i].c() * r(i,j) + givens[i].s() * rowj;
|
||||
rowj = -givens[i].s() * r(i,j) + givens[i].c() * rowj;
|
||||
r(i,j) = temp;
|
||||
}
|
||||
|
||||
/* determine a givens rotation which eliminates w(j). */
|
||||
givens[j].makeGivens(-r(j,j), rowj);
|
||||
|
||||
if (rowj == 0.)
|
||||
continue; // givens[j] is identity
|
||||
|
||||
/* apply the current transformation to r(j,j), b(j), and alpha. */
|
||||
r(j,j) = givens[j].c() * r(j,j) + givens[j].s() * rowj;
|
||||
temp = givens[j].c() * b[j] + givens[j].s() * alpha;
|
||||
alpha = -givens[j].s() * b[j] + givens[j].c() * alpha;
|
||||
b[j] = temp;
|
||||
/* apply the previous transformations to */
|
||||
/* r(i,j), i=0,1,...,j-1, and to w(j). */
|
||||
for (Index i = 0; i < j; ++i) {
|
||||
temp = givens[i].c() * r(i, j) + givens[i].s() * rowj;
|
||||
rowj = -givens[i].s() * r(i, j) + givens[i].c() * rowj;
|
||||
r(i, j) = temp;
|
||||
}
|
||||
|
||||
/* determine a givens rotation which eliminates w(j). */
|
||||
givens[j].makeGivens(-r(j, j), rowj);
|
||||
|
||||
if (rowj == 0.) continue; // givens[j] is identity
|
||||
|
||||
/* apply the current transformation to r(j,j), b(j), and alpha. */
|
||||
r(j, j) = givens[j].c() * r(j, j) + givens[j].s() * rowj;
|
||||
temp = givens[j].c() * b[j] + givens[j].s() * alpha;
|
||||
alpha = -givens[j].s() * b[j] + givens[j].c() * alpha;
|
||||
b[j] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
} // end namespace internal
|
||||
} // end namespace internal
|
||||
|
||||
} // end namespace Eigen
|
||||
} // end namespace Eigen
|
||||
|
||||
Reference in New Issue
Block a user