mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-07-23 13:24:28 +08:00
Replace the qr factorization from (c)minpack (qrfac) by Eigen's own stuff.
Results as checked by unit tests are very slightly worse, but not much.
This commit is contained in:
Thomas Capricelli
parent
1cb0be05b0
commit
ee0e39284c
@ -129,7 +129,6 @@ namespace Eigen {
|
||||
#include "src/NonLinearOptimization/r1updt.h"
|
||||
#include "src/NonLinearOptimization/r1mpyq.h"
|
||||
#include "src/NonLinearOptimization/rwupdt.h"
|
||||
#include "src/NonLinearOptimization/qrfac.h"
|
||||
#include "src/NonLinearOptimization/fdjac1.h"
|
||||
#include "src/NonLinearOptimization/qform.h"
|
||||
#include "src/NonLinearOptimization/lmpar.h"
|
||||
|
||||
@ -218,7 +218,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
|
||||
const int mode
|
||||
)
|
||||
{
|
||||
int i, j, l, iwa[1];
|
||||
int i, j, l;
|
||||
jeval = true;
|
||||
|
||||
/* calculate the jacobian matrix. */
|
||||
@ -249,7 +249,13 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
|
||||
}
|
||||
|
||||
/* compute the qr factorization of the jacobian. */
|
||||
ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), false, iwa, wa1.data());
|
||||
wa2 = fjac.colwise().blueNorm();
|
||||
HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
|
||||
fjac = qrfac.matrixQR();
|
||||
wa1 = fjac.diagonal();
|
||||
fjac.diagonal() = qrfac.hCoeffs();
|
||||
// TODO : avoid this:
|
||||
for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
|
||||
|
||||
/* form (q transpose)*fvec and store in qtf. */
|
||||
|
||||
@ -280,6 +286,11 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
|
||||
|
||||
/* accumulate the orthogonal factor in fjac. */
|
||||
ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data());
|
||||
#if 0
|
||||
std::cout << "ei_qform<Scalar>: " << fjac << std::endl;
|
||||
fjac = qrfac.matrixQ();
|
||||
std::cout << "qrfac.matrixQ():" << fjac << std::endl;
|
||||
#endif
|
||||
|
||||
/* rescale if necessary. */
|
||||
|
||||
@ -530,7 +541,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
|
||||
const int mode
|
||||
)
|
||||
{
|
||||
int i, j, l, iwa[1];
|
||||
int i, j, l;
|
||||
jeval = true;
|
||||
if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
|
||||
if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
|
||||
@ -563,7 +574,13 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
|
||||
}
|
||||
|
||||
/* compute the qr factorization of the jacobian. */
|
||||
ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), false, iwa, wa1.data());
|
||||
wa2 = fjac.colwise().blueNorm();
|
||||
HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
|
||||
fjac = qrfac.matrixQR();
|
||||
wa1 = fjac.diagonal();
|
||||
fjac.diagonal() = qrfac.hCoeffs();
|
||||
// TODO : avoid this:
|
||||
for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
|
||||
|
||||
/* form (q transpose)*fvec and store in qtf. */
|
||||
|
||||
@ -594,6 +611,11 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
|
||||
|
||||
/* accumulate the orthogonal factor in fjac. */
|
||||
ei_qform<Scalar>(n, n, fjac.data(), fjac.rows(), wa1.data());
|
||||
#if 0
|
||||
std::cout << "ei_qform<Scalar>: " << fjac << std::endl;
|
||||
fjac = qrfac.matrixQ();
|
||||
std::cout << "qrfac.matrixQ():" << fjac << std::endl;
|
||||
#endif
|
||||
|
||||
/* rescale if necessary. */
|
||||
|
||||
|
||||
@ -254,8 +254,13 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
|
||||
/* compute the qr factorization of the jacobian. */
|
||||
|
||||
wa2 = fjac.colwise().blueNorm();
|
||||
ei_qrfac<Scalar>(m, n, fjac.data(), fjac.rows(), true, ipvt.data(), wa1.data());
|
||||
ipvt.array() -= 1; // qrfac() creates ipvt with fortran convention (1->n), convert it to c (0->n-1)
|
||||
ColPivHouseholderQR<JacobianType> qrfac(fjac);
|
||||
fjac = qrfac.matrixQR();
|
||||
wa1 = fjac.diagonal();
|
||||
fjac.diagonal() = qrfac.hCoeffs();
|
||||
ipvt = qrfac.colsPermutation().indices();
|
||||
// TODO : avoid this:
|
||||
for(int i=0; i< fjac.cols(); i++) fjac.col(i).segment(i+1, fjac.rows()-i-1) *= fjac(i,i); // rescale vectors
|
||||
|
||||
/* on the first iteration and if mode is 1, scale according */
|
||||
/* to the norms of the columns of the initial jacobian. */
|
||||
@ -295,6 +300,12 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(
|
||||
qtf[j] = wa4[j];
|
||||
}
|
||||
|
||||
#if 0
|
||||
std::cout << "qtf: " << qtf << std::endl;
|
||||
FVectorType monqtf = qrfac.matrixQ().transpose() * fvec;
|
||||
std::cout << "mon qtf :" << monqtf << std::endl;
|
||||
#endif
|
||||
|
||||
/* compute the norm of the scaled gradient. */
|
||||
|
||||
gnorm = 0.;
|
||||
@ -540,10 +551,16 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
|
||||
wa2[j] = fjac.col(j).head(j).stableNorm();
|
||||
}
|
||||
if (sing) {
|
||||
ipvt.array() += 1;
|
||||
wa2 = fjac.colwise().blueNorm();
|
||||
ei_qrfac<Scalar>(n, n, fjac.data(), fjac.rows(), true, ipvt.data(), wa1.data());
|
||||
ipvt.array() -= 1; // qrfac() creates ipvt with fortran convention (1->n), convert it to c (0->n-1)
|
||||
// TODO We have no unit test covering this branch.. untested
|
||||
ColPivHouseholderQR<JacobianType> qrfac(fjac);
|
||||
fjac = qrfac.matrixQR();
|
||||
wa1 = fjac.diagonal();
|
||||
fjac.diagonal() = qrfac.hCoeffs();
|
||||
ipvt = qrfac.colsPermutation().indices();
|
||||
// TODO : avoid this:
|
||||
for(int ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
|
||||
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (fjac(j,j) != 0.) {
|
||||
sum = 0.;
|
||||
|
||||
@ -1,105 +0,0 @@
|
||||
|
||||
template <typename Scalar>
|
||||
void ei_qrfac(int m, int n, Scalar *a, int
|
||||
lda, int pivot, int *ipvt, Scalar *rdiag)
|
||||
{
|
||||
/* System generated locals */
|
||||
int a_dim1, a_offset;
|
||||
|
||||
/* Local variables */
|
||||
int i, j, k, jp1;
|
||||
Scalar sum;
|
||||
int kmax;
|
||||
Scalar temp;
|
||||
int minmn;
|
||||
Scalar ajnorm;
|
||||
|
||||
Matrix< Scalar, Dynamic, 1 > wa(n+1);
|
||||
|
||||
/* Parameter adjustments */
|
||||
--rdiag;
|
||||
a_dim1 = lda;
|
||||
a_offset = 1 + a_dim1 * 1;
|
||||
a -= a_offset;
|
||||
--ipvt;
|
||||
|
||||
/* Function Body */
|
||||
const Scalar epsmch = epsilon<Scalar>();
|
||||
|
||||
/* compute the initial column norms and initialize several arrays. */
|
||||
|
||||
for (j = 1; j <= n; ++j) {
|
||||
rdiag[j] = Map< Matrix< Scalar, Dynamic, 1 > >(&a[j * a_dim1 + 1],m).blueNorm();
|
||||
wa[j] = rdiag[j];
|
||||
if (pivot)
|
||||
ipvt[j] = j;
|
||||
}
|
||||
|
||||
/* reduce a to r with householder transformations. */
|
||||
|
||||
minmn = std::min(m,n);
|
||||
for (j = 1; j <= minmn; ++j) {
|
||||
if (! (pivot))
|
||||
goto L40;
|
||||
|
||||
/* bring the column of largest norm into the pivot position. */
|
||||
|
||||
kmax = j;
|
||||
for (k = j; k <= n; ++k)
|
||||
if (rdiag[k] > rdiag[kmax])
|
||||
kmax = k;
|
||||
if (kmax == j)
|
||||
goto L40;
|
||||
for (i = 1; i <= m; ++i) {
|
||||
temp = a[i + j * a_dim1];
|
||||
a[i + j * a_dim1] = a[i + kmax * a_dim1];
|
||||
a[i + kmax * a_dim1] = temp;
|
||||
/* L30: */
|
||||
}
|
||||
rdiag[kmax] = rdiag[j];
|
||||
wa[kmax] = wa[j];
|
||||
k = ipvt[j];
|
||||
ipvt[j] = ipvt[kmax];
|
||||
ipvt[kmax] = k;
|
||||
L40:
|
||||
|
||||
/* compute the householder transformation to reduce the */
|
||||
/* j-th column of a to a multiple of the j-th unit vector. */
|
||||
|
||||
ajnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&a[j + j * a_dim1],m-j+1).blueNorm();
|
||||
if (ajnorm == 0.)
|
||||
goto L100;
|
||||
if (a[j + j * a_dim1] < 0.)
|
||||
ajnorm = -ajnorm;
|
||||
for (i = j; i <= m; ++i)
|
||||
a[i + j * a_dim1] /= ajnorm;
|
||||
a[j + j * a_dim1] += 1.;
|
||||
|
||||
/* apply the transformation to the remaining columns */
|
||||
/* and update the norms. */
|
||||
|
||||
jp1 = j + 1;
|
||||
for (k = jp1; k <= n; ++k) {
|
||||
sum = 0.;
|
||||
for (i = j; i <= m; ++i)
|
||||
sum += a[i + j * a_dim1] * a[i + k * a_dim1];
|
||||
temp = sum / a[j + j * a_dim1];
|
||||
for (i = j; i <= m; ++i)
|
||||
a[i + k * a_dim1] -= temp * a[i + j * a_dim1];
|
||||
if (! (pivot) || rdiag[k] == 0.)
|
||||
continue;
|
||||
temp = a[j + k * a_dim1] / rdiag[k];
|
||||
/* Computing MAX */
|
||||
/* Computing 2nd power */
|
||||
rdiag[k] *= ei_sqrt((std::max(Scalar(0.), Scalar(1.)-ei_abs2(temp))));
|
||||
/* Computing 2nd power */
|
||||
if (Scalar(.05) * ei_abs2(rdiag[k] / wa[k]) > epsmch)
|
||||
continue;
|
||||
rdiag[k] = Map< Matrix< Scalar, Dynamic, 1 > >(&a[jp1 + k * a_dim1],m-j).blueNorm();
|
||||
wa[k] = rdiag[k];
|
||||
}
|
||||
L100:
|
||||
rdiag[j] = -ajnorm;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1010,7 +1010,7 @@ void testNistLanczos1(void)
|
||||
VERIFY( 79 == lm.nfev);
|
||||
VERIFY( 72 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.429604433690E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.428127827535E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats
|
||||
// check x
|
||||
VERIFY_IS_APPROX(x[0], 9.5100000027E-02 );
|
||||
VERIFY_IS_APPROX(x[1], 1.0000000001E+00 );
|
||||
@ -1031,7 +1031,7 @@ void testNistLanczos1(void)
|
||||
VERIFY( 9 == lm.nfev);
|
||||
VERIFY( 8 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.43049947737308E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.43059335827267E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats
|
||||
// check x
|
||||
VERIFY_IS_APPROX(x[0], 9.5100000027E-02 );
|
||||
VERIFY_IS_APPROX(x[1], 1.0000000001E+00 );
|
||||
@ -1170,9 +1170,9 @@ void testNistMGH10(void)
|
||||
info = lm.minimize(x);
|
||||
|
||||
// check return value
|
||||
VERIFY( 2 == info);
|
||||
VERIFY( 285 == lm.nfev);
|
||||
VERIFY( 250 == lm.njev);
|
||||
VERIFY( 3 == info);
|
||||
VERIFY( 281 == lm.nfev);
|
||||
VERIFY( 248 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 8.7945855171E+01);
|
||||
// check x
|
||||
@ -1269,9 +1269,9 @@ void testNistBoxBOD(void)
|
||||
info = lm.minimize(x);
|
||||
|
||||
// check return value
|
||||
VERIFY( 1 == info);
|
||||
VERIFY( 15 == lm.nfev);
|
||||
VERIFY( 14 == lm.njev);
|
||||
VERIFY( 1 == info);
|
||||
VERIFY( 17 == lm.nfev);
|
||||
VERIFY( 14 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.1680088766E+03);
|
||||
// check x
|
||||
@ -1331,9 +1331,9 @@ void testNistMGH17(void)
|
||||
info = lm.minimize(x);
|
||||
|
||||
// check return value
|
||||
VERIFY( 1 == info);
|
||||
VERIFY( 599 == lm.nfev);
|
||||
VERIFY( 544 == lm.njev);
|
||||
VERIFY( 2 == info);
|
||||
VERIFY( 603 == lm.nfev);
|
||||
VERIFY( 544 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.4648946975E-05);
|
||||
// check x
|
||||
@ -1418,16 +1418,16 @@ void testNistMGH09(void)
|
||||
info = lm.minimize(x);
|
||||
|
||||
// check return value
|
||||
VERIFY( 1 == info);
|
||||
VERIFY( 503== lm.nfev);
|
||||
VERIFY( 385 == lm.njev);
|
||||
VERIFY( 1 == info);
|
||||
VERIFY( 494== lm.nfev);
|
||||
VERIFY( 382 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 3.0750560385E-04);
|
||||
// check x
|
||||
VERIFY_IS_APPROX(x[0], 0.19280624); // should be 1.9280693458E-01
|
||||
VERIFY_IS_APPROX(x[1], 0.19129774); // should be 1.9128232873E-01
|
||||
VERIFY_IS_APPROX(x[2], 0.12305940); // should be 1.2305650693E-01
|
||||
VERIFY_IS_APPROX(x[3], 0.13606946); // should be 1.3606233068E-01
|
||||
VERIFY_IS_APPROX(x[0], 0.192807830); // should be 1.9280693458E-01
|
||||
VERIFY_IS_APPROX(x[1], 0.191262206); // should be 1.9128232873E-01
|
||||
VERIFY_IS_APPROX(x[2], 0.123052716); // should be 1.2305650693E-01
|
||||
VERIFY_IS_APPROX(x[3], 0.136053014); // should be 1.3606233068E-01
|
||||
|
||||
/*
|
||||
* Second try
|
||||
@ -1833,7 +1833,6 @@ void test_NonLinearOptimization()
|
||||
|
||||
/*
|
||||
* Can be useful for debugging...
|
||||
printf("info, nfev, njev : %d, %d, %d\n", info, lm.nfev, lm.njev);
|
||||
printf("info, nfev : %d, %d\n", info, lm.nfev);
|
||||
printf("info, nfev, njev : %d, %d, %d\n", info, solver.nfev, solver.njev);
|
||||
printf("info, nfev : %d, %d\n", info, solver.nfev);
|
||||
@ -1843,5 +1842,10 @@ void test_NonLinearOptimization()
|
||||
printf("x[3] : %.32g\n", x[3]);
|
||||
printf("fvec.blueNorm() : %.32g\n", solver.fvec.blueNorm());
|
||||
printf("fvec.blueNorm() : %.32g\n", lm.fvec.blueNorm());
|
||||
|
||||
printf("info, nfev, njev : %d, %d, %d\n", info, lm.nfev, lm.njev);
|
||||
printf("fvec.blueNorm() : %.32g\n", lm.fvec.blueNorm());
|
||||
printf("fvec.squaredNorm() : %.32g\n", lm.fvec.squaredNorm());
|
||||
std::cout << x << std::endl;
|
||||
*/
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user