mirror of
https://gitlab.com/libeigen/eigen.git
synced 2024-12-21 07:19:46 +08:00
Use eigen methods for solving triangular systems. We loose again very
slightly on both speed and precision on some tests.
This commit is contained in:
Thomas Capricelli
parent
92be7f461b
commit
9651e0c503
@ -199,23 +199,12 @@ void ei_lmpar2(
|
||||
/* compute and store in x the gauss-newton direction. if the */
|
||||
/* jacobian is rank-deficient, obtain a least squares solution. */
|
||||
|
||||
int nsing = n-1;
|
||||
wa1 = qtb;
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (qr.matrixQR()(j,j) == 0. && nsing == n-1)
|
||||
nsing = j - 1;
|
||||
if (nsing < n-1)
|
||||
wa1[j] = 0.;
|
||||
}
|
||||
for (j = nsing; j>=0; --j) {
|
||||
wa1[j] /= qr.matrixQR()(j,j);
|
||||
temp = wa1[j];
|
||||
for (i = 0; i < j ; ++i)
|
||||
wa1[i] -= qr.matrixQR()(i,j) * temp;
|
||||
}
|
||||
// const int rank = qr.nonzeroPivots(); // exactly double(0.)
|
||||
const int rank = qr.rank(); // use a threshold
|
||||
wa1 = qtb; wa1.segment(rank,n-rank).setZero();
|
||||
qr.matrixQR().corner(TopLeft, rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
|
||||
|
||||
for (j = 0; j < n; ++j)
|
||||
x[qr.colsPermutation().indices()(j)] = wa1[j];
|
||||
x = qr.colsPermutation()*wa1;
|
||||
|
||||
/* initialize the iteration counter. */
|
||||
/* evaluate the function at the origin, and test */
|
||||
@ -235,19 +224,12 @@ void ei_lmpar2(
|
||||
/* the function. otherwise set this bound to zero. */
|
||||
|
||||
parl = 0.;
|
||||
if (nsing >= n-1) {
|
||||
if (rank==n) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
l = qr.colsPermutation().indices()(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 += qr.matrixQR()(i,j) * wa1[i];
|
||||
wa1[j] = (wa1[j] - sum) / qr.matrixQR()(j,j);
|
||||
}
|
||||
qr.matrixQR().corner(TopLeft, n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
|
||||
temp = wa1.blueNorm();
|
||||
parl = fp / delta / temp / temp;
|
||||
}
|
||||
@ -272,7 +254,7 @@ void ei_lmpar2(
|
||||
|
||||
/* beginning of an iteration. */
|
||||
|
||||
Matrix< Scalar, Dynamic, Dynamic > r = qr.matrixQR(); // TODO : fixme
|
||||
Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR();
|
||||
while (true) {
|
||||
++iter;
|
||||
|
||||
@ -284,7 +266,7 @@ void ei_lmpar2(
|
||||
wa1 = ei_sqrt(par)* diag;
|
||||
|
||||
Matrix< Scalar, Dynamic, 1 > sdiag(n);
|
||||
ei_qrsolv<Scalar>(r, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
|
||||
ei_qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
|
||||
|
||||
wa2 = diag.cwiseProduct(x);
|
||||
dxnorm = wa2.blueNorm();
|
||||
@ -308,7 +290,7 @@ void ei_lmpar2(
|
||||
wa1[j] /= sdiag[j];
|
||||
temp = wa1[j];
|
||||
for (i = j+1; i < n; ++i)
|
||||
wa1[i] -= r(i,j) * temp;
|
||||
wa1[i] -= s(i,j) * temp;
|
||||
}
|
||||
temp = wa1.blueNorm();
|
||||
parc = fp / delta / temp / temp;
|
||||
@ -321,16 +303,8 @@ void ei_lmpar2(
|
||||
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;
|
||||
|
||||
@ -1,7 +1,8 @@
|
||||
|
||||
template <typename Scalar>
|
||||
void ei_qrsolv(
|
||||
Matrix< Scalar, Dynamic, Dynamic > &r,
|
||||
Matrix< Scalar, Dynamic, Dynamic > &s,
|
||||
// TODO : use a PermutationMatrix once ei_lmpar is no more:
|
||||
const VectorXi &ipvt,
|
||||
const Matrix< Scalar, Dynamic, 1 > &diag,
|
||||
const Matrix< Scalar, Dynamic, 1 > &qtb,
|
||||
@ -11,21 +12,23 @@ void ei_qrsolv(
|
||||
{
|
||||
/* Local variables */
|
||||
int i, j, k, l;
|
||||
Scalar sum, temp;
|
||||
int n = r.cols();
|
||||
Scalar temp;
|
||||
int n = s.cols();
|
||||
Matrix< Scalar, Dynamic, 1 > wa(n);
|
||||
|
||||
/* 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 = r.diagonal();
|
||||
x = s.diagonal();
|
||||
wa = qtb;
|
||||
|
||||
for (j = 0; j < n; ++j)
|
||||
for (i = j+1; i < n; ++i)
|
||||
r(i,j) = r(j,i);
|
||||
s(i,j) = s(j,i);
|
||||
|
||||
/* eliminate the diagonal matrix d using a givens rotation. */
|
||||
for (j = 0; j < n; ++j) {
|
||||
@ -48,43 +51,37 @@ void ei_qrsolv(
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* appropriate element in the current row of d. */
|
||||
PlanarRotation<Scalar> givens;
|
||||
givens.makeGivens(-r(k,k), sdiag[k]);
|
||||
givens.makeGivens(-s(k,k), sdiag[k]);
|
||||
|
||||
/* compute the modified diagonal element of r and */
|
||||
/* the modified element of ((q transpose)*b,0). */
|
||||
|
||||
r(k,k) = givens.c() * r(k,k) + givens.s() * sdiag[k];
|
||||
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 tranformation in the row of s. */
|
||||
for (i = k+1; i<n; ++i) {
|
||||
temp = givens.c() * r(i,k) + givens.s() * sdiag[i];
|
||||
sdiag[i] = -givens.s() * r(i,k) + givens.c() * sdiag[i];
|
||||
r(i,k) = temp;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// restore
|
||||
sdiag = r.diagonal();
|
||||
r.diagonal() = x;
|
||||
|
||||
/* solve the triangular system for z. if the system is */
|
||||
/* singular, then obtain a least squares solution. */
|
||||
|
||||
int nsing;
|
||||
for (nsing=0; nsing<n && sdiag[nsing]!=0; nsing++);
|
||||
wa.segment(nsing,n-nsing).setZero();
|
||||
nsing--; // nsing is the last nonsingular index
|
||||
|
||||
for (j = nsing; j>=0; j--) {
|
||||
sum = 0.;
|
||||
for (i = j+1; i <= nsing; ++i)
|
||||
sum += r(i,j) * wa[i];
|
||||
wa[j] = (wa[j] - sum) / sdiag[j];
|
||||
}
|
||||
s.corner(TopLeft, nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
|
||||
|
||||
// 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];
|
||||
|
||||
@ -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.428127827535E-25); // should be 1.4307867721E-25, but nist results are on 128-bit floats
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 1.427932429905E-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 );
|
||||
@ -1332,8 +1332,8 @@ void testNistMGH17(void)
|
||||
|
||||
// check return value
|
||||
VERIFY( 2 == info);
|
||||
VERIFY( 603 == lm.nfev);
|
||||
VERIFY( 544 == lm.njev);
|
||||
VERIFY( 606 == lm.nfev);
|
||||
VERIFY( 545 == lm.njev);
|
||||
// check norm^2
|
||||
VERIFY_IS_APPROX(lm.fvec.squaredNorm(), 5.4648946975E-05);
|
||||
// check x
|
||||
|
||||
Loading…
Reference in New Issue
Block a user