DogLegOptimizerExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 Example comparing DoglegOptimizer with Levenberg-Marquardt.
9 Author: Frank Dellaert
10 """
11 # pylint: disable=no-member, invalid-name
12 
13 import math
14 import argparse
15 
16 import gtsam
17 import matplotlib.pyplot as plt
18 import numpy as np
19 
20 
21 def run(args):
22  """Test Dogleg vs LM, inspired by issue #452."""
23 
24  # print parameters
25  print("num samples = {}, deltaInitial = {}".format(
26  args.num_samples, args.delta))
27 
28  # Ground truth solution
29  T11 = gtsam.Pose2(0, 0, 0)
30  T12 = gtsam.Pose2(1, 0, 0)
31  T21 = gtsam.Pose2(0, 1, 0)
32  T22 = gtsam.Pose2(1, 1, 0)
33 
34  # Factor graph
36 
37  # Priors
39  graph.add(gtsam.PriorFactorPose2(11, T11, prior))
40  graph.add(gtsam.PriorFactorPose2(21, T21, prior))
41 
42  # Odometry
43  model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
44  graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
45  graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
46 
47  # Range
48  model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)
49  graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
50 
51  params = gtsam.DoglegParams()
52  params.setDeltaInitial(args.delta) # default is 10
53 
54  # Add progressively more noise to ground truth
55  sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
56  n = len(sigmas)
57  p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n
58  for i, sigma in enumerate(sigmas):
59  dl_fails, lm_fails = 0, 0
60  # Attempt num_samples optimizations for both DL and LM
61  for _attempt in range(args.num_samples):
62  initial = gtsam.Values()
63  initial.insert(11, T11.retract(np.random.normal(0, sigma, 3)))
64  initial.insert(12, T12.retract(np.random.normal(0, sigma, 3)))
65  initial.insert(21, T21.retract(np.random.normal(0, sigma, 3)))
66  initial.insert(22, T22.retract(np.random.normal(0, sigma, 3)))
67 
68  # Run dogleg optimizer
69  dl = gtsam.DoglegOptimizer(graph, initial, params)
70  result = dl.optimize()
71  dl_fails += graph.error(result) > 1e-9
72 
73  # Run
74  lm = gtsam.LevenbergMarquardtOptimizer(graph, initial)
75  result = lm.optimize()
76  lm_fails += graph.error(result) > 1e-9
77 
78  # Calculate Bayes estimate of success probability
79  # using a beta prior of alpha=0.5, beta=0.5
80  alpha, beta = 0.5, 0.5
81  v = args.num_samples+alpha+beta
82  p_dl[i] = (args.num_samples-dl_fails+alpha)/v
83  p_lm[i] = (args.num_samples-lm_fails+alpha)/v
84 
85  def stddev(p):
86  """Calculate standard deviation."""
87  return math.sqrt(p*(1-p)/(1+v))
88 
89  s_dl[i] = stddev(p_dl[i])
90  s_lm[i] = stddev(p_lm[i])
91 
92  fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%"
93  print(fmt.format(sigma,
94  100*p_dl[i], 100*s_dl[i],
95  100*p_lm[i], 100*s_lm[i]))
96 
97  if args.plot:
98  fig, ax = plt.subplots()
99  dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg")
100  lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM")
101  plt.title("Dogleg emprical success vs. LM")
102  plt.legend(handles=[dl_plot, lm_plot])
103  ax.set_xlim(0, sigmas[-1]+1)
104  ax.set_ylim(0, 1)
105  plt.show()
106 
107 
108 if __name__ == "__main__":
109  parser = argparse.ArgumentParser(
110  description="Compare Dogleg and LM success rates")
111  parser.add_argument("-n", "--num_samples", type=int, default=1000,
112  help="Number of samples for each sigma")
113  parser.add_argument("-d", "--delta", type=float, default=10.0,
114  help="Initial delta for dogleg")
115  parser.add_argument("-p", "--plot", action="store_true",
116  help="Flag to plot results")
117  args = parser.parse_args()
118  run(args)
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
size_t len(handle h)
Definition: pytypes.h:1514
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:567


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:41:59