Skip to content

Commit 0f8353f

Browse files
jerredchenjerredchen
authored
Merge pull request #891 from borglab/origin/feature/python_examples
Pose SLAM Python examples using iSAM2
2 parents 3755f21 + 99ce18c commit 0f8353f

File tree

2 files changed

+386
-0
lines changed

2 files changed

+386
-0
lines changed

‎python/gtsam/examples/Pose2ISAM2Example.py‎

Lines changed: 178 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,178 @@
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 the 2D plane.
10+
Author: Jerred Chen, Yusuf Ali
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+
import math
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+
axes = fig.gca()
38+
plt.cla()
39+
40+
i = 1
41+
while current_estimate.exists(i):
42+
gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i))
43+
i += 1
44+
45+
plt.axis('equal')
46+
axes.set_xlim(-1, 5)
47+
axes.set_ylim(-1, 3)
48+
plt.pause(1)
49+
50+
def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
51+
key: int, xy_tol=0.6, theta_tol=17) -> int:
52+
"""Simple brute force approach which iterates through previous states
53+
and checks for loop closure.
54+
55+
Args:
56+
odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame.
57+
current_estimate: The current estimates computed by iSAM2.
58+
key: Key corresponding to the current state estimate of the robot.
59+
xy_tol: Optional argument for the x-y measurement tolerance, in meters.
60+
theta_tol: Optional argument for the theta measurement tolerance, in degrees.
61+
Returns:
62+
k: The key of the state which is helping add the loop closure constraint.
63+
If loop closure is not found, then None is returned.
64+
"""
65+
if current_estimate:
66+
prev_est = current_estimate.atPose2(key+1)
67+
rotated_odom = prev_est.rotation().matrix() @ odom[:2]
68+
curr_xy = np.array([prev_est.x() + rotated_odom[0],
69+
prev_est.y() + rotated_odom[1]])
70+
curr_theta = prev_est.theta() + odom[2]
71+
for k in range(1, key+1):
72+
pose_xy = np.array([current_estimate.atPose2(k).x(),
73+
current_estimate.atPose2(k).y()])
74+
pose_theta = current_estimate.atPose2(k).theta()
75+
if (abs(pose_xy - curr_xy) <= xy_tol).all() and \
76+
(abs(pose_theta - curr_theta) <= theta_tol*np.pi/180):
77+
return k
78+
79+
def Pose2SLAM_ISAM2_example():
80+
"""Perform 2D SLAM given the ground truth changes in pose as well as
81+
simple loop closure detection."""
82+
plt.ion()
83+
84+
# Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters.
85+
prior_xy_sigma = 0.3
86+
87+
# Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees.
88+
prior_theta_sigma = 5
89+
90+
# Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters.
91+
odometry_xy_sigma = 0.2
92+
93+
# Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees.
94+
odometry_theta_sigma = 5
95+
96+
# Although this example only uses linear measurements and Gaussian noise models, it is important
97+
# to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
98+
# simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
99+
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma,
100+
prior_xy_sigma,
101+
prior_theta_sigma*np.pi/180]))
102+
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma,
103+
odometry_xy_sigma,
104+
odometry_theta_sigma*np.pi/180]))
105+
106+
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
107+
graph = gtsam.NonlinearFactorGraph()
108+
initial_estimate = gtsam.Values()
109+
110+
# Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
111+
# update calls are required to perform the relinearization.
112+
parameters = gtsam.ISAM2Params()
113+
parameters.setRelinearizeThreshold(0.1)
114+
parameters.setRelinearizeSkip(1)
115+
isam = gtsam.ISAM2(parameters)
116+
117+
# Create the ground truth odometry measurements of the robot during the trajectory.
118+
true_odometry = [(2, 0, 0),
119+
(2, 0, math.pi/2),
120+
(2, 0, math.pi/2),
121+
(2, 0, math.pi/2),
122+
(2, 0, math.pi/2)]
123+
124+
# Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements.
125+
odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance())
126+
for true_odom in true_odometry]
127+
128+
# Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
129+
# iSAM2 incremental optimization.
130+
graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
131+
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
132+
133+
# Initialize the current estimate which is used during the incremental inference loop.
134+
current_estimate = initial_estimate
135+
136+
for i in range(len(true_odometry)):
137+
138+
# Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise.
139+
noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i]
140+
141+
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
142+
loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25)
143+
144+
# Add a binary factor in between two existing states if loop closure is detected.
145+
# Otherwise, add a binary factor between a newly observed state and the previous state.
146+
if loop:
147+
graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop,
148+
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
149+
else:
150+
graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2,
151+
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
152+
153+
# Compute and insert the initialization estimate for the current pose using the noisy odometry measurement.
154+
computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x,
155+
noisy_odom_y,
156+
noisy_odom_theta))
157+
initial_estimate.insert(i + 2, computed_estimate)
158+
159+
# Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
160+
isam.update(graph, initial_estimate)
161+
current_estimate = isam.calculateEstimate()
162+
163+
# Report all current state estimates from the iSAM2 optimzation.
164+
report_on_progress(graph, current_estimate, i)
165+
initial_estimate.clear()
166+
167+
# Print the final covariance matrix for each pose after completing inference on the trajectory.
168+
marginals = gtsam.Marginals(graph, current_estimate)
169+
i = 1
170+
for i in range(1, len(true_odometry)+1):
171+
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
172+
173+
plt.ioff()
174+
plt.show()
175+
176+
177+
if __name__ == "__main__":
178+
Pose2SLAM_ISAM2_example()

‎python/gtsam/examples/Pose3ISAM2Example.py‎

Lines changed: 208 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
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+
axes = fig.gca(projection='3d')
38+
plt.cla()
39+
40+
i = 1
41+
while current_estimate.exists(i):
42+
gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10,
43+
marginals.marginalCovariance(i))
44+
i += 1
45+
46+
axes.set_xlim3d(-30, 45)
47+
axes.set_ylim3d(-30, 45)
48+
axes.set_zlim3d(-30, 45)
49+
plt.pause(1)
50+
51+
def create_poses() -> List[gtsam.Pose3]:
52+
"""Creates ground truth poses of the robot."""
53+
P0 = np.array([[1, 0, 0, 0],
54+
[0, 1, 0, 0],
55+
[0, 0, 1, 0],
56+
[0, 0, 0, 1]])
57+
P1 = np.array([[0, -1, 0, 15],
58+
[1, 0, 0, 15],
59+
[0, 0, 1, 20],
60+
[0, 0, 0, 1]])
61+
P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30],
62+
[0, 1, 0, 30],
63+
[-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30],
64+
[0, 0, 0, 1]])
65+
P3 = np.array([[0, 1, 0, 30],
66+
[0, 0, -1, 0],
67+
[-1, 0, 0, -15],
68+
[0, 0, 0, 1]])
69+
P4 = np.array([[-1, 0, 0, 0],
70+
[0, -1, 0, -10],
71+
[0, 0, 1, -10],
72+
[0, 0, 0, 1]])
73+
P5 = P0[:]
74+
75+
return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2),
76+
gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)]
77+
78+
def determine_loop_closure(odom_tf: gtsam.Pose3, current_estimate: gtsam.Values,
79+
key: int, xyz_tol=0.6, rot_tol=17) -> int:
80+
"""Simple brute force approach which iterates through previous states
81+
and checks for loop closure.
82+
83+
Args:
84+
odom_tf: The noisy odometry transformation measurement in the body frame.
85+
current_estimate: The current estimates computed by iSAM2.
86+
key: Key corresponding to the current state estimate of the robot.
87+
xyz_tol: Optional argument for the translational tolerance, in meters.
88+
rot_tol: Optional argument for the rotational tolerance, in degrees.
89+
Returns:
90+
k: The key of the state which is helping add the loop closure constraint.
91+
If loop closure is not found, then None is returned.
92+
"""
93+
if current_estimate:
94+
prev_est = current_estimate.atPose3(key+1)
95+
curr_est = prev_est.compose(odom_tf)
96+
for k in range(1, key+1):
97+
pose = current_estimate.atPose3(k)
98+
if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol*np.pi/180).all() and \
99+
(abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all():
100+
return k
101+
102+
def Pose3_ISAM2_example():
103+
"""Perform 3D SLAM given ground truth poses as well as simple
104+
loop closure detection."""
105+
plt.ion()
106+
107+
# Declare the 3D translational standard deviations of the prior factor's Gaussian model, in meters.
108+
prior_xyz_sigma = 0.3
109+
110+
# Declare the 3D rotational standard deviations of the prior factor's Gaussian model, in degrees.
111+
prior_rpy_sigma = 5
112+
113+
# Declare the 3D translational standard deviations of the odometry factor's Gaussian model, in meters.
114+
odometry_xyz_sigma = 0.2
115+
116+
# Declare the 3D rotational standard deviations of the odometry factor's Gaussian model, in degrees.
117+
odometry_rpy_sigma = 5
118+
119+
# Although this example only uses linear measurements and Gaussian noise models, it is important
120+
# to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
121+
# simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
122+
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180,
123+
prior_rpy_sigma*np.pi/180,
124+
prior_rpy_sigma*np.pi/180,
125+
prior_xyz_sigma,
126+
prior_xyz_sigma,
127+
prior_xyz_sigma]))
128+
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_rpy_sigma*np.pi/180,
129+
odometry_rpy_sigma*np.pi/180,
130+
odometry_rpy_sigma*np.pi/180,
131+
odometry_xyz_sigma,
132+
odometry_xyz_sigma,
133+
odometry_xyz_sigma]))
134+
135+
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
136+
graph = gtsam.NonlinearFactorGraph()
137+
initial_estimate = gtsam.Values()
138+
139+
# Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
140+
# update calls are required to perform the relinearization.
141+
parameters = gtsam.ISAM2Params()
142+
parameters.setRelinearizeThreshold(0.1)
143+
parameters.setRelinearizeSkip(1)
144+
isam = gtsam.ISAM2(parameters)
145+
146+
# Create the ground truth poses of the robot trajectory.
147+
true_poses = create_poses()
148+
149+
# Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations
150+
# between each robot pose in the trajectory.
151+
odometry_tf = [true_poses[i-1].transformPoseTo(true_poses[i]) for i in range(1, len(true_poses))]
152+
odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))]
153+
odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))]
154+
155+
# Corrupt xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements.
156+
noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), \
157+
ODOMETRY_NOISE.covariance()) for i in range(len(odometry_tf))]
158+
159+
# Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
160+
# iSAM2 incremental optimization.
161+
graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE))
162+
initial_estimate.insert(1, true_poses[0].compose(gtsam.Pose3(
163+
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
164+
165+
# Initialize the current estimate which is used during the incremental inference loop.
166+
current_estimate = initial_estimate
167+
for i in range(len(odometry_tf)):
168+
169+
# Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise.
170+
noisy_odometry = noisy_measurements[i]
171+
172+
# Compute the noisy odometry transformation according to the xyz translation and roll-pitch-yaw rotation.
173+
noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1))
174+
175+
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
176+
loop = determine_loop_closure(noisy_tf, current_estimate, i, xyz_tol=18, rot_tol=30)
177+
178+
# Add a binary factor in between two existing states if loop closure is detected.
179+
# Otherwise, add a binary factor between a newly observed state and the previous state.
180+
if loop:
181+
graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE))
182+
else:
183+
graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE))
184+
185+
# Compute and insert the initialization estimate for the current pose using a noisy odometry measurement.
186+
noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf)
187+
initial_estimate.insert(i + 2, noisy_estimate)
188+
189+
# Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
190+
isam.update(graph, initial_estimate)
191+
current_estimate = isam.calculateEstimate()
192+
193+
# Report all current state estimates from the iSAM2 optimization.
194+
report_on_progress(graph, current_estimate, i)
195+
initial_estimate.clear()
196+
197+
# Print the final covariance matrix for each pose after completing inference.
198+
marginals = gtsam.Marginals(graph, current_estimate)
199+
i = 1
200+
while current_estimate.exists(i):
201+
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
202+
i += 1
203+
204+
plt.ioff()
205+
plt.show()
206+
207+
if __name__ == '__main__':
208+
Pose3_ISAM2_example()

0 commit comments

Comments
 (0)