removed ground truth; angles set in deg and converted to rad

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.
+        xy_tol: Optional argument for the x-y measurement tolerance, in meters.
-        theta_tol: Optional argument for the theta measurement tolerance.
+        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))
+    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(vector3(0.2, 0.2, 0.1))
+    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")