From 34389973c97f1d12a67bb12138fb30fec0a6bbc7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 16:54:25 -0400 Subject: [PATCH 1/4] Initial commit for dogleg test --- cython/gtsam/tests/test_DogLegOptimizer.py | 78 ++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 cython/gtsam/tests/test_DogLegOptimizer.py diff --git a/cython/gtsam/tests/test_DogLegOptimizer.py b/cython/gtsam/tests/test_DogLegOptimizer.py new file mode 100644 index 000000000..20c8bfb94 --- /dev/null +++ b/cython/gtsam/tests/test_DogLegOptimizer.py @@ -0,0 +1,78 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +DoglegOptimizer unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-member, invalid-name +import unittest + +import gtsam +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + + +class TestDoglegOptimizer(GtsamTestCase): + + def test_DoglegOptimizer(self): + # Linearization point + T11 = gtsam.Pose2(0, 0, 0) + T12 = gtsam.Pose2(1, 0, 0) + T21 = gtsam.Pose2(0, 1, 0) + T22 = gtsam.Pose2(1, 1, 0) + + # Factor graph + graph = gtsam.NonlinearFactorGraph() + + # Priors + prior = gtsam.noiseModel_Isotropic.Sigma(3, 1) + graph.add(gtsam.PriorFactorPose2(11, T11, prior)) + graph.add(gtsam.PriorFactorPose2(21, T21, prior)) + + # Odometry + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 1e6])) + graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) + graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) + + # Range + model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01) + graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) + + # Print graph + print(graph) + + sigma = 0.1 + values = gtsam.Values() + values.insert(11, T11.retract(np.random.normal(0, sigma, 3))) + values.insert(12, T12) + values.insert(21, T21) + values.insert(22, T22) + linearized = graph.linearize(values) + + # Get Jacobian + ordering = gtsam.Ordering() + ordering.push_back(11) + ordering.push_back(21) + ordering.push_back(12) + ordering.push_back(22) + A, b = linearized.jacobian(ordering) + Q = np.dot(A.transpose(), A) + print(np.linalg.det(Q)) + + bn = linearized.eliminateSequential(ordering) + + # Print gradient + linearized.gradientAtZero() + + # Run dogleg optimizer + dl = gtsam.DoglegOptimizer(graph, values) + result = dl.optimize() + print(graph.error(result)) + + +if __name__ == "__main__": + unittest.main() From c2a5d41eea15fe30b3b73a7deed16efb587ddd20 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 15 May 2019 23:59:16 -0400 Subject: [PATCH 2/4] Graphing Dogleg vs LM with beta distribution error bars --- cython/gtsam/tests/test_DogLegOptimizer.py | 85 +++++++++++++++------- 1 file changed, 58 insertions(+), 27 deletions(-) diff --git a/cython/gtsam/tests/test_DogLegOptimizer.py b/cython/gtsam/tests/test_DogLegOptimizer.py index 20c8bfb94..f9d9c06ff 100644 --- a/cython/gtsam/tests/test_DogLegOptimizer.py +++ b/cython/gtsam/tests/test_DogLegOptimizer.py @@ -9,17 +9,21 @@ DoglegOptimizer unit tests. Author: Frank Dellaert """ # pylint: disable=no-member, invalid-name + +import math import unittest import gtsam +import matplotlib.pyplot as plt import numpy as np from gtsam.utils.test_case import GtsamTestCase class TestDoglegOptimizer(GtsamTestCase): + """Test Dogleg vs LM, isnpired by issue #452.""" def test_DoglegOptimizer(self): - # Linearization point + # Ground truth solution T11 = gtsam.Pose2(0, 0, 0) T12 = gtsam.Pose2(1, 0, 0) T21 = gtsam.Pose2(0, 1, 0) @@ -34,7 +38,7 @@ class TestDoglegOptimizer(GtsamTestCase): graph.add(gtsam.PriorFactorPose2(21, T21, prior)) # Odometry - model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 1e6])) + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) @@ -42,36 +46,63 @@ class TestDoglegOptimizer(GtsamTestCase): model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01) graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) - # Print graph - print(graph) + num_samples = 1000 + print("num samples = {}%".format(num_samples)) - sigma = 0.1 - values = gtsam.Values() - values.insert(11, T11.retract(np.random.normal(0, sigma, 3))) - values.insert(12, T12) - values.insert(21, T21) - values.insert(22, T22) - linearized = graph.linearize(values) + params = gtsam.DoglegParams() + params.setDeltaInitial(10) # default was 1.0 - # Get Jacobian - ordering = gtsam.Ordering() - ordering.push_back(11) - ordering.push_back(21) - ordering.push_back(12) - ordering.push_back(22) - A, b = linearized.jacobian(ordering) - Q = np.dot(A.transpose(), A) - print(np.linalg.det(Q)) + # Add progressively more noise to ground truth + sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] + n = len(sigmas) + p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n + for i, sigma in enumerate(sigmas): + dl_fails, lm_fails = 0, 0 + # Attempt num_samples optimizations for both DL and LM + for _attempt in range(num_samples): + initial = gtsam.Values() + initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) + initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) + initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) + initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) - bn = linearized.eliminateSequential(ordering) + # Run dogleg optimizer + dl = gtsam.DoglegOptimizer(graph, initial, params) + result = dl.optimize() + dl_fails += graph.error(result) > 1e-9 - # Print gradient - linearized.gradientAtZero() + # Run + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) + result = lm.optimize() + lm_fails += graph.error(result) > 1e-9 - # Run dogleg optimizer - dl = gtsam.DoglegOptimizer(graph, values) - result = dl.optimize() - print(graph.error(result)) + # Calculate Bayes estimate of success probability + # using a beta prior of alpha=0.5, beta=0.5 + alpha, beta = 0.5, 0.5 + v = num_samples+alpha+beta + p_dl[i] = (num_samples-dl_fails+alpha)/v + p_lm[i] = (num_samples-lm_fails+alpha)/v + + def stddev(p): + """Calculate standard deviation.""" + return math.sqrt(p*(1-p)/(1+v)) + + s_dl[i] = stddev(p_dl[i]) + s_lm[i] = stddev(p_lm[i]) + + fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" + print(fmt.format(sigma, + 100*p_dl[i], 100*s_dl[i], + 100*p_lm[i], 100*s_lm[i])) + + fig, ax = plt.subplots() + dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") + lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") + plt.title("Dogleg emprical success vs. LM") + plt.legend(handles=[dl_plot, lm_plot]) + ax.set_xlim(0, sigmas[-1]+1) + ax.set_ylim(0, 1) + plt.show() if __name__ == "__main__": From 245d7eb849a9aa705c4c263b4c46779b71119370 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 May 2019 00:00:52 -0400 Subject: [PATCH 3/4] Changed default deltaInitial to 10.0 based on test_DoglegOptimizer.py --- gtsam/nonlinear/DoglegOptimizer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 7013908e5..51e6b08cc 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -37,11 +37,11 @@ public: VERBOSE }; - double deltaInitial; ///< The initial trust region radius (default: 1.0) + double deltaInitial; ///< The initial trust region radius (default: 10.0) VerbosityDL verbosityDL; ///< The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity DoglegParams() : - deltaInitial(1.0), verbosityDL(SILENT) {} + deltaInitial(10.0), verbosityDL(SILENT) {} virtual ~DoglegParams() {} From fd21f2ec71d930e8bbce5a7d79ecc03ab2178914 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 16 May 2019 10:46:10 -0400 Subject: [PATCH 4/4] Made into example rather than test --- .../gtsam/examples/DogLegOptimizerExample.py | 118 ++++++++++++++++++ cython/gtsam/tests/test_DogLegOptimizer.py | 109 ---------------- 2 files changed, 118 insertions(+), 109 deletions(-) create mode 100644 cython/gtsam/examples/DogLegOptimizerExample.py delete mode 100644 cython/gtsam/tests/test_DogLegOptimizer.py diff --git a/cython/gtsam/examples/DogLegOptimizerExample.py b/cython/gtsam/examples/DogLegOptimizerExample.py new file mode 100644 index 000000000..776ceedc4 --- /dev/null +++ b/cython/gtsam/examples/DogLegOptimizerExample.py @@ -0,0 +1,118 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Example comparing DoglegOptimizer with Levenberg-Marquardt. +Author: Frank Dellaert +""" +# pylint: disable=no-member, invalid-name + +import math +import argparse + +import gtsam +import matplotlib.pyplot as plt +import numpy as np + + +def run(args): + """Test Dogleg vs LM, inspired by issue #452.""" + + # print parameters + print("num samples = {}, deltaInitial = {}".format( + args.num_samples, args.delta)) + + # Ground truth solution + T11 = gtsam.Pose2(0, 0, 0) + T12 = gtsam.Pose2(1, 0, 0) + T21 = gtsam.Pose2(0, 1, 0) + T22 = gtsam.Pose2(1, 1, 0) + + # Factor graph + graph = gtsam.NonlinearFactorGraph() + + # Priors + prior = gtsam.noiseModel_Isotropic.Sigma(3, 1) + graph.add(gtsam.PriorFactorPose2(11, T11, prior)) + graph.add(gtsam.PriorFactorPose2(21, T21, prior)) + + # Odometry + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) + graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) + graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) + + # Range + model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01) + graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) + + params = gtsam.DoglegParams() + params.setDeltaInitial(args.delta) # default is 10 + + # Add progressively more noise to ground truth + sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] + n = len(sigmas) + p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n + for i, sigma in enumerate(sigmas): + dl_fails, lm_fails = 0, 0 + # Attempt num_samples optimizations for both DL and LM + for _attempt in range(args.num_samples): + initial = gtsam.Values() + initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) + initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) + initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) + initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) + + # Run dogleg optimizer + dl = gtsam.DoglegOptimizer(graph, initial, params) + result = dl.optimize() + dl_fails += graph.error(result) > 1e-9 + + # Run + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) + result = lm.optimize() + lm_fails += graph.error(result) > 1e-9 + + # Calculate Bayes estimate of success probability + # using a beta prior of alpha=0.5, beta=0.5 + alpha, beta = 0.5, 0.5 + v = args.num_samples+alpha+beta + p_dl[i] = (args.num_samples-dl_fails+alpha)/v + p_lm[i] = (args.num_samples-lm_fails+alpha)/v + + def stddev(p): + """Calculate standard deviation.""" + return math.sqrt(p*(1-p)/(1+v)) + + s_dl[i] = stddev(p_dl[i]) + s_lm[i] = stddev(p_lm[i]) + + fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" + print(fmt.format(sigma, + 100*p_dl[i], 100*s_dl[i], + 100*p_lm[i], 100*s_lm[i])) + + if args.plot: + fig, ax = plt.subplots() + dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") + lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") + plt.title("Dogleg emprical success vs. LM") + plt.legend(handles=[dl_plot, lm_plot]) + ax.set_xlim(0, sigmas[-1]+1) + ax.set_ylim(0, 1) + plt.show() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Compare Dogleg and LM success rates") + parser.add_argument("-n", "--num_samples", type=int, default=1000, + help="Number of samples for each sigma") + parser.add_argument("-d", "--delta", type=float, default=10.0, + help="Initial delta for dogleg") + parser.add_argument("-p", "--plot", action="store_true", + help="Flag to plot results") + args = parser.parse_args() + run(args) diff --git a/cython/gtsam/tests/test_DogLegOptimizer.py b/cython/gtsam/tests/test_DogLegOptimizer.py deleted file mode 100644 index f9d9c06ff..000000000 --- a/cython/gtsam/tests/test_DogLegOptimizer.py +++ /dev/null @@ -1,109 +0,0 @@ -""" -GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved - -See LICENSE for the license information - -DoglegOptimizer unit tests. -Author: Frank Dellaert -""" -# pylint: disable=no-member, invalid-name - -import math -import unittest - -import gtsam -import matplotlib.pyplot as plt -import numpy as np -from gtsam.utils.test_case import GtsamTestCase - - -class TestDoglegOptimizer(GtsamTestCase): - """Test Dogleg vs LM, isnpired by issue #452.""" - - def test_DoglegOptimizer(self): - # Ground truth solution - T11 = gtsam.Pose2(0, 0, 0) - T12 = gtsam.Pose2(1, 0, 0) - T21 = gtsam.Pose2(0, 1, 0) - T22 = gtsam.Pose2(1, 1, 0) - - # Factor graph - graph = gtsam.NonlinearFactorGraph() - - # Priors - prior = gtsam.noiseModel_Isotropic.Sigma(3, 1) - graph.add(gtsam.PriorFactorPose2(11, T11, prior)) - graph.add(gtsam.PriorFactorPose2(21, T21, prior)) - - # Odometry - model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) - graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) - graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) - - # Range - model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01) - graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) - - num_samples = 1000 - print("num samples = {}%".format(num_samples)) - - params = gtsam.DoglegParams() - params.setDeltaInitial(10) # default was 1.0 - - # Add progressively more noise to ground truth - sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] - n = len(sigmas) - p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n - for i, sigma in enumerate(sigmas): - dl_fails, lm_fails = 0, 0 - # Attempt num_samples optimizations for both DL and LM - for _attempt in range(num_samples): - initial = gtsam.Values() - initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) - initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) - initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) - initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) - - # Run dogleg optimizer - dl = gtsam.DoglegOptimizer(graph, initial, params) - result = dl.optimize() - dl_fails += graph.error(result) > 1e-9 - - # Run - lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) - result = lm.optimize() - lm_fails += graph.error(result) > 1e-9 - - # Calculate Bayes estimate of success probability - # using a beta prior of alpha=0.5, beta=0.5 - alpha, beta = 0.5, 0.5 - v = num_samples+alpha+beta - p_dl[i] = (num_samples-dl_fails+alpha)/v - p_lm[i] = (num_samples-lm_fails+alpha)/v - - def stddev(p): - """Calculate standard deviation.""" - return math.sqrt(p*(1-p)/(1+v)) - - s_dl[i] = stddev(p_dl[i]) - s_lm[i] = stddev(p_lm[i]) - - fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" - print(fmt.format(sigma, - 100*p_dl[i], 100*s_dl[i], - 100*p_lm[i], 100*s_lm[i])) - - fig, ax = plt.subplots() - dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") - lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") - plt.title("Dogleg emprical success vs. LM") - plt.legend(handles=[dl_plot, lm_plot]) - ax.set_xlim(0, sigmas[-1]+1) - ax.set_ylim(0, 1) - plt.show() - - -if __name__ == "__main__": - unittest.main()