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