mirror/eigen
2
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2024-12-15 07:10:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

cleaning

Browse Source
This commit is contained in:
Thomas Capricelli 2009-11-26 07:37:37 +01:00
parent db39f892a3
commit f948df5a72
1 changed files with 26 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -18,8 +18,7 @@ void ei_qrsolv(
{ {
/* Local variables */ /* Local variables */
int i, j, k, l; int i, j, k, l;
Scalar tan__, cos__, sin__, sum, temp, cotan; Scalar sum, temp;
int nsing;
Scalar qtbpj; Scalar qtbpj;
int n = r.cols(); int n = r.cols();
Matrix< Scalar, Dynamic, 1 > wa(n); Matrix< Scalar, Dynamic, 1 > wa(n);
@ -29,12 +28,12 @@ void ei_qrsolv(
/* copy r and (q transpose)*b to preserve input and initialize s. */ /* copy r and (q transpose)*b to preserve input and initialize s. */
/* in particular, save the diagonal elements of r in x. */ /* in particular, save the diagonal elements of r in x. */
for (j = 0; j < n; ++j) { x = r.diagonal();
for (i = j; i < n; ++i) wa = qtb;
for (j = 0; j < n; ++j)
for (i = j+1; i < n; ++i)
r(i,j) = r(j,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. */ /* eliminate the diagonal matrix d using a givens rotation. */
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j) {
@ -44,9 +43,8 @@ void ei_qrsolv(
l = ipvt[j]; l = ipvt[j];
if (diag[l] == 0.) if (diag[l] == 0.)
goto L90; break;
for (k = j; k < n; ++k) sdiag.segment(j,n-j).setZero();
sdiag[k] = 0.;
sdiag[j] = diag[l]; sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */ /* the transformations to eliminate the row of d */
@ -57,54 +55,39 @@ void ei_qrsolv(
for (k = j; k < n; ++k) { for (k = j; k < n; ++k) {
/* determine a givens rotation which eliminates the */ /* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */ /* appropriate element in the current row of d. */
if (sdiag[k] == 0.) PlanarRotation<Scalar> givens;
continue; givens.makeGivens(-r(k,k), 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);
/* Computing 2nd power */
cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
sin__ = cos__ * tan__;
}
/* compute the modified diagonal element of r and */ /* compute the modified diagonal element of r and */
/* the modified element of ((q transpose)*b,0). */ /* the modified element of ((q transpose)*b,0). */
r(k,k) = cos__ * r(k,k) + sin__ * sdiag[k]; r(k,k) = givens.c() * r(k,k) + givens.s() * sdiag[k];
temp = cos__ * wa[k] + sin__ * qtbpj; temp = givens.c() * wa[k] + givens.s() * qtbpj;
qtbpj = -sin__ * wa[k] + cos__ * qtbpj; qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
wa[k] = temp; wa[k] = temp;
/* accumulate the tranformation in the row of s. */ /* accumulate the tranformation in the row of s. */
for (i = k+1; i<n; ++i) { for (i = k+1; i<n; ++i) {
temp = cos__ * r(i,k) + sin__ * sdiag[i]; temp = givens.c() * r(i,k) + givens.s() * sdiag[i];
sdiag[i] = -sin__ * r(i,k) + cos__ * sdiag[i]; sdiag[i] = -givens.s() * r(i,k) + givens.c() * sdiag[i];
r(i,k) = temp; r(i,k) = temp;
} }
} }
L90:
/* store the diagonal element of s and restore */
/* the corresponding diagonal element of r. */
sdiag[j] = r(j,j);
r(j,j) = x[j];
} }
// restore
sdiag = r.diagonal();
r.diagonal() = x;
/* solve the triangular system for z. if the system is */ /* solve the triangular system for z. if the system is */
/* singular, then obtain a least squares solution. */ /* singular, then obtain a least squares solution. */
nsing = n-1; int nsing;
for (j = 0; j < n; ++j) { for (nsing=0; nsing<n && sdiag[nsing]!=0; nsing++);
if (sdiag[j] == 0. && nsing == n-1) nsing = j - 1; wa.segment(nsing,n-nsing).setZero();
if (nsing < n-1) wa[j] = 0.; nsing--; // nsing is the last nonsingular index
}
for (k = 0; k <= nsing; ++k) { for (j = nsing; j>=0; j--) {
j = nsing - k;
sum = 0.; sum = 0.;
for (i = j+1; i <= nsing; ++i) for (i = j+1; i <= nsing; ++i)
sum += r(i,j) * wa[i]; sum += r(i,j) * wa[i];
@ -112,9 +95,6 @@ L90:
} }
/* permute the components of z back to components of x. */ /* permute the components of z back to components of x. */
for (j = 0; j < n; ++j) { for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
l = ipvt[j];
x[l] = wa[j];
}
} }