1namespace Eigen {
2
3namespace internal {
4
5template <typename Scalar>
6void dogleg(
7        const Matrix< Scalar, Dynamic, Dynamic >  &qrfac,
8        const Matrix< Scalar, Dynamic, 1 >  &diag,
9        const Matrix< Scalar, Dynamic, 1 >  &qtb,
10        Scalar delta,
11        Matrix< Scalar, Dynamic, 1 >  &x)
12{
13    using std::abs;
14    using std::sqrt;
15
16    typedef DenseIndex Index;
17
18    /* Local variables */
19    Index i, j;
20    Scalar sum, temp, alpha, bnorm;
21    Scalar gnorm, qnorm;
22    Scalar sgnorm;
23
24    /* Function Body */
25    const Scalar epsmch = NumTraits<Scalar>::epsilon();
26    const Index n = qrfac.cols();
27    eigen_assert(n==qtb.size());
28    eigen_assert(n==x.size());
29    eigen_assert(n==diag.size());
30    Matrix< Scalar, Dynamic, 1 >  wa1(n), wa2(n);
31
32    /* first, calculate the gauss-newton direction. */
33    for (j = n-1; j >=0; --j) {
34        temp = qrfac(j,j);
35        if (temp == 0.) {
36            temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
37            if (temp == 0.)
38                temp = epsmch;
39        }
40        if (j==n-1)
41            x[j] = qtb[j] / temp;
42        else
43            x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
44    }
45
46    /* test whether the gauss-newton direction is acceptable. */
47    qnorm = diag.cwiseProduct(x).stableNorm();
48    if (qnorm <= delta)
49        return;
50
51    // TODO : this path is not tested by Eigen unit tests
52
53    /* the gauss-newton direction is not acceptable. */
54    /* next, calculate the scaled gradient direction. */
55
56    wa1.fill(0.);
57    for (j = 0; j < n; ++j) {
58        wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
59        wa1[j] /= diag[j];
60    }
61
62    /* calculate the norm of the scaled gradient and test for */
63    /* the special case in which the scaled gradient is zero. */
64    gnorm = wa1.stableNorm();
65    sgnorm = 0.;
66    alpha = delta / qnorm;
67    if (gnorm == 0.)
68        goto algo_end;
69
70    /* calculate the point along the scaled gradient */
71    /* at which the quadratic is minimized. */
72    wa1.array() /= (diag*gnorm).array();
73    // TODO : once unit tests cover this part,:
74    // wa2 = qrfac.template triangularView<Upper>() * wa1;
75    for (j = 0; j < n; ++j) {
76        sum = 0.;
77        for (i = j; i < n; ++i) {
78            sum += qrfac(j,i) * wa1[i];
79        }
80        wa2[j] = sum;
81    }
82    temp = wa2.stableNorm();
83    sgnorm = gnorm / temp / temp;
84
85    /* test whether the scaled gradient direction is acceptable. */
86    alpha = 0.;
87    if (sgnorm >= delta)
88        goto algo_end;
89
90    /* the scaled gradient direction is not acceptable. */
91    /* finally, calculate the point along the dogleg */
92    /* at which the quadratic is minimized. */
93    bnorm = qtb.stableNorm();
94    temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
95    temp = temp - delta / qnorm * numext::abs2(sgnorm / delta) + sqrt(numext::abs2(temp - delta / qnorm) + (1.-numext::abs2(delta / qnorm)) * (1.-numext::abs2(sgnorm / delta)));
96    alpha = delta / qnorm * (1. - numext::abs2(sgnorm / delta)) / temp;
97algo_end:
98
99    /* form appropriate convex combination of the gauss-newton */
100    /* direction and the scaled gradient direction. */
101    temp = (1.-alpha) * (std::min)(sgnorm,delta);
102    x = temp * wa1 + alpha * x;
103}
104
105} // end namespace internal
106
107} // end namespace Eigen
108