Merge pull request #438 from borglab/temp/new_python

Replace with new python tests

cython/gtsam/examples/SFMExample.py

@@ -20,7 +20,7 @@ from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
                          GenericProjectionFactorCal3_S2, Marginals,
                          NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
                          Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
-                         SimpleCamera, Values)
+                         PinholeCameraCal3_S2, Values)
 from gtsam.utils import plot
 
 

python/gtsam/__init__.py

@@ -0,0 +1,27 @@
+from .gtsam import *
+
+
+def _init():
+    """This function is to add shims for the long-gone Point2 and Point3 types"""
+
+    import numpy as np
+
+    global Point2  # export function
+
+    def Point2(x=0, y=0):
+        """Shim for the deleted Point2 type."""
+        return np.array([x, y], dtype=float)
+
+    global Point3  # export function
+
+    def Point3(x=0, y=0, z=0):
+        """Shim for the deleted Point3 type."""
+        return np.array([x, y, z], dtype=float)
+
+    # for interactive debugging
+    if __name__ == "__main__":
+        # we want all definitions accessible
+        globals().update(locals())
+
+
+_init()

python/gtsam/examples/DogLegOptimizerExample.py

@@ -35,17 +35,17 @@ def run(args):
     graph = gtsam.NonlinearFactorGraph()
 
     # Priors
-    prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
+    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]))
+    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)
+    model_rho = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)
     graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
 
     params = gtsam.DoglegParams()

python/gtsam/examples/GPSFactorExample.py

@@ -26,8 +26,8 @@ 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)
+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()

python/gtsam/examples/ImuFactorExample.py

@@ -0,0 +1,180 @@
+"""
+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 argparse
+import math
+
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam.symbol_shorthand import B, V, 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, twist_scenario="sick_twist"):
+        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:
+        twist_scenarios = dict(
+            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__(twist_scenarios[twist_scenario],
+                                               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, T=12, compute_covariances=False, verbose=True):
+        graph = gtsam.NonlinearFactorGraph()
+
+        # initialize data structure for pre-integrated IMU measurements
+        pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
+
+        T = 12
+        num_poses = T  # assumes 1 factor per second
+        initial = gtsam.Values()
+        initial.insert(BIAS_KEY, self.actualBias)
+
+        # simulate the loop
+        i = 0  # state index
+        initial_state_i = self.scenario.navState(0)
+        initial.insert(X(i), initial_state_i.pose())
+        initial.insert(V(i), initial_state_i.velocity())
+
+        # add prior on beginning
+        self.addPrior(0, graph)
+
+        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)
+
+            # Plot IMU many times
+            if k % 10 == 0:
+                self.plotImu(t, measuredOmega, measuredAcc)
+
+            if (k+1) % int(1 / self.dt) == 0:
+                # Plot every second
+                self.plotGroundTruthPose(t, scale=1)
+                plt.title("Ground Truth Trajectory")
+
+                # create IMU factor every second
+                factor = gtsam.ImuFactor(X(i), V(i),
+                                         X(i + 1), V(i + 1),
+                                         BIAS_KEY, pim)
+                graph.push_back(factor)
+
+                if verbose:
+                    print(factor)
+                    print(pim.predict(actual_state_i, self.actualBias))
+
+                pim.resetIntegration()
+
+                rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3)*0.1)
+                translationNoise = gtsam.Point3(*np.random.randn(3)*1)
+                poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
+
+                actual_state_i = self.scenario.navState(t + self.dt)
+                print("Actual state at {0}:\n{1}".format(
+                    t+self.dt, actual_state_i))
+
+                noisy_state_i = gtsam.NavState(
+                    actual_state_i.pose().compose(poseNoise),
+                    actual_state_i.velocity() + np.random.randn(3)*0.1)
+
+                initial.insert(X(i+1), noisy_state_i.pose())
+                initial.insert(V(i+1), noisy_state_i.velocity())
+                i += 1
+
+        # add priors on end
+        # self.addPrior(num_poses - 1, graph)
+
+        initial.print_("Initial values:")
+
+        # optimize using Levenberg-Marquardt optimization
+        params = gtsam.LevenbergMarquardtParams()
+        params.setVerbosityLM("SUMMARY")
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+
+        result.print_("Optimized values:")
+
+        if compute_covariances:
+            # 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+1, pose_i, 1)
+            i += 1
+        plt.title("Estimated Trajectory")
+
+        gtsam.utils.plot.set_axes_equal(POSES_FIG+1)
+
+        print("Bias Values", result.atConstantBias(BIAS_KEY))
+
+        plt.ioff()
+        plt.show()
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser("ImuFactorExample.py")
+    parser.add_argument("--twist_scenario",
+                        default="sick_twist",
+                        choices=("zero_twist", "forward_twist", "loop_twist", "sick_twist"))
+    parser.add_argument("--time", "-T", default=12,
+                        type=int, help="Total time in seconds")
+    parser.add_argument("--compute_covariances",
+                        default=False, action='store_true')
+    parser.add_argument("--verbose", default=False, action='store_true')
+    args = parser.parse_args()
+
+    ImuFactorExample(args.twist_scenario).run(
+        args.time, args.compute_covariances, args.verbose)

python/gtsam/examples/ImuFactorISAM2Example.py

@@ -1,6 +1,6 @@
 """
 iSAM2 example with ImuFactor.
-Author: Robert Truax (C++), Frank Dellaert (Python)
+Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal
 """
 # pylint: disable=invalid-name, E1101
 
@@ -9,7 +9,6 @@ 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,
@@ -17,9 +16,8 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
                    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 gtsam.symbol_shorthand import B, V, X
+from gtsam.utils import plot
 from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
 
@@ -28,46 +26,45 @@ def vector3(x, y, z):
     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 preintegration_parameters():
+    # IMU preintegration parameters
+    # Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
+    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))
+
+    return PARAMS, BIAS_COVARIANCE, DELTA
+
+
+def get_camera(radius):
+    up = Point3(0, 0, 1)
+    target = Point3(0, 0, 0)
+    position = Point3(radius, 0, 0)
+    camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
+    return camera
+
+
+def get_scenario(radius, pose_0, angular_velocity, delta_t):
+    """Create the set of ground-truth landmarks and poses"""
+
+    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)
+
+    return scenario
 
 
 def IMU_example():
@@ -75,23 +72,17 @@ def IMU_example():
 
     # 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())
+    camera = get_camera(radius)
     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 = math.radians(180)  # rad/sec
+    scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
 
-    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)
+    PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
 
     # Create a factor graph
-    newgraph = NonlinearFactorGraph()
+    graph = NonlinearFactorGraph()
 
     # Create (incremental) ISAM2 solver
     isam = ISAM2()
@@ -102,23 +93,23 @@ def IMU_example():
 
     # 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(
+    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))
+    graph.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 = 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)
+    graph.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)
+    graph.push_back(velprior)
     initialEstimate.insert(V(0), n_velocity)
 
     accum = gtsam.PreintegratedImuMeasurements(PARAMS)
@@ -139,9 +130,9 @@ def IMU_example():
             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())
+                    biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE)
+                graph.add(factor)
+                initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
 
             # Predict acceleration and gyro measurements in (actual) body frame
             nRb = scenario.rotation(t).matrix()
@@ -152,21 +143,24 @@ def IMU_example():
 
             # Add Imu Factor
             imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
-            newgraph.add(imufac)
+            graph.add(imufac)
 
             # insert new velocity, which is wrong
             initialEstimate.insert(V(i), n_velocity)
             accum.resetIntegration()
 
         # Incremental solution
-        isam.update(newgraph, initialEstimate)
+        isam.update(graph, initialEstimate)
         result = isam.calculateEstimate()
-        ISAM2_plot(result)
+        plot.plot_incremental_trajectory(0, result,
+                                         start=i, scale=3, time_interval=0.01)
 
         # reset
-        newgraph = NonlinearFactorGraph()
+        graph = NonlinearFactorGraph()
         initialEstimate.clear()
 
+    plt.show()
+
 
 if __name__ == '__main__':
     IMU_example()

python/gtsam/examples/OdometryExample.py

@@ -21,8 +21,8 @@ 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]))
+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()

python/gtsam/examples/PlanarManipulatorExample.py

@@ -167,13 +167,11 @@ class ThreeLinkArm(object):
         axes = fig.gca()
 
         sXl1 = Pose2(0, 0, math.radians(90))
-        t = sXl1.translation()
-        p1 = np.array([t.x(), t.y()])
+        p1 = sXl1.translation()
         gtsam_plot.plot_pose2_on_axes(axes, sXl1)
 
         def plot_line(p, g, color):
-            t = g.translation()
-            q = np.array([t.x(), t.y()])
+            q = g.translation()
             line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
             axes.plot(line[:, 0], line[:, 1], color)
             return q

python/gtsam/examples/PlanarSLAMExample.py

@@ -13,15 +13,15 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
 
 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
+
+import gtsam
+from gtsam.symbol_shorthand import X, L
 
 # 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]))
+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()

python/gtsam/examples/Pose2SLAMExample.py

@@ -27,10 +27,9 @@ 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))
+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()

python/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -53,7 +53,7 @@ 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))
+priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
 graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
 
 params = gtsam.GaussNewtonParams()
@@ -82,7 +82,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.utilities_extractPose2(result)
+    resultPoses = gtsam.utilities.extractPose2(result)
     for i in range(resultPoses.shape[0]):
         plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
     plt.show()

python/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -39,11 +39,11 @@ 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,
+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)
+firstKey = initial.keys()[0]
 graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
@@ -65,7 +65,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.utilities_allPose3s(result)
+    resultPoses = gtsam.utilities.allPose3s(result)
     for i in range(resultPoses.size()):
         plot.plot_pose3(1, resultPoses.atPose3(i))
     plt.show()

python/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py

@@ -24,9 +24,9 @@ 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(
+priorModel = gtsam.noiseModel.Diagonal.Variances(
     np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
-firstKey = initial.keys().at(0)
+firstKey = initial.keys()[0]
 graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
 
 # Initializing Pose3 - chordal relaxation"

python/gtsam/examples/PreintegrationExample.py

@@ -10,12 +10,11 @@ A script validating the Preintegration of IMU measurements
 
 import math
 
+import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
-from mpl_toolkits.mplot3d import Axes3D
-
-import gtsam
 from gtsam.utils.plot import plot_pose3
+from mpl_toolkits.mplot3d import Axes3D
 
 IMU_FIG = 1
 POSES_FIG = 2
@@ -68,60 +67,62 @@ class PreintegrationExample(object):
         else:
             accBias = np.array([0, 0.1, 0])
             gyroBias = np.array([0, 0, 0])
-            self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)
+            self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias)
 
         self.runner = gtsam.ScenarioRunner(
             self.scenario, self.params, self.dt, self.actualBias)
 
+        fig, self.axes = plt.subplots(4, 3)
+        fig.set_tight_layout(True)
+
     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)
+            ax = self.axes[0][i]
+            ax.scatter(t, omega_b[i], color='k', marker='.')
+            ax.scatter(t, measuredOmega[i], color=color, marker='.')
+            ax.set_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)
+            ax = self.axes[1][i]
+            ax.scatter(t, acceleration_n[i], color=color, marker='.')
+            ax.set_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)
+            ax = self.axes[2][i]
+            ax.scatter(t, acceleration_b[i], color=color, marker='.')
+            ax.set_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)
+            ax = self.axes[3][i]
+            ax.scatter(t, actual[i], color='k', marker='.')
+            ax.scatter(t, measuredAcc[i], color=color, marker='.')
+            ax.set_xlabel('specific force ' + label)
 
-    def plotGroundTruthPose(self, t):
+    def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
         # 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])
+        self.maxDim = max([max(np.abs(t)), 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)
+        plt.pause(time_interval)
 
-    def run(self):
+    def run(self, T=12):
         # 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)

python/gtsam/examples/README.md

@@ -1,17 +1,12 @@
-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 |
+| DiscreteBayesNet_FG                                   | none of the required discrete functionality is exposed through Python |
+| easyPoint2KalmanFilter                                | ExtendedKalmanFilter not yet exposed through Python |
+| elaboratePoint2KalmanFilter                           | GaussianSequentialSolver not yet exposed through Python |
 | ImuFactorExample2                                     | X      |
 | ImuFactorsExample                                     |        |
 | ISAM2Example_SmartFactor                              |        |
@@ -25,7 +20,7 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan
 | Pose2SLAMExample_g2o                                  | X      |
 | Pose2SLAMExample_graph                                |        |
 | Pose2SLAMExample_graphviz                             |        |
-| Pose2SLAMExample_lago                                 | lago not exposed through cython |
+| Pose2SLAMExample_lago                                 | lago not yet exposed through Python |
 | Pose2SLAMStressTest                                   |        |
 | Pose2SLAMwSPCG                                        |        |
 | Pose3SLAMExample_changeKeys                           |        |
@@ -47,11 +42,11 @@ Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthan
 | StereoVOExample                                       |        |
 | StereoVOExample_large                                 |        |
 | TimeTBB                                               |        |
-| UGM_chain                                             | discrete functionality not exposed |
-| UGM_small                                             | discrete functionality not exposed |
+| UGM_chain                                             | discrete functionality not yet exposed |
+| UGM_small                                             | discrete functionality not yet exposed |
 | VisualISAM2Example                                    | X      |
 | VisualISAMExample                                     | X      |
 
 Extra Examples (with no C++ equivalent)
 - PlanarManipulatorExample
-- SFMData
+- SFMData

python/gtsam/examples/SFMExample.py

@@ -13,14 +13,15 @@ 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 import symbol_shorthand
+L = symbol_shorthand.L
+X = symbol_shorthand.X
+
 from gtsam.examples import SFMdata
-from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
+from gtsam import (Cal3_S2, DoglegOptimizer,
                          GenericProjectionFactorCal3_S2, Marginals,
                          NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
-                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
-                         SimpleCamera, Values)
+                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)
 from gtsam.utils import plot
 
 
@@ -56,7 +57,7 @@ def main():
     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
+    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()
@@ -69,7 +70,7 @@ def main():
 
     # 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]))
+    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)
 
@@ -85,7 +86,7 @@ def main():
     # 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)
+    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')
@@ -97,7 +98,7 @@ def main():
         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))
+        transformed_point = point + 0.1*np.random.randn(3)
         initial_estimate.insert(L(j), transformed_point)
     initial_estimate.print_('Initial Estimates:\n')
 

python/gtsam/examples/SFMdata.py

@@ -33,7 +33,8 @@ def createPoses(K):
     poses = []
     for theta in angles:
         position = gtsam.Point3(radius*np.cos(theta),
-                                radius*np.sin(theta), height)
+                                radius*np.sin(theta),
+                                height)
         camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
         poses.append(camera.pose())
     return poses

python/gtsam/examples/SimpleRotation.py

@@ -10,10 +10,9 @@ 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
-
+import gtsam
+from gtsam.symbol_shorthand import X
 
 def main():
     """
@@ -33,7 +32,7 @@ def main():
     """
     prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
     prior.print_('goal angle')
-    model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
+    model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
     key = X(1)
     factor = gtsam.PriorFactorRot2(key, prior, model)
 

python/gtsam/examples/VisualISAM2Example.py

@@ -17,8 +17,7 @@ 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.symbol_shorthand import L, X
 from gtsam.examples import SFMdata
 from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
@@ -64,7 +63,7 @@ def visual_ISAM2_example():
     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(
+    measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
         2, 1.0)  # one pixel in u and v
 
     # Create the set of ground-truth landmarks
@@ -110,12 +109,12 @@ def visual_ISAM2_example():
         # 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(
+            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)
+            point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
             graph.push_back(gtsam.PriorFactorPoint3(
                 L(0), points[0], point_noise))  # add directly to graph
 
@@ -123,7 +122,7 @@ def visual_ISAM2_example():
             # 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))
+                    point[0]-0.25, point[1]+0.20, point[2]+0.15))
         else:
             # Update iSAM with the new factors
             isam.update(graph, initial_estimate)

python/gtsam/examples/VisualISAMExample.py

@@ -15,13 +15,11 @@ 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
-
+from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
+                   NonlinearFactorGraph, NonlinearISAM, Pose3,
+                   PriorFactorPoint3, PriorFactorPose3, Rot3,
+                   PinholeCameraCal3_S2, Values, Point3)
+from gtsam.symbol_shorthand import X, L
 
 def main():
     """
@@ -34,7 +32,8 @@ def main():
     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
+    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()
@@ -55,11 +54,13 @@ def main():
         # 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)
+            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))
+        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
@@ -71,12 +72,13 @@ def main():
         # 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]))
+            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)
+            point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
             factor = PriorFactorPoint3(L(0), points[0], point_noise)
             graph.push_back(factor)
 
@@ -84,8 +86,8 @@ def main():
             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))
+                initial_lj = points[j] + noise
+                initial_estimate.insert(L(j), initial_lj)
         else:
             # Update iSAM with the new factors
             isam.update(graph, initial_estimate)

python/gtsam/imuBias.py

@@ -0,0 +1 @@
+from .gtsam.imuBias import *

python/gtsam/noiseModel.py

@@ -0,0 +1 @@
+from .gtsam.noiseModel import *

python/gtsam/symbol_shorthand.py

@@ -0,0 +1 @@
+from .gtsam.symbol_shorthand import *

python/gtsam/tests/testScenarioRunner.py

@@ -32,7 +32,7 @@ class TestScenarioRunner(GtsamTestCase):
 
         dt = 0.1
         params = gtsam.PreintegrationParams.MakeSharedU(self.g)
-        bias = gtsam.imuBias_ConstantBias()
+        bias = gtsam.imuBias.ConstantBias()
         runner = gtsam.ScenarioRunner(
             scenario, params, dt, bias)
 

python/gtsam/tests/test_GaussianFactorGraph.py

@@ -16,7 +16,7 @@ import unittest
 
 import gtsam
 import numpy as np
-from gtsam import symbol_shorthand_X as X
+from gtsam.symbol_shorthand import X
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -28,8 +28,8 @@ def create_graph():
     x1 = X(1)
     x2 = X(2)
     
-    BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
-    PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
+    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)

python/gtsam/tests/test_JacobianFactor.py

@@ -48,7 +48,7 @@ class TestJacobianFactor(GtsamTestCase):
         # the RHS
         b2 = np.array([-1., 1.5, 2., -1.])
         sigmas = np.array([1., 1., 1., 1.])
-        model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas)
+        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
@@ -66,7 +66,7 @@ class TestJacobianFactor(GtsamTestCase):
                         [+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))
+            x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel.Unit.Create(2))
         # check if the result matches
         self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
 
@@ -82,7 +82,7 @@ class TestJacobianFactor(GtsamTestCase):
         # the RHS
         b1 = np.array([0.0, 0.894427])
 
-        model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.]))
+        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

python/gtsam/tests/test_KalmanFilter.py

@@ -22,13 +22,13 @@ class TestKalmanFilter(GtsamTestCase):
         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]))
+        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]))
+        modelR = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))
         R = 0.01 * np.eye(2)
 
         # Create the set of expected output TestValues

python/gtsam/tests/test_KarcherMeanFactor.py

@@ -18,7 +18,7 @@ import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY = 0
-MODEL = gtsam.noiseModel_Unit.Create(3)
+MODEL = gtsam.noiseModel.Unit.Create(3)
 
 
 def find_Karcher_mean_Rot3(rotations):
@@ -59,8 +59,8 @@ class TestKarcherMean(GtsamTestCase):
         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)
+        keys.append(1)
+        keys.append(2)
         graph.add(gtsam.KarcherMeanFactorRot3(keys))
 
         initial = gtsam.Values()

python/gtsam/tests/test_LocalizationExample.py

@@ -26,7 +26,7 @@ class TestLocalizationExample(GtsamTestCase):
         # 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(
+        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))
@@ -37,7 +37,7 @@ class TestLocalizationExample(GtsamTestCase):
         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.]))
+        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))
 

python/gtsam/tests/test_NonlinearOptimizer.py

@@ -15,11 +15,11 @@ from __future__ import print_function
 import unittest
 
 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
+from gtsam import (DoglegOptimizer, DoglegParams,
+                   DummyPreconditionerParameters, GaussNewtonOptimizer,
                    GaussNewtonParams, LevenbergMarquardtOptimizer,
-                   LevenbergMarquardtParams, PCGSolverParameters,
-                   DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
-                   Point2, PriorFactorPoint2, Values)
+                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   PCGSolverParameters, Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
@@ -30,7 +30,7 @@ class TestScenario(GtsamTestCase):
     def test_optimize(self):
         """Do trivial test with three optimizer variants."""
         fg = NonlinearFactorGraph()
-        model = gtsam.noiseModel_Unit.Create(2)
+        model = gtsam.noiseModel.Unit.Create(2)
         fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
 
         # test error at minimum

python/gtsam/tests/test_OdometryExample.py

@@ -25,7 +25,7 @@ class TestOdometryExample(GtsamTestCase):
 
         # 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(
+        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))
@@ -33,7 +33,7 @@ class TestOdometryExample(GtsamTestCase):
         # 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(
+        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))

python/gtsam/tests/test_PlanarSLAMExample.py

@@ -32,13 +32,13 @@ class TestPlanarSLAM(GtsamTestCase):
         # 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]))
+        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]))
+        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(
@@ -49,7 +49,7 @@ class TestPlanarSLAM(GtsamTestCase):
             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]))
+        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

python/gtsam/tests/test_Pose2SLAMExample.py

@@ -32,13 +32,13 @@ class TestPose2SLAMExample(GtsamTestCase):
         # 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]))
+        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]))
+        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(
@@ -49,7 +49,7 @@ class TestPose2SLAMExample(GtsamTestCase):
             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]))
+        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

python/gtsam/tests/test_Pose3SLAMExample.py

@@ -30,7 +30,7 @@ class TestPose3SLAMExample(GtsamTestCase):
         fg = gtsam.NonlinearFactorGraph()
         fg.add(gtsam.NonlinearEqualityPose3(0, p0))
         delta = p0.between(p1)
-        covariance = gtsam.noiseModel_Diagonal.Sigmas(
+        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))

python/gtsam/tests/test_PriorFactor.py

@@ -23,14 +23,14 @@ class TestPriorFactor(GtsamTestCase):
 
         key = 5
         priorPose3 = gtsam.Pose3()
-        model = gtsam.noiseModel_Unit.Create(6)
+        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)
+        model = gtsam.noiseModel.Unit.Create(3)
         factor = gtsam.PriorFactorVector(key, priorVector, model)
         values.insert(key, priorVector)
         self.assertEqual(factor.error(values), 0)
@@ -45,14 +45,14 @@ class TestPriorFactor(GtsamTestCase):
         # define and add Pose3 prior
         key = 5
         priorPose3 = gtsam.Pose3()
-        model = gtsam.noiseModel_Unit.Create(6)
+        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)
+        model = gtsam.noiseModel.Unit.Create(3)
         graph.addPriorVector(key, priorVector, model)
         self.assertEqual(graph.size(), 2)
 

python/gtsam/tests/test_SFMExample.py

@@ -15,8 +15,9 @@ import numpy as np
 import gtsam
 import gtsam.utils.visual_data_generator as generator
 from gtsam import symbol
+from gtsam.noiseModel import Isotropic, Diagonal
 from gtsam.utils.test_case import GtsamTestCase
-
+from gtsam.symbol_shorthand import X, P
 
 class TestSFMExample(GtsamTestCase):
 
@@ -34,29 +35,29 @@ class TestSFMExample(GtsamTestCase):
         graph = gtsam.NonlinearFactorGraph()
 
         # Add factors for all measurements
-        measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, measurementNoiseSigma)
+        measurementNoise = 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))
+                    X(i), P(j), data.K))
 
-        posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
-        graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0),
+        posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
+        graph.add(gtsam.PriorFactorPose3(X(0),
                                    truth.cameras[0].pose(), posePriorNoise))
-        pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma)
-        graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0),
+        pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
+        graph.add(gtsam.PriorFactorPoint3(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)
+            initialEstimate.insert(X(i), pose_i)
         for j in range(len(truth.points)):
             point_j = truth.points[j]
-            initialEstimate.insert(symbol(ord('p'), j), point_j)
+            initialEstimate.insert(P(j), point_j)
 
         # Optimization
         optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
@@ -66,16 +67,16 @@ class TestSFMExample(GtsamTestCase):
 
         # Marginalization
         marginals = gtsam.Marginals(graph, result)
-        marginals.marginalCovariance(symbol(ord('p'), 0))
-        marginals.marginalCovariance(symbol(ord('x'), 0))
+        marginals.marginalCovariance(P(0))
+        marginals.marginalCovariance(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))
+            pose_i = result.atPose3(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))
+            point_j = result.atPoint3(P(j))
             self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":

python/gtsam/tests/test_Scenario.py

@@ -43,8 +43,11 @@ class TestScenario(GtsamTestCase):
         # R = v/w, so test if loop crests at 2*R
         R = v / w
         T30 = scenario.pose(T)
+        xyz = T30.rotation().xyz()
+        if xyz[0] < 0:
+            xyz = -xyz
         np.testing.assert_almost_equal(
-            np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
+            np.array([math.pi, 0, math.pi]), xyz)
         self.gtsamAssertEquals(gtsam.Point3(
             0, 0, 2.0 * R), T30.translation(), 1e-9)
 

python/gtsam/tests/test_SimpleCamera.py

@@ -5,7 +5,7 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-PinholeCameraCal3_S2 unit tests.
+SimpleCamera unit tests.
 Author: Frank Dellaert & Duy Nguyen Ta (Python)
 """
 import math
@@ -14,7 +14,7 @@ import unittest
 import numpy as np
 
 import gtsam
-from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
 from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
@@ -23,14 +23,14 @@ 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)
+        camera = SimpleCamera(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)
+        camera = SimpleCamera.Level(K, pose2, 0.1)
 
         # expected
         x = Point3(1,0,0)

python/gtsam/tests/test_StereoVOExample.py

@@ -28,11 +28,11 @@ class TestStereoVOExample(GtsamTestCase):
         #  - 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)
+        x1 = symbol('x',1) 
+        x2 = symbol('x',2) 
+        l1 = symbol('l',1) 
+        l2 = symbol('l',2) 
+        l3 = symbol('l',3)
 
         ## Create graph container and add factors to it
         graph = gtsam.NonlinearFactorGraph()
@@ -44,7 +44,7 @@ class TestStereoVOExample(GtsamTestCase):
         ## 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]))
+        stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
 
         ## Add measurements
         # pose 1

python/gtsam/tests/test_Triangulation.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
+
+Test Triangulation
+Author: Frank Dellaert & Fan Jiang (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam as g
+from gtsam.utils.test_case import GtsamTestCase
+from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
+    PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
+
+class TestVisualISAMExample(GtsamTestCase):
+    def test_TriangulationExample(self):
+        # Some common constants
+        sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
+
+        # Looking along X-axis, 1 meter above ground plane (x-y)
+        upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
+        pose1 = Pose3(upright, Point3(0, 0, 1))
+        camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
+
+        # create second camera 1 meter to the right of first camera
+        pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
+        camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
+
+        # landmark ~5 meters infront of camera
+        landmark = Point3(5, 0.5, 1.2)
+
+        # 1. Project two landmarks into two cameras and triangulate
+        z1 = camera1.project(landmark)
+        z2 = camera2.project(landmark)
+
+        # twoPoses
+        poses = Pose3Vector()
+        measurements = Point2Vector()
+
+        poses.append(pose1)
+        poses.append(pose2)
+        measurements.append(z1)
+        measurements.append(z2)
+
+        optimize = True
+        rank_tol = 1e-9
+
+        triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
+        self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
+
+        # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+        measurements = Point2Vector()
+        measurements.append(z1 - np.array([0.1, 0.5]))
+        measurements.append(z2 - np.array([-0.2, 0.3]))
+
+        triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
+        self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2)
+        #
+        # # two Poses with Bundler Calibration
+        # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
+        # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
+        # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
+        #
+        # z1 = camera1.project(landmark)
+        # z2 = camera2.project(landmark)
+        #
+        # measurements = Point2Vector()
+        # measurements.append(z1)
+        # measurements.append(z2)
+        #
+        # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
+        # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam/tests/test_Values.py

@@ -15,8 +15,7 @@ import numpy as np
 
 import gtsam
 from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
-                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
-                   imuBias_ConstantBias)
+                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias)
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -37,7 +36,7 @@ class TestValues(GtsamTestCase):
         values.insert(7, Cal3DS2())
         values.insert(8, Cal3Bundler())
         values.insert(9, E)
-        values.insert(10, imuBias_ConstantBias())
+        values.insert(10, imuBias.ConstantBias())
 
         # Special cases for Vectors and Matrices
         # Note that gtsam's Eigen Vectors and Matrices requires double-precision
@@ -70,8 +69,8 @@ class TestValues(GtsamTestCase):
         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)
+        self.gtsamAssertEquals(values.atConstantBias(
+            10), imuBias.ConstantBias(), tol)
 
         # special cases for Vector and Matrix:
         actualVector = values.atVector(11)

python/gtsam/tests/test_VisualISAMExample.py

@@ -46,11 +46,11 @@ class TestVisualISAMExample(GtsamTestCase):
             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))
+            pose_i = result.atPose3(symbol('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))
+            point_j = result.atPoint3(symbol('l', j))
             self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":

python/gtsam/tests/test_dataset.py

@@ -15,7 +15,7 @@ from __future__ import print_function
 import unittest
 
 import gtsam
-from gtsam import BetweenFactorPose3, BetweenFactorPose3s
+from gtsam import BetweenFactorPose3
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -37,8 +37,8 @@ class TestDataset(GtsamTestCase):
     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)
+        self.assertEqual(len(factors), 6)
+        self.assertIsInstance(factors[0], BetweenFactorPose3)
 
 
 if __name__ == '__main__':

python/gtsam/tests/test_initialize_pose3.py

@@ -24,7 +24,7 @@ class TestValues(GtsamTestCase):
 
     def setUp(self):
 
-        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+        model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
 
         # We consider a small graph:
         #                            symbolic FG
@@ -64,9 +64,8 @@ class TestValues(GtsamTestCase):
 
     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)
@@ -77,7 +76,7 @@ class TestValues(GtsamTestCase):
         g2oFile = gtsam.findExampleDataFile("pose3example-grid")
         is3D = True
         inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
-        priorModel = gtsam.noiseModel_Unit.Create(6)
+        priorModel = gtsam.noiseModel.Unit.Create(6)
         inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
 
         initial = gtsam.InitializePose3.initialize(inputGraph)

python/gtsam/tests/test_logging_optimizer.py

@@ -21,7 +21,7 @@ from gtsam.utils.test_case import GtsamTestCase
 from gtsam.utils.logging_optimizer import gtsam_optimize
 
 KEY = 0
-MODEL = gtsam.noiseModel_Unit.Create(3)
+MODEL = gtsam.noiseModel.Unit.Create(3)
 
 
 class TestOptimizeComet(GtsamTestCase):

python/gtsam/utils/circlePose3.py

@@ -1,9 +1,10 @@
 import gtsam
+import math
 import numpy as np
-from math import pi, cos, sin
+from math import pi
 
 
-def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
+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
@@ -18,10 +19,6 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
     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
@@ -29,7 +26,7 @@ def circlePose3(numPoses=8, radius=1.0, symbolChar=0):
         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)
+        gti = gtsam.Point3(radius * math.cos(theta), radius * math.sin(theta), 0)
         oRi = gtsam.Rot3.Yaw(
             -theta)  # negative yaw goes counterclockwise, with Z down !
         gTi = gtsam.Pose3(gRo.compose(oRi), gti)

python/gtsam/utils/plot.py

@@ -109,7 +109,7 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
     t = pose.translation()
-    origin = np.array([t.x(), t.y()])
+    origin = t
 
     # draw the camera axes
     x_axis = origin + gRp[:, 0] * axis_length
@@ -155,7 +155,6 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
 
     axes.set_xlabel(axis_labels[0])
     axes.set_ylabel(axis_labels[1])
-    axes.set_zlabel(axis_labels[2])
 
     return fig
 
@@ -170,9 +169,9 @@ def plot_point3_on_axes(axes, point, linespec, P=None):
         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)
+    axes.plot([point[0]], [point[1]], [point[2]], linespec)
     if P is not None:
-        plot_covariance_ellipse_3d(axes, point.vector(), P)
+        plot_covariance_ellipse_3d(axes, point, P)
 
 
 def plot_point3(fignum, point, linespec, P=None,
@@ -222,9 +221,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
     keys = values.keys()
 
     # Plot points and covariance matrices
-    for i in range(keys.size()):
+    for key in keys:
         try:
-            key = keys.at(i)
             point = values.atPoint3(key)
             if marginals is not None:
                 covariance = marginals.marginalCovariance(key)
@@ -254,7 +252,7 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
     """
     # get rotation and translation (center)
     gRp = pose.rotation().matrix()  # rotation from pose to global
-    origin = pose.translation().vector()
+    origin = pose.translation()
 
     # draw the camera axes
     x_axis = origin + gRp[:, 0] * axis_length
@@ -320,19 +318,17 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
         title (string): The title of the plot.
         axis_labels (iterable[string]): List of axis labels to set.
     """
-    pose3Values = gtsam.utilities_allPose3s(values)
+    pose3Values = gtsam.utilities.allPose3s(values)
     keys = gtsam.KeyVector(pose3Values.keys())
-    lastIndex = None
+    lastKey = None
 
-    for i in range(keys.size()):
-        key = keys.at(i)
+    for key in keys:
         try:
             pose = pose3Values.atPose3(key)
         except:
             print("Warning: no Pose3 at key: {0}".format(key))
 
-        if lastIndex is not None:
-            lastKey = keys.at(lastIndex)
+        if lastKey is not None:
             try:
                 lastPose = pose3Values.atPose3(lastKey)
             except:
@@ -347,11 +343,10 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
             fig = plot_pose3(fignum, lastPose,  P=covariance,
                              axis_length=scale, axis_labels=axis_labels)
 
-        lastIndex = i
+        lastKey = key
 
     # Draw final pose
-    if lastIndex is not None:
-        lastKey = keys.at(lastIndex)
+    if lastKey is not None:
         try:
             lastPose = pose3Values.atPose3(lastKey)
             if marginals:
@@ -367,3 +362,40 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
 
     fig.suptitle(title)
     fig.canvas.set_window_title(title.lower())
+
+
+def plot_incremental_trajectory(fignum, values, start=0,
+                                scale=1, marginals=None,
+                                time_interval=0.0):
+    """
+    Incrementally 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.
+        start (int): Starting index to start plotting from.
+        scale (float): Value to scale the poses by.
+        marginals (gtsam.Marginals): Marginalized probability values of the estimation.
+            Used to plot uncertainty bounds.
+        time_interval (float): Time in seconds to pause between each rendering.
+            Used to create animation effect.
+    """
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+
+    pose3Values = gtsam.utilities.allPose3s(values)
+    keys = gtsam.KeyVector(pose3Values.keys())
+
+    for key in keys[start:]:
+        if values.exists(key):
+            pose_i = values.atPose3(key)
+            plot_pose3(fignum, pose_i, scale)
+
+    # Update the plot space to encompass all plotted points
+    axes.autoscale()
+
+    # Set the 3 axes equal
+    set_axes_equal(fignum)
+
+    # Pause for a fixed amount of seconds
+    plt.pause(time_interval)

python/gtsam/utils/test_case.py

@@ -21,7 +21,11 @@ class GtsamTestCase(unittest.TestCase):
             Keyword Arguments:
                 tol {float} -- tolerance passed to 'equals', default 1e-9
         """
-        equal = actual.equals(expected, tol)
+        import numpy
+        if isinstance(expected, numpy.ndarray):
+            equal = numpy.allclose(actual, expected, atol=tol)
+        else:
+            equal = actual.equals(expected, tol)
         if not equal:
             raise self.failureException(
                 "Values are not equal:\n{}!={}".format(actual, expected))

python/gtsam/utils/visual_data_generator.py

@@ -1,9 +1,10 @@
 from __future__ import print_function
 
 import numpy as np
-
+import math
+from math import pi
 import gtsam
-from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
+from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
 
 
 class Options:
@@ -31,7 +32,7 @@ class GroundTruth:
     def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
         self.cameras = [Pose3()] * nrCameras
-        self.points = [Point3()] * nrPoints
+        self.points = [Point3(0, 0, 0)] * nrPoints
 
     def print_(self, s=""):
         print(s)
@@ -55,20 +56,20 @@ class Data:
 
     def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4):
         self.K = K
-        self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras]
+        self.Z = [x[:] for x in [[gtsam.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(
+        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(
+        # 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(
+        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)
+        self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
+        self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
 
 
 def generate_data(options):
@@ -84,8 +85,8 @@ def generate_data(options):
     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)
+            theta = j * 2 * pi / nrPoints
+            truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
             Point3(10, 10, 10), Point3(-10, 10, 10),
@@ -98,10 +99,10 @@ def generate_data(options):
     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)
+        theta = i * 2 * pi / options.nrCameras
+        t = Point3(r * math.cos(theta), r * math.sin(theta), height)
         truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
-                                                       Point3(),
+                                                       Point3(0, 0, 0),
                                                        Point3(0, 0, 1),
                                                        truth.K)
         # Create measurements

python/gtsam/utils/visual_isam.py

@@ -25,7 +25,7 @@ def initialize(data, truth, options):
     newFactors = gtsam.NonlinearFactorGraph()
     initialEstimates = gtsam.Values()
     for i in range(2):
-        ii = symbol(ord('x'), i)
+        ii = symbol('x', i)
         if i == 0:
             if options.hardConstraint:  # add hard constraint
                 newFactors.add(
@@ -41,10 +41,10 @@ def initialize(data, truth, options):
     # Add visual measurement factors from two first poses and initialize
     # observed landmarks
     for i in range(2):
-        ii = symbol(ord('x'), i)
+        ii = symbol('x', i)
         for k in range(len(data.Z[i])):
             j = data.J[i][k]
-            jj = symbol(ord('l'), j)
+            jj = symbol('l', j)
             newFactors.add(
                 gtsam.GenericProjectionFactorCal3_S2(data.Z[i][
                     k], data.noiseModels.measurement, ii, jj, data.K))
@@ -59,8 +59,8 @@ def initialize(data, truth, options):
     # 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))
+            symbol('x', 0),
+            symbol('x', 1), data.odometry[1], data.noiseModels.odometry))
 
     # Update ISAM
     if options.batchInitialization:  # Do a full optimize for first two poses
@@ -98,28 +98,28 @@ def step(data, isam, result, truth, currPoseIndex):
     odometry = data.odometry[prevPoseIndex]
     newFactors.add(
         gtsam.BetweenFactorPose3(
-            symbol(ord('x'), prevPoseIndex),
-            symbol(ord('x'), currPoseIndex), odometry,
+            symbol('x', prevPoseIndex),
+            symbol('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)
+        jj = symbol('l', j)
         newFactors.add(
             gtsam.GenericProjectionFactorCal3_S2(
                 zij, data.noiseModels.measurement,
-                symbol(ord('x'), currPoseIndex), jj, data.K))
+                symbol('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))
+    prevPose = result.atPose3(symbol('x', prevPoseIndex))
     initialEstimates.insert(
-        symbol(ord('x'), currPoseIndex), prevPose.compose(odometry))
+        symbol('x', currPoseIndex), prevPose.compose(odometry))
 
     # Update ISAM
     # figure(1)tic

python/gtsam_py/python/__init__.py

@@ -1,26 +0,0 @@
-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/ImuFactorExample.py

@@ -1,153 +0,0 @@
-"""
-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_unstable/examples/FixedLagSmootherExample.py

@@ -16,16 +16,6 @@ import numpy as np
 import gtsam
 import gtsam_unstable
 
-
-def _timestamp_key_value(key, value):
-    """
-
-    """
-    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
-        key, value
-    )
-
-
 def BatchFixedLagSmootherExample():
     """
     Runs a batch fixed smoother on an agent with two odometry
@@ -45,21 +35,21 @@ def BatchFixedLagSmootherExample():
 
     # Create  a prior on the first pose, placing it at the origin
     prior_mean = gtsam.Pose2(0, 0, 0)
-    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+    prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
     X1 = 0
     new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
     new_values.insert(X1, prior_mean)
-    new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+    new_timestamps.insert((X1, 0.0))
 
     delta_time = 0.25
     time = 0.25
 
     while time <= 3.0:
-        previous_key = 1000 * (time - delta_time)
-        current_key = 1000 * time
+        previous_key = int(1000 * (time - delta_time))
+        current_key = int(1000 * time)
 
         # assign current key to the current timestamp
-        new_timestamps.insert(_timestamp_key_value(current_key, time))
+        new_timestamps.insert((current_key, time))
 
         # Add a guess for this pose to the new values
         # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
@@ -69,14 +59,14 @@ def BatchFixedLagSmootherExample():
         # Add odometry factors from two different sources with different error
         # stats
         odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
-        odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+        odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.1, 0.1, 0.05]))
         new_factors.push_back(gtsam.BetweenFactorPose2(
             previous_key, current_key, odometry_measurement_1, odometry_noise_1
         ))
 
         odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
-        odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+        odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.05, 0.05, 0.05]))
         new_factors.push_back(gtsam.BetweenFactorPose2(
             previous_key, current_key, odometry_measurement_2, odometry_noise_2

python/gtsam_unstable/examples/TimeOfArrivalExample.py

@@ -12,7 +12,7 @@ Author: Frank Dellaert
 # pylint: disable=invalid-name, no-name-in-module
 
 from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
+                   NonlinearFactorGraph, Point3, Values, noiseModel)
 from gtsam_unstable import Event, TimeOfArrival, TOAFactor
 
 # units
@@ -64,7 +64,7 @@ def create_graph(microphones, simulatedTOA):
     graph = NonlinearFactorGraph()
 
     # Create a noise model for the TOA error
-    model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
+    model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
 
     K = len(microphones)
     key = 0

python/gtsam_unstable/tests/test_FixedLagSmootherExample.py

@@ -16,13 +16,6 @@ import gtsam
 import gtsam_unstable
 from gtsam.utils.test_case import GtsamTestCase
 
-
-def _timestamp_key_value(key, value):
-    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
-        key, value
-    )
-
-
 class TestFixedLagSmootherExample(GtsamTestCase):
     '''
     Tests the fixed lag smoother wrapper
@@ -47,14 +40,14 @@ class TestFixedLagSmootherExample(GtsamTestCase):
 
         # Create  a prior on the first pose, placing it at the origin
         prior_mean = gtsam.Pose2(0, 0, 0)
-        prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
+        prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
             np.array([0.3, 0.3, 0.1]))
         X1 = 0
         new_factors.push_back(
             gtsam.PriorFactorPose2(
                 X1, prior_mean, prior_noise))
         new_values.insert(X1, prior_mean)
-        new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+        new_timestamps.insert((X1, 0.0))
 
         delta_time = 0.25
         time = 0.25
@@ -80,11 +73,11 @@ class TestFixedLagSmootherExample(GtsamTestCase):
         # and its two odometers measure the change. The smoothed
         # result is then compared to the ground truth
         while time <= 3.0:
-            previous_key = 1000 * (time - delta_time)
-            current_key = 1000 * time
+            previous_key = int(1000 * (time - delta_time))
+            current_key = int(1000 * time)
 
             # assign current key to the current timestamp
-            new_timestamps.insert(_timestamp_key_value(current_key, time))
+            new_timestamps.insert((current_key, time))
 
             # Add a guess for this pose to the new values
             # Assume that the robot moves at 2 m/s. Position is time[s] *
@@ -95,7 +88,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
             # Add odometry factors from two different sources with different
             # error stats
             odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
-            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+            odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
                 np.array([0.1, 0.1, 0.05]))
             new_factors.push_back(
                 gtsam.BetweenFactorPose2(
@@ -105,7 +98,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
                     odometry_noise_1))
 
             odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
-            odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+            odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
                 np.array([0.05, 0.05, 0.05]))
             new_factors.push_back(
                 gtsam.BetweenFactorPose2(