dogleg.h
Go to the documentation of this file.
1 namespace Eigen {
2 
3 namespace internal {
4 
5 template <typename Scalar>
6 void dogleg(
10  Scalar delta,
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
IntermediateState sqrt(const Expression &arg)
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs2_op< Scalar >, const Derived > abs2() const
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: XprHelper.h:27
EIGEN_STRONG_INLINE Index cols() const
#define eigen_assert(x)
void 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)
Definition: dogleg.h:6


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:32