GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-12 11:49:02 +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 */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x); xnorm = diag.cwiseProduct(x).stableNorm();
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm; delta = parameters.factor * xnorm;
if (delta == 0.) if (delta == 0.)
delta = parameters.factor; delta = parameters.factor;
@ -269,8 +268,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1; wa1 = -wa1;
wa2 = x + wa1; wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1); pnorm = diag.cwiseProduct(wa1).stableNorm();
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
if (iter == 1) if (iter == 1)
@ -489,8 +487,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
/* on the first iteration, calculate the norm of the scaled x */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x); xnorm = diag.cwiseProduct(x).stableNorm();
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm; delta = parameters.factor * xnorm;
if (delta == 0.) if (delta == 0.)
delta = parameters.factor; delta = parameters.factor;
@ -520,8 +517,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1; wa1 = -wa1;
wa2 = x + wa1; wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1); pnorm = diag.cwiseProduct(wa1).stableNorm();
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
if (iter == 1) 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 */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x); xnorm = diag.cwiseProduct(x).stableNorm();
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm; delta = parameters.factor * xnorm;
if (delta == 0.) if (delta == 0.)
delta = parameters.factor; delta = parameters.factor;
@ -297,8 +296,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1; wa1 = -wa1;
wa2 = x + wa1; wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1); pnorm = diag.cwiseProduct(wa1).stableNorm();
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
if (iter == 1) if (iter == 1)
@ -515,8 +513,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
/* on the first iteration, calculate the norm of the scaled x */ /* on the first iteration, calculate the norm of the scaled x */
/* and initialize the step bound delta. */ /* and initialize the step bound delta. */
wa3 = diag.cwiseProduct(x); xnorm = diag.cwiseProduct(x).stableNorm();
xnorm = wa3.stableNorm();
delta = parameters.factor * xnorm; delta = parameters.factor * xnorm;
if (delta == 0.) if (delta == 0.)
delta = parameters.factor; delta = parameters.factor;
@ -545,8 +542,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
/* store the direction p and x + p. calculate the norm of p. */ /* store the direction p and x + p. calculate the norm of p. */
wa1 = -wa1; wa1 = -wa1;
wa2 = x + wa1; wa2 = x + wa1;
wa3 = diag.cwiseProduct(wa1); pnorm = diag.cwiseProduct(wa1).stableNorm();
pnorm = wa3.stableNorm();
/* on the first iteration, adjust the initial step bound. */ /* on the first iteration, adjust the initial step bound. */
if (iter == 1) if (iter == 1)

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

@ -4,11 +4,11 @@
template<typename Scalar> template<typename Scalar>
void ei_chkder( void ei_chkder(
Matrix< Scalar, Dynamic, 1 > &x, const Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &fvec, const Matrix< Scalar, Dynamic, 1 > &fvec,
Matrix< Scalar, Dynamic, Dynamic > &fjac, const Matrix< Scalar, Dynamic, Dynamic > &fjac,
Matrix< Scalar, Dynamic, 1 > &xp, Matrix< Scalar, Dynamic, 1 > &xp,
Matrix< Scalar, Dynamic, 1 > &fvecp, const Matrix< Scalar, Dynamic, 1 > &fvecp,
int mode, int mode,
Matrix< Scalar, Dynamic, 1 > &err 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); Matrix< Scalar, Dynamic, 1 > wa(n);
assert(ipvt.size()==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; l = -1;
for (k = 0; k < n; ++k) for (k = 0; k < n; ++k)
if (ei_abs(r(k,k)) > tolr) { if (ei_abs(r(k,k)) > tolr) {
@ -24,27 +24,21 @@ void ei_covar(
for (j = 0; j <= k-1; ++j) { for (j = 0; j <= k-1; ++j) {
temp = r(k,k) * r(j,k); temp = r(k,k) * r(j,k);
r(j,k) = 0.; r(j,k) = 0.;
for (i = 0; i <= j; ++i) r.col(k).head(j+1) -= r.col(j).head(j+1) * temp;
r(i,k) -= temp * r(i,j);
} }
l = k; l = k;
} }
/* form the full upper triangle of the inverse of (r transpose)*r */ /* form the full upper triangle of the inverse of (r transpose)*r */
/* in the full upper triangle of r. */ /* in the full upper triangle of r. */
for (k = 0; k <= l; ++k) { for (k = 0; k <= l; ++k) {
for (j = 0; j <= k-1; ++j) { for (j = 0; j <= k-1; ++j)
temp = r(j,k); r.col(j).head(j+1) += r.col(k).head(j+1) * r(j,k);
for (i = 0; i <= j; ++i) r.col(k).head(k+1) *= r(k,k);
r(i,j) += temp * r(i,k);
}
temp = r(k,k);
for (i = 0; i <= k; ++i)
r(i,k) = temp * r(i,k);
} }
/* form the full lower triangle of the covariance matrix */ /* form the full lower triangle of the covariance matrix */
/* in the strict lower triangle of r and in wa. */ /* in the strict lower triangle of r and in wa. */
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j) {
jj = ipvt[j]; jj = ipvt[j];
sing = j > l; sing = j > l;
@ -60,11 +54,8 @@ void ei_covar(
wa[jj] = r(j,j); wa[jj] = r(j,j);
} }
/* symmetrize the covariance matrix in r. */ /* symmetrize the covariance matrix in r. */
for (j = 0; j < n; ++j) { r.corner(TopLeft,n,n).template triangularView<StrictlyUpper>() = r.corner(TopLeft,n,n).transpose();
for (i = 0; i <= j; ++i) r.diagonal() = wa;
r(i,j) = r(j,i);
r(j,j) = wa[j];
}
} }

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

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

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

@ -10,10 +10,11 @@ int ei_fdjac1(
{ {
/* Local variables */ /* Local variables */
Scalar h; Scalar h;
int i, j, k; int j, k;
Scalar eps, temp; Scalar eps, temp;
int msum; int msum;
int iflag; int iflag;
int start, length;
/* Function Body */ /* Function Body */
const Scalar epsmch = epsilon<Scalar>(); const Scalar epsmch = epsilon<Scalar>();
@ -55,11 +56,10 @@ int ei_fdjac1(
x[j] = wa2[j]; x[j] = wa2[j];
h = eps * ei_abs(wa2[j]); h = eps * ei_abs(wa2[j]);
if (h == 0.) h = eps; if (h == 0.) h = eps;
for (i = 0; i < n; ++i) { fjac.col(j).setZero();
fjac(i,j) = 0.; start = std::max(0,j-mu);
if (i >= j - mu && i <= j + ml) length = std::min(n-1, j+ml) - start + 1;
fjac(i,j) = (wa1[i] - fvec[i]) / h; 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 */ /* Local variables */
int i, j; int j;
Scalar fp; Scalar fp;
Scalar parc, parl; Scalar parc, parl;
int iter; int iter;
@ -183,9 +183,11 @@ void ei_lmpar2(
/* compute and store in x the gauss-newton direction. if the */ /* compute and store in x the gauss-newton direction. if the */
/* jacobian is rank-deficient, obtain a least squares solution. */ /* jacobian is rank-deficient, obtain a least squares solution. */
// const int rank = qr.nonzeroPivots(); // exactly double(0.) // const int rank = qr.nonzeroPivots(); // exactly double(0.)
const int rank = qr.rank(); // use a threshold 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)); qr.matrixQR().corner(TopLeft, rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
x = qr.colsPermutation()*wa1; x = qr.colsPermutation()*wa1;
@ -255,10 +257,12 @@ void ei_lmpar2(
/* compute the newton correction. */ /* compute the newton correction. */
wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm); 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) { for (j = 0; j < n; ++j) {
wa1[j] /= sdiag[j]; wa1[j] /= sdiag[j];
temp = wa1[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; wa1[i] -= s(i,j) * temp;
} }
temp = wa1.blueNorm(); 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> template <typename Scalar>
void ei_qrsolv( void ei_qrsolv(
Matrix< Scalar, Dynamic, Dynamic > &s, Matrix< Scalar, Dynamic, Dynamic > &s,
@ -15,6 +16,7 @@ void ei_qrsolv(
Scalar temp; Scalar temp;
int n = s.cols(); int n = s.cols();
Matrix< Scalar, Dynamic, 1 > wa(n); Matrix< Scalar, Dynamic, 1 > wa(n);
PlanarRotation<Scalar> givens;
/* Function Body */ /* Function Body */
// the following will only change the lower triangular part of s, including // the following will only change the lower triangular part of s, including
@ -25,9 +27,7 @@ void ei_qrsolv(
x = s.diagonal(); x = s.diagonal();
wa = qtb; wa = qtb;
for (j = 0; j < n; ++j) s.corner(TopLeft,n,n).template triangularView<StrictlyLower>() = s.corner(TopLeft,n,n).transpose();
for (i = j+1; i < n; ++i)
s(i,j) = s(j,i);
/* eliminate the diagonal matrix d using a givens rotation. */ /* eliminate the diagonal matrix d using a givens rotation. */
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j) {
@ -37,7 +37,7 @@ void ei_qrsolv(
l = ipvt[j]; l = ipvt[j];
if (diag[l] == 0.) if (diag[l] == 0.)
break; break;
sdiag.segment(j,n-j).setZero(); sdiag.tail(n-j).setZero();
sdiag[j] = diag[l]; sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */ /* the transformations to eliminate the row of d */
@ -47,7 +47,6 @@ void ei_qrsolv(
for (k = j; k < n; ++k) { for (k = j; k < n; ++k) {
/* determine a givens rotation which eliminates the */ /* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */ /* appropriate element in the current row of d. */
PlanarRotation<Scalar> givens;
givens.makeGivens(-s(k,k), sdiag[k]); givens.makeGivens(-s(k,k), sdiag[k]);
/* compute the modified diagonal element of r and */ /* compute the modified diagonal element of r and */
@ -70,8 +69,8 @@ void ei_qrsolv(
/* singular, then obtain a least squares solution. */ /* singular, then obtain a least squares solution. */
int nsing; int nsing;
for (nsing=0; nsing<n && sdiag[nsing]!=0; 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)); s.corner(TopLeft, nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
// restore // restore