|
17 | 17 | } |
18 | 18 |
|
19 | 19 |
|
20 | | -def optimize_using(OptimizerClass, hook) -> Callable[[Any], gtsam.Values]: |
| 20 | +def optimize_using(OptimizerClass, hook, *args) -> gtsam.Values: |
21 | 21 | """ Wraps the constructor and "optimize()" call for an Optimizer together and adds an iteration |
22 | 22 | hook. |
23 | 23 | Example usage: |
24 | | - solution = optimize_using(gtsam.GaussNewtonOptimizer, hook)(graph, init, params) |
| 24 | + ```python |
| 25 | + def hook(optimizer, error): |
| 26 | + print("iteration {:}, error = {:}".format(optimizer.iterations(), error)) |
| 27 | + solution = optimize_using(gtsam.GaussNewtonOptimizer, hook, graph, init, params) |
| 28 | + ``` |
| 29 | + Iteration hook's args are (optimizer, error) and return type should be None |
25 | 30 |
|
26 | 31 | Args: |
27 | 32 | OptimizerClass (T): A NonlinearOptimizer class (e.g. GaussNewtonOptimizer, |
28 | | - LevenbergMarquadrtOptimizer) |
| 33 | + LevenbergMarquardtOptimizer) |
29 | 34 | hook ([T, double] -> None): Function to callback after each iteration. Args are (optimizer, |
30 | 35 | error) and return should be None. |
| 36 | + *args: Arguments that would be passed into the OptimizerClass constructor, usually: |
| 37 | + graph, init, [params] |
31 | 38 | Returns: |
32 | | - (Callable[*, gtsam.Values]): Call the returned function with the usual NonlinearOptimizer |
33 | | - arguments (will be forwarded to constructor) and it will return a Values object |
34 | | - representing the solution. See example usage above. |
| 39 | + (gtsam.Values): A Values object representing the optimization solution. |
35 | 40 | """ |
36 | | - |
37 | | - def wrapped_optimize(*args): |
38 | | - for arg in args: |
39 | | - if isinstance(arg, gtsam.NonlinearOptimizerParams): |
40 | | - arg.iterationHook = lambda iteration, error_before, error_after: hook( |
41 | | - optimizer, error_after) |
42 | | - break |
43 | | - else: |
44 | | - params = OPTIMIZER_PARAMS_MAP[OptimizerClass]() |
45 | | - params.iterationHook = lambda iteration, error_before, error_after: hook( |
| 41 | + # Add the iteration hook to the NonlinearOptimizerParams |
| 42 | + for arg in args: |
| 43 | + if isinstance(arg, gtsam.NonlinearOptimizerParams): |
| 44 | + arg.iterationHook = lambda iteration, error_before, error_after: hook( |
46 | 45 | optimizer, error_after) |
47 | | - args = (*args, params) |
48 | | - optimizer = OptimizerClass(*args) |
49 | | - hook(optimizer, optimizer.error()) |
50 | | - return optimizer.optimize() |
51 | | - |
52 | | - return wrapped_optimize |
| 46 | + break |
| 47 | + else: |
| 48 | + params = OPTIMIZER_PARAMS_MAP[OptimizerClass]() |
| 49 | + params.iterationHook = lambda iteration, error_before, error_after: hook( |
| 50 | + optimizer, error_after) |
| 51 | + args = (*args, params) |
| 52 | + # Construct Optimizer and optimize |
| 53 | + optimizer = OptimizerClass(*args) |
| 54 | + hook(optimizer, optimizer.error()) # Call hook once with init values to match behavior below |
| 55 | + return optimizer.optimize() |
53 | 56 |
|
54 | 57 |
|
55 | 58 | def optimize(optimizer, check_convergence, hook): |
|
0 commit comments