2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Unit tests for IMU testing scenarios.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
13 from __future__
import print_function
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)
32 """Do trivial test with three optimizer variants."""
34 """Set up the optimization problem and ordering"""
60 self.assertAlmostEqual(0, self.
fg.
error(actual))
63 lmParams = LevenbergMarquardtParams.CeresDefaults()
66 self.assertAlmostEqual(0, self.
fg.
error(actual))
69 lmParams = LevenbergMarquardtParams.CeresDefaults()
70 lmParams.setLinearSolverType(
"ITERATIVE")
73 lmParams.setIterativeParams(cgParams)
75 self.assertAlmostEqual(0, self.
fg.
error(actual))
81 self.assertAlmostEqual(0, self.
fg.
error(actual))
84 gncParams = GncLMParams()
86 self.assertAlmostEqual(0, self.
fg.
error(actual))
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)
97 params_str =
str(params)
106 self.assertTrue(s
in params_str)
109 for loss_type
in (GncLossType.TLS, GncLossType.GM):
110 params.setLossType(loss_type)
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)
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 = []
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)
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()
166 params = LevenbergMarquardtParams.CeresDefaults()
168 params.iterationHook = iteration_hook
170 actual = optimizer.optimize()
171 self.assertAlmostEqual(0, self.
fg.
error(actual))
173 self.assertEqual(self.
fg.
error(actual), final_error)
174 self.assertEqual(optimizer.iterations(), iteration_count)
177 if __name__ ==
"__main__":