SFMExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 A structure-from-motion problem on a simulated dataset
10 """
11 
12 import gtsam
13 import matplotlib.pyplot as plt
14 import numpy as np
15 from gtsam import symbol_shorthand
16 L = symbol_shorthand.L
17 X = symbol_shorthand.X
18 
19 from gtsam.examples import SFMdata
20 from gtsam import (Cal3_S2, DoglegOptimizer,
21  GenericProjectionFactorCal3_S2, Marginals,
22  NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
23  Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
24 from gtsam.utils import plot
25 
26 
27 def main():
28  """
29  Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
30 
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
34 
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.
39 
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.
42 
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
47 
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.
53  """
54 
55  # Define the camera calibration parameters
56  K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
57 
58  # Define the camera observation noise model
59  measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
60 
61  # Create the set of ground-truth landmarks
62  points = SFMdata.createPoints()
63 
64  # Create the set of ground-truth poses
65  poses = SFMdata.createPoses()
66 
67  # Create a factor graph
68  graph = NonlinearFactorGraph()
69 
70  # Add a prior on pose x1. This indirectly specifies where the origin is.
71  # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
72  pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
73  factor = PriorFactorPose3(X(0), poses[0], pose_noise)
74  graph.push_back(factor)
75 
76  # Simulated measurements from each camera pose, adding them to the factor graph
77  for i, pose in enumerate(poses):
78  camera = PinholeCameraCal3_S2(pose, K)
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)
84 
85  # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
86  # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
87  # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
88  point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
89  factor = PriorFactorPoint3(L(0), points[0], point_noise)
90  graph.push_back(factor)
91  graph.print('Factor Graph:\n')
92 
93  # Create the data structure to hold the initial estimate to the solution
94  # Intentionally initialize the variables off from the ground truth
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')
103 
104  # Optimize the graph and print results
105  params = gtsam.DoglegParams()
106  params.setVerbosity('TERMINATION')
107  optimizer = DoglegOptimizer(graph, initial_estimate, params)
108  print('Optimizing:')
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)))
113 
114  marginals = Marginals(graph, 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)
118  plt.show()
119 
120 if __name__ == '__main__':
121  main()
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Definition: openglsupport.cpp:226
gtsam::Marginals
Definition: Marginals.h:32
GenericProjectionFactorCal3_S2
GenericProjectionFactor< Pose3, Point3, Cal3_S2 > GenericProjectionFactorCal3_S2
Definition: serialization.cpp:77
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam.examples
Definition: python/gtsam/examples/__init__.py:1
gtsam.examples.SFMExample.L
L
Definition: SFMExample.py:16
gtsam::utils
Definition: kruskal-inl.h:31
gtsam::DoglegOptimizer
Definition: DoglegOptimizer.h:68
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::DoglegParams
Definition: DoglegOptimizer.h:32
gtsam::Values
Definition: Values.h:65
PriorFactorPose3
PriorFactor< Pose3 > PriorFactorPose3
Definition: serialization.cpp:38
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam.examples.SFMExample.X
X
Definition: SFMExample.py:17
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam.examples.SFMExample.main
def main()
Definition: SFMExample.py:27
PriorFactorPoint3
PriorFactor< Point3 > PriorFactorPoint3
Definition: serialization.cpp:34


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:15