VisualISAMExample.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 visualSLAM example for the structure-from-motion problem on a simulated dataset
10 This version uses iSAM to solve the problem incrementally
11 """
12 
13 import numpy as np
14 import gtsam
15 from gtsam.examples import SFMdata
16 from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
17  NonlinearFactorGraph, NonlinearISAM, Pose3,
18  PriorFactorPoint3, PriorFactorPose3, Rot3,
19  PinholeCameraCal3_S2, Values, Point3)
20 from gtsam.symbol_shorthand import X, L
21 
22 def main():
23  """
24  A structure-from-motion example with landmarks
25  - The landmarks form a 10 meter cube
26  - The robot rotates around the landmarks, always facing towards the cube
27  """
28 
29  # Define the camera calibration parameters
30  K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
31 
32  # Define the camera observation noise model
33  camera_noise = gtsam.noiseModel.Isotropic.Sigma(
34  2, 1.0) # one pixel in u and v
35 
36  # Create the set of ground-truth landmarks
37  points = SFMdata.createPoints()
38  # Create the set of ground-truth poses
39  poses = SFMdata.createPoses()
40 
41  # Create a NonlinearISAM object which will relinearize and reorder the variables
42  # every "reorderInterval" updates
43  isam = NonlinearISAM(reorderInterval=3)
44 
45  # Create a Factor Graph and Values to hold the new data
46  graph = NonlinearFactorGraph()
47  initial_estimate = Values()
48 
49  # Loop over the different poses, adding the observations to iSAM incrementally
50  for i, pose in enumerate(poses):
51  camera = PinholeCameraCal3_S2(pose, K)
52  # Add factors for each landmark observation
53  for j, point in enumerate(points):
54  measurement = camera.project(point)
56  measurement, camera_noise, X(i), L(j), K)
57  graph.push_back(factor)
58 
59  # Intentionally initialize the variables off from the ground truth
60  noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
61  t=Point3(0.05, -0.10, 0.20))
62  initial_xi = pose.compose(noise)
63 
64  # Add an initial guess for the current pose
65  initial_estimate.insert(X(i), initial_xi)
66 
67  # If this is the first iteration, add a prior on the first pose to set the coordinate frame
68  # and a prior on the first landmark to set the scale
69  # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
70  # adding it to iSAM.
71  if i == 0:
72  # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
74  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  # Add a prior on landmark l0
79  point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
80  factor = PriorFactorPoint3(L(0), points[0], point_noise)
81  graph.push_back(factor)
82 
83  # Add initial guesses to all observed landmarks
84  noise = np.array([-0.25, 0.20, 0.15])
85  for j, point in enumerate(points):
86  # Intentionally initialize the variables off from the ground truth
87  initial_lj = points[j] + noise
88  initial_estimate.insert(L(j), initial_lj)
89  else:
90  # Update iSAM with the new factors
91  isam.update(graph, initial_estimate)
92  current_estimate = isam.estimate()
93  print('*' * 50)
94  print('Frame {}:'.format(i))
95  current_estimate.print('Current estimate: ')
96 
97  # Clear the factor graph and values for the next iteration
98  graph.resize(0)
99  initial_estimate.clear()
100 
101 
102 if __name__ == '__main__':
103  main()
gtsam.examples.VisualISAMExample.main
def main()
Definition: VisualISAMExample.py:22
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Definition: openglsupport.cpp:226
GenericProjectionFactorCal3_S2
GenericProjectionFactor< Pose3, Point3, Cal3_S2 > GenericProjectionFactorCal3_S2
Definition: serialization.cpp:77
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
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
X
#define X
Definition: icosphere.cpp:20
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::Values
Definition: Values.h:65
PriorFactorPose3
PriorFactor< Pose3 > PriorFactorPose3
Definition: serialization.cpp:38
gtsam::NonlinearISAM
Definition: NonlinearISAM.h:27
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
PriorFactorPoint3
PriorFactor< Point3 > PriorFactorPoint3
Definition: serialization.cpp:34


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