diff --git a/cython/gtsam/examples/PlanarSLAMExample.py b/cython/gtsam/examples/PlanarSLAMExample.py new file mode 100644 index 000000000..ca3d26bee --- /dev/null +++ b/cython/gtsam/examples/PlanarSLAMExample.py @@ -0,0 +1,109 @@ +import gtsam +import numpy as np +import math + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Create the keys we need for graph +# static Symbol x1('x',1), x2('x',2), x3('x',3); +# static Symbol l1('l',1), l2('l',2); +x1 = gtsam.symbol(ord('x'), 1) +x2 = gtsam.symbol(ord('x'), 2) +x3 = gtsam.symbol(ord('x'), 3) +l1 = gtsam.symbol(ord('l'), 4) +l2 = gtsam.symbol(ord('l'), 5) + +# Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) +# Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +# noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta +priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +# graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph +graph.add(gtsam.PriorFactorPose2(x1, priorMean, priorNoise)) + + +# Add two odometry factors between x1,x2 and x2,x3 +# Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta +# For simplicity, we will use the same noise model for each odometry factor +odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +# Create odometry (Between) factors between consecutive poses +# graph.emplace_shared >(x1, x2, odometry, odometryNoise); +graph.add(gtsam.BetweenFactorPose2(x1, x2, odometry, odometryNoise)) +# graph.emplace_shared >(x2, x3, odometry, odometryNoise); +graph.add(gtsam.BetweenFactorPose2(x2, x3, odometry, odometryNoise)) + +# Add Range-Bearing measurements to two different landmarks +# create a noise model for the landmark measurements +# noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range +measurementNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) +# Rot2 bearing11 = Rot2::fromDegrees(45), +# bearing21 = Rot2::fromDegrees(90), +# bearing32 = Rot2::fromDegrees(90); +# double range11 = std::sqrt(4.0+4.0), +# range21 = 2.0, +# range32 = 2.0; +bearing11 = gtsam.Rot2.fromDegrees(np.pi/4) +bearing21 = gtsam.Rot2.fromDegrees(np.pi/2) +bearing32 = gtsam.Rot2.fromDegrees(np.pi/2) +range11 = np.sqrt(4.0+4.0) +range21 = 2.0 +range32 = 2.0 + +# Add Bearing-Range factors +# graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); +# graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); +# graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); +graph.add(gtsam.BearingRangeFactor2D(x1,l1,bearing11,range11,measurementNoise)) +graph.add(gtsam.BearingRangeFactor2D(x2,l1,bearing21,range21,measurementNoise)) +graph.add(gtsam.BearingRangeFactor2D(x3,l2,bearing32,range32,measurementNoise)) + +# Print graph +graph.print_("Factor Graph:\n"); + +# Create (deliberately inaccurate) initial estimate +# Values initialEstimate; +# initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); +# initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); +# initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); +# initialEstimate.insert(l1, Point2(1.8, 2.1)); +# initialEstimate.insert(l2, Point2(4.1, 1.8)); +initialEstimate = gtsam.Values() +initialEstimate.insert(x1, gtsam.Pose2(-0.25, 0.20, 0.15)) +initialEstimate.insert(x2, gtsam.Pose2( 2.30, 0.10, -0.20)) +initialEstimate.insert(x3, gtsam.Pose2( 4.10, 0.10, 0.10)) +initialEstimate.insert(l1, gtsam.Point2( 1.80, 2.10)) +initialEstimate.insert(l2, gtsam.Point2( 4.10, 1.80)) + +# Print +initialEstimate.print_("Initial Estimate:\n"); + +# 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. +# LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); +# Values result = optimizer.optimize(); +# result.print("Final Result:\n"); +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate, params) +result = optimizer.optimize() +result.print_("\nFinal Result:\n") + +# Calculate and print marginal covariances for all variables +# Marginals marginals(graph, result); +# print(marginals.marginalCovariance(x1), "x1 covariance"); +# print(marginals.marginalCovariance(x2), "x2 covariance"); +# print(marginals.marginalCovariance(x3), "x3 covariance"); +# print(marginals.marginalCovariance(l1), "l1 covariance"); +# print(marginals.marginalCovariance(l2), "l2 covariance"); +marginals = gtsam.Marginals(graph, result) +print("x1 covariance:\n", marginals.marginalCovariance(x1)) +print("x2 covariance:\n", marginals.marginalCovariance(x2)) +print("x3 covariance:\n", marginals.marginalCovariance(x3)) +print("x4 covariance:\n", marginals.marginalCovariance(l1)) +print("x5 covariance:\n", marginals.marginalCovariance(l2))