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 11 from __future__
import print_function
14 import matplotlib.pyplot
as plt
16 from gtsam
import symbol_shorthand
17 L = symbol_shorthand.L
18 X = symbol_shorthand.X
21 from gtsam
import (Cal3_S2, DoglegOptimizer,
22 GenericProjectionFactorCal3_S2, Marginals,
23 NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
24 Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
30 Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). 32 Each variable in the system (poses and landmarks) must be identified with a unique key. 33 We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). 34 Here we will use Symbols 36 In GTSAM, measurement functions are represented as 'factors'. Several common factors 37 have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. 38 Here we will use Projection factors to model the camera's landmark observations. 39 Also, we will initialize the robot at some location using a Prior factor. 41 When the factors are created, we will add them to a Factor Graph. As the factors we are using 42 are nonlinear factors, we will need a Nonlinear Factor Graph. 44 Finally, once all of the factors have been added to our factor graph, we will want to 45 solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. 46 GTSAM includes several nonlinear optimizers to perform this step. Here we will use a 47 trust-region method known as Powell's Degleg 49 The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the 50 nonlinear functions around an initial linearization point, then solve the linear system 51 to update the linearization point. This happens repeatedly until the solver converges 52 to a consistent set of variable values. This requires us to specify an initial guess 53 for each variable, held in a Values container. 57 K =
Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
63 points = SFMdata.createPoints()
66 poses = SFMdata.createPoses(K)
75 graph.push_back(factor)
78 for i, pose
in enumerate(poses):
80 for j, point
in enumerate(points):
81 measurement = camera.project(point)
83 measurement, measurement_noise,
X(i),
L(j), K)
84 graph.push_back(factor)
91 graph.push_back(factor)
92 graph.print_(
'Factor Graph:\n')
96 initial_estimate =
Values()
97 for i, pose
in enumerate(poses):
98 transformed_pose = pose.retract(0.1*np.random.randn(6,1))
99 initial_estimate.insert(
X(i), transformed_pose)
100 for j, point
in enumerate(points):
101 transformed_point = point + 0.1*np.random.randn(3)
102 initial_estimate.insert(
L(j), transformed_point)
103 initial_estimate.print_(
'Initial Estimates:\n')
107 params.setVerbosity(
'TERMINATION')
110 result = optimizer.optimize()
111 result.print_(
'Final results:\n')
112 print(
'initial error = {}'.format(graph.error(initial_estimate)))
113 print(
'final error = {}'.format(graph.error(result)))
116 plot.plot_3d_points(1, result, marginals=marginals)
117 plot.plot_trajectory(1, result, marginals=marginals, scale=8)
118 plot.set_axes_equal(1)
121 if __name__ ==
'__main__':
void print(const Matrix &A, const string &s, ostream &stream)
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)