mirror/eigen
2
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2024-12-15 07:10:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

misc cleaning / eigenization

Browse Source
This commit is contained in:
Thomas Capricelli 2010-01-28 04:19:39 +01:00
parent fcd074c928
commit 40eac2d8a0
8 changed files with 44 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
View File

@ -238,8 +238,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x);
xnorm = wa3.stableNorm();
xnorm = diag.cwiseProduct(x).stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
@ -269,8 +268,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1);
pnorm = wa3.stableNorm();
pnorm = diag.cwiseProduct(wa1).stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
@ -489,8 +487,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x);
xnorm = wa3.stableNorm();
xnorm = diag.cwiseProduct(x).stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
@ -520,8 +517,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1);
pnorm = wa3.stableNorm();
pnorm = diag.cwiseProduct(wa1).stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)

12
unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
View File

@ -261,8 +261,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x);
xnorm = wa3.stableNorm();
xnorm = diag.cwiseProduct(x).stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
@ -297,8 +296,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1);
pnorm = wa3.stableNorm();
pnorm = diag.cwiseProduct(wa1).stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)
@ -515,8 +513,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
/* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x);
xnorm = wa3.stableNorm();
xnorm = diag.cwiseProduct(x).stableNorm();
delta = parameters.factor * xnorm;
if (delta == 0.)
delta = parameters.factor;
@ -545,8 +542,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
/* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1;
wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1);
pnorm = wa3.stableNorm();
pnorm = diag.cwiseProduct(wa1).stableNorm();
/* on the first iteration, adjust the initial step bound. */
if (iter == 1)

8
unsupported/Eigen/src/NonLinearOptimization/chkder.h
View File

@ -4,11 +4,11 @@
template<typename Scalar>
void ei_chkder(
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec,
Matrix< Scalar, Dynamic, Dynamic > &fjac,
const Matrix< Scalar, Dynamic, 1 > &x,
const Matrix< Scalar, Dynamic, 1 > &fvec,
const Matrix< Scalar, Dynamic, Dynamic > &fjac,
Matrix< Scalar, Dynamic, 1 > &xp,
Matrix< Scalar, Dynamic, 1 > &fvecp,
const Matrix< Scalar, Dynamic, 1 > &fvecp,
int mode,
Matrix< Scalar, Dynamic, 1 > &err
)

33
unsupported/Eigen/src/NonLinearOptimization/covar.h
View File

@ -16,7 +16,7 @@ void ei_covar(
Matrix< Scalar, Dynamic, 1 > wa(n);
assert(ipvt.size()==n);
/* form the inverse of r in the full upper triangle of r. */
/* form the inverse of r in the full upper triangle of r. */
l = -1;
for (k = 0; k < n; ++k)
if (ei_abs(r(k,k)) > tolr) {
@ -24,27 +24,21 @@ void ei_covar(
for (j = 0; j <= k-1; ++j) {
temp = r(k,k) * r(j,k);
r(j,k) = 0.;
for (i = 0; i <= j; ++i)
r(i,k) -= temp * r(i,j);
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. */
/* 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) {
temp = r(j,k);
for (i = 0; i <= j; ++i)
r(i,j) += temp * r(i,k);
}
temp = r(k,k);
for (i = 0; i <= k; ++i)
r(i,k) = temp * r(i,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. */
/* 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;
@ -60,11 +54,8 @@ void ei_covar(
wa[jj] = r(j,j);
}
/* symmetrize the covariance matrix in r. */
for (j = 0; j < n; ++j) {
for (i = 0; i <= j; ++i)
r(i,j) = r(j,i);
r(j,j) = wa[j];
}
/* symmetrize the covariance matrix in r. */
r.corner(TopLeft,n,n).template triangularView<StrictlyUpper>() = r.corner(TopLeft,n,n).transpose();
r.diagonal() = wa;
}

7
unsupported/Eigen/src/NonLinearOptimization/dogleg.h
View File

@ -36,8 +36,7 @@ void ei_dogleg(
}
/* test whether the gauss-newton direction is acceptable. */
wa2 = diag.cwiseProduct(x);
qnorm = wa2.stableNorm();
qnorm = diag.cwiseProduct(x).stableNorm();
if (qnorm <= delta)
return;
@ -48,9 +47,7 @@ void ei_dogleg(
wa1.fill(0.);
for (j = 0; j < n; ++j) {
temp = qtb[j];
for (i = j; i < n; ++i)
wa1[i] += qrfac(j,i) * temp;
wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
wa1[j] /= diag[j];
}

12
unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
View File

@ -10,10 +10,11 @@ int ei_fdjac1(
{
/* Local variables */
Scalar h;
int i, j, k;
int j, k;
Scalar eps, temp;
int msum;
int iflag;
int start, length;
/* Function Body */
const Scalar epsmch = epsilon<Scalar>();
@ -55,11 +56,10 @@ int ei_fdjac1(
x[j] = wa2[j];
h = eps * ei_abs(wa2[j]);
if (h == 0.) h = eps;
for (i = 0; i < n; ++i) {
fjac(i,j) = 0.;
if (i >= j - mu && i <= j + ml)
fjac(i,j) = (wa1[i] - fvec[i]) / h;
}
fjac.col(j).setZero();
start = std::max(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;
}
}
}

10
unsupported/Eigen/src/NonLinearOptimization/lmpar.h
View File

@ -164,7 +164,7 @@ void ei_lmpar2(
{
/* Local variables */
int i, j;
int j;
Scalar fp;
Scalar parc, parl;
int iter;
@ -183,9 +183,11 @@ void ei_lmpar2(
/* compute and store in x the gauss-newton direction. if the */
/* jacobian is rank-deficient, obtain a least squares solution. */
// const int rank = qr.nonzeroPivots(); // exactly double(0.)
const int rank = qr.rank(); // use a threshold
wa1 = qtb; wa1.segment(rank,n-rank).setZero();
wa1 = qtb;
wa1.tail(n-rank).setZero();
qr.matrixQR().corner(TopLeft, rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
x = qr.colsPermutation()*wa1;
@ -255,10 +257,12 @@ void ei_lmpar2(
/* 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().corner(TopLeft, n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
for (j = 0; j < n; ++j) {
wa1[j] /= sdiag[j];
temp = wa1[j];
for (i = j+1; i < n; ++i)
for (int i = j+1; i < n; ++i)
wa1[i] -= s(i,j) * temp;
}
temp = wa1.blueNorm();

11
unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
View File

@ -1,4 +1,5 @@
// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
template <typename Scalar>
void ei_qrsolv(
Matrix< Scalar, Dynamic, Dynamic > &s,
@ -15,6 +16,7 @@ void ei_qrsolv(
Scalar temp;
int n = s.cols();
Matrix< Scalar, Dynamic, 1 > wa(n);
PlanarRotation<Scalar> givens;
/* Function Body */
// the following will only change the lower triangular part of s, including
@ -25,9 +27,7 @@ void ei_qrsolv(
x = s.diagonal();
wa = qtb;
for (j = 0; j < n; ++j)
for (i = j+1; i < n; ++i)
s(i,j) = s(j,i);
s.corner(TopLeft,n,n).template triangularView<StrictlyLower>() = s.corner(TopLeft,n,n).transpose();
/* eliminate the diagonal matrix d using a givens rotation. */
for (j = 0; j < n; ++j) {
@ -37,7 +37,7 @@ void ei_qrsolv(
l = ipvt[j];
if (diag[l] == 0.)
break;
sdiag.segment(j,n-j).setZero();
sdiag.tail(n-j).setZero();
sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */
@ -47,7 +47,6 @@ void ei_qrsolv(
for (k = j; k < n; ++k) {
/* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */
PlanarRotation<Scalar> givens;
givens.makeGivens(-s(k,k), sdiag[k]);
/* compute the modified diagonal element of r and */
@ -70,8 +69,8 @@ void ei_qrsolv(
/* singular, then obtain a least squares solution. */
int nsing;
for (nsing=0; nsing<n && sdiag[nsing]!=0; nsing++);
wa.segment(nsing,n-nsing).setZero();
wa.tail(n-nsing).setZero();
s.corner(TopLeft, nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
// restore