GTSAM examples, cython versions

cython/gtsam/examples/OdometryExample.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+# 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
+priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
+
+# 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
+odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+# Create odometry (Between) factors between consecutive poses
+graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
+graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
+graph.print_("\nFactor Graph:\n")
+
+# 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))
+initial.print_("\nInitial Estimate:\n")
+
+# optimize using Levenberg-Marquardt optimization
+params = gtsam.LevenbergMarquardtParams()
+optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+result = optimizer.optimize()
+result.print_("\nFinal Result:\n")

cython/gtsam/examples/Pose2SLAMExample.py

@@ -0,0 +1,75 @@
+from __future__ import print_function
+
+import math
+
+import numpy as np
+
+import gtsam
+
+
+def Vector3(x, y, z): return np.array([x, y, z])
+
+
+# 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 model (covariance matrix)
+priorNoise = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1))
+graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise))
+
+# For simplicity, we will use the same noise model for odometry and loop closures
+model = gtsam.noiseModel_Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1))
+
+# 2b. Add odometry factors
+# Create odometry (Between) factors between consecutive poses
+graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model))
+graph.add(gtsam.BetweenFactorPose2(
+    2, 3, gtsam.Pose2(2, 0, math.pi / 2), model))
+graph.add(gtsam.BetweenFactorPose2(
+    3, 4, gtsam.Pose2(2, 0, math.pi / 2), model))
+graph.add(gtsam.BetweenFactorPose2(
+    4, 5, gtsam.Pose2(2, 0, math.pi / 2), model))
+
+# 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), model))
+graph.print_("\nFactor Graph:\n")  # 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))
+initial_estimate.print_("\nInitial Estimate:\n")  # 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()
+result.print_("Final Result:\n")
+
+# 5. Calculate and print marginal covariances for all variables
+marginals = gtsam.Marginals(graph, result)
+print("x1 covariance:\n", marginals.marginalCovariance(1))
+print("x2 covariance:\n", marginals.marginalCovariance(2))
+print("x3 covariance:\n", marginals.marginalCovariance(3))
+print("x4 covariance:\n", marginals.marginalCovariance(4))
+print("x5 covariance:\n", marginals.marginalCovariance(5))

cython/gtsam/examples/README.md

@@ -0,0 +1,4 @@
+These examples are almost identical to the old handwritten python wrapper
+examples. However, there are just some slight name changes, for example .print
+becomes .print_, and noiseModel.Diagonal becomes noiseModel_Diagonal etc...
+Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0))

cython/gtsam/examples/SFMdata.py

@@ -0,0 +1,37 @@
+"""
+A structure-from-motion example with landmarks
+ - The landmarks form a 10 meter cube
+ - The robot rotates around the landmarks, always facing towards the cube
+"""
+
+import numpy as np
+
+import gtsam
+
+
+def createPoints():
+    # Create the set of ground-truth landmarks
+    points = [gtsam.Point3(10.0, 10.0, 10.0),
+              gtsam.Point3(-10.0, 10.0, 10.0),
+              gtsam.Point3(-10.0, -10.0, 10.0),
+              gtsam.Point3(10.0, -10.0, 10.0),
+              gtsam.Point3(10.0, 10.0, -10.0),
+              gtsam.Point3(-10.0, 10.0, -10.0),
+              gtsam.Point3(-10.0, -10.0, -10.0),
+              gtsam.Point3(10.0, -10.0, -10.0)]
+    return points
+
+
+def createPoses(K):
+    # Create the set of ground-truth poses
+    radius = 30.0
+    angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
+    up = gtsam.Point3(0, 0, 1)
+    target = gtsam.Point3(0, 0, 0)
+    poses = []
+    for theta in angles:
+        position = gtsam.Point3(radius*np.cos(theta),
+                                radius*np.sin(theta), 0.0)
+        camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
+        poses.append(camera.pose())
+    return poses

cython/gtsam/examples/VisualISAM2Example.py

@@ -0,0 +1,153 @@
+"""An example of running visual SLAM using iSAM2."""
+# pylint: disable=invalid-name
+
+from __future__ import print_function
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+
+import gtsam
+import gtsam.utils.plot as gtsam_plot
+from gtsam.examples import SFMdata
+
+
+def X(i):
+    """Create key for pose i."""
+    return int(gtsam.symbol(ord('x'), i))
+
+
+def L(j):
+    """Create key for landmark j."""
+    return int(gtsam.symbol(ord('l'), j))
+
+
+def visual_ISAM2_plot(result):
+    """
+    VisualISAMPlot plots current state of ISAM2 object
+    Author: Ellon Paiva
+    Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
+    """
+
+    # Declare an id for the figure
+    fignum = 0
+
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plt.cla()
+
+    # Plot points
+    # Can't use data because current frame might not see all points
+    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
+    # gtsam.plot_3d_points(result, [], marginals)
+    gtsam_plot.plot_3d_points(fignum, result, 'rx')
+
+    # Plot cameras
+    i = 0
+    while result.exists(X(i)):
+        pose_i = result.atPose3(X(i))
+        gtsam_plot.plot_pose3(fignum, pose_i, 10)
+        i += 1
+
+    # draw
+    axes.set_xlim3d(-40, 40)
+    axes.set_ylim3d(-40, 40)
+    axes.set_zlim3d(-40, 40)
+    plt.pause(1)
+
+
+def visual_ISAM2_example():
+    plt.ion()
+
+    # Define the camera calibration parameters
+    K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Define the camera observation noise model
+    measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
+        2, 1.0)  # one pixel in u and v
+
+    # Create the set of ground-truth landmarks
+    points = SFMdata.createPoints()
+
+    # Create the set of ground-truth poses
+    poses = SFMdata.createPoses(K)
+
+    # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
+    # to maintain proper linearization and efficient variable ordering, iSAM2
+    # performs partial relinearization/reordering at each step. A parameter
+    # structure is available that allows the user to set various properties, such
+    # as the relinearization threshold and type of linear solver. For this
+    # example, we we set the relinearization threshold small so the iSAM2 result
+    # will approach the batch result.
+    parameters = gtsam.ISAM2Params()
+    parameters.setRelinearizeThreshold(0.01)
+    parameters.setRelinearizeSkip(1)
+    isam = gtsam.ISAM2(parameters)
+
+    # Create a Factor Graph and Values to hold the new data
+    graph = gtsam.NonlinearFactorGraph()
+    initial_estimate = gtsam.Values()
+
+    #  Loop over the different poses, adding the observations to iSAM incrementally
+    for i, pose in enumerate(poses):
+
+        # Add factors for each landmark observation
+        for j, point in enumerate(points):
+            camera = gtsam.PinholeCameraCal3_S2(pose, K)
+            measurement = camera.project(point)
+            graph.push_back(gtsam.GenericProjectionFactorCal3_S2(
+                measurement, measurement_noise, X(i), L(j), K))
+
+        # Add an initial guess for the current pose
+        # Intentionally initialize the variables off from the ground truth
+        initial_estimate.insert(X(i), pose.compose(gtsam.Pose3(
+            gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
+
+        # If this is the first iteration, add a prior on the first pose to set the
+        # coordinate frame and a prior on the first landmark to set the scale.
+        # Also, as iSAM solves incrementally, we must wait until each is observed
+        # at least twice before adding it to iSAM.
+        if i == 0:
+            # Add a prior on pose x0
+            pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
+                [0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+            graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
+
+            # Add a prior on landmark l0
+            point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+            graph.push_back(gtsam.PriorFactorPoint3(
+                L(0), points[0], point_noise))  # add directly to graph
+
+            # Add initial guesses to all observed landmarks
+            # Intentionally initialize the variables off from the ground truth
+            for j, point in enumerate(points):
+                initial_estimate.insert(L(j), gtsam.Point3(
+                    point.x()-0.25, point.y()+0.20, point.z()+0.15))
+        else:
+            # Update iSAM with the new factors
+            isam.update(graph, initial_estimate)
+            # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
+            # If accuracy is desired at the expense of time, update(*) can be called additional
+            # times to perform multiple optimizer iterations every step.
+            isam.update()
+            current_estimate = isam.calculateEstimate()
+            print("****************************************************")
+            print("Frame", i, ":")
+            for j in range(i + 1):
+                print(X(j), ":", current_estimate.atPose3(X(j)))
+
+            for j in range(len(points)):
+                print(L(j), ":", current_estimate.atPoint3(L(j)))
+
+            visual_ISAM2_plot(current_estimate)
+
+            # Clear the factor graph and values for the next iteration
+            graph.resize(0)
+            initial_estimate.clear()
+
+    plt.ioff()
+    plt.show()
+
+
+if __name__ == '__main__':
+    visual_ISAM2_example()

cython/gtsam/examples/__init__.py

@@ -0,0 +1 @@
+from . import SFMdata