Go to the documentation of this file.00001 namespace internal {
00002
00003 template <typename Scalar>
00004 void dogleg(
00005 const Matrix< Scalar, Dynamic, Dynamic > &qrfac,
00006 const Matrix< Scalar, Dynamic, 1 > &diag,
00007 const Matrix< Scalar, Dynamic, 1 > &qtb,
00008 Scalar delta,
00009 Matrix< Scalar, Dynamic, 1 > &x)
00010 {
00011 typedef DenseIndex Index;
00012
00013
00014 Index i, j;
00015 Scalar sum, temp, alpha, bnorm;
00016 Scalar gnorm, qnorm;
00017 Scalar sgnorm;
00018
00019
00020 const Scalar epsmch = NumTraits<Scalar>::epsilon();
00021 const Index n = qrfac.cols();
00022 assert(n==qtb.size());
00023 assert(n==x.size());
00024 assert(n==diag.size());
00025 Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
00026
00027
00028 for (j = n-1; j >=0; --j) {
00029 temp = qrfac(j,j);
00030 if (temp == 0.) {
00031 temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
00032 if (temp == 0.)
00033 temp = epsmch;
00034 }
00035 if (j==n-1)
00036 x[j] = qtb[j] / temp;
00037 else
00038 x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
00039 }
00040
00041
00042 qnorm = diag.cwiseProduct(x).stableNorm();
00043 if (qnorm <= delta)
00044 return;
00045
00046
00047
00048
00049
00050
00051 wa1.fill(0.);
00052 for (j = 0; j < n; ++j) {
00053 wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
00054 wa1[j] /= diag[j];
00055 }
00056
00057
00058
00059 gnorm = wa1.stableNorm();
00060 sgnorm = 0.;
00061 alpha = delta / qnorm;
00062 if (gnorm == 0.)
00063 goto algo_end;
00064
00065
00066
00067 wa1.array() /= (diag*gnorm).array();
00068
00069
00070 for (j = 0; j < n; ++j) {
00071 sum = 0.;
00072 for (i = j; i < n; ++i) {
00073 sum += qrfac(j,i) * wa1[i];
00074 }
00075 wa2[j] = sum;
00076 }
00077 temp = wa2.stableNorm();
00078 sgnorm = gnorm / temp / temp;
00079
00080
00081 alpha = 0.;
00082 if (sgnorm >= delta)
00083 goto algo_end;
00084
00085
00086
00087
00088 bnorm = qtb.stableNorm();
00089 temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
00090 temp = temp - delta / qnorm * abs2(sgnorm / delta) + sqrt(abs2(temp - delta / qnorm) + (1.-abs2(delta / qnorm)) * (1.-abs2(sgnorm / delta)));
00091 alpha = delta / qnorm * (1. - abs2(sgnorm / delta)) / temp;
00092 algo_end:
00093
00094
00095
00096 temp = (1.-alpha) * (std::min)(sgnorm,delta);
00097 x = temp * wa1 + alpha * x;
00098 }
00099
00100 }