Python optimizer iteration hook

gtsam/nonlinear/nonlinear.i

@@ -403,6 +403,9 @@ virtual class NonlinearOptimizerParams {
   bool isSequential() const;
   bool isCholmod() const;
   bool isIterative() const;
+
+  // This only applies to python since matlab does not have lambda machinery.
+  gtsam::NonlinearOptimizerParams::IterationHook iterationHook;
 };
 
 bool checkConvergence(double relativeErrorTreshold,

python/gtsam/tests/test_NonlinearOptimizer.py

@@ -15,76 +15,94 @@
 import unittest
 
 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams,
-                   DummyPreconditionerParameters, GaussNewtonOptimizer,
-                   GaussNewtonParams, GncLMParams, GncLMOptimizer,
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, Ordering,
-                   PCGSolverParameters, Point2, PriorFactorPoint2, Values)
+from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
+                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
+                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
+                   Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
 KEY2 = 2
 
 
 class TestScenario(GtsamTestCase):
-    def test_optimize(self):
-        """Do trivial test with three optimizer variants."""
-        fg = NonlinearFactorGraph()
+    """Do trivial test with three optimizer variants."""
+
+    def setUp(self):
+        """Set up the optimization problem and ordering"""
+        # create graph
+        self.fg = NonlinearFactorGraph()
         model = gtsam.noiseModel.Unit.Create(2)
-        fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
+        self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
 
         # test error at minimum
         xstar = Point2(0, 0)
-        optimal_values = Values()
-        optimal_values.insert(KEY1, xstar)
-        self.assertEqual(0.0, fg.error(optimal_values), 0.0)
+        self.optimal_values = Values()
+        self.optimal_values.insert(KEY1, xstar)
+        self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
 
         # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
         x0 = Point2(3, 3)
-        initial_values = Values()
-        initial_values.insert(KEY1, x0)
-        self.assertEqual(9.0, fg.error(initial_values), 1e-3)
+        self.initial_values = Values()
+        self.initial_values.insert(KEY1, x0)
+        self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
 
         # optimize parameters
-        ordering = Ordering()
-        ordering.push_back(KEY1)
+        self.ordering = Ordering()
+        self.ordering.push_back(KEY1)
 
-        # Gauss-Newton
+    def test_gauss_newton(self):
         gnParams = GaussNewtonParams()
-        gnParams.setOrdering(ordering)
-        actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual1))
+        gnParams.setOrdering(self.ordering)
+        actual = GaussNewtonOptimizer(self.fg, self.initial_values, gnParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
-        # Levenberg-Marquardt
+    def test_levenberg_marquardt(self):
         lmParams = LevenbergMarquardtParams.CeresDefaults()
-        lmParams.setOrdering(ordering)
-        actual2 = LevenbergMarquardtOptimizer(
-            fg, initial_values, lmParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual2))
+        lmParams.setOrdering(self.ordering)
+        actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
-        # Levenberg-Marquardt
+    def test_levenberg_marquardt_pcg(self):
         lmParams = LevenbergMarquardtParams.CeresDefaults()
         lmParams.setLinearSolverType("ITERATIVE")
         cgParams = PCGSolverParameters()
         cgParams.setPreconditionerParams(DummyPreconditionerParameters())
         lmParams.setIterativeParams(cgParams)
-        actual2 = LevenbergMarquardtOptimizer(
-            fg, initial_values, lmParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual2))
+        actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
-        # Dogleg
+    def test_dogleg(self):
         dlParams = DoglegParams()
-        dlParams.setOrdering(ordering)
-        actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual3))
-
-        # Graduated Non-Convexity (GNC)
-        gncParams = GncLMParams()
-        actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual4))
-
+        dlParams.setOrdering(self.ordering)
+        actual = DoglegOptimizer(self.fg, self.initial_values, dlParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
+    def test_graduated_non_convexity(self):
+        gncParams = GncLMParams()
+        actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
+
+    def test_iteration_hook(self):
+        # set up iteration hook to track some testable values
+        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)
+        params.iterationHook = iteration_hook
+        optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values, params)
+        actual = optimizer.optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
+        self.gtsamAssertEquals(final_values, actual)
+        self.assertEqual(self.fg.error(actual), final_error)
+        self.assertEqual(optimizer.iterations(), iteration_count)
 
 if __name__ == "__main__":
     unittest.main()

python/gtsam/tests/test_logging_optimizer.py

@@ -18,7 +18,7 @@
 from gtsam import Rot3
 from gtsam.utils.test_case import GtsamTestCase
 
-from gtsam.utils.logging_optimizer import gtsam_optimize
+from gtsam.utils.logging_optimizer import gtsam_optimize, optimize_using
 
 KEY = 0
 MODEL = gtsam.noiseModel.Unit.Create(3)
@@ -34,19 +34,20 @@ def setUp(self):
         rotations = {R, R.inverse()}  # mean is the identity
         self.expected = Rot3()
 
-        graph = gtsam.NonlinearFactorGraph()
+        def check(actual):
+            # Check that optimizing yields the identity
+            self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+            # Check that logging output prints out 3 lines (exact intermediate values differ by OS)
+            self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3)
+            # reset stdout catcher
+            self.capturedOutput.truncate(0)
+        self.check = check
+
+        self.graph = gtsam.NonlinearFactorGraph()
         for R in rotations:
-            graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
-        initial = gtsam.Values()
-        initial.insert(KEY, R)
-        self.params = gtsam.GaussNewtonParams()
-        self.optimizer = gtsam.GaussNewtonOptimizer(
-            graph, initial, self.params)
-
-        self.lmparams = gtsam.LevenbergMarquardtParams()
-        self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
-            graph, initial, self.lmparams
-        )
+            self.graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
+        self.initial = gtsam.Values()
+        self.initial.insert(KEY, R)
 
         # setup output capture
         self.capturedOutput = StringIO()
@@ -63,22 +64,29 @@ def test_simple_printing(self):
         def hook(_, error):
             print(error)
 
-        # Only thing we require from optimizer is an iterate method
-        gtsam_optimize(self.optimizer, self.params, hook)
-
-        # Check that optimizing yields the identity.
-        actual = self.optimizer.values()
-        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+        # Wrapper function sets the hook and calls optimizer.optimize() for us.
+        params = gtsam.GaussNewtonParams()
+        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook, self.graph, self.initial)
+        self.check(actual)
+        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook, self.graph, self.initial, params)
+        self.check(actual)
+        actual = gtsam_optimize(gtsam.GaussNewtonOptimizer(self.graph, self.initial, params),
+                                params, hook)
+        self.check(actual)
 
     def test_lm_simple_printing(self):
         """Make sure we are properly terminating LM"""
         def hook(_, error):
             print(error)
 
-        gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
-
-        actual = self.lmoptimizer.values()
-        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+        params = gtsam.LevenbergMarquardtParams()
+        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial)
+        self.check(actual)
+        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial,
+                                params)
+        self.check(actual)
+        actual = gtsam_optimize(gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, params),
+                                params, hook)
 
     @unittest.skip("Not a test we want run every time, as needs comet.ml account")
     def test_comet(self):

python/gtsam/utils/logging_optimizer.py

@@ -6,6 +6,53 @@
 
 from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
 import gtsam
+from typing import Any, Callable
+
+OPTIMIZER_PARAMS_MAP = {
+    gtsam.GaussNewtonOptimizer: gtsam.GaussNewtonParams,
+    gtsam.LevenbergMarquardtOptimizer: gtsam.LevenbergMarquardtParams,
+    gtsam.DoglegOptimizer: gtsam.DoglegParams,
+    gtsam.GncGaussNewtonOptimizer: gtsam.GaussNewtonParams,
+    gtsam.GncLMOptimizer: gtsam.LevenbergMarquardtParams
+}
+
+
+def optimize_using(OptimizerClass, hook, *args) -> gtsam.Values:
+    """ Wraps the constructor and "optimize()" call for an Optimizer together and adds an iteration
+        hook.
+        Example usage:
+            ```python
+            def hook(optimizer, error):
+                print("iteration {:}, error = {:}".format(optimizer.iterations(), error))
+            solution = optimize_using(gtsam.GaussNewtonOptimizer, hook, graph, init, params)
+            ```
+        Iteration hook's args are (optimizer, error) and return type should be None
+
+    Args:
+        OptimizerClass (T): A NonlinearOptimizer class (e.g. GaussNewtonOptimizer,
+            LevenbergMarquardtOptimizer)
+        hook ([T, double] -> None): Function to callback after each iteration.  Args are (optimizer,
+            error) and return should be None.
+        *args: Arguments that would be passed into the OptimizerClass constructor, usually:
+            graph, init, [params]
+    Returns:
+        (gtsam.Values): A Values object representing the optimization solution.
+    """
+    # Add the iteration hook to the NonlinearOptimizerParams
+    for arg in args:
+        if isinstance(arg, gtsam.NonlinearOptimizerParams):
+            arg.iterationHook = lambda iteration, error_before, error_after: hook(
+                optimizer, error_after)
+            break
+    else:
+        params = OPTIMIZER_PARAMS_MAP[OptimizerClass]()
+        params.iterationHook = lambda iteration, error_before, error_after: hook(
+            optimizer, error_after)
+        args = (*args, params)
+    # Construct Optimizer and optimize
+    optimizer = OptimizerClass(*args)
+    hook(optimizer, optimizer.error())  # Call hook once with init values to match behavior below
+    return optimizer.optimize()
 
 
 def optimize(optimizer, check_convergence, hook):
@@ -21,7 +68,8 @@ def optimize(optimizer, check_convergence, hook):
     current_error = optimizer.error()
     hook(optimizer, current_error)
 
-    # Iterative loop
+    # Iterative loop.  Cannot use `params.iterationHook` because we don't have access to params
+    # (backwards compatibility issue).
     while True:
         # Do next iteration
         optimizer.iterate()
@@ -36,6 +84,7 @@ def gtsam_optimize(optimizer,
                    params,
                    hook):
     """ Given an optimizer and params, iterate until convergence.
+        Recommend using optimize_using instead.
         After each iteration, hook(optimizer) is called.
         After the function, use values and errors to get the result.
         Arguments: