release/4.3a0
Amado Antonini 2022-07-08 16:49:04 -04:00
parent 9dd1f8ffaf
commit b4fdda4740
1 changed files with 9 additions and 6 deletions

View File

@ -15,11 +15,12 @@ from __future__ import print_function
import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLossType,
GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
PriorFactorPoint2, Values)
from gtsam import (
DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer,
GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
PriorFactorPoint2, Values
)
from gtsam.utils.test_case import GtsamTestCase
KEY1 = 1
@ -28,7 +29,6 @@ KEY2 = 2
class TestScenario(GtsamTestCase):
"""Do trivial test with three optimizer variants."""
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
@ -154,11 +154,13 @@ class TestScenario(GtsamTestCase):
iteration_count = 0
final_error = 0
final_values = None
def iteration_hook(iter, error_before, error_after):
nonlocal iteration_count, final_error, final_values
iteration_count = iter
final_error = error_after
final_values = optimizer.values()
# optimize
params = LevenbergMarquardtParams.CeresDefaults()
params.setOrdering(self.ordering)
@ -170,5 +172,6 @@ class TestScenario(GtsamTestCase):
self.assertEqual(self.fg.error(actual), final_error)
self.assertEqual(optimizer.iterations(), iteration_count)
if __name__ == "__main__":
unittest.main()