GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-13 20:26:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

make qrsolv use eigen types

Browse Source
This commit is contained in:
Thomas Capricelli 2009-11-26 04:29:51 +01:00
parent e18caf7a01
commit 905aecf803
2 changed files with 46 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
unsupported/Eigen/src/NonLinearOptimization/lmpar.h
View File

@ -122,9 +122,7 @@ void ei_lmpar(
temp = ei_sqrt(par);
wa1 = temp * diag;
ipvt.cwise()+=1; // qrsolv() expects the fortran convention (as qrfac provides)
ei_qrsolv<Scalar>(n, r.data(), r.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data());
ipvt.cwise()-=1;
ei_qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
wa2 = diag.cwise() * x;
dxnorm = wa2.blueNorm();

94
unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
View File

@ -1,44 +1,43 @@
template <typename Scalar>
void ei_qrsolv(int n, Scalar *r__, int ldr,
#if 0
int n, Scalar *r__, int ldr,
const int *ipvt, const Scalar *diag, const Scalar *qtb, Scalar *x,
Scalar *sdiag)
{
/* System generated locals */
int r_dim1, r_offset;
#endif
template <typename Scalar>
void ei_qrsolv(
Matrix< Scalar, Dynamic, Dynamic > &r,
VectorXi &ipvt, // TODO : const once ipvt mess fixed
const Matrix< Scalar, Dynamic, 1 > &diag,
const Matrix< Scalar, Dynamic, 1 > &qtb,
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &sdiag)
{
/* Local variables */
int i, j, k, l;
Scalar tan__, cos__, sin__, sum, temp, cotan;
int nsing;
Scalar qtbpj;
Matrix< Scalar, Dynamic, 1 > wa(n+1);
/* Parameter adjustments */
--sdiag;
--x;
--qtb;
--diag;
--ipvt;
r_dim1 = ldr;
r_offset = 1 + r_dim1 * 1;
r__ -= r_offset;
int n = r.cols();
Matrix< Scalar, Dynamic, 1 > wa(n);
/* Function Body */
/* copy r and (q transpose)*b to preserve input and initialize s. */
/* in particular, save the diagonal elements of r in x. */
for (j = 1; j <= n; ++j) {
for (i = j; i <= n; ++i)
r__[i + j * r_dim1] = r__[j + i * r_dim1];
x[j] = r__[j + j * r_dim1];
for (j = 0; j < n; ++j) {
for (i = j; i < n; ++i)
r(i,j) = r(j,i);
x[j] = r(j,j);
wa[j] = qtb[j];
}
/* eliminate the diagonal matrix d using a givens rotation. */
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
/* prepare the row of d to be eliminated, locating the */
/* diagonal element using p from the qr factorization. */
@ -46,7 +45,7 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
l = ipvt[j];
if (diag[l] == 0.)
goto L90;
for (k = j; k <= n; ++k)
for (k = j; k < n; ++k)
sdiag[k] = 0.;
sdiag[j] = diag[l];
@ -55,18 +54,18 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
/* beyond the first n, which is initially zero. */
qtbpj = 0.;
for (k = j; k <= n; ++k) {
for (k = j; k < n; ++k) {
/* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */
if (sdiag[k] == 0.)
continue;
if ( ei_abs(r__[k + k * r_dim1]) < ei_abs(sdiag[k])) {
cotan = r__[k + k * r_dim1] / sdiag[k];
if ( ei_abs(r(k,k)) < ei_abs(sdiag[k])) {
cotan = r(k,k) / sdiag[k];
/* Computing 2nd power */
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
cos__ = sin__ * cotan;
} else {
tan__ = sdiag[k] / r__[k + k * r_dim1];
tan__ = sdiag[k] / r(k,k);
/* Computing 2nd power */
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
sin__ = cos__ * tan__;
@ -75,17 +74,16 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
/* compute the modified diagonal element of r and */
/* the modified element of ((q transpose)*b,0). */
r__[k + k * r_dim1] = cos__ * r__[k + k * r_dim1] + sin__ * sdiag[
k];
r(k,k) = cos__ * r(k,k) + sin__ * sdiag[k];
temp = cos__ * wa[k] + sin__ * qtbpj;
qtbpj = -sin__ * wa[k] + cos__ * qtbpj;
wa[k] = temp;
/* accumulate the tranformation in the row of s. */
for (i = k+1; i <= n; ++i) {
temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i];
sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[i];
r__[i + k * r_dim1] = temp;
for (i = k+1; i<n; ++i) {
temp = cos__ * r(i,k) + sin__ * sdiag[i];
sdiag[i] = -sin__ * r(i,k) + cos__ * sdiag[i];
r(i,k) = temp;
}
}
L90:
@ -93,30 +91,28 @@ L90:
/* store the diagonal element of s and restore */
/* the corresponding diagonal element of r. */
sdiag[j] = r__[j + j * r_dim1];
r__[j + j * r_dim1] = x[j];
sdiag[j] = r(j,j);
r(j,j) = x[j];
}
/* solve the triangular system for z. if the system is */
/* singular, then obtain a least squares solution. */
nsing = n;
for (j = 1; j <= n; ++j) {
if (sdiag[j] == 0. && nsing == n) nsing = j - 1;
if (nsing < n) wa[j] = 0.;
nsing = n-1;
for (j = 0; j < n; ++j) {
if (sdiag[j] == 0. && nsing == n-1) nsing = j - 1;
if (nsing < n-1) wa[j] = 0.;
}
for (k = 0; k <= nsing; ++k) {
j = nsing - k;
sum = 0.;
for (i = j+1; i <= nsing; ++i)
sum += r(i,j) * wa[i];
wa[j] = (wa[j] - sum) / sdiag[j];
}
if (nsing >= 1)
for (k = 1; k <= nsing; ++k) {
j = nsing - k + 1;
sum = 0.;
if (nsing>j)
for (i = j+1; i <= nsing; ++i)
sum += r__[i + j * r_dim1] * wa[i];
wa[j] = (wa[j] - sum) / sdiag[j];
}
/* permute the components of z back to components of x. */
for (j = 1; j <= n; ++j) {
for (j = 0; j < n; ++j) {
l = ipvt[j];
x[l] = wa[j];
}