dogleg.h
Go to the documentation of this file.
00001 namespace Eigen { 
00002 
00003 namespace internal {
00004 
00005 template <typename Scalar>
00006 void dogleg(
00007         const Matrix< Scalar, Dynamic, Dynamic >  &qrfac,
00008         const Matrix< Scalar, Dynamic, 1 >  &diag,
00009         const Matrix< Scalar, Dynamic, 1 >  &qtb,
00010         Scalar delta,
00011         Matrix< Scalar, Dynamic, 1 >  &x)
00012 {
00013     typedef DenseIndex Index;
00014 
00015     /* Local variables */
00016     Index i, j;
00017     Scalar sum, temp, alpha, bnorm;
00018     Scalar gnorm, qnorm;
00019     Scalar sgnorm;
00020 
00021     /* Function Body */
00022     const Scalar epsmch = NumTraits<Scalar>::epsilon();
00023     const Index n = qrfac.cols();
00024     assert(n==qtb.size());
00025     assert(n==x.size());
00026     assert(n==diag.size());
00027     Matrix< Scalar, Dynamic, 1 >  wa1(n), wa2(n);
00028 
00029     /* first, calculate the gauss-newton direction. */
00030     for (j = n-1; j >=0; --j) {
00031         temp = qrfac(j,j);
00032         if (temp == 0.) {
00033             temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
00034             if (temp == 0.)
00035                 temp = epsmch;
00036         }
00037         if (j==n-1)
00038             x[j] = qtb[j] / temp;
00039         else
00040             x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
00041     }
00042 
00043     /* test whether the gauss-newton direction is acceptable. */
00044     qnorm = diag.cwiseProduct(x).stableNorm();
00045     if (qnorm <= delta)
00046         return;
00047 
00048     // TODO : this path is not tested by Eigen unit tests
00049 
00050     /* the gauss-newton direction is not acceptable. */
00051     /* next, calculate the scaled gradient direction. */
00052 
00053     wa1.fill(0.);
00054     for (j = 0; j < n; ++j) {
00055         wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
00056         wa1[j] /= diag[j];
00057     }
00058 
00059     /* calculate the norm of the scaled gradient and test for */
00060     /* the special case in which the scaled gradient is zero. */
00061     gnorm = wa1.stableNorm();
00062     sgnorm = 0.;
00063     alpha = delta / qnorm;
00064     if (gnorm == 0.)
00065         goto algo_end;
00066 
00067     /* calculate the point along the scaled gradient */
00068     /* at which the quadratic is minimized. */
00069     wa1.array() /= (diag*gnorm).array();
00070     // TODO : once unit tests cover this part,:
00071     // wa2 = qrfac.template triangularView<Upper>() * wa1;
00072     for (j = 0; j < n; ++j) {
00073         sum = 0.;
00074         for (i = j; i < n; ++i) {
00075             sum += qrfac(j,i) * wa1[i];
00076         }
00077         wa2[j] = sum;
00078     }
00079     temp = wa2.stableNorm();
00080     sgnorm = gnorm / temp / temp;
00081 
00082     /* test whether the scaled gradient direction is acceptable. */
00083     alpha = 0.;
00084     if (sgnorm >= delta)
00085         goto algo_end;
00086 
00087     /* the scaled gradient direction is not acceptable. */
00088     /* finally, calculate the point along the dogleg */
00089     /* at which the quadratic is minimized. */
00090     bnorm = qtb.stableNorm();
00091     temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
00092     temp = temp - delta / qnorm * abs2(sgnorm / delta) + sqrt(abs2(temp - delta / qnorm) + (1.-abs2(delta / qnorm)) * (1.-abs2(sgnorm / delta)));
00093     alpha = delta / qnorm * (1. - abs2(sgnorm / delta)) / temp;
00094 algo_end:
00095 
00096     /* form appropriate convex combination of the gauss-newton */
00097     /* direction and the scaled gradient direction. */
00098     temp = (1.-alpha) * (std::min)(sgnorm,delta);
00099     x = temp * wa1 + alpha * x;
00100 }
00101 
00102 } // end namespace internal
00103 
00104 } // end namespace Eigen


win_eigen
Author(s): Daniel Stonier
autogenerated on Mon Oct 6 2014 12:24:23