Pose3ISAM2Example.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2018, 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 Pose SLAM example using iSAM2 in 3D space.
10 Author: Jerred Chen
11 Modeled after:
12  - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
13  - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
14 """
15 
16 from typing import List
17 
18 import matplotlib.pyplot as plt
19 import numpy as np
20 
21 import gtsam
22 import gtsam.utils.plot as gtsam_plot
23 
24 def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values,
25  key: int):
26  """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2."""
27 
28  # Print the current estimates computed using iSAM2.
29  print("*"*50 + f"\nInference after State {key+1}:\n")
30  print(current_estimate)
31 
32  # Compute the marginals for all states in the graph.
33  marginals = gtsam.Marginals(graph, current_estimate)
34 
35  # Plot the newly updated iSAM2 inference.
36  fig = plt.figure(0)
37  if not fig.axes:
38  axes = fig.add_subplot(projection='3d')
39  else:
40  axes = fig.axes[0]
41  plt.cla()
42 
43  i = 1
44  while current_estimate.exists(i):
45  gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10,
46  marginals.marginalCovariance(i))
47  i += 1
48 
49  axes.set_xlim3d(-30, 45)
50  axes.set_ylim3d(-30, 45)
51  axes.set_zlim3d(-30, 45)
52  plt.pause(1)
53 
54 def create_poses() -> List[gtsam.Pose3]:
55  """Creates ground truth poses of the robot."""
56  P0 = np.array([[1, 0, 0, 0],
57  [0, 1, 0, 0],
58  [0, 0, 1, 0],
59  [0, 0, 0, 1]])
60  P1 = np.array([[0, -1, 0, 15],
61  [1, 0, 0, 15],
62  [0, 0, 1, 20],
63  [0, 0, 0, 1]])
64  P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30],
65  [0, 1, 0, 30],
66  [-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30],
67  [0, 0, 0, 1]])
68  P3 = np.array([[0, 1, 0, 30],
69  [0, 0, -1, 0],
70  [-1, 0, 0, -15],
71  [0, 0, 0, 1]])
72  P4 = np.array([[-1, 0, 0, 0],
73  [0, -1, 0, -10],
74  [0, 0, 1, -10],
75  [0, 0, 0, 1]])
76  P5 = P0[:]
77 
78  return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2),
79  gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)]
80 
81 def determine_loop_closure(odom_tf: gtsam.Pose3, current_estimate: gtsam.Values,
82  key: int, xyz_tol=0.6, rot_tol=17) -> int:
83  """Simple brute force approach which iterates through previous states
84  and checks for loop closure.
85 
86  Args:
87  odom_tf: The noisy odometry transformation measurement in the body frame.
88  current_estimate: The current estimates computed by iSAM2.
89  key: Key corresponding to the current state estimate of the robot.
90  xyz_tol: Optional argument for the translational tolerance, in meters.
91  rot_tol: Optional argument for the rotational tolerance, in degrees.
92  Returns:
93  k: The key of the state which is helping add the loop closure constraint.
94  If loop closure is not found, then None is returned.
95  """
96  if current_estimate:
97  prev_est = current_estimate.atPose3(key+1)
98  curr_est = prev_est.compose(odom_tf)
99  for k in range(1, key+1):
100  pose = current_estimate.atPose3(k)
101  if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol*np.pi/180).all() and \
102  (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all():
103  return k
104 
106  """Perform 3D SLAM given ground truth poses as well as simple
107  loop closure detection."""
108  plt.ion()
109 
110  # Declare the 3D translational standard deviations of the prior factor's Gaussian model, in meters.
111  prior_xyz_sigma = 0.3
112 
113  # Declare the 3D rotational standard deviations of the prior factor's Gaussian model, in degrees.
114  prior_rpy_sigma = 5
115 
116  # Declare the 3D translational standard deviations of the odometry factor's Gaussian model, in meters.
117  odometry_xyz_sigma = 0.2
118 
119  # Declare the 3D rotational standard deviations of the odometry factor's Gaussian model, in degrees.
120  odometry_rpy_sigma = 5
121 
122  # Although this example only uses linear measurements and Gaussian noise models, it is important
123  # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
124  # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
125  PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180,
126  prior_rpy_sigma*np.pi/180,
127  prior_rpy_sigma*np.pi/180,
128  prior_xyz_sigma,
129  prior_xyz_sigma,
130  prior_xyz_sigma]))
131  ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_rpy_sigma*np.pi/180,
132  odometry_rpy_sigma*np.pi/180,
133  odometry_rpy_sigma*np.pi/180,
134  odometry_xyz_sigma,
135  odometry_xyz_sigma,
136  odometry_xyz_sigma]))
137 
138  # Create a Nonlinear factor graph as well as the data structure to hold state estimates.
140  initial_estimate = gtsam.Values()
141 
142  # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
143  # update calls are required to perform the relinearization.
144  parameters = gtsam.ISAM2Params()
145  parameters.setRelinearizeThreshold(0.1)
146  parameters.relinearizeSkip = 1
147  isam = gtsam.ISAM2(parameters)
148 
149  # Create the ground truth poses of the robot trajectory.
150  true_poses = create_poses()
151 
152  # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations
153  # between each robot pose in the trajectory.
154  odometry_tf = [true_poses[i-1].transformPoseTo(true_poses[i]) for i in range(1, len(true_poses))]
155  odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))]
156  odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))]
157 
158  # Corrupt xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements.
159  noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), \
160  ODOMETRY_NOISE.covariance()) for i in range(len(odometry_tf))]
161 
162  # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
163  # iSAM2 incremental optimization.
164  graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE))
165  initial_estimate.insert(1, true_poses[0].compose(gtsam.Pose3(
166  gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
167 
168  # Initialize the current estimate which is used during the incremental inference loop.
169  current_estimate = initial_estimate
170  for i in range(len(odometry_tf)):
171 
172  # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise.
173  noisy_odometry = noisy_measurements[i]
174 
175  # Compute the noisy odometry transformation according to the xyz translation and roll-pitch-yaw rotation.
176  noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1))
177 
178  # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
179  loop = determine_loop_closure(noisy_tf, current_estimate, i, xyz_tol=18, rot_tol=30)
180 
181  # Add a binary factor in between two existing states if loop closure is detected.
182  # Otherwise, add a binary factor between a newly observed state and the previous state.
183  if loop:
184  graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE))
185  else:
186  graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE))
187 
188  # Compute and insert the initialization estimate for the current pose using a noisy odometry measurement.
189  noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf)
190  initial_estimate.insert(i + 2, noisy_estimate)
191 
192  # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
193  isam.update(graph, initial_estimate)
194  current_estimate = isam.calculateEstimate()
195 
196  # Report all current state estimates from the iSAM2 optimization.
197  report_on_progress(graph, current_estimate, i)
198  initial_estimate.clear()
199 
200  # Print the final covariance matrix for each pose after completing inference.
201  marginals = gtsam.Marginals(graph, current_estimate)
202  i = 1
203  while current_estimate.exists(i):
204  print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
205  i += 1
206 
207  plt.ioff()
208  plt.show()
209 
210 if __name__ == '__main__':
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
gtsam::ISAM2
Definition: ISAM2.h:45
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::Marginals
Definition: Marginals.h:32
gtsam.examples.Pose3ISAM2Example.Pose3_ISAM2_example
def Pose3_ISAM2_example()
Definition: Pose3ISAM2Example.py:105
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
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::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam.examples.Pose3ISAM2Example.determine_loop_closure
int determine_loop_closure(gtsam.Pose3 odom_tf, gtsam.Values current_estimate, int key, xyz_tol=0.6, rot_tol=17)
Definition: Pose3ISAM2Example.py:81
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(const Vector3 &w)
Definition: Rot3.h:240
Eigen::all
static const Eigen::internal::all_t all
Definition: IndexedViewHelper.h:171
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam.examples.Pose3ISAM2Example.report_on_progress
def report_on_progress(gtsam.NonlinearFactorGraph graph, gtsam.Values current_estimate, int key)
Definition: Pose3ISAM2Example.py:24
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam.examples.Pose3ISAM2Example.create_poses
List[gtsam.Pose3] create_poses()
Definition: Pose3ISAM2Example.py:54
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::Values
Definition: Values.h:65
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
abs
#define abs(x)
Definition: datatypes.h:17
len
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2446
gtsam::transformPoseTo
Pose3_ transformPoseTo(const Pose3_ &p, const Pose3_ &q)
Definition: slam/expressions.h:57
gtsam::utils.plot
Definition: plot.py:1
gtsam::Rot3::RzRyRx
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix,...
Definition: Rot3M.cpp:84


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