1 namespace Eigen {
2
3 namespace internal {
4
5 template <typename Scalar>
dogleg(const Matrix<Scalar,Dynamic,Dynamic> & qrfac,const Matrix<Scalar,Dynamic,1> & diag,const Matrix<Scalar,Dynamic,1> & qtb,Scalar delta,Matrix<Scalar,Dynamic,1> & x)6 void 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;
97 algo_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