From afb9bf628168a8b1119764628f716f52cf1c54ee Mon Sep 17 00:00:00 2001 From: Thomas Capricelli Date: Tue, 26 Jan 2010 17:40:55 +0100 Subject: [PATCH] use PlanarRotation<> instead of handmade givens rotation in cminpack code + cleaning. This results in some more memory being used, but not much. --- .../HybridNonLinearSolver.h | 16 ++- .../LevenbergMarquardt.h | 3 +- .../Eigen/src/NonLinearOptimization/r1mpyq.h | 43 ++----- .../Eigen/src/NonLinearOptimization/r1updt.h | 121 +++++++----------- .../Eigen/src/NonLinearOptimization/rwupdt.h | 42 ++---- 5 files changed, 78 insertions(+), 147 deletions(-) diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h index e1d10bdcc..539c58d72 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h +++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h @@ -218,6 +218,8 @@ HybridNonLinearSolver::solveOneStep( ) { int j; + std::vector > v_givens(n), w_givens(n); + jeval = true; /* calculate the jacobian matrix. */ @@ -359,9 +361,9 @@ HybridNonLinearSolver::solveOneStep( wa2 = (wa2-wa3)/pnorm; /* compute the qr factorization of the updated jacobian. */ - ei_r1updt(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing); - ei_r1mpyq(n, n, fjac.data(), wa2.data(), wa3.data()); - ei_r1mpyq(1, n, qtf.data(), wa2.data(), wa3.data()); + ei_r1updt(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing); + ei_r1mpyq(n, n, fjac.data(), v_givens, w_givens); + ei_r1mpyq(1, n, qtf.data(), v_givens, w_givens); jeval = false; } @@ -465,6 +467,8 @@ HybridNonLinearSolver::solveNumericalDiffOneStep( ) { int j; + std::vector > 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::solveNumericalDiffOneStep( wa2 = (wa2-wa3)/pnorm; /* compute the qr factorization of the updated jacobian. */ - ei_r1updt(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing); - ei_r1mpyq(n, n, fjac.data(), wa2.data(), wa3.data()); - ei_r1mpyq(1, n, qtf.data(), wa2.data(), wa3.data()); + ei_r1updt(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing); + ei_r1mpyq(n, n, fjac.data(), v_givens, w_givens); + ei_r1mpyq(1, n, qtf.data(), v_givens, w_givens); jeval = false; } diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h index 1a3ca12ae..adbb3c835 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h +++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h @@ -468,8 +468,7 @@ LevenbergMarquardt::minimizeOptimumStorageOneStep( int rownb = 2; for (i = 0; i < m; ++i) { if (functor.df(x, wa3, rownb) < 0) return UserAsked; - temp = fvec[i]; - ei_rwupdt(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data()); + ei_rwupdt(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), fvec[i]); ++rownb; } ++njev; diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h index ac1feeffc..855cb7a1b 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h +++ b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h @@ -2,46 +2,21 @@ // TODO : move this to GivensQR once there's such a thing in Eigen template -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 > &v_givens, const std::vector > &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=0; --j) + for (int i = 0; i 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 -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 > &v_givens, + std::vector > &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 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::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]; diff --git a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h index 38d614ae0..38676e720 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h +++ b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h @@ -1,18 +1,15 @@ -template -void ei_rwupdt(int n, Scalar *r__, int ldr, - const Scalar *w, Scalar *b, Scalar *alpha, Scalar *cos__, - Scalar *sin__) + template +void ei_rwupdt(int n, Scalar *r__, int ldr, const Scalar *w, Scalar *b, Scalar alpha) { + std::vector > 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; } }