diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index c8d1f1271..d8d0ae1df 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -9,20 +9,22 @@ See LICENSE for the license information A structure-from-motion problem on a simulated dataset """ -import gtsam import matplotlib.pyplot as plt import numpy as np + +import gtsam from gtsam import symbol_shorthand + L = symbol_shorthand.L X = symbol_shorthand.X from gtsam.examples import SFMdata -from gtsam import (Cal3_S2, DoglegOptimizer, - GenericProjectionFactorCal3_S2, Marginals, - NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, - Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values) from gtsam.utils import plot +from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, + Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2, + PriorFactorPoint3, PriorFactorPose3, Values) + def main(): """ @@ -43,7 +45,7 @@ def main(): Finally, once all of the factors have been added to our factor graph, we will want to solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. GTSAM includes several nonlinear optimizers to perform this step. Here we will use a - trust-region method known as Powell's Degleg + trust-region method known as Powell's Dogleg The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the nonlinear functions around an initial linearization point, then solve the linear system @@ -78,8 +80,7 @@ def main(): camera = PinholeCameraCal3_S2(pose, K) for j, point in enumerate(points): measurement = camera.project(point) - factor = GenericProjectionFactorCal3_S2( - measurement, measurement_noise, X(i), L(j), K) + factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K) graph.push_back(factor) # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained @@ -88,28 +89,29 @@ def main(): point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) - graph.print('Factor Graph:\n') + graph.print("Factor Graph:\n") # Create the data structure to hold the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initial_estimate = Values() + rng = np.random.default_rng() for i, pose in enumerate(poses): - transformed_pose = pose.retract(0.1*np.random.randn(6,1)) + transformed_pose = pose.retract(0.1 * rng.standard_normal(6).reshape(6, 1)) initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): - transformed_point = point + 0.1*np.random.randn(3) + transformed_point = point + 0.1 * rng.standard_normal(3) initial_estimate.insert(L(j), transformed_point) - initial_estimate.print('Initial Estimates:\n') + initial_estimate.print("Initial Estimates:\n") # Optimize the graph and print results params = gtsam.DoglegParams() - params.setVerbosity('TERMINATION') + params.setVerbosity("TERMINATION") optimizer = DoglegOptimizer(graph, initial_estimate, params) - print('Optimizing:') + print("Optimizing:") result = optimizer.optimize() - result.print('Final results:\n') - print('initial error = {}'.format(graph.error(initial_estimate))) - print('final error = {}'.format(graph.error(result))) + result.print("Final results:\n") + print("initial error = {}".format(graph.error(initial_estimate))) + print("final error = {}".format(graph.error(result))) marginals = Marginals(graph, result) plot.plot_3d_points(1, result, marginals=marginals) @@ -117,5 +119,6 @@ def main(): plot.set_axes_equal(1) plt.show() -if __name__ == '__main__': + +if __name__ == "__main__": main()