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 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
