From 430530ca544be50001252ec1209bf1c95cf40d0a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Oct 2021 14:40:30 -0400 Subject: [PATCH 1/2] put all example scripts inside main() functions and apply formatting --- python/gtsam/examples/CustomFactorExample.py | 224 ++++++++++-------- python/gtsam/examples/GPSFactorExample.py | 51 ++-- python/gtsam/examples/OdometryExample.py | 95 ++++---- python/gtsam/examples/PlanarSLAMExample.py | 134 ++++++----- python/gtsam/examples/Pose2SLAMExample.py | 156 ++++++------ python/gtsam/examples/Pose2SLAMExample_g2o.py | 145 ++++++------ python/gtsam/examples/Pose3SLAMExample_g2o.py | 113 +++++---- ...Pose3SLAMExample_initializePose3Chordal.py | 32 ++- 8 files changed, 514 insertions(+), 436 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index c7fe1e202c..03b1384108 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -9,15 +9,17 @@ Author: Fan Jiang, Frank Dellaert """ +from functools import partial +from typing import List, Optional + import gtsam import numpy as np -from typing import List, Optional -from functools import partial +I = np.eye(1) def simulate_car(): - # Simulate a car for one second + """Simulate a car for one second""" x0 = 0 dt = 0.25 # 4 Hz, typical GPS v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast @@ -26,46 +28,8 @@ def simulate_car(): return x -x = simulate_car() -print(f"Simulated car trajectory: {x}") - -# %% -add_noise = True # set this to False to run with "perfect" measurements - -# GPS measurements -sigma_gps = 3.0 # assume GPS is +/- 3m -g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) - for k in range(5)] - -# Odometry measurements -sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz -o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0) - for k in range(4)] - -# Landmark measurements: -sigma_lm = 1 # assume landmark measurement is accurate up to 1m - -# Assume first landmark is at x=5, we measure it at time k=0 -lm_0 = 5.0 -z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) - -# Assume other landmark is at x=28, we measure it at time k=3 -lm_3 = 28.0 -z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) - -unknown = [gtsam.symbol('x', k) for k in range(5)] - -print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) - -# We now can use nonlinear factor graphs -factor_graph = gtsam.NonlinearFactorGraph() - -# Add factors for GPS measurements -I = np.eye(1) -gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) - - -def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, + jacobians: Optional[List[np.ndarray]]): """GPS Factor error function :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -82,36 +46,8 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobia return error -# Add the GPS factors -for k in range(5): - gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]]))) - factor_graph.add(gf) - -# New Values container -v = gtsam.Values() - -# Add initial estimates to the Values container -for i in range(5): - v.insert(unknown[i], np.array([0.0])) - -# Initialize optimizer -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) - -# Optimize the factor graph -result = optimizer.optimize() - -# calculate the error from ground truth -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) - -print("Result with only GPS") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") - -# Adding odometry will improve things a lot -odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) - - -def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, + jacobians: Optional[List[np.ndarray]]): """Odometry Factor error function :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -130,25 +66,8 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobi return error -for k in range(4): - odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]]))) - factor_graph.add(odof) - -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) - -result = optimizer.optimize() - -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) - -print("Result with GPS+Odometry") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") - -# This is great, but GPS noise is still apparent, so now we add the two landmarks -lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) - - -def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, + jacobians: Optional[List[np.ndarray]]): """Landmark Factor error function :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -165,15 +84,120 @@ def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobian return error -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0])))) -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3])))) +def main(): + """Main runner.""" + + x = simulate_car() + print(f"Simulated car trajectory: {x}") + + add_noise = True # set this to False to run with "perfect" measurements + + # GPS measurements + sigma_gps = 3.0 # assume GPS is +/- 3m + g = [ + x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) + for k in range(5) + ] + + # Odometry measurements + sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz + o = [ + x[k + 1] - x[k] + + (np.random.normal(scale=sigma_odo) if add_noise else 0) + for k in range(4) + ] + + # Landmark measurements: + sigma_lm = 1 # assume landmark measurement is accurate up to 1m + + # Assume first landmark is at x=5, we measure it at time k=0 + lm_0 = 5.0 + z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + + # Assume other landmark is at x=28, we measure it at time k=3 + lm_3 = 28.0 + z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + + unknown = [gtsam.symbol('x', k) for k in range(5)] + + print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) + + # We now can use nonlinear factor graphs + factor_graph = gtsam.NonlinearFactorGraph() + + # Add factors for GPS measurements + gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) + + # Add the GPS factors + for k in range(5): + gf = gtsam.CustomFactor(gps_model, [unknown[k]], + partial(error_gps, np.array([g[k]]))) + factor_graph.add(gf) + + # New Values container + v = gtsam.Values() + + # Add initial estimates to the Values container + for i in range(5): + v.insert(unknown[i], np.array([0.0])) + + # Initialize optimizer + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + # Optimize the factor graph + result = optimizer.optimize() + + # calculate the error from ground truth + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with only GPS") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + # Adding odometry will improve things a lot + odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) + + for k in range(4): + odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], + partial(error_odom, np.array([o[k]]))) + factor_graph.add(odof) + + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + result = optimizer.optimize() + + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) + + print("Result with GPS+Odometry") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") + + # This is great, but GPS noise is still apparent, so now we add the two landmarks + lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) + + factor_graph.add( + gtsam.CustomFactor(lm_model, [unknown[0]], + partial(error_lm, np.array([lm_0 + z_0])))) + factor_graph.add( + gtsam.CustomFactor(lm_model, [unknown[3]], + partial(error_lm, np.array([lm_3 + z_3])))) + + params = gtsam.GaussNewtonParams() + optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + + result = optimizer.optimize() -params = gtsam.GaussNewtonParams() -optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + error = np.array([(result.atVector(unknown[k]) - x[k])[0] + for k in range(5)]) -result = optimizer.optimize() + print("Result with GPS+Odometry+Landmark") + print(result, np.round(error, 2), + f"\nJ(X)={0.5 * np.sum(np.square(error))}") -error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) -print("Result with GPS+Odometry+Landmark") -print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/GPSFactorExample.py b/python/gtsam/examples/GPSFactorExample.py index 0bc0d1bf32..8eb663cb4c 100644 --- a/python/gtsam/examples/GPSFactorExample.py +++ b/python/gtsam/examples/GPSFactorExample.py @@ -13,13 +13,8 @@ from __future__ import print_function -import numpy as np - import gtsam -import matplotlib.pyplot as plt -import gtsam.utils.plot as gtsam_plot - # ENU Origin is where the plane was in hold next to runway lat0 = 33.86998 lon0 = -84.30626 @@ -29,28 +24,34 @@ GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() -# Add a prior on the first point, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose3() # prior at origin -graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) +def main(): + """Main runner.""" + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() + + # Add a prior on the first point, setting it to the origin + # A prior factor consists of a mean and a noise model (covariance matrix) + priorMean = gtsam.Pose3() # prior at origin + graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) + + # Add GPS factors + gps = gtsam.Point3(lat0, lon0, h0) + graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) -# Add GPS factors -gps = gtsam.Point3(lat0, lon0, h0) -graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) + # Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initial = gtsam.Values() + initial.insert(1, gtsam.Pose3()) + print("\nInitial Estimate:\n{}".format(initial)) -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose3()) -print("\nInitial Estimate:\n{}".format(initial)) + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/OdometryExample.py b/python/gtsam/examples/OdometryExample.py index 8b519ce9a7..210aeb8083 100644 --- a/python/gtsam/examples/OdometryExample.py +++ b/python/gtsam/examples/OdometryExample.py @@ -13,57 +13,60 @@ from __future__ import print_function -import numpy as np - import gtsam - -import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt +import numpy as np # Create noise models ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() - -# Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise model (covariance matrix) -priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin -graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) - -# Add odometry factors -odometry = gtsam.Pose2(2.0, 0.0, 0.0) -# For simplicity, we will use the same noise model for each odometry factor -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) - -# Create the data structure to hold the initialEstimate estimate to the solution -# For illustrative purposes, these have been deliberately set to incorrect values -initial = gtsam.Values() -initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) -print("\nInitial Estimate:\n{}".format(initial)) - -# optimize using Levenberg-Marquardt optimization -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) - -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 4): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) - -fig = plt.figure(0) -for i in range(1, 4): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) -plt.axis('equal') -plt.show() - - +def main(): + """Main runner""" + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() + + # Add a prior on the first pose, setting it to the origin + # A prior factor consists of a mean and a noise model (covariance matrix) + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE)) + + # Add odometry factors + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + # For simplicity, we will use the same noise model for each odometry factor + # Create odometry (Between) factors between consecutive poses + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE)) + graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) + + # Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initial = gtsam.Values() + initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) + print("\nInitial Estimate:\n{}".format(initial)) + + # optimize using Levenberg-Marquardt optimization + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for i in range(1, 4): + print("X{} covariance:\n{}\n".format(i, + marginals.marginalCovariance(i))) + + for i in range(1, 4): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, + marginals.marginalCovariance(i)) + plt.axis('equal') + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py index 5ffdf048d0..d2ee92c95a 100644 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ b/python/gtsam/examples/PlanarSLAMExample.py @@ -13,69 +13,85 @@ from __future__ import print_function -import numpy as np - import gtsam -from gtsam.symbol_shorthand import X, L +import numpy as np +from gtsam.symbol_shorthand import L, X # Create noise models PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) -# Create an empty nonlinear factor graph -graph = gtsam.NonlinearFactorGraph() - -# Create the keys corresponding to unknown variables in the factor graph -X1 = X(1) -X2 = X(2) -X3 = X(3) -L1 = L(4) -L2 = L(5) - -# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model -graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) - -# Add odometry factors between X1,X2 and X2,X3, respectively -graph.add(gtsam.BetweenFactorPose2( - X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE)) - -# Add Range-Bearing measurements to two different landmarks L1 and L2 -graph.add(gtsam.BearingRangeFactor2D( - X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) -graph.add(gtsam.BearingRangeFactor2D( - X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE)) - -# Print graph -print("Factor Graph:\n{}".format(graph)) - -# Create (deliberately inaccurate) initial estimate -initial_estimate = gtsam.Values() -initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) -initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) -initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) -initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) -initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) - -# Print -print("Initial Estimate:\n{}".format(initial_estimate)) - -# Optimize using Levenberg-Marquardt optimization. The optimizer -# accepts an optional set of configuration parameters, controlling -# things like convergence criteria, the type of linear system solver -# to use, and the amount of information displayed during optimization. -# Here we will use the default set of parameters. See the -# documentation for the full set of parameters. -params = gtsam.LevenbergMarquardtParams() -optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) -result = optimizer.optimize() -print("\nFinal Result:\n{}".format(result)) - -# Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]: - print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key))) + +def main(): + """Main runner""" + + # Create an empty nonlinear factor graph + graph = gtsam.NonlinearFactorGraph() + + # Create the keys corresponding to unknown variables in the factor graph + X1 = X(1) + X2 = X(2) + X3 = X(3) + L1 = L(4) + L2 = L(5) + + # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model + graph.add( + gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) + + # Add odometry factors between X1,X2 and X2,X3, respectively + graph.add( + gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), + ODOMETRY_NOISE)) + + # Add Range-Bearing measurements to two different landmarks L1 and L2 + graph.add( + gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), + np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE)) + graph.add( + gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, + MEASUREMENT_NOISE)) + graph.add( + gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, + MEASUREMENT_NOISE)) + + # Print graph + print("Factor Graph:\n{}".format(graph)) + + # Create (deliberately inaccurate) initial estimate + initial_estimate = gtsam.Values() + initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) + initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) + initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) + initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) + initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) + + # Print + print("Initial Estimate:\n{}".format(initial_estimate)) + + # Optimize using Levenberg-Marquardt optimization. The optimizer + # accepts an optional set of configuration parameters, controlling + # things like convergence criteria, the type of linear system solver + # to use, and the amount of information displayed during optimization. + # Here we will use the default set of parameters. See the + # documentation for the full set of parameters. + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, + params) + result = optimizer.optimize() + print("\nFinal Result:\n{}".format(result)) + + # Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), + (L2, "L2")]: + print("{} covariance:\n{}\n".format(s, + marginals.marginalCovariance(key))) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py index 2ec190d73a..300a70fbda 100644 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ b/python/gtsam/examples/Pose2SLAMExample.py @@ -15,82 +15,88 @@ import math -import numpy as np - import gtsam - -import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot +import matplotlib.pyplot as plt -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) - -# Create noise models -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)) - -# 1. Create a factor graph container and add factors to it -graph = gtsam.NonlinearFactorGraph() - -# 2a. Add a prior on the first pose, setting it to the origin -# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) -graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) - -# 2b. Add odometry factors -# Create odometry (Between) factors between consecutive poses -graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -graph.add(gtsam.BetweenFactorPose2( - 4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) - -# 2c. Add the loop closure constraint -# This factor encodes the fact that we have returned to the same pose. In real -# systems, these constraints may be identified in many ways, such as appearance-based -# techniques with camera images. We will use another Between Factor to enforce this constraint: -graph.add(gtsam.BetweenFactorPose2( - 5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE)) -print("\nFactor Graph:\n{}".format(graph)) # print - -# 3. Create the data structure to hold the initial_estimate estimate to the -# solution. For illustrative purposes, these have been deliberately set to incorrect values -initial_estimate = gtsam.Values() -initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) -initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) -initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) -initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) -initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) -print("\nInitial Estimate:\n{}".format(initial_estimate)) # print - -# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer -# The optimizer accepts an optional set of configuration parameters, -# controlling things like convergence criteria, the type of linear -# system solver to use, and the amount of information displayed during -# optimization. We will set a few parameters as a demonstration. -parameters = gtsam.GaussNewtonParams() - -# Stop iterating once the change in error between steps is less than this value -parameters.setRelativeErrorTol(1e-5) -# Do not perform more than N iteration steps -parameters.setMaxIterations(100) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) -# ... and optimize -result = optimizer.optimize() -print("Final Result:\n{}".format(result)) - -# 5. Calculate and print marginal covariances for all variables -marginals = gtsam.Marginals(graph, result) -for i in range(1, 6): - print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) - -fig = plt.figure(0) -for i in range(1, 6): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) - -plt.axis('equal') -plt.show() +def main(): + """Main runner.""" + # Create noise models + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas( + gtsam.Point3(0.2, 0.2, 0.1)) + + # 1. Create a factor graph container and add factors to it + graph = gtsam.NonlinearFactorGraph() + + # 2a. Add a prior on the first pose, setting it to the origin + # A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) + graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + + # 2b. Add odometry factors + # Create odometry (Between) factors between consecutive poses + graph.add( + gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + graph.add( + gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + + # 2c. Add the loop closure constraint + # This factor encodes the fact that we have returned to the same pose. In real + # systems, these constraints may be identified in many ways, such as appearance-based + # techniques with camera images. We will use another Between Factor to enforce this constraint: + graph.add( + gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), + ODOMETRY_NOISE)) + print("\nFactor Graph:\n{}".format(graph)) # print + + # 3. Create the data structure to hold the initial_estimate estimate to the + # solution. For illustrative purposes, these have been deliberately set to incorrect values + initial_estimate = gtsam.Values() + initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) + initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) + initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) + print("\nInitial Estimate:\n{}".format(initial_estimate)) # print + + # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + # The optimizer accepts an optional set of configuration parameters, + # controlling things like convergence criteria, the type of linear + # system solver to use, and the amount of information displayed during + # optimization. We will set a few parameters as a demonstration. + parameters = gtsam.GaussNewtonParams() + + # Stop iterating once the change in error between steps is less than this value + parameters.setRelativeErrorTol(1e-5) + # Do not perform more than N iteration steps + parameters.setMaxIterations(100) + # Create the optimizer ... + optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) + # ... and optimize + result = optimizer.optimize() + print("Final Result:\n{}".format(result)) + + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + for i in range(1, 6): + print("X{} covariance:\n{}\n".format(i, + marginals.marginalCovariance(i))) + + for i in range(1, 6): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, + marginals.marginalCovariance(i)) + + plt.axis('equal') + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose2SLAMExample_g2o.py b/python/gtsam/examples/Pose2SLAMExample_g2o.py index 97fb46c5f2..cf029c0499 100644 --- a/python/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose2SLAMExample_g2o.py @@ -12,77 +12,86 @@ # pylint: disable=invalid-name, E1101 from __future__ import print_function + import argparse -import math -import numpy as np -import matplotlib.pyplot as plt import gtsam +import matplotlib.pyplot as plt from gtsam.utils import plot -def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) - - -parser = argparse.ArgumentParser( - description="A 2D Pose SLAM example that reads input from g2o, " - "converts it to a factor graph and does the optimization. " - "Output is written on a file, in g2o format") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument('-m', '--maxiter', type=int, - help="maximum number of iterations for optimizer") -parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'], - default="none", help="Type of kernel used") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() - -g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ - else args.input - -maxIterations = 100 if args.maxiter is None else args.maxiter - -is3D = False - -graph, initial = gtsam.readG2o(g2oFile, is3D) - -assert args.kernel == "none", "Supplied kernel type is not yet implemented" - -# Add prior on the pose having index (key) = 0 -priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) - -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") -params.setMaxIterations(maxIterations) -# parameters.setRelativeErrorTol(1e-5) -# Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -# ... and optimize -result = optimizer.optimize() - -print("Optimization complete") -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) - - -if args.output is None: - print("\nFactor Graph:\n{}".format(graph)) - print("\nInitial Estimate:\n{}".format(initial)) - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") - -if args.plot: - resultPoses = gtsam.utilities.extractPose2(result) - for i in range(resultPoses.shape[0]): - plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) - plt.show() +def main(): + """Main runner.""" + + parser = argparse.ArgumentParser( + description="A 2D Pose SLAM example that reads input from g2o, " + "converts it to a factor graph and does the optimization. " + "Output is written on a file, in g2o format") + parser.add_argument('-i', '--input', help='input file g2o format') + parser.add_argument( + '-o', + '--output', + help="the path to the output file with optimized graph") + parser.add_argument('-m', + '--maxiter', + type=int, + help="maximum number of iterations for optimizer") + parser.add_argument('-k', + '--kernel', + choices=['none', 'huber', 'tukey'], + default="none", + help="Type of kernel used") + parser.add_argument("-p", + "--plot", + action="store_true", + help="Flag to plot results") + args = parser.parse_args() + + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\ + else args.input + + maxIterations = 100 if args.maxiter is None else args.maxiter + + is3D = False + + graph, initial = gtsam.readG2o(g2oFile, is3D) + + assert args.kernel == "none", "Supplied kernel type is not yet implemented" + + # Add prior on the pose having index (key) = 0 + priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8)) + graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) + + params = gtsam.GaussNewtonParams() + params.setVerbosity("Termination") + params.setMaxIterations(maxIterations) + # parameters.setRelativeErrorTol(1e-5) + # Create the optimizer ... + optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) + # ... and optimize + result = optimizer.optimize() + + print("Optimization complete") + print("initial error = ", graph.error(initial)) + print("final error = ", graph.error(result)) + + if args.output is None: + print("\nFactor Graph:\n{}".format(graph)) + print("\nInitial Estimate:\n{}".format(initial)) + print("Final Result:\n{}".format(result)) + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print("Done!") + + if args.plot: + resultPoses = gtsam.utilities.extractPose2(result) + for i in range(resultPoses.shape[0]): + plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :])) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose3SLAMExample_g2o.py b/python/gtsam/examples/Pose3SLAMExample_g2o.py index 501a75dc17..dcdfc34a3b 100644 --- a/python/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/python/gtsam/examples/Pose3SLAMExample_g2o.py @@ -8,13 +8,14 @@ # pylint: disable=invalid-name, E1101 from __future__ import print_function + import argparse -import numpy as np -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D import gtsam +import matplotlib.pyplot as plt +import numpy as np from gtsam.utils import plot +from mpl_toolkits.mplot3d import Axes3D def vector6(x, y, z, a, b, c): @@ -22,50 +23,62 @@ def vector6(x, y, z, a, b, c): return np.array([x, y, z, a, b, c], dtype=float) -parser = argparse.ArgumentParser( - description="A 3D Pose SLAM example that reads input from g2o, and " - "initializes Pose3") -parser.add_argument('-i', '--input', help='input file g2o format') -parser.add_argument('-o', '--output', - help="the path to the output file with optimized graph") -parser.add_argument("-p", "--plot", action="store_true", - help="Flag to plot results") -args = parser.parse_args() - -g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ - else args.input - -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) - -# Add Prior on the first key -priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, - 1e-4, 1e-4, 1e-4)) - -print("Adding prior to g2o file ") -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) - -params = gtsam.GaussNewtonParams() -params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) -result = optimizer.optimize() -print("Optimization complete") - -print("initial error = ", graph.error(initial)) -print("final error = ", graph.error(result)) - -if args.output is None: - print("Final Result:\n{}".format(result)) -else: - outputFile = args.output - print("Writing results to file: ", outputFile) - graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) - gtsam.writeG2o(graphNoKernel, result, outputFile) - print ("Done!") - -if args.plot: - resultPoses = gtsam.utilities.allPose3s(result) - for i in range(resultPoses.size()): - plot.plot_pose3(1, resultPoses.atPose3(i)) - plt.show() +def main(): + """Main runner.""" + + parser = argparse.ArgumentParser( + description="A 3D Pose SLAM example that reads input from g2o, and " + "initializes Pose3") + parser.add_argument('-i', '--input', help='input file g2o format') + parser.add_argument( + '-o', + '--output', + help="the path to the output file with optimized graph") + parser.add_argument("-p", + "--plot", + action="store_true", + help="Flag to plot results") + args = parser.parse_args() + + g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ + else args.input + + is3D = True + graph, initial = gtsam.readG2o(g2oFile, is3D) + + # Add Prior on the first key + priorModel = gtsam.noiseModel.Diagonal.Variances( + vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) + + print("Adding prior to g2o file ") + firstKey = initial.keys()[0] + graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) + + params = gtsam.GaussNewtonParams() + params.setVerbosity( + "Termination") # this will show info about stopping conds + optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) + result = optimizer.optimize() + print("Optimization complete") + + print("initial error = ", graph.error(initial)) + print("final error = ", graph.error(result)) + + if args.output is None: + print("Final Result:\n{}".format(result)) + else: + outputFile = args.output + print("Writing results to file: ", outputFile) + graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) + gtsam.writeG2o(graphNoKernel, result, outputFile) + print("Done!") + + if args.plot: + resultPoses = gtsam.utilities.allPose3s(result) + for i in range(resultPoses.size()): + plot.plot_pose3(1, resultPoses.atPose3(i)) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py index 2b2c5f9919..e439bbaeb1 100644 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -13,23 +13,29 @@ from __future__ import print_function +import gtsam import numpy as np -import gtsam -# Read graph from file -g2oFile = gtsam.findExampleDataFile("pose3example.txt") +def main(): + """Main runner.""" + # Read graph from file + g2oFile = gtsam.findExampleDataFile("pose3example.txt") + + is3D = True + graph, initial = gtsam.readG2o(g2oFile, is3D) + + # Add prior on the first key. TODO: assumes first key ios z + priorModel = gtsam.noiseModel.Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) + firstKey = initial.keys()[0] + graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) -is3D = True -graph, initial = gtsam.readG2o(g2oFile, is3D) + # Initializing Pose3 - chordal relaxation" + initialization = gtsam.InitializePose3.initialize(graph) -# Add prior on the first key. TODO: assumes first key ios z -priorModel = gtsam.noiseModel.Diagonal.Variances( - np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) -firstKey = initial.keys()[0] -graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + print(initialization) -# Initializing Pose3 - chordal relaxation" -initialization = gtsam.InitializePose3.initialize(graph) -print(initialization) +if __name__ == "__main__": + main() From f8f93cab21128efd163af0abfc8a3dc6824694f8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Oct 2021 19:28:11 -0400 Subject: [PATCH 2/2] add type annotations --- python/gtsam/examples/CustomFactorExample.py | 17 ++++++++++------- .../Pose3SLAMExample_initializePose3Chordal.py | 2 +- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index 03b1384108..36c1e003d3 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -18,7 +18,7 @@ I = np.eye(1) -def simulate_car(): +def simulate_car() -> List[float]: """Simulate a car for one second""" x0 = 0 dt = 0.25 # 4 Hz, typical GPS @@ -28,8 +28,9 @@ def simulate_car(): return x -def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, - jacobians: Optional[List[np.ndarray]]): +def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """GPS Factor error function :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -46,8 +47,9 @@ def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, return error -def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, - jacobians: Optional[List[np.ndarray]]): +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """Odometry Factor error function :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle @@ -66,8 +68,9 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, return error -def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, - jacobians: Optional[List[np.ndarray]]): +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, + values: gtsam.Values, + jacobians: Optional[List[np.ndarray]]) -> float: """Landmark Factor error function :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle diff --git a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py index e439bbaeb1..a96da07745 100644 --- a/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py +++ b/python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -31,7 +31,7 @@ def main(): firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) - # Initializing Pose3 - chordal relaxation" + # Initializing Pose3 - chordal relaxation initialization = gtsam.InitializePose3.initialize(graph) print(initialization)