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
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
array
int array[24]
Definition: Map_general_stride.cpp:1
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
eigen_assert
#define eigen_assert(x)
Definition: Macros.h:1037
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
Eigen::internal::dogleg
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
n
int n
Definition: BiCGSTAB_simple.cpp:1
epsilon
static double epsilon
Definition: testRot3.cpp:37
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
anyset::size
size_t size() const
Definition: pytypes.h:2220
Eigen::numext::abs2
EIGEN_DEVICE_FUNC bool abs2(bool x)
Definition: Eigen/src/Core/MathFunctions.h:1294
Eigen::PlainObjectBase::cols
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:145
Eigen::DenseIndex
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: Meta.h:66
min
#define min(a, b)
Definition: datatypes.h:19
Eigen::Matrix< Scalar, Dynamic, Dynamic >
abs
#define abs(x)
Definition: datatypes.h:17
internal
Definition: BandTriangularSolver.h:13
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:26