Copy cython files

python/gtsam_py/python/__init__.py

@@ -0,0 +1,26 @@
+from .gtsam import *
+
+try:
+    import gtsam_unstable
+
+
+    def _deprecated_wrapper(item, name):
+        def wrapper(*args, **kwargs):
+            from warnings import warn
+            message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
+                       'Please import it from gtsam_unstable.')
+            warn(message)
+            return item(*args, **kwargs)
+        return wrapper
+
+
+    for name in dir(gtsam_unstable):
+        if not name.startswith('__'):
+            item = getattr(gtsam_unstable, name)
+            if callable(item):
+                item = _deprecated_wrapper(item, name)
+            globals()[name] = item
+
+except ImportError:
+    pass
+

python/gtsam_py/python/examples/DogLegOptimizerExample.py

@@ -0,0 +1,118 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Example comparing DoglegOptimizer with Levenberg-Marquardt.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-member, invalid-name
+
+import math
+import argparse
+
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def run(args):
+    """Test Dogleg vs LM, inspired by issue #452."""
+
+    # print parameters
+    print("num samples = {}, deltaInitial = {}".format(
+        args.num_samples, args.delta))
+
+    # Ground truth solution
+    T11 = gtsam.Pose2(0, 0, 0)
+    T12 = gtsam.Pose2(1, 0, 0)
+    T21 = gtsam.Pose2(0, 1, 0)
+    T22 = gtsam.Pose2(1, 1, 0)
+
+    # Factor graph
+    graph = gtsam.NonlinearFactorGraph()
+
+    # Priors
+    prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
+    graph.add(gtsam.PriorFactorPose2(11, T11, prior))
+    graph.add(gtsam.PriorFactorPose2(21, T21, prior))
+
+    # Odometry
+    model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
+    graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
+    graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
+
+    # Range
+    model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01)
+    graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
+
+    params = gtsam.DoglegParams()
+    params.setDeltaInitial(args.delta)  # default is 10
+
+    # Add progressively more noise to ground truth
+    sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
+    n = len(sigmas)
+    p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n
+    for i, sigma in enumerate(sigmas):
+        dl_fails, lm_fails = 0, 0
+        # Attempt num_samples optimizations for both DL and LM
+        for _attempt in range(args.num_samples):
+            initial = gtsam.Values()
+            initial.insert(11, T11.retract(np.random.normal(0, sigma, 3)))
+            initial.insert(12, T12.retract(np.random.normal(0, sigma, 3)))
+            initial.insert(21, T21.retract(np.random.normal(0, sigma, 3)))
+            initial.insert(22, T22.retract(np.random.normal(0, sigma, 3)))
+
+            # Run dogleg optimizer
+            dl = gtsam.DoglegOptimizer(graph, initial, params)
+            result = dl.optimize()
+            dl_fails += graph.error(result) > 1e-9
+
+            # Run
+            lm = gtsam.LevenbergMarquardtOptimizer(graph, initial)
+            result = lm.optimize()
+            lm_fails += graph.error(result) > 1e-9
+
+        # Calculate Bayes estimate of success probability
+        # using a beta prior of alpha=0.5, beta=0.5
+        alpha, beta = 0.5, 0.5
+        v = args.num_samples+alpha+beta
+        p_dl[i] = (args.num_samples-dl_fails+alpha)/v
+        p_lm[i] = (args.num_samples-lm_fails+alpha)/v
+
+        def stddev(p):
+            """Calculate standard deviation."""
+            return math.sqrt(p*(1-p)/(1+v))
+
+        s_dl[i] = stddev(p_dl[i])
+        s_lm[i] = stddev(p_lm[i])
+
+        fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%"
+        print(fmt.format(sigma,
+                         100*p_dl[i], 100*s_dl[i],
+                         100*p_lm[i], 100*s_lm[i]))
+
+    if args.plot:
+        fig, ax = plt.subplots()
+        dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg")
+        lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM")
+        plt.title("Dogleg emprical success vs. LM")
+        plt.legend(handles=[dl_plot, lm_plot])
+        ax.set_xlim(0, sigmas[-1]+1)
+        ax.set_ylim(0, 1)
+        plt.show()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description="Compare Dogleg and LM success rates")
+    parser.add_argument("-n", "--num_samples", type=int, default=1000,
+                        help="Number of samples for each sigma")
+    parser.add_argument("-d", "--delta", type=float, default=10.0,
+                        help="Initial delta for dogleg")
+    parser.add_argument("-p", "--plot", action="store_true",
+                        help="Flag to plot results")
+    args = parser.parse_args()
+    run(args)

python/gtsam_py/python/examples/GPSFactorExample.py

@@ -0,0 +1,56 @@
+"""
+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
+
+Simple robot motion example, with prior and one GPS measurements
+Author: Mandy Xie
+"""
+# pylint: disable=invalid-name, E1101
+
+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
+h0 = 274
+
+# Create noise models
+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))
+
+# 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))
+
+# optimize using Levenberg-Marquardt optimization
+params = gtsam.LevenbergMarquardtParams()
+optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+result = optimizer.optimize()
+print("\nFinal Result:\n{}".format(result))
+

python/gtsam_py/python/examples/ImuFactorExample.py

@@ -0,0 +1,153 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+A script validating and demonstrating the ImuFactor inference.
+
+Author: Frank Dellaert, Varun Agrawal
+"""
+
+from __future__ import print_function
+
+import math
+
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import symbol_shorthand_B as B
+from gtsam import symbol_shorthand_V as V
+from gtsam import symbol_shorthand_X as X
+from gtsam.utils.plot import plot_pose3
+from mpl_toolkits.mplot3d import Axes3D
+
+from PreintegrationExample import POSES_FIG, PreintegrationExample
+
+BIAS_KEY = B(0)
+
+
+np.set_printoptions(precision=3, suppress=True)
+
+
+class ImuFactorExample(PreintegrationExample):
+
+    def __init__(self):
+        self.velocity = np.array([2, 0, 0])
+        self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+        self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+
+        # Choose one of these twists to change scenario:
+        zero_twist = (np.zeros(3), np.zeros(3))
+        forward_twist = (np.zeros(3), self.velocity)
+        loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
+        sick_twist = (
+            np.array([math.radians(30), -math.radians(30), 0]), self.velocity)
+
+        accBias = np.array([-0.3, 0.1, 0.2])
+        gyroBias = np.array([0.1, 0.3, -0.1])
+        bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
+
+        dt = 1e-2
+        super(ImuFactorExample, self).__init__(sick_twist, bias, dt)
+
+    def addPrior(self, i, graph):
+        state = self.scenario.navState(i)
+        graph.push_back(gtsam.PriorFactorPose3(
+            X(i), state.pose(), self.priorNoise))
+        graph.push_back(gtsam.PriorFactorVector(
+            V(i), state.velocity(), self.velNoise))
+
+    def run(self):
+        graph = gtsam.NonlinearFactorGraph()
+
+        # initialize data structure for pre-integrated IMU measurements
+        pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
+
+        T = 12
+        num_poses = T + 1  # assumes 1 factor per second
+        initial = gtsam.Values()
+        initial.insert(BIAS_KEY, self.actualBias)
+        for i in range(num_poses):
+            state_i = self.scenario.navState(float(i))
+
+            poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
+            pose = state_i.pose().compose(poseNoise)
+
+            velocity = state_i.velocity() + np.random.randn(3)*0.1
+
+            initial.insert(X(i), pose)
+            initial.insert(V(i), velocity)
+
+        # simulate the loop
+        i = 0  # state index
+        actual_state_i = self.scenario.navState(0)
+        for k, t in enumerate(np.arange(0, T, self.dt)):
+            # get measurements and add them to PIM
+            measuredOmega = self.runner.measuredAngularVelocity(t)
+            measuredAcc = self.runner.measuredSpecificForce(t)
+            pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
+
+            poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
+
+            actual_state_i = gtsam.NavState(
+                actual_state_i.pose().compose(poseNoise),
+                actual_state_i.velocity() + np.random.randn(3)*0.1)
+
+            # Plot IMU many times
+            if k % 10 == 0:
+                self.plotImu(t, measuredOmega, measuredAcc)
+
+            # Plot every second
+            if k % int(1 / self.dt) == 0:
+                self.plotGroundTruthPose(t)
+
+            # create IMU factor every second
+            if (k + 1) % int(1 / self.dt) == 0:
+                factor = gtsam.ImuFactor(X(i), V(i), X(
+                    i + 1), V(i + 1), BIAS_KEY, pim)
+                graph.push_back(factor)
+                if True:
+                    print(factor)
+                    print(pim.predict(actual_state_i, self.actualBias))
+                pim.resetIntegration()
+                actual_state_i = self.scenario.navState(t + self.dt)
+                i += 1
+
+        # add priors on beginning and end
+        self.addPrior(0, graph)
+        self.addPrior(num_poses - 1, graph)
+
+        # optimize using Levenberg-Marquardt optimization
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("SUMMARY")
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+
+        # Calculate and print marginal covariances
+        marginals = gtsam.Marginals(graph, result)
+        print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
+        for i in range(num_poses):
+            print("Covariance on pose {}:\n{}\n".format(
+                i, marginals.marginalCovariance(X(i))))
+            print("Covariance on vel {}:\n{}\n".format(
+                i, marginals.marginalCovariance(V(i))))
+
+        # Plot resulting poses
+        i = 0
+        while result.exists(X(i)):
+            pose_i = result.atPose3(X(i))
+            plot_pose3(POSES_FIG, pose_i, 0.1)
+            i += 1
+
+        gtsam.utils.plot.set_axes_equal(POSES_FIG)
+
+        print(result.atimuBias_ConstantBias(BIAS_KEY))
+
+        plt.ioff()
+        plt.show()
+
+
+if __name__ == '__main__':
+    ImuFactorExample().run()

python/gtsam_py/python/examples/ImuFactorExample2.py

@@ -0,0 +1,172 @@
+"""
+iSAM2 example with ImuFactor.
+Author: Robert Truax (C++), Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+
+import gtsam
+import gtsam.utils.plot as gtsam_plot
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
+                   ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
+                   PinholeCameraCal3_S2, Point3, Pose3,
+                   PriorFactorConstantBias, PriorFactorPose3,
+                   PriorFactorVector, Rot3, Values)
+from gtsam import symbol_shorthand_B as B
+from gtsam import symbol_shorthand_V as V
+from gtsam import symbol_shorthand_X as X
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+
+
+def vector3(x, y, z):
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=np.float)
+
+
+def ISAM2_plot(values, fignum=0):
+    """Plot poses."""
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plt.cla()
+
+    i = 0
+    min_bounds = 0, 0, 0
+    max_bounds = 0, 0, 0
+    while values.exists(X(i)):
+        pose_i = values.atPose3(X(i))
+        gtsam_plot.plot_pose3(fignum, pose_i, 10)
+        position = pose_i.translation().vector()
+        min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
+        max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
+        # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
+        i += 1
+
+    # draw
+    axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
+    axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
+    axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
+    plt.pause(1)
+
+
+# IMU preintegration parameters
+# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+g = 9.81
+n_gravity = vector3(0, 0, -g)
+PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
+I = np.eye(3)
+PARAMS.setAccelerometerCovariance(I * 0.1)
+PARAMS.setGyroscopeCovariance(I * 0.1)
+PARAMS.setIntegrationCovariance(I * 0.1)
+PARAMS.setUse2ndOrderCoriolis(False)
+PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
+
+BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
+DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
+              Point3(0.05, -0.10, 0.20))
+
+
+def IMU_example():
+    """Run iSAM 2 example with IMU factor."""
+
+    # Start with a camera on x-axis looking at origin
+    radius = 30
+    up = Point3(0, 0, 1)
+    target = Point3(0, 0, 0)
+    position = Point3(radius, 0, 0)
+    camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
+    pose_0 = camera.pose()
+
+    # Create the set of ground-truth landmarks and poses
+    angular_velocity = math.radians(180)  # rad/sec
+    delta_t = 1.0/18  # makes for 10 degrees per step
+
+    angular_velocity_vector = vector3(0, -angular_velocity, 0)
+    linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
+    scenario = ConstantTwistScenario(
+        angular_velocity_vector, linear_velocity_vector, pose_0)
+
+    # Create a factor graph
+    newgraph = NonlinearFactorGraph()
+
+    # Create (incremental) ISAM2 solver
+    isam = ISAM2()
+
+    # Create the initial estimate to the solution
+    # Intentionally initialize the variables off from the ground truth
+    initialEstimate = Values()
+
+    # Add a prior on pose x0. This indirectly specifies where the origin is.
+    # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+    noise = gtsam.noiseModel_Diagonal.Sigmas(
+        np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
+    newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
+
+    # Add imu priors
+    biasKey = B(0)
+    biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+    biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
+                                        biasnoise)
+    newgraph.push_back(biasprior)
+    initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
+    velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+
+    # Calculate with correct initial velocity
+    n_velocity = vector3(0, angular_velocity * radius, 0)
+    velprior = PriorFactorVector(V(0), n_velocity, velnoise)
+    newgraph.push_back(velprior)
+    initialEstimate.insert(V(0), n_velocity)
+
+    accum = gtsam.PreintegratedImuMeasurements(PARAMS)
+
+    # Simulate poses and imu measurements, adding them to the factor graph
+    for i in range(80):
+        t = i * delta_t  # simulation time
+        if i == 0:  # First time add two poses
+            pose_1 = scenario.pose(delta_t)
+            initialEstimate.insert(X(0), pose_0.compose(DELTA))
+            initialEstimate.insert(X(1), pose_1.compose(DELTA))
+        elif i >= 2:  # Add more poses as necessary
+            pose_i = scenario.pose(t)
+            initialEstimate.insert(X(i), pose_i.compose(DELTA))
+
+        if i > 0:
+            # Add Bias variables periodically
+            if i % 5 == 0:
+                biasKey += 1
+                factor = BetweenFactorConstantBias(
+                    biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
+                newgraph.add(factor)
+                initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
+
+            # Predict acceleration and gyro measurements in (actual) body frame
+            nRb = scenario.rotation(t).matrix()
+            bRn = np.transpose(nRb)
+            measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
+            measuredOmega = scenario.omega_b(t)
+            accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
+
+            # Add Imu Factor
+            imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
+            newgraph.add(imufac)
+
+            # insert new velocity, which is wrong
+            initialEstimate.insert(V(i), n_velocity)
+            accum.resetIntegration()
+
+        # Incremental solution
+        isam.update(newgraph, initialEstimate)
+        result = isam.calculateEstimate()
+        ISAM2_plot(result)
+
+        # reset
+        newgraph = NonlinearFactorGraph()
+        initialEstimate.clear()
+
+
+if __name__ == '__main__':
+    IMU_example()

python/gtsam_py/python/examples/OdometryExample.py

@@ -0,0 +1,69 @@
+"""
+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
+
+Simple robot motion example, with prior and two odometry measurements
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+import matplotlib.pyplot as plt
+import gtsam.utils.plot as gtsam_plot
+
+# 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()
+
+
+

python/gtsam_py/python/examples/PlanarManipulatorExample.py

@@ -0,0 +1,334 @@
+"""
+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
+
+Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+import unittest
+from functools import reduce
+
+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 import Pose2
+from gtsam.utils.test_case import GtsamTestCase
+
+
+def vector3(x, y, z):
+    """Create 3D double numpy array."""
+    return np.array([x, y, z], dtype=np.float)
+
+
+def compose(*poses):
+    """Compose all Pose2 transforms given as arguments from left to right."""
+    return reduce((lambda x, y: x.compose(y)), poses)
+
+
+def vee(M):
+    """Pose2 vee operator."""
+    return vector3(M[0, 2], M[1, 2], M[1, 0])
+
+
+def delta(g0, g1):
+    """Difference between x,y,,theta components of SE(2) poses."""
+    return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
+
+
+def trajectory(g0, g1, N=20):
+    """ Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
+        g0 and g1 are the initial and final pose, respectively.
+        N is the number of *intervals*
+        Returns N+1 poses
+    """
+    e = delta(g0, g1)
+    return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
+
+
+class ThreeLinkArm(object):
+    """Three-link arm class."""
+
+    def __init__(self):
+        self.L1 = 3.5
+        self.L2 = 3.5
+        self.L3 = 2.5
+        self.xi1 = vector3(0, 0, 1)
+        self.xi2 = vector3(self.L1, 0, 1)
+        self.xi3 = vector3(self.L1+self.L2, 0, 1)
+        self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
+
+    def fk(self, q):
+        """ Forward kinematics.
+            Takes numpy array of joint angles, in radians.
+        """
+        sXl1 = Pose2(0, 0, math.radians(90))
+        l1Zl1 = Pose2(0, 0, q[0])
+        l1Xl2 = Pose2(self.L1, 0, 0)
+        l2Zl2 = Pose2(0, 0, q[1])
+        l2Xl3 = Pose2(self.L2, 0, 0)
+        l3Zl3 = Pose2(0, 0, q[2])
+        l3Xt = Pose2(self.L3, 0, 0)
+        return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
+
+    def jacobian(self, q):
+        """ Calculate manipulator Jacobian.
+            Takes numpy array of joint angles, in radians.
+        """
+        a = q[0]+q[1]
+        b = a+q[2]
+        return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
+                          -self.L1*math.cos(a)-self.L3*math.cos(b),
+                          - self.L3*math.cos(b)],
+                         [-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
+                          -self.L1*math.sin(a)-self.L3*math.sin(b),
+                          - self.L3*math.sin(b)],
+                         [1, 1, 1]], np.float)
+
+    def poe(self, q):
+        """ Forward kinematics.
+            Takes numpy array of joint angles, in radians.
+        """
+        l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
+        l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
+        l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
+        return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
+
+    def con(self, q):
+        """ Forward kinematics, conjugation form.
+            Takes numpy array of joint angles, in radians.
+        """
+        def expmap(x, y, theta):
+            """Implement exponential map via conjugation with axis (x,y)."""
+            return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
+
+        l1Zl1 = expmap(0.0, 0.0, q[0])
+        l2Zl2 = expmap(0.0, self.L1, q[1])
+        l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
+        return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
+
+    def ik(self, sTt_desired, e=1e-9):
+        """ Inverse kinematics.
+            Takes desired Pose2 of tool T with respect to base S.
+            Optional: mu, gradient descent rate; e: error norm threshold
+        """
+        q = np.radians(vector3(30, -30, 45))  # well within workspace
+        error = vector3(100, 100, 100)
+
+        while np.linalg.norm(error) > e:
+            error = delta(sTt_desired, self.fk(q))
+            J = self.jacobian(q)
+            q -= np.dot(np.linalg.pinv(J), error)
+
+        # return result in interval [-pi,pi)
+        return np.remainder(q+math.pi, 2*math.pi)-math.pi
+
+    def manipulator_jacobian(self, q):
+        """ Calculate manipulator Jacobian.
+            Takes numpy array of joint angles, in radians.
+            Returns the manipulator Jacobian of differential twists. When multiplied with
+            a vector of joint velocities, will yield a single differential twist which is
+            the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
+            Just like always, differential twists can be hatted and multiplied with spatial
+            coordinates of a point to give the spatial velocity of the point.
+        """
+        l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
+        l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
+        # l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
+
+        p1 = self.xi1
+        # p1 = Pose2().Adjoint(self.xi1)
+
+        sTl1 = l1Zl1
+        p2 = sTl1.Adjoint(self.xi2)
+
+        sTl2 = compose(l1Zl1, l2Zl2)
+        p3 = sTl2.Adjoint(self.xi3)
+
+        differential_twists = [p1, p2, p3]
+        return np.stack(differential_twists, axis=1)
+
+    def plot(self, fignum, q):
+        """ Plot arm.
+            Takes figure number, and numpy array of joint angles, in radians.
+        """
+        fig = plt.figure(fignum)
+        axes = fig.gca()
+
+        sXl1 = Pose2(0, 0, math.radians(90))
+        t = sXl1.translation()
+        p1 = np.array([t.x(), t.y()])
+        gtsam_plot.plot_pose2_on_axes(axes, sXl1)
+
+        def plot_line(p, g, color):
+            t = g.translation()
+            q = np.array([t.x(), t.y()])
+            line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
+            axes.plot(line[:, 0], line[:, 1], color)
+            return q
+
+        l1Zl1 = Pose2(0, 0, q[0])
+        l1Xl2 = Pose2(self.L1, 0, 0)
+        sTl2 = compose(sXl1, l1Zl1, l1Xl2)
+        p2 = plot_line(p1, sTl2, 'r-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTl2)
+
+        l2Zl2 = Pose2(0, 0, q[1])
+        l2Xl3 = Pose2(self.L2, 0, 0)
+        sTl3 = compose(sTl2, l2Zl2, l2Xl3)
+        p3 = plot_line(p2, sTl3, 'g-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTl3)
+
+        l3Zl3 = Pose2(0, 0, q[2])
+        l3Xt = Pose2(self.L3, 0, 0)
+        sTt = compose(sTl3, l3Zl3, l3Xt)
+        plot_line(p3, sTt, 'b-')
+        gtsam_plot.plot_pose2_on_axes(axes, sTt)
+
+
+# Create common example configurations.
+Q0 = vector3(0, 0, 0)
+Q1 = np.radians(vector3(-30, -45, -90))
+Q2 = np.radians(vector3(-90, 90, 0))
+
+
+class TestPose2SLAMExample(GtsamTestCase):
+    """Unit tests for functions used below."""
+
+    def setUp(self):
+        self.arm = ThreeLinkArm()
+
+    def assertPose2Equals(self, actual, expected, tol=1e-2):
+        """Helper function that prints out actual and expected if not equal."""
+        equal = actual.equals(expected, tol)
+        if not equal:
+            raise self.failureException(
+                "Poses are not equal:\n{}!={}".format(actual, expected))
+
+    def test_fk_arm(self):
+        """Make sure forward kinematics is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.fk(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.fk(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_jacobian(self):
+        """Test Jacobian calculation."""
+        # at rest
+        expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.jacobian(Q0)
+        np.testing.assert_array_almost_equal(J, expected)
+
+        # at -90, 90, 0
+        expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.jacobian(Q2)
+        np.testing.assert_array_almost_equal(J, expected)
+
+    def test_con_arm(self):
+        """Make sure POE is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.con(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.con(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_poe_arm(self):
+        """Make sure POE is correct for some known test configurations."""
+        # at rest
+        expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
+        sTt = self.arm.poe(Q0)
+        self.assertIsInstance(sTt, Pose2)
+        self.assertPose2Equals(sTt, expected)
+
+        # -30, -45, -90
+        expected = Pose2(5.78, 1.52, math.radians(-75))
+        sTt = self.arm.poe(Q1)
+        self.assertPose2Equals(sTt, expected)
+
+    def test_ik(self):
+        """Check iterative inverse kinematics function."""
+        # at rest
+        actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
+        np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
+
+        # -30, -45, -90
+        sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
+        actual = self.arm.ik(sTt_desired)
+        self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
+        np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
+
+    def test_manipulator_jacobian(self):
+        """Test Jacobian calculation."""
+        # at rest
+        expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
+        J = self.arm.manipulator_jacobian(Q0)
+        np.testing.assert_array_almost_equal(J, expected)
+
+        # at -90, 90, 0
+        expected = np.array(
+            [[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
+        J = self.arm.manipulator_jacobian(Q2)
+        np.testing.assert_array_almost_equal(J, expected)
+
+
+def run_example():
+    """ Use trajectory interpolation and then trajectory tracking a la Murray
+        to move a 3-link arm on a straight line.
+    """
+    # Create arm
+    arm = ThreeLinkArm()
+
+    # Get initial pose using forward kinematics
+    q = np.radians(vector3(30, -30, 45))
+    sTt_initial = arm.fk(q)
+
+    # Create interpolated trajectory in task space to desired goal pose
+    sTt_goal = Pose2(2.4, 4.3, math.radians(0))
+    poses = trajectory(sTt_initial, sTt_goal, 50)
+
+    # Setup figure and plot initial pose
+    fignum = 0
+    fig = plt.figure(fignum)
+    axes = fig.gca()
+    axes.set_xlim(-5, 5)
+    axes.set_ylim(0, 10)
+    gtsam_plot.plot_pose2(fignum, arm.fk(q))
+
+    # For all poses in interpolated trajectory, calculate dq to move to next pose.
+    # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
+    for pose in poses:
+        sTt = arm.fk(q)
+        error = delta(sTt, pose)
+        J = arm.jacobian(q)
+        q += np.dot(np.linalg.inv(J), error)
+        arm.plot(fignum, q)
+        plt.pause(0.01)
+
+    plt.pause(10)
+
+
+if __name__ == "__main__":
+    run_example()
+    unittest.main()

python/gtsam_py/python/examples/PlanarSLAMExample.py

@@ -0,0 +1,81 @@
+"""
+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
+
+Simple robotics example using odometry measurements and bearing-range (laser) measurements
+Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as 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)))

python/gtsam_py/python/examples/Pose2SLAMExample.py

@@ -0,0 +1,97 @@
+"""
+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
+
+Simple robotics example using odometry measurements and bearing-range (laser) measurements
+Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+
+import numpy as np
+
+import gtsam
+
+import matplotlib.pyplot as plt
+import gtsam.utils.plot as gtsam_plot
+
+
+def vector3(x, y, z):
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=np.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()

python/gtsam_py/python/examples/Pose2SLAMExample_g2o.py

@@ -0,0 +1,88 @@
+"""
+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
+
+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
+"""
+# 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
+from gtsam.utils import plot
+
+
+def vector3(x, y, z):
+    """Create 3d double numpy array."""
+    return np.array([x, y, z], dtype=np.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()

python/gtsam_py/python/examples/Pose3SLAMExample_g2o.py

@@ -0,0 +1,71 @@
+"""
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
+ *  Pose3 using InitializePose3
+ * @date Jan 17, 2019
+ * @author Vikrant Shah based on CPP example by Luca Carlone
+"""
+# 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
+from gtsam.utils import plot
+
+
+def vector6(x, y, z, a, b, c):
+    """Create 6d double numpy array."""
+    return np.array([x, y, z, a, b, c], dtype=np.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().at(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()

python/gtsam_py/python/examples/Pose3SLAMExample_initializePose3Chordal.py

@@ -0,0 +1,35 @@
+"""
+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
+
+Initialize PoseSLAM with Chordal init
+Author: Luca Carlone, Frank Dellaert (python port)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+# 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().at(0)
+graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
+
+# Initializing Pose3 - chordal relaxation"
+initialization = gtsam.InitializePose3.initialize(graph)
+
+print(initialization)

python/gtsam_py/python/examples/PreintegrationExample.py

@@ -0,0 +1,140 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+A script validating the Preintegration of IMU measurements
+"""
+
+import math
+
+import matplotlib.pyplot as plt
+import numpy as np
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
+from gtsam.utils.plot import plot_pose3
+
+IMU_FIG = 1
+POSES_FIG = 2
+
+
+class PreintegrationExample(object):
+
+    @staticmethod
+    def defaultParams(g):
+        """Create default parameters with Z *up* and realistic noise parameters"""
+        params = gtsam.PreintegrationParams.MakeSharedU(g)
+        kGyroSigma = math.radians(0.5) / 60  # 0.5 degree ARW
+        kAccelSigma = 0.1 / 60  # 10 cm VRW
+        params.setGyroscopeCovariance(
+            kGyroSigma ** 2 * np.identity(3, np.float))
+        params.setAccelerometerCovariance(
+            kAccelSigma ** 2 * np.identity(3, np.float))
+        params.setIntegrationCovariance(
+            0.0000001 ** 2 * np.identity(3, np.float))
+        return params
+
+    def __init__(self, twist=None, bias=None, dt=1e-2):
+        """Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
+
+        # setup interactive plotting
+        plt.ion()
+
+        # Setup loop as default scenario
+        if twist is not None:
+            (W, V) = twist
+        else:
+            # default = loop with forward velocity 2m/s, while pitching up
+            # with angular velocity 30 degree/sec (negative in FLU)
+            W = np.array([0, -math.radians(30), 0])
+            V = np.array([2, 0, 0])
+
+        self.scenario = gtsam.ConstantTwistScenario(W, V)
+        self.dt = dt
+
+        self.maxDim = 5
+        self.labels = list('xyz')
+        self.colors = list('rgb')
+
+        # Create runner
+        self.g = 10  # simple gravity constant
+        self.params = self.defaultParams(self.g)
+
+        if bias is not None:
+            self.actualBias = bias
+        else:
+            accBias = np.array([0, 0.1, 0])
+            gyroBias = np.array([0, 0, 0])
+            self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
+
+        self.runner = gtsam.ScenarioRunner(
+            self.scenario, self.params, self.dt, self.actualBias)
+
+    def plotImu(self, t, measuredOmega, measuredAcc):
+        plt.figure(IMU_FIG)
+
+        # plot angular velocity
+        omega_b = self.scenario.omega_b(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 1)
+            plt.scatter(t, omega_b[i], color='k', marker='.')
+            plt.scatter(t, measuredOmega[i], color=color, marker='.')
+            plt.xlabel('angular velocity ' + label)
+
+        # plot acceleration in nav
+        acceleration_n = self.scenario.acceleration_n(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 4)
+            plt.scatter(t, acceleration_n[i], color=color, marker='.')
+            plt.xlabel('acceleration in nav ' + label)
+
+        # plot acceleration in body
+        acceleration_b = self.scenario.acceleration_b(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 7)
+            plt.scatter(t, acceleration_b[i], color=color, marker='.')
+            plt.xlabel('acceleration in body ' + label)
+
+        # plot actual specific force, as well as corrupted
+        actual = self.runner.actualSpecificForce(t)
+        for i, (label, color) in enumerate(zip(self.labels, self.colors)):
+            plt.subplot(4, 3, i + 10)
+            plt.scatter(t, actual[i], color='k', marker='.')
+            plt.scatter(t, measuredAcc[i], color=color, marker='.')
+            plt.xlabel('specific force ' + label)
+
+    def plotGroundTruthPose(self, t):
+        # plot ground truth pose, as well as prediction from integrated IMU measurements
+        actualPose = self.scenario.pose(t)
+        plot_pose3(POSES_FIG, actualPose, 0.3)
+        t = actualPose.translation()
+        self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
+        ax = plt.gca()
+        ax.set_xlim3d(-self.maxDim, self.maxDim)
+        ax.set_ylim3d(-self.maxDim, self.maxDim)
+        ax.set_zlim3d(-self.maxDim, self.maxDim)
+
+        plt.pause(0.01)
+
+    def run(self):
+        # simulate the loop
+        T = 12
+        for i, t in enumerate(np.arange(0, T, self.dt)):
+            measuredOmega = self.runner.measuredAngularVelocity(t)
+            measuredAcc = self.runner.measuredSpecificForce(t)
+            if i % 25 == 0:
+                self.plotImu(t, measuredOmega, measuredAcc)
+                self.plotGroundTruthPose(t)
+                pim = self.runner.integrate(t, self.actualBias, True)
+                predictedNavState = self.runner.predict(pim, self.actualBias)
+                plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
+
+        plt.ioff()
+        plt.show()
+
+
+if __name__ == '__main__':
+    PreintegrationExample().run()

python/gtsam_py/python/examples/README.md

@@ -0,0 +1,57 @@
+These examples are almost identical to the old handwritten python wrapper
+examples. However, there are just some slight name changes, for example
+`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
+Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
+
+# Porting Progress
+
+| C++ Example Name                                      | Ported |
+|-------------------------------------------------------|--------|
+| CameraResectioning                                    |        |
+| CreateSFMExampleData                                  |        |
+| DiscreteBayesNet_FG                                   | none of the required discrete functionality is exposed through cython |
+| easyPoint2KalmanFilter                                | ExtendedKalmanFilter not exposed through cython |
+| elaboratePoint2KalmanFilter                           | GaussianSequentialSolver not exposed through cython |
+| ImuFactorExample2                                     | X      |
+| ImuFactorsExample                                     |        |
+| ISAM2Example_SmartFactor                              |        |
+| ISAM2_SmartFactorStereo_IMU                           |        |
+| LocalizationExample                                   | impossible? |
+| METISOrderingExample                                  |        |
+| OdometryExample                                       | X      |
+| PlanarSLAMExample                                     | X      |
+| Pose2SLAMExample                                      | X      |
+| Pose2SLAMExampleExpressions                           |        |
+| Pose2SLAMExample_g2o                                  | X      |
+| Pose2SLAMExample_graph                                |        |
+| Pose2SLAMExample_graphviz                             |        |
+| Pose2SLAMExample_lago                                 | lago not exposed through cython |
+| Pose2SLAMStressTest                                   |        |
+| Pose2SLAMwSPCG                                        |        |
+| Pose3SLAMExample_changeKeys                           |        |
+| Pose3SLAMExampleExpressions_BearingRangeWithTransform |        |
+| Pose3SLAMExample_g2o                                  | X      |
+| Pose3SLAMExample_initializePose3Chordal               |        |
+| Pose3SLAMExample_initializePose3Gradient              |        |
+| RangeISAMExample_plaza2                               |        |
+| SelfCalibrationExample                                |        |
+| SFMExample_bal_COLAMD_METIS                           |        |
+| SFMExample_bal                                        |        |
+| SFMExample                                            | X      |
+| SFMExampleExpressions_bal                             |        |
+| SFMExampleExpressions                                 |        |
+| SFMExample_SmartFactor                                |        |
+| SFMExample_SmartFactorPCG                             |        |
+| SimpleRotation                                        | X      |
+| SolverComparer                                        |        |
+| StereoVOExample                                       |        |
+| StereoVOExample_large                                 |        |
+| TimeTBB                                               |        |
+| UGM_chain                                             | discrete functionality not exposed |
+| UGM_small                                             | discrete functionality not exposed |
+| VisualISAM2Example                                    | X      |
+| VisualISAMExample                                     | X      |
+
+Extra Examples (with no C++ equivalent)
+- PlanarManipulatorExample
+- SFMData

python/gtsam_py/python/examples/SFMExample.py

@@ -0,0 +1,121 @@
+"""
+GTSAM Copyright 2010, 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
+
+A structure-from-motion problem on a simulated dataset
+"""
+from __future__ import print_function
+
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
+from gtsam.examples import SFMdata
+from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
+                         GenericProjectionFactorCal3_S2, Marginals,
+                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
+                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         SimpleCamera, Values)
+from gtsam.utils import plot
+
+
+def main():
+    """
+    Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
+
+    Each variable in the system (poses and landmarks) must be identified with a unique key.
+    We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+    Here we will use Symbols
+
+    In GTSAM, measurement functions are represented as 'factors'. Several common factors
+    have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
+    Here we will use Projection factors to model the camera's landmark observations.
+    Also, we will initialize the robot at some location using a Prior factor.
+
+    When the factors are created, we will add them to a Factor Graph. As the factors we are using
+    are nonlinear factors, we will need a Nonlinear Factor Graph.
+
+    Finally, once all of the factors have been added to our factor graph, we will want to
+    solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
+    GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
+    trust-region method known as Powell's Degleg
+
+    The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
+    nonlinear functions around an initial linearization point, then solve the linear system
+    to update the linearization point. This happens repeatedly until the solver converges
+    to a consistent set of variable values. This requires us to specify an initial guess
+    for each variable, held in a Values container.
+    """
+
+    # Define the camera calibration parameters
+    K = 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 a factor graph
+    graph = NonlinearFactorGraph()
+
+    # Add a prior on pose x1. This indirectly specifies where the origin is.
+    # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
+    pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+    factor = PriorFactorPose3(X(0), poses[0], pose_noise)
+    graph.push_back(factor)
+
+    # Simulated measurements from each camera pose, adding them to the factor graph
+    for i, pose in enumerate(poses):
+        camera = PinholeCameraCal3_S2(pose, K)
+        for j, point in enumerate(points):
+            measurement = camera.project(point)
+            factor = GenericProjectionFactorCal3_S2(
+                measurement, measurement_noise, X(i), L(j), K)
+            graph.push_back(factor)
+
+    # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
+    # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
+    # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
+    point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+    factor = PriorFactorPoint3(L(0), points[0], point_noise)
+    graph.push_back(factor)
+    graph.print_('Factor Graph:\n')
+
+    # Create the data structure to hold the initial estimate to the solution
+    # Intentionally initialize the variables off from the ground truth
+    initial_estimate = Values()
+    for i, pose in enumerate(poses):
+        transformed_pose = pose.retract(0.1*np.random.randn(6,1))
+        initial_estimate.insert(X(i), transformed_pose)
+    for j, point in enumerate(points):
+        transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
+        initial_estimate.insert(L(j), transformed_point)
+    initial_estimate.print_('Initial Estimates:\n')
+
+    # Optimize the graph and print results
+    params = gtsam.DoglegParams()
+    params.setVerbosity('TERMINATION')
+    optimizer = DoglegOptimizer(graph, initial_estimate, params)
+    print('Optimizing:')
+    result = optimizer.optimize()
+    result.print_('Final results:\n')
+    print('initial error = {}'.format(graph.error(initial_estimate)))
+    print('final error = {}'.format(graph.error(result)))
+
+    marginals = Marginals(graph, result)
+    plot.plot_3d_points(1, result, marginals=marginals)
+    plot.plot_trajectory(1, result, marginals=marginals, scale=8)
+    plot.set_axes_equal(1)
+    plt.show()
+
+if __name__ == '__main__':
+    main()

python/gtsam_py/python/examples/SFMdata.py

@@ -0,0 +1,39 @@
+"""
+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
+"""
+# pylint: disable=invalid-name, E1101
+
+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 = 40.0
+    height = 10.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), height)
+        camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
+        poses.append(camera.pose())
+    return poses

python/gtsam_py/python/examples/SimpleRotation.py

@@ -0,0 +1,85 @@
+"""
+GTSAM Copyright 2010, 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
+
+This example will perform a relatively trivial optimization on
+a single variable with a single factor.
+"""
+
+import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_X as X
+
+
+def main():
+    """
+    Step 1: Create a factor to express a unary constraint
+
+    The "prior" in this case is the measurement from a sensor,
+    with a model of the noise on the measurement.
+
+    The "Key" created here is a label used to associate parts of the
+    state (stored in "RotValues") with particular factors.  They require
+    an index to allow for lookup, and should be unique.
+
+    In general, creating a factor requires:
+    - A key or set of keys labeling the variables that are acted upon
+    - A measurement value
+    - A measurement model with the correct dimensionality for the factor
+    """
+    prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
+    prior.print_('goal angle')
+    model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
+    key = X(1)
+    factor = gtsam.PriorFactorRot2(key, prior, model)
+
+    """
+    Step 2: Create a graph container and add the factor to it
+
+    Before optimizing, all factors need to be added to a Graph container,
+    which provides the necessary top-level functionality for defining a
+    system of constraints.
+
+    In this case, there is only one factor, but in a practical scenario,
+    many more factors would be added.
+    """
+    graph = gtsam.NonlinearFactorGraph()
+    graph.push_back(factor)
+    graph.print_('full graph')
+
+    """
+    Step 3: Create an initial estimate
+
+    An initial estimate of the solution for the system is necessary to
+    start optimization.  This system state is the "Values" instance,
+    which is similar in structure to a dictionary, in that it maps
+    keys (the label created in step 1) to specific values.
+
+    The initial estimate provided to optimization will be used as
+    a linearization point for optimization, so it is important that
+    all of the variables in the graph have a corresponding value in
+    this structure.
+    """
+    initial = gtsam.Values()
+    initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
+    initial.print_('initial estimate')
+
+    """
+    Step 4: Optimize
+
+    After formulating the problem with a graph of constraints
+    and an initial estimate, executing optimization is as simple
+    as calling a general optimization function with the graph and
+    initial estimate.  This will yield a new RotValues structure
+    with the final state of the optimization.
+    """
+    result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
+    result.print_('final result')
+
+
+if __name__ == '__main__':
+    main()

python/gtsam_py/python/examples/VisualISAM2Example.py

@@ -0,0 +1,154 @@
+"""
+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
+
+An example of running visual SLAM using iSAM2.
+Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import gtsam
+import gtsam.utils.plot as gtsam_plot
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
+from gtsam.examples import SFMdata
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+
+
+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.1, 0.1, 0.1, 0.3, 0.3, 0.3]))  # 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()

python/gtsam_py/python/examples/VisualISAMExample.py

@@ -0,0 +1,103 @@
+"""
+GTSAM Copyright 2010, 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
+
+A visualSLAM example for the structure-from-motion problem on a simulated dataset
+This version uses iSAM to solve the problem incrementally
+"""
+
+from __future__ import print_function
+
+import numpy as np
+import gtsam
+from gtsam.examples import SFMdata
+from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
+                         NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
+                         PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         PinholeCameraCal3_S2, Values)
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
+
+
+def main():
+    """
+    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
+    """
+
+    # Define the camera calibration parameters
+    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
+
+    # Define the camera observation noise model
+    camera_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 a NonlinearISAM object which will relinearize and reorder the variables
+    # every "reorderInterval" updates
+    isam = NonlinearISAM(reorderInterval=3)
+
+    # Create a Factor Graph and Values to hold the new data
+    graph = NonlinearFactorGraph()
+    initial_estimate = Values()
+
+    # Loop over the different poses, adding the observations to iSAM incrementally
+    for i, pose in enumerate(poses):
+        camera = PinholeCameraCal3_S2(pose, K)
+        # Add factors for each landmark observation
+        for j, point in enumerate(points):
+            measurement = camera.project(point)
+            factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
+            graph.push_back(factor)
+
+        # Intentionally initialize the variables off from the ground truth
+        noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20))
+        initial_xi = pose.compose(noise)
+
+        # Add an initial guess for the current pose
+        initial_estimate.insert(X(i), initial_xi)
+
+        # 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, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
+            pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+            factor = PriorFactorPose3(X(0), poses[0], pose_noise)
+            graph.push_back(factor)
+
+            # Add a prior on landmark l0
+            point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+            factor = PriorFactorPoint3(L(0), points[0], point_noise)
+            graph.push_back(factor)
+
+            # Add initial guesses to all observed landmarks
+            noise = np.array([-0.25, 0.20, 0.15])
+            for j, point in enumerate(points):
+                # Intentionally initialize the variables off from the ground truth
+                initial_lj = points[j].vector() + noise
+                initial_estimate.insert(L(j), Point3(initial_lj))
+        else:
+            # Update iSAM with the new factors
+            isam.update(graph, initial_estimate)
+            current_estimate = isam.estimate()
+            print('*' * 50)
+            print('Frame {}:'.format(i))
+            current_estimate.print_('Current estimate: ')
+
+            # Clear the factor graph and values for the next iteration
+            graph.resize(0)
+            initial_estimate.clear()
+
+
+if __name__ == '__main__':
+    main()

python/gtsam_py/python/tests/testScenarioRunner.py

@@ -0,0 +1,47 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+ScenarioRunner unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import math
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestScenarioRunner(GtsamTestCase):
+    def setUp(self):
+        self.g = 10  # simple gravity constant
+
+    def test_loop_runner(self):
+        # Forward velocity 2m/s
+        # Pitch up with angular velocity 6 degree/sec (negative in FLU)
+        v = 2
+        w = math.radians(6)
+        W = np.array([0, -w, 0])
+        V = np.array([v, 0, 0])
+        scenario = gtsam.ConstantTwistScenario(W, V)
+
+        dt = 0.1
+        params = gtsam.PreintegrationParams.MakeSharedU(self.g)
+        bias = gtsam.imuBias_ConstantBias()
+        runner = gtsam.ScenarioRunner(
+            scenario, params, dt, bias)
+
+        # Test specific force at time 0: a is pointing up
+        t = 0.0
+        a = w * v
+        np.testing.assert_almost_equal(
+            np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
+
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam_py/python/tests/test_Cal3Unified.py

@@ -0,0 +1,38 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Cal3Unified unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestCal3Unified(GtsamTestCase):
+
+    def test_Cal3Unified(self):
+        K = gtsam.Cal3Unified()
+        self.assertEqual(K.fx(), 1.)
+        self.assertEqual(K.fx(), 1.)
+
+    def test_retract(self):
+        expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+                                     1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
+        K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240,
+                              1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
+        d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
+        actual = K.retract(d)
+        self.gtsamAssertEquals(actual, expected)
+        np.testing.assert_allclose(d, K.localCoordinates(actual))
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_FrobeniusFactor.py

@@ -0,0 +1,56 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+FrobeniusFactor unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error, invalid-name
+import unittest
+
+import numpy as np
+from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
+                   FrobeniusWormholeFactor, SOn)
+
+id = SO4()
+v1 = np.array([0, 0, 0, 0.1, 0, 0])
+Q1 = SO4.Expmap(v1)
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q2 = SO4.Expmap(v2)
+
+
+class TestFrobeniusFactorSO4(unittest.TestCase):
+    """Test FrobeniusFactor factors."""
+
+    def test_frobenius_factor(self):
+        """Test creation of a factor that calculates the Frobenius norm."""
+        factor = FrobeniusFactorSO4(1, 2)
+        actual = factor.evaluateError(Q1, Q2)
+        expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
+        np.testing.assert_array_equal(actual, expected)
+
+    def test_frobenius_between_factor(self):
+        """Test creation of a Frobenius BetweenFactor."""
+        factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
+        actual = factor.evaluateError(Q1, Q2)
+        expected = np.zeros((16,))
+        np.testing.assert_array_almost_equal(actual, expected)
+
+    def test_frobenius_wormhole_factor(self):
+        """Test creation of a factor that calculates Shonan error."""
+        R1 = SO3.Expmap(v1[3:])
+        R2 = SO3.Expmap(v2[3:])
+        factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4)
+        I4 = SOn(4)
+        Q1 = I4.retract(v1)
+        Q2 = I4.retract(v2)
+        actual = factor.evaluateError(Q1, Q2)
+        expected = np.zeros((12,))
+        np.testing.assert_array_almost_equal(actual, expected, decimal=4)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_GaussianFactorGraph.py

@@ -0,0 +1,92 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Linear Factor Graphs.
+Author: Frank Dellaert & Gerry Chen
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_X as X
+from gtsam.utils.test_case import GtsamTestCase
+
+
+def create_graph():
+    """Create a basic linear factor graph for testing"""
+    graph = gtsam.GaussianFactorGraph()
+    
+    x0 = X(0)
+    x1 = X(1)
+    x2 = X(2)
+    
+    BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
+    PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
+
+    graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
+    graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
+    graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
+
+    return graph, (x0, x1, x2)
+
+class TestGaussianFactorGraph(GtsamTestCase):
+    """Tests for Gaussian Factor Graphs."""
+
+    def test_fg(self):
+        """Test solving a linear factor graph"""
+        graph, X = create_graph()
+        result = graph.optimize()
+
+        EXPECTEDX = [0, 1, 3]
+
+        # check solutions
+        self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8)
+
+    def test_convertNonlinear(self):
+        """Test converting a linear factor graph to a nonlinear one"""
+        graph, X = create_graph()
+
+        EXPECTEDM = [1, 2, 3]
+
+        # create nonlinear factor graph for marginalization
+        nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph)
+        optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values())
+        nlresult = optimizer.optimizeSafely()
+
+        # marginalize
+        marginals = gtsam.Marginals(nfg, nlresult)
+        m = [marginals.marginalCovariance(x) for x in X]
+
+        # check linear marginalizations
+        self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
+    
+    def test_linearMarginalization(self):
+        """Marginalize a linear factor graph"""
+        graph, X = create_graph()
+        result = graph.optimize()
+
+        EXPECTEDM = [1, 2, 3]
+
+        # linear factor graph marginalize
+        marginals = gtsam.Marginals(graph, result)
+        m = [marginals.marginalCovariance(x) for x in X]
+
+        # check linear marginalizations
+        self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
+        self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam_py/python/tests/test_JacobianFactor.py

@@ -0,0 +1,92 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+JacobianFactor unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestJacobianFactor(GtsamTestCase):
+
+    def test_eliminate(self):
+        # Recommended way to specify a matrix (see cython/README)
+        Ax2 = np.array(
+           [[-5., 0.],
+            [+0., -5.],
+            [10., 0.],
+            [+0., 10.]], order='F')
+
+        # This is good too
+        Al1 = np.array(
+           [[5, 0],
+            [0, 5],
+            [0, 0],
+            [0, 0]], dtype=float, order = 'F')
+
+        # Not recommended for performance reasons, but should still work
+        # as the wrapper should convert it to the correct type and storage order
+        Ax1 = np.array(
+           [[0,  0],  # f4
+            [0,  0],  # f4
+            [-10,  0],  # f2
+            [0, -10]])  # f2
+
+        x2 = 1
+        l1 = 2
+        x1 = 3
+
+        # the RHS
+        b2 = np.array([-1., 1.5, 2., -1.])
+        sigmas = np.array([1., 1., 1., 1.])
+        model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
+        combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4)
+
+        # eliminate the first variable (x2) in the combined factor, destructive
+        # !
+        ord = gtsam.Ordering()
+        ord.push_back(x2)
+        actualCG, lf = combined.eliminate(ord)
+
+        # create expected Conditional Gaussian
+        R11 = np.array([[11.1803,  0.00],
+                        [0.00, 11.1803]])
+        S12 = np.array([[-2.23607, 0.00],
+                        [+0.00, -2.23607]])
+        S13 = np.array([[-8.94427, 0.00],
+                        [+0.00, -8.94427]])
+        d = np.array([2.23607, -1.56525])
+        expectedCG = gtsam.GaussianConditional(
+            x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
+        # check if the result matches
+        self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
+
+        # the expected linear factor
+        Bl1 = np.array([[4.47214, 0.00],
+                        [0.00, 4.47214]])
+
+        Bx1 = np.array(
+            # x1
+            [[-4.47214,  0.00],
+             [+0.00, -4.47214]])
+
+        # the RHS
+        b1 = np.array([0.0, 0.894427])
+
+        model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
+        expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
+
+        # check if the result matches the combined (reduced) factor
+        self.gtsamAssertEquals(lf, expectedLF, 1e-4)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_KalmanFilter.py

@@ -0,0 +1,83 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+KalmanFilter unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestKalmanFilter(GtsamTestCase):
+
+    def test_KalmanFilter(self):
+        F = np.eye(2)
+        B = np.eye(2)
+        u = np.array([1.0, 0.0])
+        modelQ = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
+        Q = 0.01 * np.eye(2)
+        H = np.eye(2)
+        z1 = np.array([1.0, 0.0])
+        z2 = np.array([2.0, 0.0])
+        z3 = np.array([3.0, 0.0])
+        modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1]))
+        R = 0.01 * np.eye(2)
+
+        # Create the set of expected output TestValues
+        expected0 = np.array([0.0, 0.0])
+        P00 = 0.01 * np.eye(2)
+
+        expected1 = np.array([1.0, 0.0])
+        P01 = P00 + Q
+        I11 = np.linalg.inv(P01) + np.linalg.inv(R)
+
+        expected2 = np.array([2.0, 0.0])
+        P12 = np.linalg.inv(I11) + Q
+        I22 = np.linalg.inv(P12) + np.linalg.inv(R)
+
+        expected3 = np.array([3.0, 0.0])
+        P23 = np.linalg.inv(I22) + Q
+        I33 = np.linalg.inv(P23) + np.linalg.inv(R)
+
+        # Create an KalmanFilter object
+        KF = gtsam.KalmanFilter(n=2)
+
+        # Create the Kalman Filter initialization point
+        x_initial = np.array([0.0, 0.0])
+        P_initial = 0.01 * np.eye(2)
+
+        # Create an KF object
+        state = KF.init(x_initial, P_initial)
+        self.assertTrue(np.allclose(expected0, state.mean()))
+        self.assertTrue(np.allclose(P00, state.covariance()))
+
+        # Run iteration 1
+        state = KF.predict(state, F, B, u, modelQ)
+        self.assertTrue(np.allclose(expected1, state.mean()))
+        self.assertTrue(np.allclose(P01, state.covariance()))
+        state = KF.update(state, H, z1, modelR)
+        self.assertTrue(np.allclose(expected1, state.mean()))
+        self.assertTrue(np.allclose(I11, state.information()))
+
+        # Run iteration 2
+        state = KF.predict(state, F, B, u, modelQ)
+        self.assertTrue(np.allclose(expected2, state.mean()))
+        state = KF.update(state, H, z2, modelR)
+        self.assertTrue(np.allclose(expected2, state.mean()))
+
+        # Run iteration 3
+        state = KF.predict(state, F, B, u, modelQ)
+        self.assertTrue(np.allclose(expected3, state.mean()))
+        state = KF.update(state, H, z3, modelR)
+        self.assertTrue(np.allclose(expected3, state.mean()))
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_KarcherMeanFactor.py

@@ -0,0 +1,80 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+KarcherMeanFactor unit tests.
+Author: Frank Dellaert
+"""
+
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+KEY = 0
+MODEL = gtsam.noiseModel_Unit.Create(3)
+
+
+def find_Karcher_mean_Rot3(rotations):
+    """Find the Karcher mean of given values."""
+    # Cost function C(R) = \sum PriorFactor(R_i)::error(R)
+    # No closed form solution.
+    graph = gtsam.NonlinearFactorGraph()
+    for R in rotations:
+        graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
+    initial = gtsam.Values()
+    initial.insert(KEY, gtsam.Rot3())
+    result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
+    return result.atRot3(KEY)
+
+
+# Rot3 version
+R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
+
+
+class TestKarcherMean(GtsamTestCase):
+
+    def test_find(self):
+        # Check that optimizing for Karcher mean (which minimizes Between distance)
+        # gets correct result.
+        rotations = {R, R.inverse()}
+        expected = gtsam.Rot3()
+        actual = find_Karcher_mean_Rot3(rotations)
+        self.gtsamAssertEquals(expected, actual)
+
+    def test_factor(self):
+        """Check that the InnerConstraint factor leaves the mean unchanged."""
+        # Make a graph with two variables, one between, and one InnerConstraint
+        # The optimal result should satisfy the between, while moving the other
+        # variable to make the mean the same as before.
+        # Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
+        # R*R*R, i.e. geodesic length is 3 rather than 2.
+        graph = gtsam.NonlinearFactorGraph()
+        R12 = R.compose(R.compose(R))
+        graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
+        keys = gtsam.KeyVector()
+        keys.push_back(1)
+        keys.push_back(2)
+        graph.add(gtsam.KarcherMeanFactorRot3(keys))
+
+        initial = gtsam.Values()
+        initial.insert(1, R.inverse())
+        initial.insert(2, R)
+        expected = find_Karcher_mean_Rot3([R, R.inverse()])
+
+        result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
+        actual = find_Karcher_mean_Rot3(
+            [result.atRot3(1), result.atRot3(2)])
+        self.gtsamAssertEquals(expected, actual)
+        self.gtsamAssertEquals(
+            R12, result.atRot3(1).between(result.atRot3(2)))
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_LocalizationExample.py

@@ -0,0 +1,64 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Localization unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestLocalizationExample(GtsamTestCase):
+
+    def test_LocalizationExample(self):
+        # Create the graph (defined in pose2SLAM.h, derived from
+        # NonlinearFactorGraph)
+        graph = gtsam.NonlinearFactorGraph()
+
+        # Add two odometry factors
+        # create a measurement for both factors (the same in this case)
+        odometry = gtsam.Pose2(2.0, 0.0, 0.0)
+        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.2, 0.2, 0.1]))  # 20cm std on x,y, 0.1 rad on theta
+        graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
+
+        # Add three "GPS" measurements
+        # We use Pose2 Priors here with high variance on theta
+        groundTruth = gtsam.Values()
+        groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0))
+        groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0))
+        groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0))
+        model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.]))
+        for i in range(3):
+            graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model))
+
+        # Initialize to noisy points
+        initialEstimate = gtsam.Values()
+        initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2))
+        initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2))
+        initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1))
+
+        # Optimize using Levenberg-Marquardt optimization with an ordering from
+        # colamd
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+        result = optimizer.optimizeSafely()
+
+        # Plot Covariance Ellipses
+        marginals = gtsam.Marginals(graph, result)
+        P = [None] * result.size()
+        for i in range(0, result.size()):
+            pose_i = result.atPose2(i)
+            self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4)
+            P[i] = marginals.marginalCovariance(i)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_NonlinearOptimizer.py

@@ -0,0 +1,83 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for IMU testing scenarios.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, no-name-in-module
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
+                   GaussNewtonParams, LevenbergMarquardtOptimizer,
+                   LevenbergMarquardtParams, PCGSolverParameters,
+                   DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
+                   Point2, PriorFactorPoint2, Values)
+from gtsam.utils.test_case import GtsamTestCase
+
+KEY1 = 1
+KEY2 = 2
+
+
+class TestScenario(GtsamTestCase):
+    def test_optimize(self):
+        """Do trivial test with three optimizer variants."""
+        fg = NonlinearFactorGraph()
+        model = gtsam.noiseModel_Unit.Create(2)
+        fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
+
+        # test error at minimum
+        xstar = Point2(0, 0)
+        optimal_values = Values()
+        optimal_values.insert(KEY1, xstar)
+        self.assertEqual(0.0, fg.error(optimal_values), 0.0)
+
+        # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
+        x0 = Point2(3, 3)
+        initial_values = Values()
+        initial_values.insert(KEY1, x0)
+        self.assertEqual(9.0, fg.error(initial_values), 1e-3)
+
+        # optimize parameters
+        ordering = Ordering()
+        ordering.push_back(KEY1)
+
+        # Gauss-Newton
+        gnParams = GaussNewtonParams()
+        gnParams.setOrdering(ordering)
+        actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual1))
+
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setOrdering(ordering)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setLinearSolverType("ITERATIVE")
+        cgParams = PCGSolverParameters()
+        cgParams.setPreconditionerParams(DummyPreconditionerParameters())
+        lmParams.setIterativeParams(cgParams)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
+        # Dogleg
+        dlParams = DoglegParams()
+        dlParams.setOrdering(ordering)
+        actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual3))
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_OdometryExample.py

@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Odometry unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestOdometryExample(GtsamTestCase):
+
+    def test_OdometryExample(self):
+        # Create the graph (defined in pose2SLAM.h, derived from
+        # NonlinearFactorGraph)
+        graph = gtsam.NonlinearFactorGraph()
+
+        # Add a Gaussian prior on pose x_1
+        priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior mean is at origin
+        priorNoise = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.3, 0.3, 0.1]))  # 30cm std on x,y, 0.1 rad on theta
+        # add directly to graph
+        graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
+
+        # Add two odometry factors
+        # create a measurement for both factors (the same in this case)
+        odometry = gtsam.Pose2(2.0, 0.0, 0.0)
+        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.2, 0.2, 0.1]))  # 20cm std on x,y, 0.1 rad on theta
+        graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
+
+        # Initialize to noisy points
+        initialEstimate = gtsam.Values()
+        initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
+        initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
+        initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
+
+        # Optimize using Levenberg-Marquardt optimization with an ordering from
+        # colamd
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+        result = optimizer.optimizeSafely()
+        marginals = gtsam.Marginals(graph, result)
+        marginals.marginalCovariance(1)
+
+        # Check first pose equality
+        pose_1 = result.atPose2(1)
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_PlanarSLAMExample.py

@@ -0,0 +1,78 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PlanarSLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPlanarSLAM(GtsamTestCase):
+
+    def test_PlanarSLAM(self):
+        # Assumptions
+        #  - All values are axis aligned
+        #  - Robot poses are facing along the X axis (horizontal, to the right in images)
+        #  - We have full odometry for measurements
+        #  - The robot is on a grid, moving 2 meters each step
+
+        # Create graph container and add factors to it
+        graph = gtsam.NonlinearFactorGraph()
+
+        # Add prior
+        # gaussian for prior
+        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]))
+        # add directly to graph
+        graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
+
+        # Add odometry
+        # general noisemodel for odometry
+        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        graph.add(gtsam.BetweenFactorPose2(
+            1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(
+            2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(
+            3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(
+            4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
+
+        # Add pose constraint
+        model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
+
+        # Initialize to noisy points
+        initialEstimate = gtsam.Values()
+        initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
+        initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
+        initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
+        initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
+        initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
+
+        # Optimize using Levenberg-Marquardt optimization with an ordering from
+        # colamd
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+        result = optimizer.optimizeSafely()
+
+        # Plot Covariance Ellipses
+        marginals = gtsam.Marginals(graph, result)
+        P = marginals.marginalCovariance(1)
+
+        pose_1 = result.atPose2(1)
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
+
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_Pose2.py

@@ -0,0 +1,32 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose2 unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import Pose2
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPose2(GtsamTestCase):
+    """Test selected Pose2 methods."""
+
+    def test_adjoint(self):
+        """Test adjoint method."""
+        xi = np.array([1, 2, 3])
+        expected = np.dot(Pose2.adjointMap_(xi), xi)
+        actual = Pose2.adjoint_(xi, xi)
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_Pose2SLAMExample.py

@@ -0,0 +1,76 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose2SLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPose2SLAMExample(GtsamTestCase):
+
+    def test_Pose2SLAMExample(self):
+        # Assumptions
+        #  - All values are axis aligned
+        #  - Robot poses are facing along the X axis (horizontal, to the right in images)
+        #  - We have full odometry for measurements
+        #  - The robot is on a grid, moving 2 meters each step
+
+        # Create graph container and add factors to it
+        graph = gtsam.NonlinearFactorGraph()
+
+        # Add prior
+        # gaussian for prior
+        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]))
+        # add directly to graph
+        graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
+
+        # Add odometry
+        # general noisemodel for odometry
+        odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        graph.add(gtsam.BetweenFactorPose2(
+            1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(
+            2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(
+            3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
+        graph.add(gtsam.BetweenFactorPose2(
+            4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
+
+        # Add pose constraint
+        model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
+        graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
+
+        # Initialize to noisy points
+        initialEstimate = gtsam.Values()
+        initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
+        initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
+        initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
+        initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
+        initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
+
+        # Optimize using Levenberg-Marquardt optimization with an ordering from
+        # colamd
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+        result = optimizer.optimizeSafely()
+
+        # Plot Covariance Ellipses
+        marginals = gtsam.Marginals(graph, result)
+        P = marginals.marginalCovariance(1)
+
+        pose_1 = result.atPose2(1)
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_Pose3.py

@@ -0,0 +1,70 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose3 unit tests.
+Author: Frank Dellaert, Duy Nguyen Ta
+"""
+# pylint: disable=no-name-in-module
+import math
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import Point3, Pose3, Rot3
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPose3(GtsamTestCase):
+    """Test selected Pose3 methods."""
+
+    def test_between(self):
+        """Test between method."""
+        T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
+        T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
+        expected = T2.inverse().compose(T3)
+        actual = T2.between(T3)
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+    def test_transform_to(self):
+        """Test transformTo method."""
+        transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
+        actual = transform.transformTo(Point3(3, 2, 10))
+        expected = Point3(2, 1, 10)
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+    def test_range(self):
+        """Test range method."""
+        l1 = Point3(1, 0, 0)
+        l2 = Point3(1, 1, 0)
+        x1 = Pose3()
+
+        xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
+        xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
+
+        # establish range is indeed zero
+        self.assertEqual(1, x1.range(point=l1))
+
+        # establish range is indeed sqrt2
+        self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
+
+        # establish range is indeed zero
+        self.assertEqual(1, x1.range(pose=xl1))
+
+        # establish range is indeed sqrt2
+        self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
+
+    def test_adjoint(self):
+        """Test adjoint method."""
+        xi = np.array([1, 2, 3, 4, 5, 6])
+        expected = np.dot(Pose3.adjointMap_(xi), xi)
+        actual = Pose3.adjoint_(xi, xi)
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_Pose3SLAMExample.py

@@ -0,0 +1,60 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PoseSLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+from gtsam.utils.circlePose3 import *
+
+
+class TestPose3SLAMExample(GtsamTestCase):
+
+    def test_Pose3SLAMExample(self):
+        # Create a hexagon of poses
+        hexagon = circlePose3(6, 1.0)
+        p0 = hexagon.atPose3(0)
+        p1 = hexagon.atPose3(1)
+
+        # create a Pose graph with one equality constraint and one measurement
+        fg = gtsam.NonlinearFactorGraph()
+        fg.add(gtsam.NonlinearEqualityPose3(0, p0))
+        delta = p0.between(p1)
+        covariance = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180]))
+        fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
+        fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
+        fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
+        fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
+        fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
+        fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
+
+        # Create initial config
+        initial = gtsam.Values()
+        s = 0.10
+        initial.insert(0, p0)
+        initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
+        initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1)))
+        initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1)))
+        initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1)))
+        initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
+
+        # optimize
+        optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial)
+        result = optimizer.optimizeSafely()
+
+        pose_1 = result.atPose3(1)
+        self.gtsamAssertEquals(pose_1, p1, 1e-4)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_PriorFactor.py

@@ -0,0 +1,61 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PriorFactor unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPriorFactor(GtsamTestCase):
+
+    def test_PriorFactor(self):
+        values = gtsam.Values()
+
+        key = 5
+        priorPose3 = gtsam.Pose3()
+        model = gtsam.noiseModel_Unit.Create(6)
+        factor = gtsam.PriorFactorPose3(key, priorPose3, model)
+        values.insert(key, priorPose3)
+        self.assertEqual(factor.error(values), 0)
+
+        key = 3
+        priorVector = np.array([0., 0., 0.])
+        model = gtsam.noiseModel_Unit.Create(3)
+        factor = gtsam.PriorFactorVector(key, priorVector, model)
+        values.insert(key, priorVector)
+        self.assertEqual(factor.error(values), 0)
+
+    def test_AddPrior(self):
+        """
+        Test adding prior factors directly to factor graph via the .addPrior method.
+        """
+        # define factor graph
+        graph = gtsam.NonlinearFactorGraph()
+
+        # define and add Pose3 prior
+        key = 5
+        priorPose3 = gtsam.Pose3()
+        model = gtsam.noiseModel_Unit.Create(6)
+        graph.addPriorPose3(key, priorPose3, model)
+        self.assertEqual(graph.size(), 1)
+
+        # define and add Vector prior
+        key = 3
+        priorVector = np.array([0., 0., 0.])
+        model = gtsam.noiseModel_Unit.Create(3)
+        graph.addPriorVector(key, priorVector, model)
+        self.assertEqual(graph.size(), 2)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_SFMExample.py

@@ -0,0 +1,82 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SFM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+import gtsam.utils.visual_data_generator as generator
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSFMExample(GtsamTestCase):
+
+    def test_SFMExample(self):
+        options = generator.Options()
+        options.triangle = False
+        options.nrCameras = 10
+
+        [data, truth] = generator.generate_data(options)
+
+        measurementNoiseSigma = 1.0
+        pointNoiseSigma = 0.1
+        poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
+
+        graph = gtsam.NonlinearFactorGraph()
+
+        # Add factors for all measurements
+        measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
+        for i in range(len(data.Z)):
+            for k in range(len(data.Z[i])):
+                j = data.J[i][k]
+                graph.add(gtsam.GenericProjectionFactorCal3_S2(
+                    data.Z[i][k], measurementNoise,
+                    symbol(ord('x'), i), symbol(ord('p'), j), data.K))
+
+        posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
+        graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
+                                   truth.cameras[0].pose(), posePriorNoise))
+        pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
+        graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
+                                    truth.points[0], pointPriorNoise))
+
+        # Initial estimate
+        initialEstimate = gtsam.Values()
+        for i in range(len(truth.cameras)):
+            pose_i = truth.cameras[i].pose()
+            initialEstimate.insert(symbol(ord('x'), i), pose_i)
+        for j in range(len(truth.points)):
+            point_j = truth.points[j]
+            initialEstimate.insert(symbol(ord('p'), j), point_j)
+
+        # Optimization
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+        for i in range(5):
+            optimizer.iterate()
+        result = optimizer.values()
+
+        # Marginalization
+        marginals = gtsam.Marginals(graph, result)
+        marginals.marginalCovariance(symbol(ord('p'), 0))
+        marginals.marginalCovariance(symbol(ord('x'), 0))
+
+        # Check optimized results, should be equal to ground truth
+        for i in range(len(truth.cameras)):
+            pose_i = result.atPose3(symbol(ord('x'), i))
+            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
+
+        for j in range(len(truth.points)):
+            point_j = result.atPoint3(symbol(ord('p'), j))
+            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_SO4.py

@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SO4 unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error
+import unittest
+
+import numpy as np
+from gtsam import SO4
+
+I4 = SO4()
+v1 = np.array([0, 0, 0, .1, 0, 0])
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q1 = SO4.Expmap(v1)
+Q2 = SO4.Expmap(v2)
+
+
+class TestSO4(unittest.TestCase):
+    """Test selected SO4 methods."""
+
+    def test_constructor(self):
+        """Construct from matrix."""
+        matrix = np.eye(4)
+        so4 = SO4(matrix)
+        self.assertIsInstance(so4, SO4)
+
+    def test_retract(self):
+        """Test retraction to manifold."""
+        v = np.zeros((6,), np.float)
+        actual = I4.retract(v)
+        self.assertTrue(actual.equals(I4, 1e-9))
+
+    def test_local(self):
+        """Check localCoordinates for trivial case."""
+        v0 = np.zeros((6,), np.float)
+        actual = I4.localCoordinates(I4)
+        np.testing.assert_array_almost_equal(actual, v0)
+
+    def test_compose(self):
+        """Check compose works in subgroup."""
+        expected = SO4.Expmap(2*v1)
+        actual = Q1.compose(Q1)
+        self.assertTrue(actual.equals(expected, 1e-9))
+
+    def test_vec(self):
+        """Check wrapping of vec."""
+        expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
+        actual = I4.vec()
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_SOn.py

@@ -0,0 +1,59 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Dynamic SO(n) unit tests.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-name-in-module, import-error
+import unittest
+
+import numpy as np
+from gtsam import SOn
+
+I4 = SOn(4)
+v1 = np.array([0, 0, 0, .1, 0, 0])
+v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
+Q1 = I4.retract(v1)
+Q2 = I4.retract(v2)
+
+
+class TestSO4(unittest.TestCase):
+    """Test selected SOn methods."""
+
+    def test_constructor(self):
+        """Construct from matrix."""
+        matrix = np.eye(4)
+        so4 = SOn.FromMatrix(matrix)
+        self.assertIsInstance(so4, SOn)
+
+    def test_retract(self):
+        """Test retraction to manifold."""
+        v = np.zeros((6,), np.float)
+        actual = I4.retract(v)
+        self.assertTrue(actual.equals(I4, 1e-9))
+
+    def test_local(self):
+        """Check localCoordinates for trivial case."""
+        v0 = np.zeros((6,), np.float)
+        actual = I4.localCoordinates(I4)
+        np.testing.assert_array_almost_equal(actual, v0)
+
+    def test_compose(self):
+        """Check compose works in subgroup."""
+        expected = I4.retract(2*v1)
+        actual = Q1.compose(Q1)
+        self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
+
+    def test_vec(self):
+        """Check wrapping of vec."""
+        expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
+        actual = I4.vec()
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_Scenario.py

@@ -0,0 +1,53 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Scenario unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+from __future__ import print_function
+
+import math
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+# pylint: disable=invalid-name, E1101
+
+
+class TestScenario(GtsamTestCase):
+    def setUp(self):
+        pass
+
+    def test_loop(self):
+        # Forward velocity 2m/s
+        # Pitch up with angular velocity 6 degree/sec (negative in FLU)
+        v = 2
+        w = math.radians(6)
+        W = np.array([0, -w, 0])
+        V = np.array([v, 0, 0])
+        scenario = gtsam.ConstantTwistScenario(W, V)
+
+        T = 30
+        np.testing.assert_almost_equal(W, scenario.omega_b(T))
+        np.testing.assert_almost_equal(V, scenario.velocity_b(T))
+        np.testing.assert_almost_equal(
+            np.cross(W, V), scenario.acceleration_b(T))
+
+        # R = v/w, so test if loop crests at 2*R
+        R = v / w
+        T30 = scenario.pose(T)
+        np.testing.assert_almost_equal(
+            np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
+        self.gtsamAssertEquals(gtsam.Point3(
+            0, 0, 2.0 * R), T30.translation(), 1e-9)
+
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam_py/python/tests/test_SimpleCamera.py

@@ -0,0 +1,45 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PinholeCameraCal3_S2 unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import math
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
+from gtsam.utils.test_case import GtsamTestCase
+
+K = Cal3_S2(625, 625, 0, 0, 0)
+
+class TestSimpleCamera(GtsamTestCase):
+
+    def test_constructor(self):
+        pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
+        camera = PinholeCameraCal3_S2(pose1, K)
+        self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
+        self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
+
+    def test_level2(self):
+        # Create a level camera, looking in Y-direction
+        pose2 = Pose2(0.4,0.3,math.pi/2.0)
+        camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1)
+
+        # expected
+        x = Point3(1,0,0)
+        y = Point3(0,0,-1)
+        z = Point3(0,1,0)
+        wRc = Rot3(x,y,z)
+        expected = Pose3(wRc,Point3(0.4,0.3,0.1))
+        self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_StereoVOExample.py

@@ -0,0 +1,82 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Stereo VO unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestStereoVOExample(GtsamTestCase):
+
+    def test_StereoVOExample(self):
+        ## Assumptions
+        #  - For simplicity this example is in the camera's coordinate frame
+        #  - X: right, Y: down, Z: forward
+        #  - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
+        #  - x1 is fixed with a constraint, x2 is initialized with noisy values
+        #  - No noise on measurements
+
+        ## Create keys for variables
+        x1 = symbol(ord('x'),1) 
+        x2 = symbol(ord('x'),2) 
+        l1 = symbol(ord('l'),1) 
+        l2 = symbol(ord('l'),2) 
+        l3 = symbol(ord('l'),3)
+
+        ## Create graph container and add factors to it
+        graph = gtsam.NonlinearFactorGraph()
+
+        ## add a constraint on the starting pose
+        first_pose = gtsam.Pose3()
+        graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
+
+        ## Create realistic calibration and measurement noise model
+        # format: fx fy skew cx cy baseline
+        K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
+        stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
+
+        ## Add measurements
+        # pose 1
+        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
+        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120,  80, 440), stereo_model, x1, l2, K))
+        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
+
+        #pose 2
+        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
+        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K))
+        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
+
+        ## Create initial estimate for camera poses and landmarks
+        initialEstimate = gtsam.Values()
+        initialEstimate.insert(x1, first_pose)
+        # noisy estimate for pose 2
+        initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1)))
+        expected_l1 = gtsam.Point3( 1,  1, 5)
+        initialEstimate.insert(l1, expected_l1)
+        initialEstimate.insert(l2, gtsam.Point3(-1,  1, 5))
+        initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5))
+
+        ## optimize
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
+        result = optimizer.optimize()
+
+        ## check equality for the first pose and point
+        pose_x1 = result.atPose3(x1)
+        self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
+
+        point_l1 = result.atPoint3(l1)
+        self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_Values.py

@@ -0,0 +1,86 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Values unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
+                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
+                   imuBias_ConstantBias)
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestValues(GtsamTestCase):
+
+    def test_values(self):
+        values = Values()
+        E = EssentialMatrix(Rot3(), Unit3())
+        tol = 1e-9
+
+        values.insert(0, Point2(0, 0))
+        values.insert(1, Point3(0, 0, 0))
+        values.insert(2, Rot2())
+        values.insert(3, Pose2())
+        values.insert(4, Rot3())
+        values.insert(5, Pose3())
+        values.insert(6, Cal3_S2())
+        values.insert(7, Cal3DS2())
+        values.insert(8, Cal3Bundler())
+        values.insert(9, E)
+        values.insert(10, imuBias_ConstantBias())
+
+        # Special cases for Vectors and Matrices
+        # Note that gtsam's Eigen Vectors and Matrices requires double-precision
+        # floating point numbers in column-major (Fortran style) storage order,
+        # whereas by default, numpy.array is in row-major order and the type is
+        # in whatever the number input type is, e.g. np.array([1,2,3])
+        # will have 'int' type.
+        #
+        # The wrapper will automatically fix the type and storage order for you,
+        # but for performance reasons, it's recommended to specify the correct
+        # type and storage order.
+        # for vectors, the order is not important, but dtype still is
+        vec = np.array([1., 2., 3.])
+        values.insert(11, vec)
+        mat = np.array([[1., 2.], [3., 4.]], order='F')
+        values.insert(12, mat)
+        # Test with dtype int and the default order='C'
+        # This still works as the wrapper converts to the correct type and order for you
+        # but is nornally not recommended!
+        mat2 = np.array([[1, 2, ], [3, 5]])
+        values.insert(13, mat2)
+
+        self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol)
+        self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol)
+        self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol)
+        self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol)
+        self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol)
+        self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol)
+        self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol)
+        self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
+        self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
+        self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
+        self.gtsamAssertEquals(values.atimuBias_ConstantBias(
+            10), imuBias_ConstantBias(), tol)
+
+        # special cases for Vector and Matrix:
+        actualVector = values.atVector(11)
+        np.testing.assert_allclose(vec, actualVector, tol)
+        actualMatrix = values.atMatrix(12)
+        np.testing.assert_allclose(mat, actualMatrix, tol)
+        actualMatrix2 = values.atMatrix(13)
+        np.testing.assert_allclose(mat2, actualMatrix2, tol)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_VisualISAMExample.py

@@ -0,0 +1,57 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+visual_isam unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+import gtsam.utils.visual_data_generator as generator
+import gtsam.utils.visual_isam as visual_isam
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestVisualISAMExample(GtsamTestCase):
+
+    def test_VisualISAMExample(self):
+        # Data Options
+        options = generator.Options()
+        options.triangle = False
+        options.nrCameras = 20
+
+        # iSAM Options
+        isamOptions = visual_isam.Options()
+        isamOptions.hardConstraint = False
+        isamOptions.pointPriors = False
+        isamOptions.batchInitialization = True
+        isamOptions.reorderInterval = 10
+        isamOptions.alwaysRelinearize = False
+
+        # Generate data
+        data, truth = generator.generate_data(options)
+
+        # Initialize iSAM with the first pose and points
+        isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions)
+
+        # Main loop for iSAM: stepping through all poses
+        for currentPose in range(nextPose, options.nrCameras):
+            isam, result = visual_isam.step(data, isam, result, truth, currentPose)
+
+        for i in range(len(truth.cameras)):
+            pose_i = result.atPose3(symbol(ord('x'), i))
+            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
+
+        for j in range(len(truth.points)):
+            point_j = result.atPoint3(symbol(ord('l'), j))
+            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_dataset.py

@@ -0,0 +1,45 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import BetweenFactorPose3, BetweenFactorPose3s
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestDataset(GtsamTestCase):
+    """Tests for datasets.h wrapper."""
+
+    def setUp(self):
+        """Get some common paths."""
+        self.pose3_example_g2o_file = gtsam.findExampleDataFile(
+            "pose3example.txt")
+
+    def test_readG2o3D(self):
+        """Test reading directly into factor graph."""
+        is3D = True
+        graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
+        self.assertEqual(graph.size(), 6)
+        self.assertEqual(initial.size(), 5)
+
+    def test_parse3Dfactors(self):
+        """Test parsing into data structure."""
+        factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
+        self.assertEqual(factors.size(), 6)
+        self.assertIsInstance(factors.at(0), BetweenFactorPose3)
+
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam_py/python/tests/test_dsf_map.py

@@ -0,0 +1,40 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Disjoint Set Forest.
+Author: Frank Dellaert & Varun Agrawal
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestDSFMap(GtsamTestCase):
+    """Tests for DSFMap."""
+
+    def test_all(self):
+        """Test everything in DFSMap."""
+        def key(index_pair):
+            return index_pair.i(), index_pair.j()
+
+        dsf = gtsam.DSFMapIndexPair()
+        pair1 = gtsam.IndexPair(1, 18)
+        self.assertEqual(key(dsf.find(pair1)), key(pair1))
+        pair2 = gtsam.IndexPair(2, 2)
+        
+        # testing the merge feature of dsf
+        dsf.merge(pair1, pair2)
+        self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
+
+
+if __name__ == '__main__':
+    unittest.main()

python/gtsam_py/python/tests/test_initialize_pose3.py

@@ -0,0 +1,89 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for 3D SLAM initialization, using rotation relaxation.
+Author: Luca Carlone and Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
+from gtsam.utils.test_case import GtsamTestCase
+
+x0, x1, x2, x3 = 0, 1, 2, 3
+
+
+class TestValues(GtsamTestCase):
+
+    def setUp(self):
+
+        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+
+        # We consider a small graph:
+        #                            symbolic FG
+        #               x2               0  1
+        #             / | \              1  2
+        #            /  |  \             2  3
+        #          x3   |   x1           2  0
+        #           \   |   /            0  3
+        #            \  |  /
+        #               x0
+        #
+        p0 = Point3(0, 0, 0)
+        self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
+        p1 = Point3(1, 2, 0)
+        self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
+        p2 = Point3(0, 2, 0)
+        self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
+        p3 = Point3(-1, 1, 0)
+        self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
+
+        pose0 = Pose3(self.R0, p0)
+        pose1 = Pose3(self.R1, p1)
+        pose2 = Pose3(self.R2, p2)
+        pose3 = Pose3(self.R3, p3)
+
+        g = NonlinearFactorGraph()
+        g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
+        g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
+        g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
+        g.add(gtsam.PriorFactorPose3(x0, pose0, model))
+        self.graph = g
+
+    def test_buildPose3graph(self):
+        pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+    def test_orientations(self):
+        pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+        initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
+
+        # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+        self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6)
+
+    def test_initializePoses(self):
+        g2oFile = gtsam.findExampleDataFile("pose3example-grid")
+        is3D = True
+        inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
+        priorModel = gtsam.noiseModel_Unit.Create(6)
+        inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
+
+        initial = gtsam.InitializePose3.initialize(inputGraph)
+        # TODO(frank): very loose !!
+        self.gtsamAssertEquals(initial, expectedValues, 0.1)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/tests/test_logging_optimizer.py

@@ -0,0 +1,108 @@
+"""
+Unit tests for optimization that logs to comet.ml.
+Author: Jing Wu and Frank Dellaert
+"""
+# pylint: disable=invalid-name
+
+import sys
+if sys.version_info.major >= 3:
+    from io import StringIO
+else:
+    from cStringIO import StringIO
+
+import unittest
+from datetime import datetime
+
+import gtsam
+import numpy as np
+from gtsam import Rot3
+from gtsam.utils.test_case import GtsamTestCase
+
+from gtsam.utils.logging_optimizer import gtsam_optimize
+
+KEY = 0
+MODEL = gtsam.noiseModel_Unit.Create(3)
+
+
+class TestOptimizeComet(GtsamTestCase):
+    """Check correct logging to comet.ml."""
+
+    def setUp(self):
+        """Set up a small Karcher mean optimization example."""
+        # Grabbed from KarcherMeanFactor unit tests.
+        R = Rot3.Expmap(np.array([0.1, 0, 0]))
+        rotations = {R, R.inverse()}  # mean is the identity
+        self.expected = Rot3()
+
+        graph = gtsam.NonlinearFactorGraph()
+        for R in rotations:
+            graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
+        initial = gtsam.Values()
+        initial.insert(KEY, R)
+        self.params = gtsam.GaussNewtonParams()
+        self.optimizer = gtsam.GaussNewtonOptimizer(
+            graph, initial, self.params)
+
+        self.lmparams = gtsam.LevenbergMarquardtParams()
+        self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
+            graph, initial, self.lmparams
+        )
+
+        # setup output capture
+        self.capturedOutput = StringIO()
+        sys.stdout = self.capturedOutput
+
+    def tearDown(self):
+        """Reset print capture."""
+        sys.stdout = sys.__stdout__
+
+    def test_simple_printing(self):
+        """Test with a simple hook."""
+
+        # Provide a hook that just prints
+        def hook(_, error):
+            print(error)
+
+        # Only thing we require from optimizer is an iterate method
+        gtsam_optimize(self.optimizer, self.params, hook)
+
+        # Check that optimizing yields the identity.
+        actual = self.optimizer.values()
+        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+
+    def test_lm_simple_printing(self):
+        """Make sure we are properly terminating LM"""
+        def hook(_, error):
+            print(error)
+
+        gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
+
+        actual = self.lmoptimizer.values()
+        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+
+    @unittest.skip("Not a test we want run every time, as needs comet.ml account")
+    def test_comet(self):
+        """Test with a comet hook."""
+        from comet_ml import Experiment
+        comet = Experiment(project_name="Testing",
+                           auto_output_logging="native")
+        comet.log_dataset_info(name="Karcher", path="shonan")
+        comet.add_tag("GaussNewton")
+        comet.log_parameter("method", "GaussNewton")
+        time = datetime.now()
+        comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " "
+                       + str(time.hour)+":"+str(time.minute)+":"+str(time.second))
+
+        # I want to do some comet thing here
+        def hook(optimizer, error):
+            comet.log_metric("Karcher error",
+                             error, optimizer.iterations())
+
+        gtsam_optimize(self.optimizer, self.params, hook)
+        comet.end()
+
+        actual = self.optimizer.values()
+        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam_py/python/utils/circlePose3.py

@@ -0,0 +1,38 @@
+import gtsam
+import numpy as np
+from math import pi, cos, sin
+
+
+def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
+    """
+    circlePose3 generates a set of poses in a circle. This function
+    returns those poses inside a gtsam.Values object, with sequential
+    keys starting from 0. An optional character may be provided, which
+    will be stored in the msb of each key (i.e. gtsam.Symbol).
+
+    We use aerospace/navlab convention, X forward, Y right, Z down
+    First pose will be at (R,0,0)
+    ^y   ^ X
+    |    |
+    z-->xZ--> Y  (z pointing towards viewer, Z pointing away from viewer)
+    Vehicle at p0 is looking towards y axis (X-axis points towards world y)
+    """
+
+    # Force symbolChar to be a single character
+    if type(symbolChar) is str:
+        symbolChar = ord(symbolChar[0])
+
+    values = gtsam.Values()
+    theta = 0.0
+    dtheta = 2 * pi / numPoses
+    gRo = gtsam.Rot3(
+        np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F'))
+    for i in range(numPoses):
+        key = gtsam.symbol(symbolChar, i)
+        gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0)
+        oRi = gtsam.Rot3.Yaw(
+            -theta)  # negative yaw goes counterclockwise, with Z down !
+        gTi = gtsam.Pose3(gRo.compose(oRi), gti)
+        values.insert(key, gTi)
+        theta = theta + dtheta
+    return values

python/gtsam_py/python/utils/logging_optimizer.py

@@ -0,0 +1,52 @@
+"""
+Optimization with logging via a hook.
+Author: Jing Wu and Frank Dellaert
+"""
+# pylint: disable=invalid-name
+
+from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
+import gtsam
+
+
+def optimize(optimizer, check_convergence, hook):
+    """ Given an optimizer and a convergence check, iterate until convergence.
+        After each iteration, hook(optimizer, error) is called.
+        After the function, use values and errors to get the result.
+        Arguments:
+            optimizer (T): needs an iterate and an error function.
+            check_convergence: T * float * float -> bool
+            hook -- hook function to record the error
+    """
+    # the optimizer is created with default values which incur the error below
+    current_error = optimizer.error()
+    hook(optimizer, current_error)
+
+    # Iterative loop
+    while True:
+        # Do next iteration
+        optimizer.iterate()
+        new_error = optimizer.error()
+        hook(optimizer, new_error)
+        if check_convergence(optimizer, current_error, new_error):
+            return
+        current_error = new_error
+
+
+def gtsam_optimize(optimizer,
+                   params,
+                   hook):
+    """ Given an optimizer and params, iterate until convergence.
+        After each iteration, hook(optimizer) is called.
+        After the function, use values and errors to get the result.
+        Arguments:
+                optimizer {NonlinearOptimizer} -- Nonlinear optimizer
+                params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
+                hook -- hook function to record the error
+    """
+    def check_convergence(optimizer, current_error, new_error):
+        return (optimizer.iterations() >= params.getMaxIterations()) or (
+            gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
+                                   current_error, new_error)) or (
+            isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
+    optimize(optimizer, check_convergence, hook)
+    return optimizer.values()

python/gtsam_py/python/utils/plot.py

@@ -0,0 +1,369 @@
+"""Various plotting utlities."""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import patches
+from mpl_toolkits.mplot3d import Axes3D
+
+import gtsam
+
+
+def set_axes_equal(fignum):
+    """
+    Make axes of 3D plot have equal scale so that spheres appear as spheres,
+    cubes as cubes, etc..  This is one possible solution to Matplotlib's
+    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
+
+    Args:
+      fignum (int): An integer representing the figure number for Matplotlib.
+    """
+    fig = plt.figure(fignum)
+    ax = fig.gca(projection='3d')
+
+    limits = np.array([
+        ax.get_xlim3d(),
+        ax.get_ylim3d(),
+        ax.get_zlim3d(),
+    ])
+
+    origin = np.mean(limits, axis=1)
+    radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
+
+    ax.set_xlim3d([origin[0] - radius, origin[0] + radius])
+    ax.set_ylim3d([origin[1] - radius, origin[1] + radius])
+    ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
+
+
+def ellipsoid(xc, yc, zc, rx, ry, rz, n):
+    """
+    Numpy equivalent of Matlab's ellipsoid function.
+
+    Args:
+        xc (double): Center of ellipsoid in X-axis.
+        yc (double): Center of ellipsoid in Y-axis.
+        zc (double): Center of ellipsoid in Z-axis.
+        rx (double): Radius of ellipsoid in X-axis.
+        ry (double): Radius of ellipsoid in Y-axis.
+        rz (double): Radius of ellipsoid in Z-axis.
+        n (int): The granularity of the ellipsoid plotted. 
+
+    Returns:
+        tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
+    """
+    u = np.linspace(0, 2*np.pi, n+1)
+    v = np.linspace(0, np.pi, n+1)
+    x = -rx * np.outer(np.cos(u), np.sin(v)).T
+    y = -ry * np.outer(np.sin(u), np.sin(v)).T
+    z = -rz * np.outer(np.ones_like(u), np.cos(v)).T
+
+    return x, y, z
+
+
+def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
+    """
+    Plots a Gaussian as an uncertainty ellipse
+
+    Based on Maybeck Vol 1, page 366
+    k=2.296 corresponds to 1 std, 68.26% of all probability
+    k=11.82 corresponds to 3 std, 99.74% of all probability
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        origin (gtsam.Point3): The origin in the world frame.
+        P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
+        scale (float): Scaling factor of the radii of the covariance ellipse.
+        n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
+        alpha (float): Transparency value for the plotted surface in the range [0, 1].
+    """
+    k = 11.82
+    U, S, _ = np.linalg.svd(P)
+
+    radii = k * np.sqrt(S)
+    radii = radii * scale
+    rx, ry, rz = radii
+
+    # generate data for "unrotated" ellipsoid
+    xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n)
+
+    # rotate data with orientation matrix U and center c
+    data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
+        np.kron(U[:, 2:3], zc)
+    n = data.shape[1]
+    x = data[0:n, :] + origin[0]
+    y = data[n:2*n, :] + origin[1]
+    z = data[2*n:, :] + origin[2]
+
+    axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
+
+
+def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
+    """
+    Plot a 2D pose on given axis `axes` with given `axis_length`.
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        pose (gtsam.Pose2): The pose to be plotted.
+        axis_length (float): The length of the camera axes.
+        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+    """
+    # get rotation and translation (center)
+    gRp = pose.rotation().matrix()  # rotation from pose to global
+    t = pose.translation()
+    origin = np.array([t.x(), t.y()])
+
+    # draw the camera axes
+    x_axis = origin + gRp[:, 0] * axis_length
+    line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], 'r-')
+
+    y_axis = origin + gRp[:, 1] * axis_length
+    line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], 'g-')
+
+    if covariance is not None:
+        pPp = covariance[0:2, 0:2]
+        gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
+
+        w, v = np.linalg.eig(gPp)
+
+        # k = 2.296
+        k = 5.0
+
+        angle = np.arctan2(v[1, 0], v[0, 0])
+        e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k),
+                             np.rad2deg(angle), fill=False)
+        axes.add_patch(e1)
+
+
+def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
+               axis_labels=('X axis', 'Y axis', 'Z axis')):
+    """
+    Plot a 2D pose on given figure with given `axis_length`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        pose (gtsam.Pose2): The pose to be plotted.
+        axis_length (float): The length of the camera axes.
+        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        axis_labels (iterable[string]): List of axis labels to set.
+    """
+    # get figure object
+    fig = plt.figure(fignum)
+    axes = fig.gca()
+    plot_pose2_on_axes(axes, pose, axis_length=axis_length,
+                       covariance=covariance)
+
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
+
+    return fig
+
+
+def plot_point3_on_axes(axes, point, linespec, P=None):
+    """
+    Plot a 3D point on given axis `axes` with given `linespec`.
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        point (gtsam.Point3): The point to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+    """
+    axes.plot([point.x()], [point.y()], [point.z()], linespec)
+    if P is not None:
+        plot_covariance_ellipse_3d(axes, point.vector(), P)
+
+
+def plot_point3(fignum, point, linespec, P=None,
+                axis_labels=('X axis', 'Y axis', 'Z axis')):
+    """
+    Plot a 3D point on given figure with given `linespec`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        point (gtsam.Point3): The point to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        axis_labels (iterable[string]): List of axis labels to set.
+
+    Returns:
+        fig: The matplotlib figure.
+
+    """
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plot_point3_on_axes(axes, point, linespec, P)
+
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
+
+    return fig
+
+
+def plot_3d_points(fignum, values, linespec="g*", marginals=None,
+                   title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
+    """
+    Plots the Point3s in `values`, with optional covariances.
+    Finds all the Point3 objects in the given Values object and plots them.
+    If a Marginals object is given, this function will also plot marginal
+    covariance ellipses for each point.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        values (gtsam.Values): Values dictionary consisting of points to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        title (string): The title of the plot.
+        axis_labels (iterable[string]): List of axis labels to set.
+    """
+
+    keys = values.keys()
+
+    # Plot points and covariance matrices
+    for i in range(keys.size()):
+        try:
+            key = keys.at(i)
+            point = values.atPoint3(key)
+            if marginals is not None:
+                covariance = marginals.marginalCovariance(key)
+            else:
+                covariance = None
+
+            fig = plot_point3(fignum, point, linespec, covariance,
+                              axis_labels=axis_labels)
+
+        except RuntimeError:
+            continue
+            # I guess it's not a Point3
+
+    fig.suptitle(title)
+    fig.canvas.set_window_title(title.lower())
+
+
+def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
+    """
+    Plot a 3D pose on given axis `axes` with given `axis_length`.
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        point (gtsam.Point3): The point to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+    """
+    # get rotation and translation (center)
+    gRp = pose.rotation().matrix()  # rotation from pose to global
+    origin = pose.translation().vector()
+
+    # draw the camera axes
+    x_axis = origin + gRp[:, 0] * axis_length
+    line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')
+
+    y_axis = origin + gRp[:, 1] * axis_length
+    line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')
+
+    z_axis = origin + gRp[:, 2] * axis_length
+    line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
+    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
+
+    # plot the covariance
+    if P is not None:
+        # covariance matrix in pose coordinate frame
+        pPp = P[3:6, 3:6]
+        # convert the covariance matrix to global coordinate frame
+        gPp = gRp @ pPp @ gRp.T
+        plot_covariance_ellipse_3d(axes, origin, gPp)
+
+
+def plot_pose3(fignum, pose, axis_length=0.1, P=None,
+               axis_labels=('X axis', 'Y axis', 'Z axis')):
+    """
+    Plot a 3D pose on given figure with given `axis_length`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        pose (gtsam.Pose3): 3D pose to be plotted.
+        linespec (string): String representing formatting options for Matplotlib.
+        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        axis_labels (iterable[string]): List of axis labels to set.
+
+    Returns:
+        fig: The matplotlib figure.
+    """
+    # get figure object
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plot_pose3_on_axes(axes, pose, P=P,
+                       axis_length=axis_length)
+
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
+
+    return fig
+
+
+def plot_trajectory(fignum, values, scale=1, marginals=None,
+                    title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
+    """
+    Plot a complete 3D trajectory using poses in `values`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        values (gtsam.Values): Values dict containing the poses.
+        scale (float): Value to scale the poses by.
+        marginals (gtsam.Marginals): Marginalized probability values of the estimation.
+            Used to plot uncertainty bounds.
+        title (string): The title of the plot.
+        axis_labels (iterable[string]): List of axis labels to set.
+    """
+    pose3Values = gtsam.utilities_allPose3s(values)
+    keys = gtsam.KeyVector(pose3Values.keys())
+    lastIndex = None
+
+    for i in range(keys.size()):
+        key = keys.at(i)
+        try:
+            pose = pose3Values.atPose3(key)
+        except:
+            print("Warning: no Pose3 at key: {0}".format(key))
+
+        if lastIndex is not None:
+            lastKey = keys.at(lastIndex)
+            try:
+                lastPose = pose3Values.atPose3(lastKey)
+            except:
+                print("Warning: no Pose3 at key: {0}".format(lastKey))
+                pass
+
+            if marginals:
+                covariance = marginals.marginalCovariance(lastKey)
+            else:
+                covariance = None
+
+            fig = plot_pose3(fignum, lastPose,  P=covariance,
+                             axis_length=scale, axis_labels=axis_labels)
+
+        lastIndex = i
+
+    # Draw final pose
+    if lastIndex is not None:
+        lastKey = keys.at(lastIndex)
+        try:
+            lastPose = pose3Values.atPose3(lastKey)
+            if marginals:
+                covariance = marginals.marginalCovariance(lastKey)
+            else:
+                covariance = None
+
+            fig = plot_pose3(fignum, lastPose, P=covariance,
+                             axis_length=scale, axis_labels=axis_labels)
+
+        except:
+            pass
+
+    fig.suptitle(title)
+    fig.canvas.set_window_title(title.lower())

python/gtsam_py/python/utils/test_case.py

@@ -0,0 +1,27 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+TestCase class with GTSAM assert utils.
+Author: Frank Dellaert
+"""
+import unittest
+
+
+class GtsamTestCase(unittest.TestCase):
+    """TestCase class with GTSAM assert utils."""
+
+    def gtsamAssertEquals(self, actual, expected, tol=1e-9):
+        """ AssertEqual function that prints out actual and expected if not equal.
+            Usage:
+                self.gtsamAssertEqual(actual,expected)
+            Keyword Arguments:
+                tol {float} -- tolerance passed to 'equals', default 1e-9
+        """
+        equal = actual.equals(expected, tol)
+        if not equal:
+            raise self.failureException(
+                "Values are not equal:\n{}!={}".format(actual, expected))

python/gtsam_py/python/utils/visual_data_generator.py

@@ -0,0 +1,118 @@
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
+
+
+class Options:
+    """
+    Options to generate test scenario
+    """
+
+    def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()):
+        """
+        Options to generate test scenario
+        @param triangle: generate a triangle scene with 3 points if True, otherwise
+                  a cube with 8 points
+        @param nrCameras: number of cameras to generate
+        @param K: camera calibration object
+        """
+        self.triangle = triangle
+        self.nrCameras = nrCameras
+
+
+class GroundTruth:
+    """
+    Object holding generated ground-truth data
+    """
+
+    def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
+        self.K = K
+        self.cameras = [Pose3()] * nrCameras
+        self.points = [Point3()] * nrPoints
+
+    def print_(self, s=""):
+        print(s)
+        print("K = ", self.K)
+        print("Cameras: ", len(self.cameras))
+        for camera in self.cameras:
+            print("\t", camera)
+        print("Points: ", len(self.points))
+        for point in self.points:
+            print("\t", point)
+        pass
+
+
+class Data:
+    """
+    Object holding generated measurement data
+    """
+
+    class NoiseModels:
+        pass
+
+    def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
+        self.K = K
+        self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
+        self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
+        self.odometry = [Pose3()] * nrCameras
+
+        # Set Noise parameters
+        self.noiseModels = Data.NoiseModels()
+        self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
+        # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
+        #    np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
+        self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
+        self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
+        self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)
+
+
+def generate_data(options):
+    """ Generate ground-truth and measurement data. """
+
+    K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.)
+    nrPoints = 3 if options.triangle else 8
+
+    truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
+    data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints)
+
+    # Generate simulated data
+    if options.triangle:  # Create a triangle target, just 3 points on a plane
+        r = 10
+        for j in range(len(truth.points)):
+            theta = j * 2 * np.pi / nrPoints
+            truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
+    else:  # 3D landmarks as vertices of a cube
+        truth.points = [
+            Point3(10, 10, 10), Point3(-10, 10, 10),
+            Point3(-10, -10, 10), Point3(10, -10, 10),
+            Point3(10, 10, -10), Point3(-10, 10, -10),
+            Point3(-10, -10, -10), Point3(10, -10, -10)
+        ]
+
+    # Create camera cameras on a circle around the triangle
+    height = 10
+    r = 40
+    for i in range(options.nrCameras):
+        theta = i * 2 * np.pi / options.nrCameras
+        t = Point3(r * np.cos(theta), r * np.sin(theta), height)
+        truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
+                                                       Point3(),
+                                                       Point3(0, 0, 1),
+                                                       truth.K)
+        # Create measurements
+        for j in range(nrPoints):
+            # All landmarks seen in every frame
+            data.Z[i][j] = truth.cameras[i].project(truth.points[j])
+            data.J[i][j] = j
+
+    # Calculate odometry between cameras
+    for i in range(1, options.nrCameras):
+        data.odometry[i] = truth.cameras[i - 1].pose().between(
+            truth.cameras[i].pose())
+
+    return data, truth

python/gtsam_py/python/utils/visual_isam.py

@@ -0,0 +1,131 @@
+import gtsam
+from gtsam import symbol
+
+
+class Options:
+    """ Options for visual isam example. """
+
+    def __init__(self):
+        self.hardConstraint = False
+        self.pointPriors = False
+        self.batchInitialization = True
+        self.reorderInterval = 10
+        self.alwaysRelinearize = False
+
+
+def initialize(data, truth, options):
+    # Initialize iSAM
+    params = gtsam.ISAM2Params()
+    if options.alwaysRelinearize:
+        params.setRelinearizeSkip(1)
+    isam = gtsam.ISAM2(params=params)
+
+    # Add constraints/priors
+    # TODO: should not be from ground truth!
+    newFactors = gtsam.NonlinearFactorGraph()
+    initialEstimates = gtsam.Values()
+    for i in range(2):
+        ii = symbol(ord('x'), i)
+        if i == 0:
+            if options.hardConstraint:  # add hard constraint
+                newFactors.add(
+                    gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
+            else:
+                newFactors.add(
+                    gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
+                                           data.noiseModels.posePrior))
+        initialEstimates.insert(ii, truth.cameras[i].pose())
+
+    nextPoseIndex = 2
+
+    # Add visual measurement factors from two first poses and initialize
+    # observed landmarks
+    for i in range(2):
+        ii = symbol(ord('x'), i)
+        for k in range(len(data.Z[i])):
+            j = data.J[i][k]
+            jj = symbol(ord('l'), j)
+            newFactors.add(
+                gtsam.GenericProjectionFactorCal3_S2(data.Z[i][
+                    k], data.noiseModels.measurement, ii, jj, data.K))
+            # TODO: initial estimates should not be from ground truth!
+            if not initialEstimates.exists(jj):
+                initialEstimates.insert(jj, truth.points[j])
+            if options.pointPriors:  # add point priors
+                newFactors.add(
+                    gtsam.PriorFactorPoint3(jj, truth.points[j],
+                                            data.noiseModels.pointPrior))
+
+    # Add odometry between frames 0 and 1
+    newFactors.add(
+        gtsam.BetweenFactorPose3(
+            symbol(ord('x'), 0),
+            symbol(ord('x'), 1), data.odometry[1], data.noiseModels.odometry))
+
+    # Update ISAM
+    if options.batchInitialization:  # Do a full optimize for first two poses
+        batchOptimizer = gtsam.LevenbergMarquardtOptimizer(newFactors,
+                                                           initialEstimates)
+        fullyOptimized = batchOptimizer.optimize()
+        isam.update(newFactors, fullyOptimized)
+    else:
+        isam.update(newFactors, initialEstimates)
+
+    # figure(1)tic
+    # t=toc plot(frame_i,t,'r.') tic
+    result = isam.calculateEstimate()
+    # t=toc plot(frame_i,t,'g.')
+
+    return isam, result, nextPoseIndex
+
+
+def step(data, isam, result, truth, currPoseIndex):
+    '''
+    Do one step isam update
+    @param[in] data: measurement data (odometry and visual measurements and their noiseModels)
+    @param[in/out] isam: current isam object, will be updated
+    @param[in/out] result: current result object, will be updated
+    @param[in] truth: ground truth data, used to initialize new variables
+    @param[currPoseIndex]: index of the current pose
+    '''
+    # iSAM expects us to give it a new set of factors
+    # along with initial estimates for any new variables introduced.
+    newFactors = gtsam.NonlinearFactorGraph()
+    initialEstimates = gtsam.Values()
+
+    # Add odometry
+    prevPoseIndex = currPoseIndex - 1
+    odometry = data.odometry[prevPoseIndex]
+    newFactors.add(
+        gtsam.BetweenFactorPose3(
+            symbol(ord('x'), prevPoseIndex),
+            symbol(ord('x'), currPoseIndex), odometry,
+            data.noiseModels.odometry))
+
+    # Add visual measurement factors and initializations as necessary
+    for k in range(len(data.Z[currPoseIndex])):
+        zij = data.Z[currPoseIndex][k]
+        j = data.J[currPoseIndex][k]
+        jj = symbol(ord('l'), j)
+        newFactors.add(
+            gtsam.GenericProjectionFactorCal3_S2(
+                zij, data.noiseModels.measurement,
+                symbol(ord('x'), currPoseIndex), jj, data.K))
+        # TODO: initialize with something other than truth
+        if not result.exists(jj) and not initialEstimates.exists(jj):
+            lmInit = truth.points[j]
+            initialEstimates.insert(jj, lmInit)
+
+    # Initial estimates for the new pose.
+    prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
+    initialEstimates.insert(
+        symbol(ord('x'), currPoseIndex), prevPose.compose(odometry))
+
+    # Update ISAM
+    # figure(1)tic
+    isam.update(newFactors, initialEstimates)
+    # t=toc plot(frame_i,t,'r.') tic
+    newResult = isam.calculateEstimate()
+    # t=toc plot(frame_i,t,'g.')
+
+    return isam, newResult