made changes according to Frank
jerredchen
parent
00c541aca6
commit
79f7861f0c
|
|
@ -13,23 +13,28 @@ Modelled after:
|
|||
- Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
import math
|
||||
import numpy as np
|
||||
from numpy.random import multivariate_normal as mult_gauss
|
||||
import gtsam
|
||||
import matplotlib.pyplot as plt
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
|
||||
def Pose2SLAM_ISAM2_plot(graph, current_estimate):
|
||||
"""Plots incremental state of the robot for 2D Pose SLAM using iSAM2
|
||||
def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values,
|
||||
key: int):
|
||||
"""Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.
|
||||
|
||||
Based on version by:
|
||||
- Ellon Paiva (Python),
|
||||
- Duy Nguyen Ta and Frank Dellaert (MATLAB)
|
||||
"""
|
||||
# Print the current estimates computed using iSAM2.
|
||||
print("*"*50 + f"\nInference after State {key+1}:\n")
|
||||
print(current_estimate)
|
||||
|
||||
# Compute the marginals for all states in the graph.
|
||||
marginals = gtsam.Marginals(graph, current_estimate)
|
||||
|
||||
# Plot the newly updated iSAM2 inference.
|
||||
fig = plt.figure(0)
|
||||
axes = fig.gca()
|
||||
plt.cla()
|
||||
|
|
@ -43,19 +48,47 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate):
|
|||
axes.set_xlim(-1, 5)
|
||||
axes.set_ylim(-1, 3)
|
||||
plt.pause(1)
|
||||
|
||||
return marginals
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create a 3D double numpy array."""
|
||||
return np.array([x, y, z], dtype=float)
|
||||
|
||||
def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
|
||||
key: int, xy_tol=0.6, theta_tol=0.3) -> int:
|
||||
"""Simple brute force approach which iterates through previous states
|
||||
and checks for loop closure.
|
||||
|
||||
Args:
|
||||
odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame.
|
||||
current_estimate: The current estimates computed by iSAM2.
|
||||
key: Key corresponding to the current state estimate of the robot.
|
||||
xy_tol: Optional argument for the x-y measurement tolerance.
|
||||
theta_tol: Optional argument for the theta measurement tolerance.
|
||||
Returns:
|
||||
k: The key of the state which is helping add the loop closure constraint.
|
||||
If loop closure is not found, then None is returned.
|
||||
"""
|
||||
if current_estimate:
|
||||
prev_est = current_estimate.atPose2(key+1)
|
||||
rotated_odom = prev_est.rotation().matrix() @ odom[:2]
|
||||
curr_xy = np.array([prev_est.x() + rotated_odom[0],
|
||||
prev_est.y() + rotated_odom[1]])
|
||||
curr_theta = prev_est.theta() + odom[2]
|
||||
for k in range(1, key+1):
|
||||
pose_xy = np.array([current_estimate.atPose2(k).x(),
|
||||
current_estimate.atPose2(k).y()])
|
||||
pose_theta = current_estimate.atPose2(k).theta()
|
||||
if (abs(pose_xy - curr_xy) <= xy_tol).all() and \
|
||||
(abs(pose_theta - curr_theta) <= theta_tol):
|
||||
return k
|
||||
|
||||
def Pose2SLAM_ISAM2_example():
|
||||
"""Perform 2D SLAM given the ground truth changes in pose as well as
|
||||
simple loop closure detection.
|
||||
"""
|
||||
simple loop closure detection."""
|
||||
plt.ion()
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=float)
|
||||
|
||||
# Although this example only uses linear measurements and Gaussian noise models, it is important
|
||||
# to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
|
||||
# simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
|
||||
|
|
@ -66,85 +99,68 @@ def Pose2SLAM_ISAM2_example():
|
|||
graph = gtsam.NonlinearFactorGraph()
|
||||
initial_estimate = gtsam.Values()
|
||||
|
||||
# iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
|
||||
# Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
|
||||
# update calls are required to perform the relinearization.
|
||||
parameters = gtsam.ISAM2Params()
|
||||
parameters.setRelinearizeThreshold(0.1)
|
||||
parameters.setRelinearizeSkip(1)
|
||||
isam = gtsam.ISAM2(parameters)
|
||||
|
||||
# The ground truth odometry measurements (without noise) of the robot during the trajectory.
|
||||
odom_arr = [(2, 0, 0),
|
||||
(2, 0, math.pi/2),
|
||||
(2, 0, math.pi/2),
|
||||
(2, 0, math.pi/2),
|
||||
(2, 0, math.pi/2)]
|
||||
# Create the ground truth odometry measurements of the robot during the trajectory.
|
||||
true_odometry = [(2, 0, 0),
|
||||
(2, 0, math.pi/2),
|
||||
(2, 0, math.pi/2),
|
||||
(2, 0, math.pi/2),
|
||||
(2, 0, math.pi/2)]
|
||||
|
||||
# The initial estimates for robot poses 2-5. Pose 1 is initialized by the prior.
|
||||
# To demonstrate iSAM2 incremental optimization, poor initializations were intentionally inserted.
|
||||
pose_est = [gtsam.Pose2(2.3, 0.1, -0.2),
|
||||
gtsam.Pose2(4.1, 0.1, math.pi/2),
|
||||
gtsam.Pose2(4.0, 2.0, math.pi),
|
||||
gtsam.Pose2(2.1, 2.1, -math.pi/2),
|
||||
gtsam.Pose2(1.9, -0.2, 0.2)]
|
||||
# Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements.
|
||||
odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance())
|
||||
for true_odom in true_odometry]
|
||||
|
||||
# Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
|
||||
# iSAM2 incremental optimization.
|
||||
graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
|
||||
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
|
||||
def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
|
||||
xy_tol=0.6, theta_tol=0.3) -> int:
|
||||
"""Simple brute force approach which iterates through previous states
|
||||
and checks for loop closure.
|
||||
# Initialize the current estimate which is used during the incremental inference loop.
|
||||
current_estimate = initial_estimate
|
||||
|
||||
Args:
|
||||
odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame.
|
||||
current_estimate: The current estimates computed by iSAM2.
|
||||
xy_tol: Optional argument for the x-y measurement tolerance.
|
||||
theta_tol: Optional argument for the theta measurement tolerance.
|
||||
Returns:
|
||||
k: The key of the state which is helping add the loop closure constraint.
|
||||
If loop closure is not found, then None is returned.
|
||||
"""
|
||||
if current_estimate:
|
||||
prev_est = current_estimate.atPose2(i+1)
|
||||
rotated_odom = prev_est.rotation().matrix() @ odom[:2]
|
||||
curr_xy = np.array([prev_est.x() + rotated_odom[0],
|
||||
prev_est.y() + rotated_odom[1]])
|
||||
curr_theta = prev_est.theta() + odom[2]
|
||||
for k in range(1, i+1):
|
||||
pose_xy = np.array([current_estimate.atPose2(k).x(),
|
||||
current_estimate.atPose2(k).y()])
|
||||
pose_theta = current_estimate.atPose2(k).theta()
|
||||
if (abs(pose_xy - curr_xy) <= xy_tol).all() and \
|
||||
(abs(pose_theta - curr_theta) <= theta_tol):
|
||||
return k
|
||||
for i in range(len(true_odometry)):
|
||||
|
||||
current_estimate = None
|
||||
for i in range(len(odom_arr)):
|
||||
# The "ground truth" change between poses
|
||||
odom_x, odom_y, odom_theta = odom_arr[i]
|
||||
# Odometry measurement that is received by the robot and corrupted by gaussian noise
|
||||
odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance())
|
||||
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state
|
||||
loop = determine_loop_closure(odom, current_estimate)
|
||||
# Obtain "ground truth" change between the current pose and the previous pose.
|
||||
true_odom_x, true_odom_y, true_odom_theta = true_odometry[i]
|
||||
|
||||
# Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise.
|
||||
noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i]
|
||||
|
||||
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
|
||||
loop = determine_loop_closure(odometry_measurements[i], current_estimate, i)
|
||||
|
||||
# Add a binary factor in between two existing states if loop closure is detected.
|
||||
# Otherwise, add a binary factor between a newly observed state and the previous state.
|
||||
# Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement.
|
||||
# This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used.
|
||||
if loop:
|
||||
graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE))
|
||||
graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop,
|
||||
gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE))
|
||||
else:
|
||||
graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE))
|
||||
initial_estimate.insert(i + 2, pose_est[i])
|
||||
# Incremental update to iSAM2's internal Baye's tree, which only optimizes upon the added variables
|
||||
# as well as any affected variables
|
||||
graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2,
|
||||
gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE))
|
||||
|
||||
# Compute and insert the initialization estimate for the current pose using the noisy odometry measurement.
|
||||
computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta))
|
||||
initial_estimate.insert(i + 2, computed_estimate)
|
||||
|
||||
# Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
|
||||
isam.update(graph, initial_estimate)
|
||||
# Another iSAM2 update can be performed for additional optimization
|
||||
isam.update()
|
||||
current_estimate = isam.calculateEstimate()
|
||||
print("*"*50)
|
||||
print(f"Inference after State {i+1}:")
|
||||
print(current_estimate)
|
||||
marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate)
|
||||
|
||||
# Report all current state estimates from the iSAM2 optimzation.
|
||||
marginals = report_on_progress(graph, current_estimate, i)
|
||||
initial_estimate.clear()
|
||||
# Print the final covariance matrix for each pose after completing inference
|
||||
for i in range(1, len(odom_arr)+1):
|
||||
|
||||
# Print the final covariance matrix for each pose after completing inference on the trajectory.
|
||||
for i in range(1, len(true_odometry)+1):
|
||||
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
|
||||
|
||||
plt.ioff()
|
||||
|
|
|
|||
|
|
@ -12,34 +12,36 @@ Modelled after version by:
|
|||
- VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
||||
- Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
from __future__ import print_function
|
||||
|
||||
import gtsam
|
||||
from gtsam import Pose3, Rot3
|
||||
from gtsam.symbol_shorthand import X
|
||||
import gtsam.utils.plot as gtsam_plot
|
||||
import numpy as np
|
||||
from numpy import cos, sin, pi
|
||||
from numpy.random import multivariate_normal as mult_gauss
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
def Pose3SLAM_ISAM2_plot(graph, current_estimate):
|
||||
"""Plots incremental state of the robot for 3D Pose SLAM using iSAM2
|
||||
def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values,
|
||||
key: int):
|
||||
"""Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.
|
||||
|
||||
Based on version by:
|
||||
- Ellon Paiva (Python),
|
||||
- Duy Nguyen Ta and Frank Dellaert (MATLAB)
|
||||
"""
|
||||
# Print the current estimates computed using iSAM2.
|
||||
print("*"*50 + f"\nInference after State {key+1}:\n")
|
||||
print(current_estimate)
|
||||
|
||||
# Compute the marginals for all states in the graph.
|
||||
marginals = gtsam.Marginals(graph, current_estimate)
|
||||
|
||||
# Plot the newly updated iSAM2 inference.
|
||||
fig = plt.figure(0)
|
||||
axes = fig.gca(projection='3d')
|
||||
plt.cla()
|
||||
|
||||
i = 0
|
||||
while current_estimate.exists(X(i)):
|
||||
gtsam_plot.plot_pose3(0, current_estimate.atPose3(X(i)), 10,
|
||||
marginals.marginalCovariance(X(i)))
|
||||
i = 1
|
||||
while current_estimate.exists(i):
|
||||
gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10,
|
||||
marginals.marginalCovariance(i))
|
||||
i += 1
|
||||
|
||||
axes.set_xlim3d(-30, 45)
|
||||
|
|
@ -49,7 +51,6 @@ def Pose3SLAM_ISAM2_plot(graph, current_estimate):
|
|||
|
||||
return marginals
|
||||
|
||||
|
||||
def createPoses():
|
||||
"""Creates ground truth poses of the robot."""
|
||||
P0 = np.array([[1, 0, 0, 0],
|
||||
|
|
@ -60,9 +61,9 @@ def createPoses():
|
|||
[1, 0, 0, 15],
|
||||
[0, 0, 1, 20],
|
||||
[0, 0, 0, 1]])
|
||||
P2 = np.array([[cos(pi/4), 0, sin(pi/4), 30],
|
||||
P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30],
|
||||
[0, 1, 0, 30],
|
||||
[-sin(pi/4), 0, cos(pi/4), 30],
|
||||
[-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30],
|
||||
[0, 0, 0, 1]])
|
||||
P3 = np.array([[0, 1, 0, 30],
|
||||
[0, 0, -1, 0],
|
||||
|
|
@ -74,99 +75,120 @@ def createPoses():
|
|||
[0, 0, 0, 1]])
|
||||
P5 = P0[:]
|
||||
|
||||
return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)]
|
||||
return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2),
|
||||
gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)]
|
||||
|
||||
def vector6(x, y, z, a, b, c):
|
||||
"""Create a 6D double numpy array."""
|
||||
return np.array([x, y, z, a, b, c], dtype=float)
|
||||
|
||||
def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
|
||||
key: int, xyz_tol=0.6, rot_tol=0.3) -> int:
|
||||
"""Simple brute force approach which iterates through previous states
|
||||
and checks for loop closure.
|
||||
|
||||
Args:
|
||||
odom: Vector representing noisy odometry transformation measurement in the body frame,
|
||||
where the vector represents [roll, pitch, yaw, x, y, z].
|
||||
current_estimate: The current estimates computed by iSAM2.
|
||||
key: Key corresponding to the current state estimate of the robot.
|
||||
xyz_tol: Optional argument for the translational tolerance.
|
||||
rot_tol: Optional argument for the rotational tolerance.
|
||||
Returns:
|
||||
k: The key of the state which is helping add the loop closure constraint.
|
||||
If loop closure is not found, then None is returned.
|
||||
"""
|
||||
if current_estimate:
|
||||
rot = gtsam.Rot3.RzRyRx(odom[:3])
|
||||
odom_tf = gtsam.Pose3(rot, odom[3:6].reshape(-1,1))
|
||||
prev_est = current_estimate.atPose3(key+1)
|
||||
curr_est = prev_est.transformPoseFrom(odom_tf)
|
||||
for k in range(1, key+1):
|
||||
pose = current_estimate.atPose3(k)
|
||||
if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \
|
||||
(abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all():
|
||||
return k
|
||||
|
||||
def Pose3_ISAM2_example():
|
||||
"""Perform 3D SLAM given ground truth poses as well as simple
|
||||
loop closure detection."""
|
||||
plt.ion()
|
||||
|
||||
def vector6(x, y, z, a, b, c):
|
||||
"""Create 6d double numpy array."""
|
||||
return np.array([x, y, z, a, b, c], dtype=float)
|
||||
|
||||
# Although this example only uses linear measurements and Gaussian noise models, it is important
|
||||
# to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
|
||||
# simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
|
||||
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2))
|
||||
|
||||
# Create the ground truth poses of the robot trajectory.
|
||||
poses = createPoses()
|
||||
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
initial_estimate = gtsam.Values()
|
||||
|
||||
# iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
|
||||
# Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
|
||||
# update calls are required to perform the relinearization.
|
||||
parameters = gtsam.ISAM2Params()
|
||||
parameters.setRelinearizeThreshold(0.1)
|
||||
parameters.setRelinearizeSkip(1)
|
||||
isam = gtsam.ISAM2(parameters)
|
||||
|
||||
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
initial_estimate = gtsam.Values()
|
||||
# Create the ground truth poses of the robot trajectory.
|
||||
true_poses = createPoses()
|
||||
|
||||
# Add prior factor to the first pose with intentionally poor initial estimate
|
||||
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE))
|
||||
initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3(
|
||||
# Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations
|
||||
# between each robot pose in the trajectory.
|
||||
odometry_tf = [true_poses[i-1].transformPoseTo(true_poses[i]) for i in range(1, len(true_poses))]
|
||||
odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))]
|
||||
odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))]
|
||||
|
||||
# Corrupt the xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements.
|
||||
noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), ODOMETRY_NOISE.covariance())
|
||||
for i in range(len(odometry_tf))]
|
||||
|
||||
# Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
|
||||
# iSAM2 incremental optimization.
|
||||
graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE))
|
||||
initial_estimate.insert(1, true_poses[0].compose(gtsam.Pose3(
|
||||
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
|
||||
|
||||
def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
|
||||
xyz_tol=0.6, rot_tol=0.3) -> int:
|
||||
"""Simple brute force approach which iterates through previous states
|
||||
and checks for loop closure.
|
||||
# Initialize the current estimate which is used during the incremental inference loop.
|
||||
current_estimate = initial_estimate
|
||||
for i in range(len(odometry_tf)):
|
||||
|
||||
Args:
|
||||
odom: Vector representing noisy odometry transformation measurement in the body frame,
|
||||
where the vector represents [roll, pitch, yaw, x, y, z].
|
||||
current_estimate: The current estimates computed by iSAM2.
|
||||
xyz_tol: Optional argument for the translational tolerance.
|
||||
rot_tol: Optional argument for the rotational tolerance.
|
||||
Returns:
|
||||
k: The key of the state which is helping add the loop closure constraint.
|
||||
If loop closure is not found, then None is returned.
|
||||
"""
|
||||
if current_estimate:
|
||||
rot = Rot3.RzRyRx(odom[:3])
|
||||
odom_tf = Pose3(rot, odom[3:6].reshape(-1,1))
|
||||
prev_est = current_estimate.atPose3(X(i-1))
|
||||
curr_est = prev_est.transformPoseFrom(odom_tf)
|
||||
for k in range(i):
|
||||
pose = current_estimate.atPose3(X(k))
|
||||
if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \
|
||||
(abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all():
|
||||
return k
|
||||
# Obtain "ground truth" transformation between the current pose and the previous pose.
|
||||
true_odometry = odometry_tf[i]
|
||||
|
||||
current_estimate = None
|
||||
for i in range(1, len(poses)):
|
||||
# The "ground truth" change between poses
|
||||
odom_tf = poses[i-1].transformPoseTo(poses[i])
|
||||
odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z()
|
||||
odom_rpy = odom_tf.rotation().rpy()
|
||||
# Odometry measurement that is received by the robot and corrupted by gaussian noise
|
||||
measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance())
|
||||
loop = determine_loop_closure(measurement, current_estimate)
|
||||
# If loop closure is detected, then adds a constraint between existing states in the factor graph
|
||||
if loop is not None:
|
||||
graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE))
|
||||
# Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise.
|
||||
noisy_odometry = noisy_measurements[i]
|
||||
|
||||
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
|
||||
loop = determine_loop_closure(noisy_odometry, current_estimate, i)
|
||||
|
||||
# Add a binary factor in between two existing states if loop closure is detected.
|
||||
# Otherwise, add a binary factor between a newly observed state and the previous state.
|
||||
# Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement.
|
||||
# This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used.
|
||||
if loop:
|
||||
graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, true_odometry, ODOMETRY_NOISE))
|
||||
else:
|
||||
graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(i), gtsam.Pose3(odom_tf), ODOMETRY_NOISE))
|
||||
# Intentionally insert poor initializations
|
||||
initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3(
|
||||
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
|
||||
# Performs iSAM2 incremental update based on newly added factors
|
||||
graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, true_odometry, ODOMETRY_NOISE))
|
||||
|
||||
# Compute and insert the initialization estimate for the current pose using the noisy odometry measurement.
|
||||
noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1))
|
||||
computed_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf)
|
||||
initial_estimate.insert(i + 2, computed_estimate)
|
||||
|
||||
# Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
|
||||
isam.update(graph, initial_estimate)
|
||||
# Additional iSAM2 optimization
|
||||
isam.update()
|
||||
current_estimate = isam.calculateEstimate()
|
||||
print("*"*50)
|
||||
print(f"Inference after State {i}:\n")
|
||||
print(current_estimate)
|
||||
marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate)
|
||||
|
||||
# Report all current state estimates from the iSAM2 optimization.
|
||||
marginals = report_on_progress(graph, current_estimate, i)
|
||||
initial_estimate.clear()
|
||||
# Print the final covariance matrix for each pose after completing inference
|
||||
i = 0
|
||||
while current_estimate.exists(X(i)):
|
||||
print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n")
|
||||
|
||||
# Print the final covariance matrix for each pose after completing inference.
|
||||
i = 1
|
||||
while current_estimate.exists(i):
|
||||
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
|
||||
i += 1
|
||||
|
||||
plt.ioff()
|
||||
|
|
|
|||
Loading…
Reference in New Issue