Merge branch 'develop' into feature/Eigen_3.3.4

cmake/GtsamCythonWrap.cmake

@@ -52,7 +52,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
   add_custom_command(
     OUTPUT ${generated_cpp}
     COMMAND
-      ${CYTHON_EXECUTABLE} -X boundscheck=False -a -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
+      ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
     VERBATIM)
   add_custom_target(${target} ALL DEPENDS ${generated_cpp})
 endfunction()
@@ -95,7 +95,7 @@ function(cythonize target pyx_file output_lib_we output_dir include_dirs libs in
 
   # Late dependency injection, to make sure this gets called whenever the interface header is updated
   # See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
-  add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} APPEND)
+  add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
   if (NOT "${dependencies}" STREQUAL "")
     add_dependencies(${target}_pyx2cpp "${dependencies}")
   endif()

cython/gtsam/examples/ImuFactorExample2.py

@@ -0,0 +1,176 @@
+"""
+iSAM2 example with ImuFactor.
+Author: Robert Truax (C++), Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import math
+
+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
+
+
+def X(key):
+    """Create symbol for pose key."""
+    return gtsam.symbol(ord('x'), key)
+
+
+def V(key):
+    """Create symbol for velocity key."""
+    return gtsam.symbol(ord('v'), key)
+
+
+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 = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
+                    gtsam.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 = gtsam.Point3(0, 0, 1)
+    target = gtsam.Point3(0, 0, 0)
+    position = gtsam.Point3(radius, 0, 0)
+    camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.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 = gtsam.ConstantTwistScenario(
+        angular_velocity_vector, linear_velocity_vector, pose_0)
+
+    # Create a factor graph
+    newgraph = gtsam.NonlinearFactorGraph()
+
+    # Create (incremental) ISAM2 solver
+    isam = gtsam.ISAM2()
+
+    # Create the initial estimate to the solution
+    # Intentionally initialize the variables off from the ground truth
+    initialEstimate = gtsam.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.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+    newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
+
+    # Add imu priors
+    biasKey = gtsam.symbol(ord('b'), 0)
+    biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+    biasprior = gtsam.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 = gtsam.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 = gtsam.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 = gtsam.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 = gtsam.NonlinearFactorGraph()
+        initialEstimate.clear()
+
+
+if __name__ == '__main__':
+    IMU_example()

cython/gtsam/examples/OdometryExample.py

@@ -0,0 +1,52 @@
+"""
+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
+
+# 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))

cython/gtsam/examples/PlanarSLAMExample.py

@@ -0,0 +1,80 @@
+"""
+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 numpy as np
+
+import gtsam
+
+# 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 = gtsam.symbol(ord('x'), 1)
+X2 = gtsam.symbol(ord('x'), 2)
+X3 = gtsam.symbol(ord('x'), 3)
+L1 = gtsam.symbol(ord('l'), 4)
+L2 = gtsam.symbol(ord('l'), 5)
+
+# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
+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)))

cython/gtsam/examples/Pose2SLAMExample.py

@@ -0,0 +1,87 @@
+"""
+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
+
+
+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)))

cython/gtsam/examples/README.md

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

cython/gtsam/examples/SFMdata.py

@@ -0,0 +1,38 @@
+"""
+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 = 30.0
+    angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
+    up = gtsam.Point3(0, 0, 1)
+    target = gtsam.Point3(0, 0, 0)
+    poses = []
+    for theta in angles:
+        position = gtsam.Point3(radius*np.cos(theta),
+                                radius*np.sin(theta), 0.0)
+        camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
+        poses.append(camera.pose())
+    return poses

cython/gtsam/examples/VisualISAM2Example.py

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

cython/gtsam/tests/test_Scenario.py

@@ -0,0 +1,36 @@
+import math
+import unittest
+import numpy as np
+
+import gtsam
+
+
+class TestScenario(unittest.TestCase):
+    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.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
+
+
+if __name__ == '__main__':
+    unittest.main()

cython/gtsam/utils/plot.py

@@ -0,0 +1,77 @@
+"""Various plotting utlities."""
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+def plot_point3_on_axes(axes, point, linespec):
+    """Plot a 3D point on given axis 'axes' with given 'linespec'."""
+    axes.plot([point.x()], [point.y()], [point.z()], linespec)
+
+
+def plot_point3(fignum, point, linespec):
+    """Plot a 3D point on given figure with given 'linespec'."""
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plot_point3_on_axes(axes, point, linespec)
+
+
+def plot_3d_points(fignum, values, linespec, marginals=None):
+    """
+    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.
+    """
+
+    keys = values.keys()
+
+    # Plot points and covariance matrices
+    for i in range(keys.size()):
+        try:
+            p = values.atPoint3(keys.at(i))
+            # if haveMarginals
+            #     P = marginals.marginalCovariance(key);
+            #     gtsam.plot_point3(p, linespec, P);
+            # else
+            plot_point3(fignum, p, linespec)
+        except RuntimeError:
+            continue
+            # I guess it's not a Point3
+
+
+def plot_pose3_on_axes(axes, pose, axis_length=0.1):
+    """Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
+    # get rotation and translation (center)
+    gRp = pose.rotation().matrix()  # rotation from pose to global
+    t = pose.translation()
+    origin = np.array([t.x(), t.y(), t.z()])
+
+    # 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
+    # TODO (dellaert): make this work
+    # if (nargin>2) && (~isempty(P))
+    #     pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
+    #     gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
+    #     gtsam.covarianceEllipse3D(origin,gPp);
+    # end
+
+
+def plot_pose3(fignum, pose, axis_length=0.1):
+    """Plot a 3D pose on given figure with given 'axis_length'."""
+    # get figure object
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+    plot_pose3_on_axes(axes, pose, axis_length)

cython/gtsam_eigency/eigency_cpp.h

@@ -306,10 +306,17 @@ public:
                   : cols),
                stride
             )  {}
+
+    MapBase &operator=(const MatrixType &other) {
+        Base::operator=(other);
+        return *this;
+    }
+
+    virtual ~MapBase() { }
 };
 
 
-template <template<class,int,int,int,int,int> class DenseBase,
+template <template<class,int,int,int,int,int> class EigencyDenseBase,
           typename Scalar,
           int _Rows, int _Cols,
           int _Options = Eigen::AutoAlign |
@@ -341,16 +348,18 @@ template <template<class,int,int,int,int,int> class DenseBase,
           int _StrideOuter=0, int _StrideInner=0,
           int _MaxRows = _Rows,
           int _MaxCols = _Cols>
-class FlattenedMap: public MapBase<DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> >  {
+class FlattenedMap: public MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> >  {
 public:
-    typedef MapBase<DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
+    typedef MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
 
     FlattenedMap()
-        : Base(NULL, 0, 0) {}
+        : Base(NULL, 0, 0),
+          object_(NULL) {}
     
     FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
         : Base(data, rows, cols,
-               Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)) {
+               Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
+          object_(NULL) {
     }
 
     FlattenedMap(PyArrayObject *object)
@@ -359,18 +368,26 @@ public:
                (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
                (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
                Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
-                                                         _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])) {
+                                                         _StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])),
+          object_(object) {
 
         if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
             throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
+
+        Py_XINCREF(object_);
     }
     FlattenedMap &operator=(const FlattenedMap &other) {
-        // Replace the memory that we point to (not a memory allocation)
+        if (other.object_) {
-        new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
+            new (this) FlattenedMap(other.object_);
-                                other.rows(),
+        } else {
-                                other.cols(),
+            // Replace the memory that we point to (not a memory allocation)
-                                other.outerStride(),
+            new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
-                                other.innerStride());
+                                    other.rows(),
+                                    other.cols(),
+                                    other.outerStride(),
+                                    other.innerStride());
+        }
+
         return *this;
     }
     
@@ -382,9 +399,16 @@ public:
         return static_cast<Base&>(*this);
     }
 
-    operator DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
+    operator EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
-        return DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
+        return EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
     }
+
+    virtual ~FlattenedMap() {
+        Py_XDECREF(object_);
+    }
+
+private:
+    PyArrayObject * const object_;
 };
 
 
@@ -394,39 +418,60 @@ public:
     typedef MapBase<MatrixType> Base;
     typedef typename MatrixType::Scalar Scalar;
 
+    enum {
+        RowsAtCompileTime = Base::Base::RowsAtCompileTime,
+        ColsAtCompileTime = Base::Base::ColsAtCompileTime
+    };
+
     Map()
-        : Base(NULL, 0, 0) {
+        : Base(NULL,
+               (RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime,
+               (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
+          object_(NULL) {
     }
     
     Map(Scalar *data, long rows, long cols)
-        : Base(data, rows, cols) {}
+        : Base(data, rows, cols),
+          object_(NULL) {}
 
     Map(PyArrayObject *object)
         : Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data,
                // ROW: If array is in row-major order, transpose (see README)
                (PyObject*)object == Py_None? 0 :
-               (PyArray_IS_C_CONTIGUOUS(object)
+               (!PyArray_IS_F_CONTIGUOUS(object)
                 ? ((object->nd == 1)
                    ? 1  // ROW: If 1D row-major numpy array, set to 1 (row vector)
                    : object->dimensions[1])
                 : object->dimensions[0]),
                // COLUMN: If array is in row-major order: transpose (see README)
                (PyObject*)object == Py_None? 0 :
-               (PyArray_IS_C_CONTIGUOUS(object)
+               (!PyArray_IS_F_CONTIGUOUS(object)
                 ? object->dimensions[0]
                 : ((object->nd == 1)
                    ? 1  // COLUMN: If 1D col-major numpy array, set to length (column vector)
-                   : object->dimensions[1]))) {
+                   : object->dimensions[1]))),
+          object_(object) {
 
         if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
             throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
+        Py_XINCREF(object_);
     }
     
     Map &operator=(const Map &other) {
-        // Replace the memory that we point to (not a memory allocation)
+        if (other.object_) {
-        new (this) Map(const_cast<Scalar*>(other.data()),
+            new (this) Map(other.object_);
-                       other.rows(),
+        } else {
-                       other.cols());
+            // Replace the memory that we point to (not a memory allocation)
+            new (this) Map(const_cast<Scalar*>(other.data()),
+                          other.rows(),
+                          other.cols());
+        }
+
+        return *this;
+    }
+
+    Map &operator=(const MatrixType &other) {
+        MapBase<MatrixType>::operator=(other);
         return *this;
     }
     
@@ -440,7 +485,14 @@ public:
 
     operator MatrixType() const {
         return MatrixType(static_cast<Base>(*this));
-    }    
+    }
+
+    virtual ~Map() {
+        Py_XDECREF(object_);
+    }
+
+private:
+    PyArrayObject * const object_;
 };
 
 

examples/ImuFactorExample2.cpp

@@ -0,0 +1,136 @@
+
+#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/Scenario.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+// Shorthand for velocity and pose variables
+using symbol_shorthand::V;
+using symbol_shorthand::X;
+
+const double kGravity = 9.81;
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  auto params = PreintegrationParams::MakeSharedU(kGravity);
+  params->setAccelerometerCovariance(I_3x3 * 0.1);
+  params->setGyroscopeCovariance(I_3x3 * 0.1);
+  params->setIntegrationCovariance(I_3x3 * 0.1);
+  params->setUse2ndOrderCoriolis(false);
+  params->setOmegaCoriolis(Vector3(0, 0, 0));
+
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+
+  // Start with a camera on x-axis looking at origin
+  double radius = 30;
+  const Point3 up(0, 0, 1), target(0, 0, 0);
+  const Point3 position(radius, 0, 0);
+  const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
+  const auto pose_0 = camera.pose();
+
+  // Now, create a constant-twist scenario that makes the camera orbit the
+  // origin
+  double angular_velocity = M_PI,  // rad/sec
+      delta_t = 1.0 / 18;          // makes for 10 degrees per step
+  Vector3 angular_velocity_vector(0, -angular_velocity, 0);
+  Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
+  auto scenario = ConstantTwistScenario(angular_velocity_vector,
+                                        linear_velocity_vector, pose_0);
+
+  // Create a factor graph
+  NonlinearFactorGraph newgraph;
+
+  // Create (incremental) ISAM2 solver
+  ISAM2 isam;
+
+  // Create the initial estimate to the solution
+  // Intentionally initialize the variables off from the ground truth
+  Values initialEstimate, totalEstimate, result;
+
+  // 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
+  auto noise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+  newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
+
+  // Add imu priors
+  Key biasKey = Symbol('b', 0);
+  auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
+  PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
+                                               biasnoise);
+  newgraph.push_back(biasprior);
+  initialEstimate.insert(biasKey, imuBias::ConstantBias());
+  auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
+
+  Vector n_velocity(3);
+  n_velocity << 0, angular_velocity * radius, 0;
+  PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
+  newgraph.push_back(velprior);
+
+  initialEstimate.insert(V(0), n_velocity);
+
+  // IMU preintegrator
+  PreintegratedImuMeasurements accum(params);
+
+  // Simulate poses and imu measurements, adding them to the factor graph
+  for (size_t i = 0; i < 36; ++i) {
+    double t = i * delta_t;
+    if (i == 0) {  // First time add two poses
+      auto pose_1 = scenario.pose(delta_t);
+      initialEstimate.insert(X(0), pose_0.compose(delta));
+      initialEstimate.insert(X(1), pose_1.compose(delta));
+    } else if (i >= 2) {  // Add more poses as necessary
+      auto 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++;
+        Symbol b1 = biasKey - 1;
+        Symbol b2 = biasKey;
+        Vector6 covvec;
+        covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
+        auto cov = noiseModel::Diagonal::Variances(covvec);
+        Vector6 zerovec;
+        zerovec << 0, 0, 0, 0, 0, 0;
+        auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
+            b1, b2, imuBias::ConstantBias(), cov);
+        newgraph.add(f);
+        initialEstimate.insert(biasKey, imuBias::ConstantBias());
+      }
+      // Predict acceleration and gyro measurements in (actual) body frame
+      auto measuredAcc = scenario.acceleration_b(t) -
+                         scenario.rotation(t).transpose() * params->n_gravity;
+      auto measuredOmega = scenario.omega_b(t);
+      accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
+
+      // Add Imu Factor
+      ImuFactor imufac(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();
+    newgraph = NonlinearFactorGraph();
+    initialEstimate.clear();
+  }
+  GTSAM_PRINT(result);
+  return 0;
+}
+/* ************************************************************************* */

gtsam.h

@@ -568,7 +568,7 @@ class Pose2 {
   static gtsam::Pose2 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose2& p);
   Matrix AdjointMap() const;
-  Vector Adjoint(const Vector& xi) const;
+  Vector Adjoint(Vector xi) const;
   static Matrix wedge(double vx, double vy, double w);
 
   // Group Actions on Point2
@@ -865,7 +865,7 @@ class CalibratedCamera {
   // Standard Constructors and Named Constructors
   CalibratedCamera();
   CalibratedCamera(const gtsam::Pose3& pose);
-  CalibratedCamera(const Vector& v);
+  CalibratedCamera(Vector v);
   static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
 
   // Testable
@@ -875,7 +875,7 @@ class CalibratedCamera {
   // Manifold
   static size_t Dim();
   size_t dim() const;
-  gtsam::CalibratedCamera retract(const Vector& d) const;
+  gtsam::CalibratedCamera retract(Vector d) const;
   Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
 
   // Action on Point3
@@ -911,7 +911,7 @@ class PinholeCamera {
   CALIBRATION calibration() const;
 
   // Manifold
-  This retract(const Vector& d) const;
+  This retract(Vector d) const;
   Vector localCoordinates(const This& T2) const;
   size_t dim() const;
   static size_t Dim();
@@ -938,6 +938,8 @@ virtual class SimpleCamera {
   static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
   static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
       const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
+  static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+      const gtsam::Point3& upVector);
 
   // Testable
   void print(string s) const;
@@ -948,7 +950,7 @@ virtual class SimpleCamera {
   gtsam::Cal3_S2 calibration() const;
 
   // Manifold
-  gtsam::SimpleCamera retract(const Vector& d) const;
+  gtsam::SimpleCamera retract(Vector d) const;
   Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
   size_t dim() const;
   static size_t Dim();
@@ -990,7 +992,7 @@ class StereoCamera {
   gtsam::Cal3_S2Stereo calibration() const;
 
   // Manifold
-  gtsam::StereoCamera retract(const Vector& d) const;
+  gtsam::StereoCamera retract(Vector d) const;
   Vector localCoordinates(const gtsam::StereoCamera& T2) const;
   size_t dim() const;
   static size_t Dim();
@@ -1225,12 +1227,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
 };
 
 virtual class Constrained : gtsam::noiseModel::Diagonal {
-    static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas);
+    static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas);
-    static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas);
+    static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas);
-    static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances);
+    static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances);
-    static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances);
+    static gtsam::noiseModel::Constrained* MixedVariances(Vector variances);
-    static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions);
+    static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions);
-    static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions);
+    static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions);
 
     static gtsam::noiseModel::Constrained* All(size_t dim);
     static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
@@ -1413,12 +1415,12 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   pair<Matrix, Vector> jacobianUnweighted() const;
   Matrix augmentedJacobianUnweighted() const;
 
-  void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
+  void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
   gtsam::JacobianFactor whiten() const;
 
   pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
 
-  void setModel(bool anyConstrained, const Vector& sigmas);
+  void setModel(bool anyConstrained, Vector sigmas);
 
   gtsam::noiseModel::Diagonal* get_model() const;
 
@@ -2502,6 +2504,11 @@ class NavState {
 #include <gtsam/navigation/PreintegratedRotation.h>
 virtual class PreintegratedRotationParams {
   PreintegratedRotationParams();
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
+
   void setGyroscopeCovariance(Matrix cov);
   void setOmegaCoriolis(Vector omega);
   void setBodyPSensor(const gtsam::Pose3& pose);
@@ -2509,13 +2516,23 @@ virtual class PreintegratedRotationParams {
   Matrix getGyroscopeCovariance() const;
 
   // TODO(frank): allow optional
-  //  boost::optional<Vector3> getOmegaCoriolis() const;
+  //  boost::optional<Vector> getOmegaCoriolis() const;
   //  boost::optional<Pose3>   getBodyPSensor()   const;
 };
 
 #include <gtsam/navigation/PreintegrationParams.h>
 virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(Vector n_gravity);
+
+  static gtsam::PreintegrationParams* MakeSharedD(double g);
+  static gtsam::PreintegrationParams* MakeSharedU(double g);
+  static gtsam::PreintegrationParams* MakeSharedD();  // default g = 9.81
+  static gtsam::PreintegrationParams* MakeSharedU();  // default g = 9.81
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::PreintegrationParams& expected, double tol);
+
   void setAccelerometerCovariance(Matrix cov);
   void setIntegrationCovariance(Matrix cov);
   void setUse2ndOrderCoriolis(bool flag);
@@ -2523,7 +2540,6 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   Matrix getAccelerometerCovariance() const;
   Matrix getIntegrationCovariance()   const;
   bool   getUse2ndOrderCoriolis()     const;
-  void print(string s) const;
 };
 
 #include <gtsam/navigation/ImuFactor.h>
@@ -2649,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
   gtsam::Unit3 bRef() const;
 };
 
-virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
+virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
-      const gtsam::Unit3& bRef);
+                      const gtsam::noiseModel::Diagonal* model,
-  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
+                      const gtsam::Unit3& bRef);
+  Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
+                      const gtsam::noiseModel::Diagonal* model);
   Pose3AttitudeFactor();
   void print(string s) const;
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
@@ -2660,6 +2678,30 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
   gtsam::Unit3 bRef() const;
 };
 
+#include <gtsam/navigation/Scenario.h>
+virtual class Scenario {
+  gtsam::Pose3 pose(double t) const;
+  Vector omega_b(double t) const;
+  Vector velocity_n(double t) const;
+  Vector acceleration_n(double t) const;
+  gtsam::Rot3 rotation(double t) const;
+  gtsam::NavState navState(double t) const;
+  Vector velocity_b(double t) const;
+  Vector acceleration_b(double t) const;
+};
+
+virtual class ConstantTwistScenario : gtsam::Scenario {
+  ConstantTwistScenario(Vector w, Vector v);
+  ConstantTwistScenario(Vector w, Vector v,
+                        const Pose3& nTb0);
+};
+
+virtual class AcceleratingScenario : gtsam::Scenario {
+  AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
+                       Vector v0, Vector a_n,
+                       Vector omega_b);
+};
+
 //*************************************************************************
 // Utilities
 //*************************************************************************

gtsam/geometry/Unit3.cpp

@@ -65,69 +65,76 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
   return Unit3(d[0], d[1], d[2]);
 }
 
+/* ************************************************************************* */
+// Get the axis of rotation with the minimum projected length of the point
+static Point3 CalculateBestAxis(const Point3& n) {
+  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
+  if ((mx <= my) && (mx <= mz)) {
+    return Point3(1.0, 0.0, 0.0);
+  } else if ((my <= mx) && (my <= mz)) {
+    return Point3(0.0, 1.0, 0.0);
+  } else {
+    return Point3(0, 0, 1);
+  }
+}
+
 /* ************************************************************************* */
 const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
 #ifdef GTSAM_USE_TBB
-  // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I
+  // NOTE(hayk): At some point it seemed like this reproducably resulted in
-  // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or
+  // deadlock. However, I don't know why and I can no longer reproduce it.
-  // there is still a latent bug to watch out for.
+  // It either was a red herring or there is still a latent bug left to debug.
   tbb::mutex::scoped_lock lock(B_mutex_);
 #endif
 
-  // Return cached basis if available and the Jacobian isn't needed.
+  const bool cachedBasis = static_cast<bool>(B_);
-  if (B_ && !H) {
+  const bool cachedJacobian = static_cast<bool>(H_B_);
-    return *B_;
-  }
-
-  // Return cached basis and derivatives if available.
-  if (B_ && H && H_B_) {
-    *H = *H_B_;
-    return *B_;
-  }
-
-  // Get the unit vector and derivative wrt this.
-  // NOTE(hayk): We can't call point3(), because it would recursively call basis().
-  const Point3 n(p_);
-
-  // Get the axis of rotation with the minimum projected length of the point
-  Point3 axis(0, 0, 1);
-  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
-  if ((mx <= my) && (mx <= mz)) {
-    axis = Point3(1.0, 0.0, 0.0);
-  } else if ((my <= mx) && (my <= mz)) {
-    axis = Point3(0.0, 1.0, 0.0);
-  }
-
-  // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
-  // the chosen axis.
-  Matrix33 H_B1_n;
-  Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr);
-
-  // Normalize result to get a unit vector: b1 = B1 / |B1|.
-  Matrix33 H_b1_B1;
-  Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr);
-
-  // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
-  // No need to normalize this, p and b1 are orthogonal unit vectors.
-  Matrix33 H_b2_n, H_b2_b1;
-  Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr);
-
-  // Create the basis by stacking b1 and b2.
-  B_.reset(Matrix32());
-  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
 
   if (H) {
-    // Chain rule tomfoolery to compute the derivative.
+    if (!cachedJacobian) {
-    const Matrix32& H_n_p = *B_;
+      // Compute Jacobian. Recomputes B_
-    const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p;
+      Matrix32 B;
-    const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
+      Matrix62 jacobian;
+      Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1;
 
-    // Cache the derivative and fill the result.
+      // Choose the direction of the first basis vector b1 in the tangent plane
-    H_B_.reset(Matrix62());
+      // by crossing n with the chosen axis.
-    (*H_B_) << H_b1_p, H_b2_p;
+      const Point3 n(p_), axis = CalculateBestAxis(n);
+      const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);
+
+      // Normalize result to get a unit vector: b1 = B1 / |B1|.
+      B.col(0) = normalize(B1, &H_b1_B1);
+
+      // Get the second basis vector b2, which is orthogonal to n and b1.
+      B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1);
+
+      // Chain rule tomfoolery to compute the jacobian.
+      const Matrix32& H_n_p = B;
+      jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p;
+      auto H_b1_p = jacobian.block<3, 2>(0, 0);
+      jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
+
+      // Cache the result and jacobian
+      H_B_.reset(jacobian);
+      B_.reset(B);
+    }
+
+    // Return cached jacobian, possibly computed just above
     *H = *H_B_;
   }
 
+  if (!cachedBasis) {
+    // Same calculation as above, without derivatives.
+    // Done after H block, as that possibly computes B_ for the first time
+    Matrix32 B;
+
+    const Point3 n(p_), axis = CalculateBestAxis(n);
+    const Point3 B1 = gtsam::cross(n, axis);
+    B.col(0) = normalize(B1);
+    B.col(1) = gtsam::cross(n, B.col(0));
+    B_.reset(B);
+  }
+
   return *B_;
 }
 
@@ -257,7 +264,7 @@ Unit3 Unit3::retract(const Vector2& v) const {
       std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
   return Unit3(exp_p_xi_hat);
 }
- 
+
 /* ************************************************************************* */
 Vector2 Unit3::localCoordinates(const Unit3& other) const {
   const double x = p_.dot(other.p_);

gtsam/geometry/tests/testUnit3.cpp

@@ -314,15 +314,24 @@ TEST(Unit3, basis) {
   Unit3 p(0.1, -0.2, 0.9);
 
   Matrix expected(3, 2);
-  expected << 0.0, -0.994169047, 0.97618706,
+  expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958;
-             -0.0233922129, 0.216930458,  0.105264958;
 
   Matrix62 actualH;
-  Matrix actual = p.basis(actualH);
-  EXPECT(assert_equal(expected, actual, 1e-6));
-
   Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
-                         boost::bind(BasisTest, _1, boost::none), p);
+      boost::bind(BasisTest, _1, boost::none), p);
+
+  // without H, first time
+  EXPECT(assert_equal(expected, p.basis(), 1e-6));
+
+  // without H, cached
+  EXPECT(assert_equal(expected, p.basis(), 1e-6));
+
+  // with H, first time
+  EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+
+  // with H, cached
+  EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
@@ -432,7 +441,8 @@ TEST(Unit3, ErrorBetweenFactor) {
   // Add process factors using the dot product error function.
   SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
   for (size_t i = 0; i < data.size() - 1; i++) {
-    Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1)));
+    Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector,
+                            Expression<Unit3>(U(i + 1)));
     graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
   }
 

gtsam/linear/Scatter.cpp

@@ -70,9 +70,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
   iterator first = begin();
   if (ordering) first += ordering->size();
   if (first != end()) std::sort(first, end());
-
-  // Filter out keys with zero dimensions (if ordering had more keys)
-  erase(std::remove_if(begin(), end(), SlotEntry::Zero), end());
 }
 
 /* ************************************************************************* */

gtsam/navigation/Scenario.h

@@ -57,10 +57,13 @@ class Scenario {
 class ConstantTwistScenario : public Scenario {
  public:
   /// Construct scenario with constant twist [w,v]
-  ConstantTwistScenario(const Vector3& w, const Vector3& v)
+  ConstantTwistScenario(const Vector3& w, const Vector3& v,
-      : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {}
+                        const Pose3& nTb0 = Pose3())
+      : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
 
-  Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); }
+  Pose3 pose(double t) const override {
+    return nTb0_ * Pose3::Expmap(twist_ * t);
+  }
   Vector3 omega_b(double t) const override { return twist_.head<3>(); }
   Vector3 velocity_n(double t) const override {
     return rotation(t).matrix() * twist_.tail<3>();
@@ -70,6 +73,7 @@ class ConstantTwistScenario : public Scenario {
  private:
   const Vector6 twist_;
   const Vector3 a_b_;  // constant centripetal acceleration in body = w_b * v_b
+  const Pose3 nTb0_;
 };
 
 /// Accelerating from an arbitrary initial state, with optional rotation

gtsam/navigation/tests/testScenario.cpp

@@ -15,8 +15,8 @@
  * @author  Frank Dellaert
  */
 
-#include <gtsam/navigation/Scenario.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/navigation/Scenario.h>
 
 #include <CppUnitLite/TestHarness.h>
 #include <boost/bind.hpp>
@@ -96,9 +96,33 @@ TEST(Scenario, Loop) {
   const double R = v / w;
   const Pose3 T30 = scenario.pose(30);
   EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
+  EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz()));
   EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
 }
 
+/* ************************************************************************* */
+TEST(Scenario, LoopWithInitialPose) {
+  // Forward velocity 2m/s
+  // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
+  const double v = 2, w = 6 * kDegree;
+  const Vector3 W(0, -w, 0), V(v, 0, 0);
+  const Rot3 nRb0 = Rot3::yaw(M_PI);
+  const Pose3 nTb0(nRb0, Point3(1, 2, 3));
+  const ConstantTwistScenario scenario(W, V, nTb0);
+
+  const double T = 30;
+  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
+  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
+  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
+
+  // R = v/w, so test if loop crests at 2*R
+  const double R = v / w;
+  const Pose3 T30 = scenario.pose(30);
+  EXPECT(
+      assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
+  EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9));
+}
+
 /* ************************************************************************* */
 TEST(Scenario, Accelerating) {
   // Set up body pointing towards y axis, and start at 10,20,0 with velocity

gtsam/nonlinear/internal/LevenbergMarquardtState.h

@@ -113,9 +113,9 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
   // Small cache of A|b|model indexed by dimension. Can save many mallocs.
   mutable std::vector<CachedModel> noiseModelCache;
   CachedModel* getCachedModel(size_t dim) const {
-    if (dim > noiseModelCache.size())
+    if (dim >= noiseModelCache.size())
-      noiseModelCache.resize(dim);
+      noiseModelCache.resize(dim+1);
-    CachedModel* item = &noiseModelCache[dim - 1];
+    CachedModel* item = &noiseModelCache[dim];
     if (!item->model)
       *item = CachedModel(dim, 1.0 / std::sqrt(lambda));
     return item;

gtsam_unstable/slam/SmartRangeFactor.h

@@ -121,17 +121,18 @@ class SmartRangeFactor: public NoiseModelFactor {
 
     // use best fh to find actual intersection points
     if (bestCircle2 && best_fh) {
-    std::list<Point2> intersections = circleCircleIntersection(
+      auto bestCircleCenter = bestCircle2->center;
-        circle1.center, bestCircle2->center, best_fh);
+      std::list<Point2> intersections =
+          circleCircleIntersection(circle1.center, bestCircleCenter, best_fh);
 
-    // pick winner based on other measurements
+      // pick winner based on other measurements
-    double error1 = 0, error2 = 0;
+      double error1 = 0, error2 = 0;
-    Point2 p1 = intersections.front(), p2 = intersections.back();
+      Point2 p1 = intersections.front(), p2 = intersections.back();
-    for (const Circle2& it : circles) {
+      for (const Circle2& it : circles) {
-      error1 += distance2(it.center, p1);
+        error1 += distance2(it.center, p1);
-      error2 += distance2(it.center, p2);
+        error2 += distance2(it.center, p2);
-    }
+      }
-    return (error1 < error2) ? p1 : p2;
+      return (error1 < error2) ? p1 : p2;
     } else {
       throw std::runtime_error("triangulate failed");
     }

wrap/Method.cpp

@@ -106,7 +106,7 @@ void Method::emit_cython_pyx_no_overload(FileWriter& file,
 
   // leverage python's special treatment for print
   if (funcName == "print_") {
-    file.oss << "    def __str__(self):\n";
+    file.oss << "    def __repr__(self):\n";
     file.oss << "        strBuf = RedirectCout()\n";
     file.oss << "        self.print_('')\n";
     file.oss << "        return strBuf.str()\n";