diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index be4118b1f..c70dbfa6f 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -14,12 +14,14 @@ Modeled after: """ import math -import numpy as np -import gtsam + import matplotlib.pyplot as plt +import numpy as np + +import gtsam import gtsam.utils.plot as gtsam_plot -def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, +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.""" @@ -94,8 +96,12 @@ def Pose2SLAM_ISAM2_example(): # 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(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])) + 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() @@ -145,7 +151,9 @@ def Pose2SLAM_ISAM2_example(): 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)) + 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. diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 82458822a..59b9fb2ac 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -8,15 +8,18 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen -Modeled after version by: +Modeled after: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ +from typing import List + +import matplotlib.pyplot as plt +import numpy as np + import gtsam import gtsam.utils.plot as gtsam_plot -import numpy as np -import matplotlib.pyplot as plt def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): @@ -45,7 +48,7 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_zlim3d(-30, 45) plt.pause(1) -def createPoses(): +def create_poses() -> List[gtsam.Pose3]: """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], @@ -118,7 +121,7 @@ def Pose3_ISAM2_example(): # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180, prior_rpy_sigma*np.pi/180, - prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, prior_xyz_sigma, prior_xyz_sigma, prior_xyz_sigma])) @@ -141,7 +144,7 @@ def Pose3_ISAM2_example(): isam = gtsam.ISAM2(parameters) # Create the ground truth poses of the robot trajectory. - true_poses = createPoses() + true_poses = create_poses() # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations # between each robot pose in the trajectory. @@ -149,9 +152,9 @@ def Pose3_ISAM2_example(): 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))] + # Corrupt 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. @@ -179,7 +182,7 @@ def Pose3_ISAM2_example(): else: graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE)) - # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + # Compute and insert the initialization estimate for the current pose using a noisy odometry measurement. noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) initial_estimate.insert(i + 2, noisy_estimate)