DoglegOptimizerImpl.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #include <cmath>
20 
21 using namespace std;
22 
23 namespace gtsam {
24 /* ************************************************************************* */
25 VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
26  double delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) {
27 
28  // Get magnitude of each update and find out which segment delta falls in
29  assert(delta >= 0.0);
30  double deltaSq = delta*delta;
31  double x_u_norm_sq = dx_u.squaredNorm();
32  double x_n_norm_sq = dx_n.squaredNorm();
33  if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl;
34  if(deltaSq < x_u_norm_sq) {
35  // Trust region is smaller than steepest descent update
36  VectorValues x_d = std::sqrt(deltaSq / x_u_norm_sq) * dx_u;
37  if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(deltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
38  return x_d;
39  } else if(deltaSq < x_n_norm_sq) {
40  // Trust region boundary is between steepest descent point and Newton's method point
41  return ComputeBlend(delta, dx_u, dx_n, verbose);
42  } else {
43  assert(deltaSq >= x_n_norm_sq);
44  if(verbose) cout << "In pure Newton's method region" << endl;
45  // Trust region is larger than Newton's method point
46  return dx_n;
47  }
48 }
49 
50 /* ************************************************************************* */
51 VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) {
52 
53  // See doc/trustregion.lyx or doc/trustregion.pdf
54 
55  // Compute inner products
56  const double un = dot(x_u, x_n);
57  const double uu = dot(x_u, x_u);
58  const double nn = dot(x_n, x_n);
59 
60  // Compute quadratic formula terms
61  const double a = uu - 2.*un + nn;
62  const double b = 2. * (un - uu);
63  const double c = uu - delta*delta;
64  double sqrt_b_m4ac = std::sqrt(b*b - 4*a*c);
65 
66  // Compute blending parameter
67  double tau1 = (-b + sqrt_b_m4ac) / (2.*a);
68  double tau2 = (-b - sqrt_b_m4ac) / (2.*a);
69 
70  // Determine correct solution accounting for machine precision
71  double tau;
72  const double eps = std::numeric_limits<double>::epsilon();
73  if(-eps <= tau1 && tau1 <= 1.0 + eps) {
74  assert(!(-eps <= tau2 && tau2 <= 1.0 + eps));
75  tau = tau1;
76  } else {
77  assert(-eps <= tau2 && tau2 <= 1.0 + eps);
78  tau = tau2;
79  }
80 
81  // Compute blended point
82  if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
83  VectorValues blend = (1. - tau) * x_u;
84  blend += tau * x_n;
85  return blend;
86 }
87 
88 }
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:49
epsilon
static double epsilon
Definition: testRot3.cpp:37
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::VectorValues::squaredNorm
double squaredNorm() const
Definition: VectorValues.cpp:260
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
std
Definition: BFloat16.h:88
DoglegOptimizerImpl.h
Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
nn
idx_t * nn
Definition: include/metis.h:207
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:13