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 matplotlib.pyplot as plt
13 import numpy as np
14 
15 import gtsam
16 from gtsam import symbol_shorthand
17 
18 L = symbol_shorthand.L
19 X = symbol_shorthand.X
20 
21 from gtsam.examples import SFMdata
22 from gtsam.utils import plot
23 
24 from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2,
25  Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2,
26  PriorFactorPoint3, PriorFactorPose3, Values)
27 
28 
29 def main():
30  """
31  Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
32 
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
36 
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.
41 
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.
44 
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
49 
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.
55  """
56 
57  # Define the camera calibration parameters
58  K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
59 
60  # Define the camera observation noise model
61  measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
62 
63  # Create the set of ground-truth landmarks
64  points = SFMdata.createPoints()
65 
66  # Create the set of ground-truth poses
67  poses = SFMdata.createPoses()
68 
69  # Create a factor graph
70  graph = NonlinearFactorGraph()
71 
72  # Add a prior on pose x1. This indirectly specifies where the origin is.
73  # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
74  pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
75  factor = PriorFactorPose3(X(0), poses[0], pose_noise)
76  graph.push_back(factor)
77 
78  # Simulated measurements from each camera pose, adding them to the factor graph
79  for i, pose in enumerate(poses):
80  camera = PinholeCameraCal3_S2(pose, K)
81  for j, point in enumerate(points):
82  measurement = camera.project(point)
83  factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)
84  graph.push_back(factor)
85 
86  # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
87  # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
88  # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
89  point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
90  factor = PriorFactorPoint3(L(0), points[0], point_noise)
91  graph.push_back(factor)
92  graph.print("Factor Graph:\n")
93 
94  # Create the data structure to hold the initial estimate to the solution
95  # Intentionally initialize the variables off from the ground truth
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")
105 
106  # Optimize the graph and print results
107  params = gtsam.DoglegParams()
108  params.setVerbosity("TERMINATION")
109  optimizer = DoglegOptimizer(graph, initial_estimate, params)
110  print("Optimizing:")
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)))
115 
116  marginals = Marginals(graph, 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)
120  plt.show()
121 
122 
123 if __name__ == "__main__":
124  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::reshape
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType reshape(const Eigen::Matrix< double, InM, InN, InOptions > &m)
Definition: base/Matrix.h:281
gtsam.examples.SFMExample.L
L
Definition: SFMExample.py:18
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:19
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:29
PriorFactorPoint3
PriorFactor< Point3 > PriorFactorPoint3
Definition: serialization.cpp:34


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:11