2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
9 Pose SLAM example using iSAM2 in 3D space.
12 - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
13 - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
16 from typing
import List
18 import matplotlib.pyplot
as plt
26 """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2."""
29 print(
"*"*50 + f
"\nInference after State {key+1}:\n")
30 print(current_estimate)
38 axes = fig.add_subplot(projection=
'3d')
44 while current_estimate.exists(i):
45 gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10,
46 marginals.marginalCovariance(i))
49 axes.set_xlim3d(-30, 45)
50 axes.set_ylim3d(-30, 45)
51 axes.set_zlim3d(-30, 45)
55 """Creates ground truth poses of the robot."""
56 P0 = np.array([[1, 0, 0, 0],
60 P1 = np.array([[0, -1, 0, 15],
64 P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30],
66 [-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30],
68 P3 = np.array([[0, 1, 0, 30],
72 P4 = np.array([[-1, 0, 0, 0],
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.
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.
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.
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():
106 """Perform 3D SLAM given ground truth poses as well as simple
107 loop closure detection."""
111 prior_xyz_sigma = 0.3
117 odometry_xyz_sigma = 0.2
120 odometry_rpy_sigma = 5
126 prior_rpy_sigma*np.pi/180,
127 prior_rpy_sigma*np.pi/180,
132 odometry_rpy_sigma*np.pi/180,
133 odometry_rpy_sigma*np.pi/180,
136 odometry_xyz_sigma]))
145 parameters.setRelinearizeThreshold(0.1)
146 parameters.relinearizeSkip = 1
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))]
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))]
164 graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE))
169 current_estimate = initial_estimate
173 noisy_odometry = noisy_measurements[i]
184 graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE))
186 graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE))
189 noisy_estimate = current_estimate.atPose3(i + 1).
compose(noisy_tf)
190 initial_estimate.insert(i + 2, noisy_estimate)
193 isam.update(graph, initial_estimate)
194 current_estimate = isam.calculateEstimate()
198 initial_estimate.clear()
203 while current_estimate.exists(i):
204 print(f
"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
210 if __name__ ==
'__main__':