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  double tau;
71  if(0.0 <= tau1 && tau1 <= 1.0) {
72  assert(!(0.0 <= tau2 && tau2 <= 1.0));
73  tau = tau1;
74  } else {
75  assert(0.0 <= tau2 && tau2 <= 1.0);
76  tau = tau2;
77  }
78 
79  // Compute blended point
80  if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
81  VectorValues blend = (1. - tau) * x_u;
82  blend += tau * x_n;
83  return blend;
84 }
85 
86 }
Scalar * b
Definition: benchVecAdd.cpp:17
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Definition: BFloat16.h:88
Nonlinear factor graph optimizer using Powell&#39;s Dogleg algorithm (detail implementation) ...
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
traits
Definition: chartTesting.h:28
double squaredNorm() const
idx_t * nn
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:10