mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-03-07 18:27:40 +08:00
use PlanarRotation<> instead of handmade givens rotation in cminpack code
+ cleaning. This results in some more memory being used, but not much.
This commit is contained in:
Thomas Capricelli
parent
c04a93df31
commit
afb9bf6281
@ -218,6 +218,8 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
|
||||
)
|
||||
{
|
||||
int j;
|
||||
std::vector<PlanarRotation<Scalar> > v_givens(n), w_givens(n);
|
||||
|
||||
jeval = true;
|
||||
|
||||
/* calculate the jacobian matrix. */
|
||||
@ -359,9 +361,9 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
|
||||
wa2 = (wa2-wa3)/pnorm;
|
||||
|
||||
/* compute the qr factorization of the updated jacobian. */
|
||||
ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing);
|
||||
ei_r1mpyq<Scalar>(n, n, fjac.data(), wa2.data(), wa3.data());
|
||||
ei_r1mpyq<Scalar>(1, n, qtf.data(), wa2.data(), wa3.data());
|
||||
ei_r1updt<Scalar>(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing);
|
||||
ei_r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
|
||||
ei_r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
|
||||
|
||||
jeval = false;
|
||||
}
|
||||
@ -465,6 +467,8 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
|
||||
)
|
||||
{
|
||||
int j;
|
||||
std::vector<PlanarRotation<Scalar> > v_givens(n), w_givens(n);
|
||||
|
||||
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;
|
||||
@ -608,9 +612,9 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
|
||||
wa2 = (wa2-wa3)/pnorm;
|
||||
|
||||
/* compute the qr factorization of the updated jacobian. */
|
||||
ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing);
|
||||
ei_r1mpyq<Scalar>(n, n, fjac.data(), wa2.data(), wa3.data());
|
||||
ei_r1mpyq<Scalar>(1, n, qtf.data(), wa2.data(), wa3.data());
|
||||
ei_r1updt<Scalar>(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing);
|
||||
ei_r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
|
||||
ei_r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
|
||||
|
||||
jeval = false;
|
||||
}
|
||||
|
||||
@ -468,8 +468,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
|
||||
int rownb = 2;
|
||||
for (i = 0; i < m; ++i) {
|
||||
if (functor.df(x, wa3, rownb) < 0) return UserAsked;
|
||||
temp = fvec[i];
|
||||
ei_rwupdt<Scalar>(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data());
|
||||
ei_rwupdt<Scalar>(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), fvec[i]);
|
||||
++rownb;
|
||||
}
|
||||
++njev;
|
||||
|
||||
@ -2,46 +2,21 @@
|
||||
// TODO : move this to GivensQR once there's such a thing in Eigen
|
||||
|
||||
template <typename Scalar>
|
||||
void ei_r1mpyq(int m, int n, Scalar *a, const Scalar *v, const Scalar *w)
|
||||
void ei_r1mpyq(int m, int n, Scalar *a, const std::vector<PlanarRotation<Scalar> > &v_givens, const std::vector<PlanarRotation<Scalar> > &w_givens)
|
||||
{
|
||||
/* Local variables */
|
||||
int i, j;
|
||||
Scalar cos__=0., sin__=0., temp;
|
||||
|
||||
/* Function Body */
|
||||
if (n<=1)
|
||||
return;
|
||||
|
||||
/* apply the first set of givens rotations to a. */
|
||||
for (j = n-2; j>=0; --j) {
|
||||
if (ei_abs(v[j]) > 1.) {
|
||||
cos__ = 1. / v[j];
|
||||
sin__ = ei_sqrt(1. - ei_abs2(cos__));
|
||||
} else {
|
||||
sin__ = v[j];
|
||||
cos__ = ei_sqrt(1. - ei_abs2(sin__));
|
||||
}
|
||||
for (i = 0; i<m; ++i) {
|
||||
temp = cos__ * a[i+m*j] - sin__ * a[i+m*(n-1)];
|
||||
a[i+m*(n-1)] = sin__ * a[i+m*j] + cos__ * a[i+m*(n-1)];
|
||||
for (int j = n-2; j>=0; --j)
|
||||
for (int 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 (j = 0; j<n-1; ++j) {
|
||||
if (ei_abs(w[j]) > 1.) {
|
||||
cos__ = 1. / w[j];
|
||||
sin__ = ei_sqrt(1. - ei_abs2(cos__));
|
||||
} else {
|
||||
sin__ = w[j];
|
||||
cos__ = ei_sqrt(1. - ei_abs2(sin__));
|
||||
}
|
||||
for (i = 0; i<m; ++i) {
|
||||
temp = cos__ * a[i+m*j] + sin__ * a[i+m*(n-1)];
|
||||
a[i+m*(n-1)] = -sin__ * a[i+m*j] + cos__ * a[i+m*(n-1)];
|
||||
for (int j = 0; j<n-1; ++j)
|
||||
for (int 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;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -1,12 +1,16 @@
|
||||
|
||||
template <typename Scalar>
|
||||
void ei_r1updt(int m, int n, Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar *u, Scalar *v, Scalar *w, bool *sing)
|
||||
void ei_r1updt(Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar *u,
|
||||
std::vector<PlanarRotation<Scalar> > &v_givens,
|
||||
std::vector<PlanarRotation<Scalar> > &w_givens,
|
||||
Scalar *v, Scalar *w, bool *sing)
|
||||
{
|
||||
/* Local variables */
|
||||
int i, j=1, nm1;
|
||||
Scalar tan__;
|
||||
int nmj;
|
||||
Scalar cos__, sin__, tau, temp, cotan;
|
||||
const int m = s.rows();
|
||||
const int n = s.cols();
|
||||
int i, j=1;
|
||||
Scalar temp;
|
||||
PlanarRotation<Scalar> givens;
|
||||
|
||||
// ei_r1updt had a broader usecase, but we dont use it here. And, more
|
||||
// importantly, we can not test it.
|
||||
@ -17,52 +21,31 @@ void ei_r1updt(int m, int n, Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar
|
||||
--u;
|
||||
--v;
|
||||
|
||||
/* Function Body */
|
||||
const Scalar giant = std::numeric_limits<Scalar>::max();
|
||||
|
||||
/* move the nontrivial part of the last column of s into w. */
|
||||
w[n] = 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. */
|
||||
nm1 = n - 1;
|
||||
if (nm1 >= 1)
|
||||
for (nmj = 1; nmj <= nm1; ++nmj) {
|
||||
j = n - nmj;
|
||||
w[j] = 0.;
|
||||
if (v[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of v. */
|
||||
if (ei_abs(v[n]) < ei_abs(v[j])) {
|
||||
cotan = v[n] / v[j];
|
||||
/* Computing 2nd power */
|
||||
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
|
||||
cos__ = sin__ * cotan;
|
||||
tau = 1.;
|
||||
if (ei_abs(cos__) * giant > 1.) {
|
||||
tau = 1. / cos__;
|
||||
}
|
||||
} else {
|
||||
tan__ = v[j] / v[n];
|
||||
/* Computing 2nd power */
|
||||
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
|
||||
sin__ = cos__ * tan__;
|
||||
tau = sin__;
|
||||
}
|
||||
for (j=n-1; j>=1; --j) {
|
||||
w[j] = 0.;
|
||||
if (v[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of v. */
|
||||
givens.makeGivens(-v[n], v[j]);
|
||||
|
||||
/* apply the transformation to v and store the information */
|
||||
/* necessary to recover the givens rotation. */
|
||||
v[n] = sin__ * v[j] + cos__ * v[n];
|
||||
v[j] = tau;
|
||||
/* apply the transformation to v and store the information */
|
||||
/* necessary to recover the givens rotation. */
|
||||
v[n] = givens.s() * v[j] + givens.c() * v[n];
|
||||
v_givens[j-1] = givens;
|
||||
|
||||
/* apply the transformation to s and extend the spike in w. */
|
||||
for (i = j; i <= m; ++i) {
|
||||
temp = cos__ * s(j-1,i-1) - sin__ * w[i];
|
||||
w[i] = sin__ * s(j-1,i-1) + cos__ * w[i];
|
||||
s(j-1,i-1) = temp;
|
||||
}
|
||||
/* apply the transformation to s and extend the spike in w. */
|
||||
for (i = j; i <= m; ++i) {
|
||||
temp = givens.c() * s(j-1,i-1) - givens.s() * w[i];
|
||||
w[i] = givens.s() * s(j-1,i-1) + givens.c() * w[i];
|
||||
s(j-1,i-1) = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* add the spike from the rank 1 update to w. */
|
||||
for (i = 1; i <= m; ++i)
|
||||
@ -70,45 +53,29 @@ void ei_r1updt(int m, int n, Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar
|
||||
|
||||
/* eliminate the spike. */
|
||||
*sing = false;
|
||||
if (nm1 >= 1)
|
||||
for (j = 1; j <= nm1; ++j) {
|
||||
if (w[j] != 0.) {
|
||||
/* determine a givens rotation which eliminates the */
|
||||
/* j-th element of the spike. */
|
||||
if (ei_abs(s(j-1,j-1)) < ei_abs(w[j])) {
|
||||
cotan = s(j-1,j-1) / w[j];
|
||||
/* Computing 2nd power */
|
||||
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
|
||||
cos__ = sin__ * cotan;
|
||||
tau = 1.;
|
||||
if (ei_abs(cos__) * giant > 1.) {
|
||||
tau = 1. / cos__;
|
||||
}
|
||||
} else {
|
||||
tan__ = w[j] / s(j-1,j-1);
|
||||
/* Computing 2nd power */
|
||||
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
|
||||
sin__ = cos__ * tan__;
|
||||
tau = sin__;
|
||||
}
|
||||
for (j = 1; 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-1,j-1), w[j]);
|
||||
|
||||
/* apply the transformation to s and reduce the spike in w. */
|
||||
for (i = j; i <= m; ++i) {
|
||||
temp = cos__ * s(j-1,i-1) + sin__ * w[i];
|
||||
w[i] = -sin__ * s(j-1,i-1) + cos__ * w[i];
|
||||
s(j-1,i-1) = temp;
|
||||
}
|
||||
|
||||
/* store the information necessary to recover the */
|
||||
/* givens rotation. */
|
||||
w[j] = tau;
|
||||
/* apply the transformation to s and reduce the spike in w. */
|
||||
for (i = j; i <= m; ++i) {
|
||||
temp = givens.c() * s(j-1,i-1) + givens.s() * w[i];
|
||||
w[i] = -givens.s() * s(j-1,i-1) + givens.c() * w[i];
|
||||
s(j-1,i-1) = temp;
|
||||
}
|
||||
|
||||
/* test for zero diagonal elements in the output s. */
|
||||
if (s(j-1,j-1) == 0.) {
|
||||
*sing = true;
|
||||
}
|
||||
/* store the information necessary to recover the */
|
||||
/* givens rotation. */
|
||||
w_givens[j-1] = givens;
|
||||
}
|
||||
|
||||
/* test for zero diagonal elements in the output s. */
|
||||
if (s(j-1,j-1) == 0.) {
|
||||
*sing = true;
|
||||
}
|
||||
}
|
||||
/* move w back into the last column of the output s. */
|
||||
s(n-1,n-1) = w[n];
|
||||
|
||||
|
||||
@ -1,18 +1,15 @@
|
||||
|
||||
template <typename Scalar>
|
||||
void ei_rwupdt(int n, Scalar *r__, int ldr,
|
||||
const Scalar *w, Scalar *b, Scalar *alpha, Scalar *cos__,
|
||||
Scalar *sin__)
|
||||
template <typename Scalar>
|
||||
void ei_rwupdt(int n, Scalar *r__, int ldr, const Scalar *w, Scalar *b, Scalar alpha)
|
||||
{
|
||||
std::vector<PlanarRotation<Scalar> > givens(n);
|
||||
/* System generated locals */
|
||||
int r_dim1, r_offset;
|
||||
|
||||
/* Local variables */
|
||||
Scalar tan__, temp, rowj, cotan;
|
||||
Scalar temp, rowj;
|
||||
|
||||
/* Parameter adjustments */
|
||||
--sin__;
|
||||
--cos__;
|
||||
--b;
|
||||
--w;
|
||||
r_dim1 = ldr;
|
||||
@ -23,34 +20,23 @@ void ei_rwupdt(int n, Scalar *r__, int ldr,
|
||||
for (int j = 1; j <= n; ++j) {
|
||||
rowj = w[j];
|
||||
|
||||
/* apply the previous transformations to */
|
||||
/* r(i,j), i=1,2,...,j-1, and to w(j). */
|
||||
/* apply the previous transformations to */
|
||||
/* r(i,j), i=1,2,...,j-1, and to w(j). */
|
||||
if (j-1>=1)
|
||||
for (int i = 1; i <= j-1; ++i) {
|
||||
temp = cos__[i] * r__[i + j * r_dim1] + sin__[i] * rowj;
|
||||
rowj = -sin__[i] * r__[i + j * r_dim1] + cos__[i] * rowj;
|
||||
temp = givens[i-1].c() * r__[i + j * r_dim1] + givens[i-1].s() * rowj;
|
||||
rowj = -givens[i-1].s() * r__[i + j * r_dim1] + givens[i-1].c() * rowj;
|
||||
r__[i + j * r_dim1] = temp;
|
||||
}
|
||||
|
||||
/* determine a givens rotation which eliminates w(j). */
|
||||
cos__[j] = 1.;
|
||||
sin__[j] = 0.;
|
||||
/* determine a givens rotation which eliminates w(j). */
|
||||
if (rowj != 0.) {
|
||||
if (ei_abs(r__[j + j * r_dim1]) < ei_abs(rowj)) {
|
||||
cotan = r__[j + j * r_dim1] / rowj;
|
||||
sin__[j] = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
|
||||
cos__[j] = sin__[j] * cotan;
|
||||
}
|
||||
else {
|
||||
tan__ = rowj / r__[j + j * r_dim1];
|
||||
cos__[j] = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
|
||||
sin__[j] = cos__[j] * tan__;
|
||||
}
|
||||
givens[j-1].makeGivens(-r__[j + j * r_dim1], rowj);
|
||||
|
||||
/* apply the current transformation to r(j,j), b(j), and alpha. */
|
||||
r__[j + j * r_dim1] = cos__[j] * r__[j + j * r_dim1] + sin__[j] * rowj;
|
||||
temp = cos__[j] * b[j] + sin__[j] * *alpha;
|
||||
*alpha = -sin__[j] * b[j] + cos__[j] * *alpha;
|
||||
/* apply the current transformation to r(j,j), b(j), and alpha. */
|
||||
r__[j + j * r_dim1] = givens[j-1].c() * r__[j + j * r_dim1] + givens[j-1].s() * rowj;
|
||||
temp = givens[j-1].c() * b[j] + givens[j-1].s() * alpha;
|
||||
alpha = -givens[j-1].s() * b[j] + givens[j-1].c() * alpha;
|
||||
b[j] = temp;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user