From 859c5f8d07abcd5f933bbb540b6281f3f30d04bb Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 13 Oct 2021 00:13:05 -0400 Subject: [PATCH 1/7] added new Python examples using iSAM2 --- python/gtsam/examples/Pose2ISAM2Example.py | 153 +++++++++++++++++++ python/gtsam/examples/Pose3ISAM2Example.py | 165 +++++++++++++++++++++ 2 files changed, 318 insertions(+) create mode 100644 python/gtsam/examples/Pose2ISAM2Example.py create mode 100644 python/gtsam/examples/Pose3ISAM2Example.py diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py new file mode 100644 index 000000000..ba2fbb283 --- /dev/null +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -0,0 +1,153 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Pose SLAM example using iSAM2 in the 2D plane. +Author: Jerred Chen, Yusuf Ali +Modelled after: + - 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 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 + Author: Jerred Chen + Based on version by: + - Ellon Paiva (Python), + - Duy Nguyen Ta and Frank Dellaert (MATLAB) + """ + marginals = gtsam.Marginals(graph, current_estimate) + + fig = plt.figure(0) + axes = fig.gca() + plt.cla() + + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i)) + i += 1 + + plt.axis('equal') + axes.set_xlim(-1, 5) + axes.set_ylim(-1, 3) + plt.pause(1) + return marginals + + +def Pose2SLAM_ISAM2_example(): + 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. + 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)) + + # 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 + # 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)] + + # 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)] + + 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, current_estimate, xy_tol=0.6, theta_tol=0.3): + """ + Simple brute force approach which iterates through previous states + and checks for loop closure. + ### Parameters: + odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) + measurement in the body frame. + current_estimate: (gtsam.Values) The current estimates computed by iSAM2. + xy_tol: (double) Optional argument for the x-y measurement tolerance. + theta_tol: (double) Optional argument for the theta measurement tolerance. + ### Returns: + k: (int) 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 + + current_estimate = None + for i in range(len(odom_arr)): + + 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) + if loop: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(odom_x, odom_y, 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 + 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) + initial_estimate.clear() + + for i in range(1, len(odom_arr)+1): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") + + plt.ioff() + plt.show() + + +if __name__ == "__main__": + Pose2SLAM_ISAM2_example() diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py new file mode 100644 index 000000000..26d82ae33 --- /dev/null +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -0,0 +1,165 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Pose SLAM example using iSAM2 in 3D space. +Author: Jerred Chen +Modelled after version by: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), 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 + Author: Jerred Chen + Based on version by: + - Ellon Paiva (Python), + - Duy Nguyen Ta and Frank Dellaert (MATLAB) + """ + marginals = gtsam.Marginals(graph, current_estimate) + + 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 + + axes.set_xlim3d(-30, 45) + axes.set_ylim3d(-30, 45) + axes.set_zlim3d(-30, 45) + plt.pause(1) + + return marginals + + +def createPoses(): + """ + Creates ground truth poses of the robot. + """ + P0 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + P1 = np.array([[0, -1, 0, 15], + [1, 0, 0, 15], + [0, 0, 1, 20], + [0, 0, 0, 1]]) + P2 = np.array([[cos(pi/4), 0, sin(pi/4), 30], + [0, 1, 0, 30], + [-sin(pi/4), 0, cos(pi/4), 30], + [0, 0, 0, 1]]) + P3 = np.array([[0, 1, 0, 30], + [0, 0, -1, 0], + [-1, 0, 0, -15], + [0, 0, 0, 1]]) + P4 = np.array([[-1, 0, 0, 0], + [0, -1, 0, -10], + [0, 0, 1, -10], + [0, 0, 0, 1]]) + P5 = P0[:] + + return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] + +def Pose3_ISAM2_example(): + """ + """ + 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) + + 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)) + + poses = createPoses() + + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + # Add prior factor to the first pose + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) + initial_estimate.insert(X(0), 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, current_estimate, xyz_tol=0.6, rot_tol=0.3): + """ + Simple brute force approach which iterates through previous states + and checks for loop closure. + ### Parameters: + odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation + measurement in the body frame, [roll, pitch, yaw, x, y, z] + current_estimate: (gtsam.Values) The current estimates computed by iSAM2. + xyz_tol: (double) Optional argument for the translational tolerance. + rot_tol: (double) Optional argument for the rotational tolerance. + ### Returns: + k: (int) 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 + + current_estimate = None + for i in range(1, len(poses)): + # The odometry "ground truth" measurement 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 is not None: + graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), 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)))) + isam.update(graph, initial_estimate) + isam.update() + current_estimate = isam.calculateEstimate() + print("*"*50) + print(f"Inference after State {i}:") + print(current_estimate) + marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) + initial_estimate.clear() + i = 0 + while current_estimate.exists(X(i)): + print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") + i += 1 + + plt.ioff() + plt.show() + +if __name__ == '__main__': + Pose3_ISAM2_example() From 47c45c633f607790940859589292c03d35f4ceb4 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 13 Oct 2021 11:03:40 -0400 Subject: [PATCH 2/7] Added minor comments for documentation --- python/gtsam/examples/Pose2ISAM2Example.py | 7 +++++-- python/gtsam/examples/Pose3ISAM2Example.py | 22 +++++++++++++++++++--- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index ba2fbb283..6710dc706 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -48,6 +48,8 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): def Pose2SLAM_ISAM2_example(): + """ + """ plt.ion() def vector3(x, y, z): @@ -93,6 +95,7 @@ def Pose2SLAM_ISAM2_example(): """ Simple brute force approach which iterates through previous states and checks for loop closure. + Author: Jerred ### Parameters: odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) measurement in the body frame. @@ -119,7 +122,7 @@ def Pose2SLAM_ISAM2_example(): 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()) @@ -141,7 +144,7 @@ def Pose2SLAM_ISAM2_example(): print(current_estimate) marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() - + # Print the final covariance matrix for each pose after completing inference for i in range(1, len(odom_arr)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 26d82ae33..a15424d04 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -10,6 +10,7 @@ Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen 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 @@ -80,6 +81,8 @@ def createPoses(): def Pose3_ISAM2_example(): """ + Perform 3D SLAM given ground truth poses as well as simple + loop closure detection. """ plt.ion() @@ -87,19 +90,27 @@ def Pose3_ISAM2_example(): """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() + # 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() - # Add prior factor to the first pose + initial_estimate = gtsam.Values() + + # 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( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) @@ -108,6 +119,7 @@ def Pose3_ISAM2_example(): """ Simple brute force approach which iterates through previous states and checks for loop closure. + Author: Jerred Chen ### Parameters: odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation measurement in the body frame, [roll, pitch, yaw, x, y, z] @@ -131,13 +143,14 @@ def Pose3_ISAM2_example(): current_estimate = None for i in range(1, len(poses)): - # The odometry "ground truth" measurement between 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)) else: @@ -145,7 +158,9 @@ def Pose3_ISAM2_example(): # 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 isam.update(graph, initial_estimate) + # Additional iSAM2 optimization isam.update() current_estimate = isam.calculateEstimate() print("*"*50) @@ -153,6 +168,7 @@ def Pose3_ISAM2_example(): print(current_estimate) marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) 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") From 00c541aca660268e79834b8ab90f3782896482be Mon Sep 17 00:00:00 2001 From: jerredchen Date: Thu, 14 Oct 2021 13:42:21 -0400 Subject: [PATCH 3/7] adjusted docstrings to match google style guide --- python/gtsam/examples/Pose2ISAM2Example.py | 33 ++++++++--------- python/gtsam/examples/Pose3ISAM2Example.py | 43 ++++++++++------------ 2 files changed, 35 insertions(+), 41 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index 6710dc706..5336bc34e 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -22,9 +22,8 @@ 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 - Author: Jerred Chen + """Plots incremental state of the robot for 2D Pose SLAM using iSAM2 + Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) @@ -48,7 +47,8 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): def Pose2SLAM_ISAM2_example(): - """ + """Perform 2D SLAM given the ground truth changes in pose as well as + simple loop closure detection. """ plt.ion() @@ -91,20 +91,19 @@ def Pose2SLAM_ISAM2_example(): 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, current_estimate, xy_tol=0.6, theta_tol=0.3): - """ - Simple brute force approach which iterates through previous states + 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. - Author: Jerred - ### Parameters: - odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) - measurement in the body frame. - current_estimate: (gtsam.Values) The current estimates computed by iSAM2. - xy_tol: (double) Optional argument for the x-y measurement tolerance. - theta_tol: (double) Optional argument for the theta measurement tolerance. - ### Returns: - k: (int) The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. + + 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) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index a15424d04..3fcdcd7ec 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -24,9 +24,8 @@ import matplotlib.pyplot as plt def Pose3SLAM_ISAM2_plot(graph, current_estimate): - """ - Plots incremental state of the robot for 3D Pose SLAM using iSAM2 - Author: Jerred Chen + """Plots incremental state of the robot for 3D Pose SLAM using iSAM2 + Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) @@ -52,9 +51,7 @@ def Pose3SLAM_ISAM2_plot(graph, current_estimate): def createPoses(): - """ - Creates ground truth poses of the robot. - """ + """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], @@ -80,10 +77,8 @@ def createPoses(): return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] def Pose3_ISAM2_example(): - """ - Perform 3D SLAM given ground truth poses as well as simple - loop closure detection. - """ + """Perform 3D SLAM given ground truth poses as well as simple + loop closure detection.""" plt.ion() def vector6(x, y, z, a, b, c): @@ -115,20 +110,20 @@ def Pose3_ISAM2_example(): initial_estimate.insert(X(0), 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, current_estimate, xyz_tol=0.6, rot_tol=0.3): - """ - Simple brute force approach which iterates through previous states + 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. - Author: Jerred Chen - ### Parameters: - odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation - measurement in the body frame, [roll, pitch, yaw, x, y, z] - current_estimate: (gtsam.Values) The current estimates computed by iSAM2. - xyz_tol: (double) Optional argument for the translational tolerance. - rot_tol: (double) Optional argument for the rotational tolerance. - ### Returns: - k: (int) The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. + + 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]) @@ -164,7 +159,7 @@ def Pose3_ISAM2_example(): isam.update() current_estimate = isam.calculateEstimate() print("*"*50) - print(f"Inference after State {i}:") + print(f"Inference after State {i}:\n") print(current_estimate) marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() From 79f7861f0c14a0c3a502be0ff3447a1dc737ea28 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Sun, 24 Oct 2021 15:34:49 -0400 Subject: [PATCH 4/7] made changes according to Frank --- python/gtsam/examples/Pose2ISAM2Example.py | 158 ++++++++++-------- python/gtsam/examples/Pose3ISAM2Example.py | 182 ++++++++++++--------- 2 files changed, 189 insertions(+), 151 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index 5336bc34e..fac7ecc1a 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -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() diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 3fcdcd7ec..3e78339bd 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -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() From e31beee22b2a38676e792ba86503b763562d8f35 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 27 Oct 2021 22:33:11 -0400 Subject: [PATCH 5/7] removed ground truth; angles set in deg and converted to rad --- python/gtsam/examples/Pose2ISAM2Example.py | 53 +++++++++++----------- 1 file changed, 26 insertions(+), 27 deletions(-) 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") From c51a1a23098c87a13849a4e77cb36ed165983282 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 27 Oct 2021 22:35:03 -0400 Subject: [PATCH 6/7] removed ground truth; set ang in deg and convert to rad for Pose3iSAM2 --- python/gtsam/examples/Pose3ISAM2Example.py | 79 ++++++++++++---------- 1 file changed, 43 insertions(+), 36 deletions(-) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 3e78339bd..82458822a 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -8,7 +8,7 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen -Modelled after version by: +Modeled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ @@ -20,12 +20,8 @@ import matplotlib.pyplot as plt 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,8 +45,6 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_zlim3d(-30, 45) plt.pause(1) - return marginals - def createPoses(): """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], @@ -78,34 +72,27 @@ def createPoses(): 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: +def determine_loop_closure(odom_tf: gtsam.Pose3, current_estimate: gtsam.Values, + key: int, xyz_tol=0.6, rot_tol=17) -> 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]. + odom_tf: The noisy odometry transformation measurement in the body frame. 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. + xyz_tol: Optional argument for the translational tolerance, in meters. + rot_tol: Optional argument for the rotational 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. """ 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) + curr_est = prev_est.compose(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 \ + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol*np.pi/180).all() and \ (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): return k @@ -114,11 +101,33 @@ def Pose3_ISAM2_example(): loop closure detection.""" plt.ion() + # Declare the 3D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xyz_sigma = 0.3 + + # Declare the 3D rotational standard deviations of the prior factor's Gaussian model, in degrees. + prior_rpy_sigma = 5 + + # Declare the 3D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xyz_sigma = 0.2 + + # Declare the 3D rotational standard deviations of the odometry factor's Gaussian model, in degrees. + odometry_rpy_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(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)) + 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_xyz_sigma, + prior_xyz_sigma, + prior_xyz_sigma])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_xyz_sigma, + odometry_xyz_sigma, + odometry_xyz_sigma])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -154,38 +163,36 @@ def Pose3_ISAM2_example(): current_estimate = initial_estimate for i in range(len(odometry_tf)): - # Obtain "ground truth" transformation between the current pose and the previous pose. - true_odometry = odometry_tf[i] - # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise. noisy_odometry = noisy_measurements[i] + # Compute the noisy odometry transformation according to the xyz translation and roll-pitch-yaw rotation. + noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) + # 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) + loop = determine_loop_closure(noisy_tf, current_estimate, i, xyz_tol=18, rot_tol=30) # 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)) + graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, true_odometry, ODOMETRY_NOISE)) + 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. - 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) + noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) + initial_estimate.insert(i + 2, noisy_estimate) # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) current_estimate = isam.calculateEstimate() # Report all current state estimates from the iSAM2 optimization. - 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. + marginals = gtsam.Marginals(graph, current_estimate) i = 1 while current_estimate.exists(i): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") From 99ce18c85735e7722b744446d13ecf65d99f88ae Mon Sep 17 00:00:00 2001 From: jerredchen Date: Thu, 28 Oct 2021 12:29:00 -0400 Subject: [PATCH 7/7] formatting by Google style --- python/gtsam/examples/Pose2ISAM2Example.py | 20 +++++++++++++------ python/gtsam/examples/Pose3ISAM2Example.py | 23 ++++++++++++---------- 2 files changed, 27 insertions(+), 16 deletions(-) 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)