2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Example comparing DoglegOptimizer with Levenberg-Marquardt.
17 import matplotlib.pyplot
as plt
22 """Test Dogleg vs LM, inspired by issue #452."""
26 args.num_samples, args.delta))
39 graph.add(gtsam.PriorFactorPose2(11, T11, prior))
40 graph.add(gtsam.PriorFactorPose2(21, T21, prior))
44 graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
45 graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
49 graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
52 params.setDeltaInitial(args.delta)
55 sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
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
61 for _attempt
in range(args.num_samples):
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)))
70 result = dl.optimize()
71 dl_fails += graph.error(result) > 1e-9
75 result = lm.optimize()
76 lm_fails += graph.error(result) > 1e-9
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
86 """Calculate standard deviation."""
87 return math.sqrt(p*(1-p)/(1+v))
89 s_dl[i] = stddev(p_dl[i])
90 s_lm[i] = stddev(p_lm[i])
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]))
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)
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()