diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index fac7ecc1a..be4118b1f 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -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")