removed ground truth; angles set in deg and converted to rad
jerredchen
parent
79f7861f0c
commit
e31beee22b
|
|
@ -8,7 +8,7 @@ See LICENSE for the license information
|
|||
|
||||
Pose SLAM example using iSAM2 in the 2D plane.
|
||||
Author: Jerred Chen, Yusuf Ali
|
||||
Modelled after:
|
||||
Modeled after:
|
||||
- VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
||||
- Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
|
|
@ -21,12 +21,8 @@ import gtsam.utils.plot as gtsam_plot
|
|||
|
||||
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.
|
||||
"""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)
|
||||
|
|
@ -49,14 +45,8 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa
|
|||
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:
|
||||
key: int, xy_tol=0.6, theta_tol=17) -> int:
|
||||
"""Simple brute force approach which iterates through previous states
|
||||
and checks for loop closure.
|
||||
|
||||
|
|
@ -64,8 +54,8 @@ def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
|
|||
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.
|
||||
xy_tol: Optional argument for the x-y measurement tolerance, in meters.
|
||||
theta_tol: Optional argument for the theta measurement tolerance, in degrees.
|
||||
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.
|
||||
|
|
@ -81,7 +71,7 @@ def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values,
|
|||
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):
|
||||
(abs(pose_theta - curr_theta) <= theta_tol*np.pi/180):
|
||||
return k
|
||||
|
||||
def Pose2SLAM_ISAM2_example():
|
||||
|
|
@ -89,11 +79,23 @@ def Pose2SLAM_ISAM2_example():
|
|||
simple loop closure detection."""
|
||||
plt.ion()
|
||||
|
||||
# Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters.
|
||||
prior_xy_sigma = 0.3
|
||||
|
||||
# Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees.
|
||||
prior_theta_sigma = 5
|
||||
|
||||
# Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters.
|
||||
odometry_xy_sigma = 0.2
|
||||
|
||||
# Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees.
|
||||
odometry_theta_sigma = 5
|
||||
|
||||
# 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(vector3(0.3, 0.3, 0.1))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
|
||||
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, prior_xy_sigma, prior_theta_sigma*np.pi/180]))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, odometry_xy_sigma, odometry_theta_sigma*np.pi/180]))
|
||||
|
||||
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
|
@ -127,25 +129,20 @@ def Pose2SLAM_ISAM2_example():
|
|||
|
||||
for i in range(len(true_odometry)):
|
||||
|
||||
# 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)
|
||||
loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25)
|
||||
|
||||
# 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(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE))
|
||||
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
|
||||
else:
|
||||
graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2,
|
||||
gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE))
|
||||
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_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))
|
||||
|
|
@ -156,10 +153,12 @@ def Pose2SLAM_ISAM2_example():
|
|||
current_estimate = isam.calculateEstimate()
|
||||
|
||||
# Report all current state estimates from the iSAM2 optimzation.
|
||||
marginals = report_on_progress(graph, current_estimate, i)
|
||||
report_on_progress(graph, current_estimate, i)
|
||||
initial_estimate.clear()
|
||||
|
||||
# Print the final covariance matrix for each pose after completing inference on the trajectory.
|
||||
marginals = gtsam.Marginals(graph, current_estimate)
|
||||
i = 1
|
||||
for i in range(1, len(true_odometry)+1):
|
||||
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue