test_NonlinearOptimizer.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 Unit tests for IMU testing scenarios.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 # pylint: disable=invalid-name, no-name-in-module
12 
13 from __future__ import print_function
14 
15 import unittest
16 
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 import gtsam
20 from gtsam import (DoglegOptimizer, DoglegParams,
21  DummyPreconditionerParameters, GaussNewtonOptimizer,
22  GaussNewtonParams, GncLMOptimizer, GncLMParams, GncLossType,
23  LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
24  NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
25  PriorFactorPoint2, Values)
26 
27 KEY1 = 1
28 KEY2 = 2
29 
30 
32  """Do trivial test with three optimizer variants."""
33  def setUp(self):
34  """Set up the optimization problem and ordering"""
35  # create graph
38  self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
39 
40  # test error at minimum
41  xstar = Point2(0, 0)
43  self.optimal_values.insert(KEY1, xstar)
44  self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
45 
46  # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
47  x0 = Point2(3, 3)
49  self.initial_values.insert(KEY1, x0)
50  self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
51 
52  # optimize parameters
53  self.ordering = Ordering()
54  self.ordering.push_back(KEY1)
55 
56  def test_gauss_newton(self):
57  gnParams = GaussNewtonParams()
58  gnParams.setOrdering(self.ordering)
59  actual = GaussNewtonOptimizer(self.fg, self.initial_values, gnParams).optimize()
60  self.assertAlmostEqual(0, self.fg.error(actual))
61 
63  lmParams = LevenbergMarquardtParams.CeresDefaults()
64  lmParams.setOrdering(self.ordering)
65  actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
66  self.assertAlmostEqual(0, self.fg.error(actual))
67 
69  lmParams = LevenbergMarquardtParams.CeresDefaults()
70  lmParams.setLinearSolverType("ITERATIVE")
71  cgParams = PCGSolverParameters()
72  cgParams.preconditioner = DummyPreconditionerParameters()
73  lmParams.setIterativeParams(cgParams)
74  actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
75  self.assertAlmostEqual(0, self.fg.error(actual))
76 
77  def test_dogleg(self):
78  dlParams = DoglegParams()
79  dlParams.setOrdering(self.ordering)
80  actual = DoglegOptimizer(self.fg, self.initial_values, dlParams).optimize()
81  self.assertAlmostEqual(0, self.fg.error(actual))
82 
84  gncParams = GncLMParams()
85  actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
86  self.assertAlmostEqual(0, self.fg.error(actual))
87 
88  def test_gnc_params(self):
89  base_params = LevenbergMarquardtParams()
90  # Test base params
91  for base_max_iters in (50, 100):
92  base_params.setMaxIterations(base_max_iters)
93  params = GncLMParams(base_params)
94  self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters)
95 
96  # Test printing
97  params_str = str(params)
98  for s in (
99  "lossType",
100  "maxIterations",
101  "muStep",
102  "relativeCostTol",
103  "weightsTol",
104  "verbosity",
105  ):
106  self.assertTrue(s in params_str)
107 
108  # Test each parameter
109  for loss_type in (GncLossType.TLS, GncLossType.GM):
110  params.setLossType(loss_type) # Default is TLS
111  self.assertEqual(params.lossType, loss_type)
112  for max_iter in (1, 10, 100):
113  params.setMaxIterations(max_iter)
114  self.assertEqual(params.maxIterations, max_iter)
115  for mu_step in (1.1, 1.2, 1.5):
116  params.setMuStep(mu_step)
117  self.assertEqual(params.muStep, mu_step)
118  for rel_cost_tol in (1e-5, 1e-6, 1e-7):
119  params.setRelativeCostTol(rel_cost_tol)
120  self.assertEqual(params.relativeCostTol, rel_cost_tol)
121  for weights_tol in (1e-4, 1e-3, 1e-2):
122  params.setWeightsTol(weights_tol)
123  self.assertEqual(params.weightsTol, weights_tol)
124  for i in (0, 1, 2):
125  verb = GncLMParams.Verbosity(i)
126  params.setVerbosityGNC(verb)
127  self.assertEqual(params.verbosity, verb)
128  for inl in ([], [10], [0, 100]):
129  params.setKnownInliers(inl)
130  self.assertEqual(params.knownInliers, inl)
131  params.knownInliers = []
132  for out in ([], [1], [0, 10]):
133  params.setKnownInliers(out)
134  self.assertEqual(params.knownInliers, out)
135  params.knownInliers = []
136 
137  # Test optimizer params
138  optimizer = GncLMOptimizer(self.fg, self.initial_values, params)
139  for ict_factor in (0.9, 1.1):
140  new_ict = ict_factor * optimizer.getInlierCostThresholds().item()
141  optimizer.setInlierCostThresholds(new_ict)
142  self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict)
143  for w_factor in (0.8, 0.9):
144  new_weights = w_factor * optimizer.getWeights()
145  optimizer.setWeights(new_weights)
146  self.assertAlmostEqual(optimizer.getWeights(), new_weights)
147  optimizer.setInlierCostThresholdsAtProbability(0.9)
148  w1 = optimizer.getInlierCostThresholds()
149  optimizer.setInlierCostThresholdsAtProbability(0.8)
150  w2 = optimizer.getInlierCostThresholds()
151  self.assertLess(w2, w1)
152 
154  # set up iteration hook to track some testable values
155  iteration_count = 0
156  final_error = 0
157  final_values = None
158 
159  def iteration_hook(iter, error_before, error_after):
160  nonlocal iteration_count, final_error, final_values
161  iteration_count = iter
162  final_error = error_after
163  final_values = optimizer.values()
164 
165  # optimize
166  params = LevenbergMarquardtParams.CeresDefaults()
167  params.setOrdering(self.ordering)
168  params.iterationHook = iteration_hook
169  optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values, params)
170  actual = optimizer.optimize()
171  self.assertAlmostEqual(0, self.fg.error(actual))
172  self.gtsamAssertEquals(final_values, actual)
173  self.assertEqual(self.fg.error(actual), final_error)
174  self.assertEqual(optimizer.iterations(), iteration_count)
175 
176 
177 if __name__ == "__main__":
178  unittest.main()
test_NonlinearOptimizer.TestScenario.test_levenberg_marquardt
def test_levenberg_marquardt(self)
Definition: test_NonlinearOptimizer.py:62
test_NonlinearOptimizer.TestScenario.test_levenberg_marquardt_pcg
def test_levenberg_marquardt_pcg(self)
Definition: test_NonlinearOptimizer.py:68
gtsam::DummyPreconditionerParameters
Definition: Preconditioner.h:95
test_NonlinearOptimizer.TestScenario.setUp
def setUp(self)
Definition: test_NonlinearOptimizer.py:33
test_NonlinearOptimizer.TestScenario.test_iteration_hook
def test_iteration_hook(self)
Definition: test_NonlinearOptimizer.py:153
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
PriorFactorPoint2
PriorFactor< Point2 > PriorFactorPoint2
Definition: serialization.cpp:32
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
test_NonlinearOptimizer.TestScenario.test_gnc_params
def test_gnc_params(self)
Definition: test_NonlinearOptimizer.py:88
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
test_NonlinearOptimizer.TestScenario.optimal_values
optimal_values
Definition: test_NonlinearOptimizer.py:42
gtsam::DoglegOptimizer
Definition: DoglegOptimizer.h:68
gtsam::PCGSolverParameters
Definition: PCGSolver.h:36
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
test_NonlinearOptimizer.TestScenario.test_gauss_newton
def test_gauss_newton(self)
Definition: test_NonlinearOptimizer.py:56
add
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
test_NonlinearOptimizer.TestScenario.initial_values
initial_values
Definition: test_NonlinearOptimizer.py:48
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::DoglegParams
Definition: DoglegOptimizer.h:32
str
Definition: pytypes.h:1558
error
static double error
Definition: testRot3.cpp:37
gtsam::Values
Definition: Values.h:65
test_NonlinearOptimizer.TestScenario
Definition: test_NonlinearOptimizer.py:31
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
test_NonlinearOptimizer.TestScenario.fg
fg
Definition: test_NonlinearOptimizer.py:36
test_NonlinearOptimizer.TestScenario.test_dogleg
def test_dogleg(self)
Definition: test_NonlinearOptimizer.py:77
test_NonlinearOptimizer.TestScenario.ordering
ordering
Definition: test_NonlinearOptimizer.py:53
insert
A insert(1, 2)=0
gtsam::Ordering
Definition: inference/Ordering.h:33
test_NonlinearOptimizer.TestScenario.test_graduated_non_convexity
def test_graduated_non_convexity(self)
Definition: test_NonlinearOptimizer.py:83


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:27