2 GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
9 A structure-from-motion problem on a simulated dataset
12 import matplotlib.pyplot
as plt
16 from gtsam
import symbol_shorthand
18 L = symbol_shorthand.L
19 X = symbol_shorthand.X
24 from gtsam
import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2,
25 Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2,
26 PriorFactorPoint3, PriorFactorPose3, Values)
31 Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
33 Each variable in the system (poses and landmarks) must be identified with a unique key.
34 We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
35 Here we will use Symbols
37 In GTSAM, measurement functions are represented as 'factors'. Several common factors
38 have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
39 Here we will use Projection factors to model the camera's landmark observations.
40 Also, we will initialize the robot at some location using a Prior factor.
42 When the factors are created, we will add them to a Factor Graph. As the factors we are using
43 are nonlinear factors, we will need a Nonlinear Factor Graph.
45 Finally, once all of the factors have been added to our factor graph, we will want to
46 solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
47 GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
48 trust-region method known as Powell's Dogleg
50 The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
51 nonlinear functions around an initial linearization point, then solve the linear system
52 to update the linearization point. This happens repeatedly until the solver converges
53 to a consistent set of variable values. This requires us to specify an initial guess
54 for each variable, held in a Values container.
58 K =
Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
64 points = SFMdata.createPoints()
67 poses = SFMdata.createPoses()
76 graph.push_back(factor)
79 for i, pose
in enumerate(poses):
81 for j, point
in enumerate(points):
82 measurement = camera.project(point)
84 graph.push_back(factor)
91 graph.push_back(factor)
92 graph.print(
"Factor Graph:\n")
96 initial_estimate =
Values()
97 rng = np.random.default_rng()
98 for i, pose
in enumerate(poses):
99 transformed_pose = pose.retract(0.1 * rng.standard_normal(6).
reshape(6, 1))
100 initial_estimate.insert(
X(i), transformed_pose)
101 for j, point
in enumerate(points):
102 transformed_point = point + 0.1 * rng.standard_normal(3)
103 initial_estimate.insert(
L(j), transformed_point)
104 initial_estimate.print(
"Initial Estimates:\n")
108 params.setVerbosity(
"TERMINATION")
111 result = optimizer.optimize()
112 result.print(
"Final results:\n")
113 print(
"initial error = {}".
format(graph.error(initial_estimate)))
114 print(
"final error = {}".
format(graph.error(result)))
117 plot.plot_3d_points(1, result, marginals=marginals)
118 plot.plot_trajectory(1, result, marginals=marginals, scale=8)
119 plot.set_axes_equal(1)
123 if __name__ ==
"__main__":