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
13 import matplotlib.pyplot
as plt
15 from gtsam
import symbol_shorthand
16 L = symbol_shorthand.L
17 X = symbol_shorthand.X
20 from gtsam
import (Cal3_S2, DoglegOptimizer,
21 GenericProjectionFactorCal3_S2, Marginals,
22 NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
23 Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
29 Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
31 Each variable in the system (poses and landmarks) must be identified with a unique key.
32 We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
33 Here we will use Symbols
35 In GTSAM, measurement functions are represented as 'factors'. Several common factors
36 have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
37 Here we will use Projection factors to model the camera's landmark observations.
38 Also, we will initialize the robot at some location using a Prior factor.
40 When the factors are created, we will add them to a Factor Graph. As the factors we are using
41 are nonlinear factors, we will need a Nonlinear Factor Graph.
43 Finally, once all of the factors have been added to our factor graph, we will want to
44 solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
45 GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
46 trust-region method known as Powell's Degleg
48 The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
49 nonlinear functions around an initial linearization point, then solve the linear system
50 to update the linearization point. This happens repeatedly until the solver converges
51 to a consistent set of variable values. This requires us to specify an initial guess
52 for each variable, held in a Values container.
56 K =
Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
62 points = SFMdata.createPoints()
65 poses = SFMdata.createPoses()
74 graph.push_back(factor)
77 for i, pose
in enumerate(poses):
79 for j, point
in enumerate(points):
80 measurement = camera.project(point)
82 measurement, measurement_noise,
X(i),
L(j), K)
83 graph.push_back(factor)
90 graph.push_back(factor)
91 graph.print(
'Factor Graph:\n')
95 initial_estimate =
Values()
96 for i, pose
in enumerate(poses):
97 transformed_pose = pose.retract(0.1*np.random.randn(6,1))
98 initial_estimate.insert(
X(i), transformed_pose)
99 for j, point
in enumerate(points):
100 transformed_point = point + 0.1*np.random.randn(3)
101 initial_estimate.insert(
L(j), transformed_point)
102 initial_estimate.print(
'Initial Estimates:\n')
106 params.setVerbosity(
'TERMINATION')
109 result = optimizer.optimize()
110 result.print(
'Final results:\n')
111 print(
'initial error = {}'.
format(graph.error(initial_estimate)))
112 print(
'final error = {}'.
format(graph.error(result)))
115 plot.plot_3d_points(1, result, marginals=marginals)
116 plot.plot_trajectory(1, result, marginals=marginals, scale=8)
117 plot.set_axes_equal(1)
120 if __name__ ==
'__main__':