Merge branch 'develop' into isam2_imu_example

.github/workflows/trigger-python.yml

@@ -0,0 +1,14 @@
+# This triggers Cython builds on `gtsam-manylinux-build`
+name: Trigger Python Builds
+on: push
+jobs:
+  triggerCython:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Repository Dispatch
+        uses: ProfFan/repository-dispatch@master
+        with:
+          token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }}
+          repository: borglab/gtsam-manylinux-build
+          event-type: cython-wrapper
+          client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}'

.travis.sh

@@ -63,7 +63,7 @@ function configure()
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
       -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-      -DCMAKE_VERBOSE_MAKEFILE=ON
+      -DCMAKE_VERBOSE_MAKEFILE=OFF
 }
 
 

.travis.yml

@@ -14,7 +14,8 @@ addons:
     - clang-9
     - build-essential pkg-config
     - cmake
-    - libpython-dev python-numpy
+    - python3-dev libpython-dev
+    - python3-numpy
     - libboost-all-dev
 
 # before_install:
@@ -94,7 +95,7 @@ jobs:
     os: linux
     compiler: gcc
     env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
-    script: bash .travis.sh -b
+    script: bash .travis.sh -t
 # -------- STAGE 2: TESTS -----------
 # on Mac, GCC
   - stage: test

CMakeLists.txt

@@ -22,6 +22,7 @@ set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM
 
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 include(GtsamMakeConfigFile)
+include(GNUInstallDirs)
 
 # Record the root dir for gtsam - needed during external builds, e.g., ROS
 set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
@@ -200,7 +201,7 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
 # Set up variables if we're using TBB
 if(TBB_FOUND AND GTSAM_WITH_TBB)
     set(GTSAM_USE_TBB 1)  # This will go into config.h
-    if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020)
+    if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
         set(TBB_GREATER_EQUAL_2020 1)
     else()
         set(TBB_GREATER_EQUAL_2020 0)
@@ -597,7 +598,11 @@ print_config_flag(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preint
 print_config_flag(${GTSAM_BUILD_WRAP}                  "Build Wrap                      ")
 
 message(STATUS "MATLAB toolbox flags                                      ")
-print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install matlab toolbox         ")
+print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install MATLAB toolbox          ")
+if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
+    message(STATUS "  MATLAB root                     : ${MATLAB_ROOT}")
+    message(STATUS "  MEX binary                      : ${MEX_COMMAND}")
+endif()
 
 message(STATUS "Cython toolbox flags                                      ")
 print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX}      "Install Cython toolbox          ")

CppUnitLite/CMakeLists.txt

@@ -12,6 +12,6 @@ gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC
 
 option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
 if (GTSAM_INSTALL_CPPUNITLITE)
-    install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite)
+    install(FILES ${cppunitlite_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/CppUnitLite)
-    install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
+    install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
 endif(GTSAM_INSTALL_CPPUNITLITE)

cmake/FindMKL.cmake

@@ -231,6 +231,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
                         FIND_LIBRARY(MKL_IOMP5_LIBRARY
                           iomp5
                           PATHS
+                                ${MKL_ROOT_DIR}/lib/intel64
                                 ${MKL_ROOT_DIR}/../lib/intel64
                         )
                 ELSE()

cmake/GtsamBuildTypes.cmake

@@ -1,3 +1,8 @@
+# Set cmake policy to recognize the AppleClang compiler
+# independently from the Clang compiler.
+if(POLICY CMP0025)
+  cmake_policy(SET CMP0025 NEW)
+endif()
 
 # function:  list_append_cache(var [new_values ...])
 # Like "list(APPEND ...)" but working for CACHE variables.

cmake/GtsamCythonWrap.cmake

@@ -87,6 +87,15 @@ endfunction()
 #    - output_dir:   The output directory
 function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
   add_library(${target} MODULE ${cpp_file})
+  
+  if(WIN32)
+    # Use .pyd extension instead of .dll on Windows
+    set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
+
+    # Add full path to the Python library
+    target_link_libraries(${target} ${PYTHON_LIBRARIES})
+  endif()
+  
   if(APPLE)
     set(link_flags "-undefined dynamic_lookup")
   endif()

cmake/example_cmake_find_gtsam/main.cpp

@@ -33,7 +33,6 @@
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // We will also use a Between Factor to encode the loop closure constraint
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -69,7 +68,7 @@ int main(int argc, char** argv) {
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+  graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

cython/CMakeLists.txt

@@ -7,6 +7,11 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   add_subdirectory(gtsam_eigency)
   include_directories(${PROJECT_BINARY_DIR}/cython/gtsam_eigency)
 
+  # Fix for error "C1128: number of sections exceeded object file format limit"
+  if(MSVC)
+    add_compile_options(/bigobj)
+  endif()
+
   # wrap gtsam
   add_custom_target(gtsam_header DEPENDS "../gtsam.h")
   wrap_and_install_library_cython("../gtsam.h" # interface_header

cython/gtsam/examples/ImuFactorExample.py

@@ -5,32 +5,27 @@ All Rights Reserved
 
 See LICENSE for the license information
 
-A script validating the ImuFactor inference.
+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
 
-import gtsam
-from gtsam.utils.plot import plot_pose3
 from PreintegrationExample import POSES_FIG, PreintegrationExample
 
-BIAS_KEY = int(gtsam.symbol(ord('b'), 0))
+BIAS_KEY = B(0)
-
-
-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)
 
 
 np.set_printoptions(precision=3, suppress=True)
@@ -76,8 +71,14 @@ class ImuFactorExample(PreintegrationExample):
         initial.insert(BIAS_KEY, self.actualBias)
         for i in range(num_poses):
             state_i = self.scenario.navState(float(i))
-            initial.insert(X(i), state_i.pose())
+
-            initial.insert(V(i), state_i.velocity())
+            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
@@ -88,6 +89,12 @@ class ImuFactorExample(PreintegrationExample):
             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)
@@ -133,6 +140,9 @@ class ImuFactorExample(PreintegrationExample):
             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()

cython/gtsam/examples/ImuFactorExample2.py

@@ -8,27 +8,19 @@ 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
+import matplotlib.pyplot as plt
+import numpy as np
 from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
                    ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
                    PinholeCameraCal3_S2, Point3, Pose3,
                    PriorFactorConstantBias, PriorFactorPose3,
                    PriorFactorVector, Rot3, Values)
-
+from gtsam import symbol_shorthand_B as B
-
+from gtsam import symbol_shorthand_V as V
-def X(key):
+from gtsam import symbol_shorthand_X as X
-    """Create symbol for pose key."""
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
-    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):
@@ -115,7 +107,7 @@ def IMU_example():
     newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors
-    biasKey = gtsam.symbol(ord('b'), 0)
+    biasKey = B(0)
     biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
     biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
                                         biasnoise)

cython/gtsam/examples/PlanarSLAMExample.py

@@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
 
 from __future__ import print_function
 
-import numpy as np
-
 import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 
 # Create noise models
 PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
 graph = gtsam.NonlinearFactorGraph()
 
 # Create the keys corresponding to unknown variables in the factor graph
-X1 = gtsam.symbol(ord('x'), 1)
+X1 = X(1)
-X2 = gtsam.symbol(ord('x'), 2)
+X2 = X(2)
-X3 = gtsam.symbol(ord('x'), 3)
+X3 = X(3)
-L1 = gtsam.symbol(ord('l'), 4)
+L1 = L(4)
-L2 = gtsam.symbol(ord('l'), 5)
+L2 = L(5)
 
 # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
 graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))

cython/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -82,7 +82,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.extractPose2(result)
+    resultPoses = gtsam.utilities_extractPose2(result)
     for i in range(resultPoses.shape[0]):
         plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
     plt.show()

cython/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -65,7 +65,7 @@ else:
     print ("Done!")
 
 if args.plot:
-    resultPoses = gtsam.allPose3s(result)
+    resultPoses = gtsam.utilities_allPose3s(result)
     for i in range(resultPoses.size()):
         plot.plot_pose3(1, resultPoses.atPose3(i))
     plt.show()

cython/gtsam/examples/README.md

@@ -1,7 +1,7 @@
 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))`
+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
 

cython/gtsam/examples/SFMExample.py

@@ -11,17 +11,17 @@ A structure-from-motion problem on a simulated dataset
 from __future__ import print_function
 
 import gtsam
+import matplotlib.pyplot as plt
 import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 from gtsam.examples import SFMdata
 from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
-                         GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
+                         GenericProjectionFactorCal3_S2, Marginals,
-                         Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
+                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
-                         Rot3, PinholeCameraCal3_S2, Values)
+                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
-
+                         SimpleCamera, Values)
-
+from gtsam.utils import plot
-def symbol(name: str, index: int) -> int:
-    """ helper for creating a symbol without explicitly casting 'name' from str to int """
-    return gtsam.symbol(ord(name), index)
 
 
 def main():
@@ -70,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]))
-    factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
+    factor = PriorFactorPose3(X(0), poses[0], pose_noise)
     graph.push_back(factor)
 
     # Simulated measurements from each camera pose, adding them to the factor graph
@@ -79,14 +79,14 @@ def main():
         for j, point in enumerate(points):
             measurement = camera.project(point)
             factor = GenericProjectionFactorCal3_S2(
-                measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
+                measurement, measurement_noise, X(i), L(j), K)
             graph.push_back(factor)
 
     # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
     # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
     # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
     point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
-    factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
+    factor = PriorFactorPoint3(L(0), points[0], point_noise)
     graph.push_back(factor)
     graph.print_('Factor Graph:\n')
 
@@ -94,13 +94,11 @@ def main():
     # Intentionally initialize the variables off from the ground truth
     initial_estimate = Values()
     for i, pose in enumerate(poses):
-        r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
+        transformed_pose = pose.retract(0.1*np.random.randn(6,1))
-        t = Point3(0.05, -0.10, 0.20)
+        initial_estimate.insert(X(i), transformed_pose)
-        transformed_pose = pose.compose(Pose3(r, t))
-        initial_estimate.insert(symbol('x', i), transformed_pose)
     for j, point in enumerate(points):
-        transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
+        transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
-        initial_estimate.insert(symbol('l', j), transformed_point)
+        initial_estimate.insert(L(j), transformed_point)
     initial_estimate.print_('Initial Estimates:\n')
 
     # Optimize the graph and print results
@@ -113,6 +111,11 @@ def main():
     print('initial error = {}'.format(graph.error(initial_estimate)))
     print('final error = {}'.format(graph.error(result)))
 
+    marginals = Marginals(graph, result)
+    plot.plot_3d_points(1, result, marginals=marginals)
+    plot.plot_trajectory(1, result, marginals=marginals, scale=8)
+    plot.set_axes_equal(1)
+    plt.show()
 
 if __name__ == '__main__':
     main()

cython/gtsam/examples/SFMdata.py

@@ -25,14 +25,15 @@ def createPoints():
 
 def createPoses(K):
     # Create the set of ground-truth poses
-    radius = 30.0
+    radius = 40.0
+    height = 10.0
     angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
     up = gtsam.Point3(0, 0, 1)
     target = gtsam.Point3(0, 0, 0)
     poses = []
     for theta in angles:
         position = gtsam.Point3(radius*np.cos(theta),
-                                radius*np.sin(theta), 0.0)
+                                radius*np.sin(theta), height)
         camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
         poses.append(camera.pose())
     return poses

cython/gtsam/examples/SimpleRotation.py

@@ -10,8 +10,9 @@ This example will perform a relatively trivial optimization on
 a single variable with a single factor.
 """
 
-import numpy as np
 import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_X as X
 
 
 def main():
@@ -33,7 +34,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))
-    key = gtsam.symbol(ord('x'), 1)
+    key = X(1)
     factor = gtsam.PriorFactorRot2(key, prior, model)
 
     """

cython/gtsam/examples/VisualISAM2Example.py

@@ -13,23 +13,14 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
 
 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
+import matplotlib.pyplot as plt
+import numpy as np
+from gtsam import symbol_shorthand_L as L
+from gtsam import symbol_shorthand_X as X
 from gtsam.examples import SFMdata
-
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
-
-def 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):

cython/gtsam/examples/VisualISAMExample.py

@@ -19,11 +19,8 @@ from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
                          NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
                          PriorFactorPoint3, PriorFactorPose3, Rot3,
                          PinholeCameraCal3_S2, Values)
-
+from gtsam import symbol_shorthand_L as L
-
+from gtsam import symbol_shorthand_X as X
-def symbol(name: str, index: int) -> int:
-    """ helper for creating a symbol without explicitly casting 'name' from str to int """
-    return gtsam.symbol(ord(name), index)
 
 
 def main():
@@ -58,7 +55,7 @@ def main():
         # Add factors for each landmark observation
         for j, point in enumerate(points):
             measurement = camera.project(point)
-            factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('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
@@ -66,7 +63,7 @@ def main():
         initial_xi = pose.compose(noise)
 
         # Add an initial guess for the current pose
-        initial_estimate.insert(symbol('x', i), initial_xi)
+        initial_estimate.insert(X(i), initial_xi)
 
         # If this is the first iteration, add a prior on the first pose to set the coordinate frame
         # and a prior on the first landmark to set the scale
@@ -75,12 +72,12 @@ def main():
         if i == 0:
             # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
             pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
-            factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
+            factor = PriorFactorPose3(X(0), poses[0], pose_noise)
             graph.push_back(factor)
 
             # Add a prior on landmark l0
             point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
-            factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
+            factor = PriorFactorPoint3(L(0), points[0], point_noise)
             graph.push_back(factor)
 
             # Add initial guesses to all observed landmarks
@@ -88,7 +85,7 @@ def main():
             for j, point in enumerate(points):
                 # Intentionally initialize the variables off from the ground truth
                 initial_lj = points[j].vector() + noise
-                initial_estimate.insert(symbol('l', j), Point3(initial_lj))
+                initial_estimate.insert(L(j), Point3(initial_lj))
         else:
             # Update iSAM with the new factors
             isam.update(graph, initial_estimate)

cython/gtsam/tests/test_FrobeniusFactor.py

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

cython/gtsam/tests/test_GaussianFactorGraph.py

@@ -15,17 +15,18 @@ from __future__ import print_function
 import unittest
 
 import gtsam
+import numpy as np
+from gtsam import symbol_shorthand_X as X
 from gtsam.utils.test_case import GtsamTestCase
 
-import numpy as np
 
 def create_graph():
     """Create a basic linear factor graph for testing"""
     graph = gtsam.GaussianFactorGraph()
     
-    x0 = gtsam.symbol(ord('x'), 0)
+    x0 = X(0)
-    x1 = gtsam.symbol(ord('x'), 1)
+    x1 = X(1)
-    x2 = gtsam.symbol(ord('x'), 2)
+    x2 = X(2)
     
     BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
     PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))

cython/gtsam/tests/test_NonlinearOptimizer.py

@@ -17,7 +17,8 @@ import unittest
 import gtsam
 from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
                    GaussNewtonParams, LevenbergMarquardtOptimizer,
-                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   LevenbergMarquardtParams, PCGSolverParameters,
+                   DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
                    Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
@@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase):
             fg, initial_values, lmParams).optimize()
         self.assertAlmostEqual(0, fg.error(actual2))
 
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setLinearSolverType("ITERATIVE")
+        cgParams = PCGSolverParameters()
+        cgParams.setPreconditionerParams(DummyPreconditionerParameters())
+        lmParams.setIterativeParams(cgParams)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
         # Dogleg
         dlParams = DoglegParams()
         dlParams.setOrdering(ordering)

cython/gtsam/tests/test_PriorFactor.py

@@ -35,5 +35,27 @@ class TestPriorFactor(GtsamTestCase):
         values.insert(key, priorVector)
         self.assertEqual(factor.error(values), 0)
 
+    def test_AddPrior(self):
+        """
+        Test adding prior factors directly to factor graph via the .addPrior method.
+        """
+        # define factor graph
+        graph = gtsam.NonlinearFactorGraph()
+
+        # define and add Pose3 prior
+        key = 5
+        priorPose3 = gtsam.Pose3()
+        model = gtsam.noiseModel_Unit.Create(6)
+        graph.addPriorPose3(key, priorPose3, model)
+        self.assertEqual(graph.size(), 1)
+
+        # define and add Vector prior
+        key = 3
+        priorVector = np.array([0., 0., 0.])
+        model = gtsam.noiseModel_Unit.Create(3)
+        graph.addPriorVector(key, priorVector, model)
+        self.assertEqual(graph.size(), 2)
+
+
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_dsf_map.py

@@ -33,7 +33,7 @@ class TestDSFMap(GtsamTestCase):
         
         # testing the merge feature of dsf
         dsf.merge(pair1, pair2)
-        self.assertEquals(key(dsf.find(pair1)), key(dsf.find(pair2)))
+        self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
 
 
 if __name__ == '__main__':

cython/gtsam/tests/test_logging_optimizer.py

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

cython/gtsam/utils/logging_optimizer.py

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

cython/gtsam/utils/plot.py

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

cython/requirements.txt

@@ -1,3 +1,3 @@
 Cython>=0.25.2
 backports_abc>=0.5
-numpy>=1.12.0
+numpy>=1.11.0

cython/setup.py.in

@@ -35,7 +35,7 @@ setup(
 
     packages=packages,
     package_data={package:
-        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')]
+        [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.pyd')]
         for package in packages
     },
     install_requires=[line.strip() for line in '''

debian/README.md

@@ -1,12 +0,0 @@
-# How to build a GTSAM debian package
-
-To use the ``debuild`` command, install the ``devscripts`` package
-
-    sudo apt install devscripts
-
-Change into the gtsam directory, then run:
-
-    debuild -us -uc -j4
-
-Adjust the ``-j4`` depending on how many CPUs you want to build on in
-parallel. 

debian/changelog

@@ -1,5 +0,0 @@
-gtsam (4.0.0-1berndpfrommer) bionic; urgency=medium
-
-  * initial release
-
- -- Bernd Pfrommer <bernd.pfrommer@gmail.com>  Wed, 18 Jul 2018 20:36:44 -0400

debian/compat

@@ -1 +0,0 @@
-9

debian/control

@@ -1,15 +0,0 @@
-Source: gtsam
-Section: libs
-Priority: optional
-Maintainer: Frank Dellaert <frank@cc.gatech.edu>
-Uploaders: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>, Bernd Pfrommer <bernd.pfrommer@gmail.com>
-Build-Depends: cmake, libboost-all-dev (>= 1.58), libeigen3-dev, libtbb-dev, debhelper (>=9)
-Standards-Version: 3.9.7
-Homepage: https://github.com/borglab/gtsam
-Vcs-Browser: https://github.com/borglab/gtsam
-
-Package: libgtsam-dev
-Architecture: any
-Depends: ${shlibs:Depends}, ${misc:Depends}, libboost-serialization-dev, libboost-system-dev, libboost-filesystem-dev, libboost-thread-dev, libboost-program-options-dev, libboost-date-time-dev, libboost-timer-dev, libboost-chrono-dev, libboost-regex-dev
-Description: Georgia Tech Smoothing and Mapping Library
- gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications

debian/copyright

@@ -1,15 +0,0 @@
-Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
-Upstream-Name: gtsam
-Source: https://bitbucket.org/gtborg/gtsam.git
-
-Files: *
-Copyright: 2017, Frank Dellaert
-License: BSD
-
-Files: gtsam/3rdparty/CCOLAMD/*
-Copyright: 2005-2011, Univ. of Florida.  Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore.  Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. http://www.cise.ufl.edu/research/sparse
-License: GNU LESSER GENERAL PUBLIC LICENSE
-
-Files: gtsam/3rdparty/Eigen/*
-Copyright: 2017, Multiple Authors
-License: MPL2

debian/rules

@@ -1,25 +0,0 @@
-#!/usr/bin/make -f
-# See debhelper(7) (uncomment to enable)
-# output every command that modifies files on the build system.
-export DH_VERBOSE = 1
-
-
-# see FEATURE AREAS in dpkg-buildflags(1)
-#export DEB_BUILD_MAINT_OPTIONS = hardening=+all
-
-# see ENVIRONMENT in dpkg-buildflags(1)
-# package maintainers to append CFLAGS
-#export DEB_CFLAGS_MAINT_APPEND  = -Wall -pedantic
-# package maintainers to append LDFLAGS
-#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed
-
-
-%:
-	dh $@ --parallel
-
-
-# dh_make generated override targets
-# This is example for Cmake (See https://bugs.debian.org/641051 )
-override_dh_auto_configure:
-	dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF
-

debian/source/format

@@ -1 +0,0 @@
-3.0 (quilt)

doc/Code/OdometryExample.cpp

@@ -5,7 +5,7 @@ NonlinearFactorGraph graph;
 Pose2 priorMean(0.0, 0.0, 0.0);
 noiseModel::Diagonal::shared_ptr priorNoise =
   noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+graph.addPrior(1, priorMean, priorNoise);
 
 // Add two odometry factors
 Pose2 odometry(2.0, 0.0, 0.0);

doc/Code/Pose2SLAMExample.cpp

@@ -1,7 +1,7 @@
 NonlinearFactorGraph graph;
 noiseModel::Diagonal::shared_ptr priorNoise =
   noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
+graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
 // Add odometry factors
 noiseModel::Diagonal::shared_ptr model =

doc/gtsam-coordinate-frames.lyx

@@ -2291,15 +2291,11 @@ uncalibration
  used in the residual).
 \end_layout
 
-\begin_layout Standard
-\begin_inset Note Note
-status collapsed
-
 \begin_layout Section
 Noise models of prior factors
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 The simplest way to describe noise models is by an example.
  Let's take a prior factor on a 3D pose 
 \begin_inset Formula $x\in\SE 3$
@@ -2353,7 +2349,7 @@ e\left(x\right)=\norm{h\left(x\right)}_{\Sigma}^{2}=h\left(x\right)^{\t}\Sigma^{
  useful answer out quickly ]
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 The density induced by a noise model on the prior factor is Gaussian in
  the tangent space about the linearization point.
  Suppose that the pose is linearized at 
@@ -2431,7 +2427,7 @@ Here we see that the update
 .
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 This means that to draw random pose samples, we actually draw random samples
  of 
 \begin_inset Formula $\delta x$
@@ -2456,7 +2452,7 @@ This means that to draw random pose samples, we actually draw random samples
 Noise models of between factors
 \end_layout
 
-\begin_layout Plain Layout
+\begin_layout Standard
 The noise model of a BetweenFactor is a bit more complicated.
  The unwhitened error is
 \begin_inset Formula 
@@ -2516,11 +2512,6 @@ e\left(\delta x_{1}\right) & \approx\norm{\log\left(z^{-1}\left(x_{1}\exp\delta
 \end_inset
 
 
-\end_layout
-
-\end_inset
-
-
 \end_layout
 
 \end_body

doc/math.lyx

@@ -1,7 +1,9 @@
-#LyX 2.1 created this file. For more info see http://www.lyx.org/
+#LyX 2.3 created this file. For more info see http://www.lyx.org/
-\lyxformat 474
+\lyxformat 544
 \begin_document
 \begin_header
+\save_transient_properties true
+\origin unavailable
 \textclass article
 \use_default_options false
 \begin_modules
@@ -14,16 +16,18 @@ theorems-ams-bytype
 \language_package default
 \inputencoding auto
 \fontencoding global
-\font_roman times
+\font_roman "times" "default"
-\font_sans default
+\font_sans "default" "default"
-\font_typewriter default
+\font_typewriter "default" "default"
-\font_math auto
+\font_math "auto" "auto"
 \font_default_family rmdefault
 \use_non_tex_fonts false
 \font_sc false
 \font_osf false
-\font_sf_scale 100
+\font_sf_scale 100 100
-\font_tt_scale 100
+\font_tt_scale 100 100
+\use_microtype false
+\use_dash_ligatures true
 \graphics default
 \default_output_format default
 \output_sync 0
@@ -53,6 +57,7 @@ theorems-ams-bytype
 \suppress_date false
 \justification true
 \use_refstyle 0
+\use_minted 0
 \index Index
 \shortcut idx
 \color #008000
@@ -65,7 +70,10 @@ theorems-ams-bytype
 \tocdepth 3
 \paragraph_separation indent
 \paragraph_indentation default
-\quotes_language english
+\is_math_indent 0
+\math_numbering_side default
+\quotes_style english
+\dynamic_quotes 0
 \papercolumns 1
 \papersides 1
 \paperpagestyle default
@@ -98,6 +106,11 @@ width "100col%"
 special "none"
 height "1in"
 height_special "totalheight"
+thickness "0.4pt"
+separation "3pt"
+shadowsize "4pt"
+framecolor "black"
+backgroundcolor "none"
 status collapsed
 
 \begin_layout Plain Layout
@@ -654,6 +667,7 @@ reference "eq:LocalBehavior"
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -934,6 +948,7 @@ See
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -1025,6 +1040,7 @@ See
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -2209,6 +2225,7 @@ instantaneous velocity
 LatexCommand cite
 after "page 51 for rotations, page 419 for SE(3)"
 key "Murray94book"
+literal "true"
 
 \end_inset
 
@@ -2965,7 +2982,7 @@ B^{T} & I_{3}\end{array}\right]
 \begin_layout Subsection
 \begin_inset CommandInset label
 LatexCommand label
-name "sub:Pushforward-of-Between"
+name "subsec:Pushforward-of-Between"
 
 \end_inset
 
@@ -3419,6 +3436,7 @@ A retraction
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Absil07book"
+literal "true"
 
 \end_inset
 
@@ -3873,7 +3891,7 @@ BetweenFactor
 , derived in Section 
 \begin_inset CommandInset ref
 LatexCommand ref
-reference "sub:Pushforward-of-Between"
+reference "subsec:Pushforward-of-Between"
 
 \end_inset
 
@@ -4502,7 +4520,7 @@ reference "Th:InverseAction"
 \begin_layout Subsection
 \begin_inset CommandInset label
 LatexCommand label
-name "sub:3DAngularVelocities"
+name "subsec:3DAngularVelocities"
 
 \end_inset
 
@@ -4695,6 +4713,7 @@ Absil
 LatexCommand cite
 after "page 58"
 key "Absil07book"
+literal "true"
 
 \end_inset
 
@@ -5395,6 +5414,7 @@ While not a Lie group, we can define an exponential map, which is given
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Ma01ijcv"
+literal "true"
 
 \end_inset
 
@@ -5402,6 +5422,7 @@ key "Ma01ijcv"
 \begin_inset CommandInset href
 LatexCommand href
 name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf"
+literal "false"
 
 \end_inset
 
@@ -5605,6 +5626,7 @@ The exponential map uses
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Absil07book"
+literal "true"
 
 \end_inset
 
@@ -6293,7 +6315,7 @@ d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)
 \end_layout
 
 \begin_layout Section
-Line3 (Ocaml)
+Line3
 \end_layout
 
 \begin_layout Standard
@@ -6345,6 +6367,14 @@ R'=R(I+\Omega)
 
 \end_inset
 
+
+\end_layout
+
+\begin_layout Subsection
+Projecting Line3
+\end_layout
+
+\begin_layout Standard
 Projecting a line to 2D can be done easily, as both 
 \begin_inset Formula $v$
 \end_inset
@@ -6430,13 +6460,21 @@ or the
 
 \end_layout
 
+\begin_layout Subsection
+Action of 
+\begin_inset Formula $\SEthree$
+\end_inset
+
+ on the line
+\end_layout
+
 \begin_layout Standard
 Transforming a 3D line 
 \begin_inset Formula $(R,(a,b))$
 \end_inset
 
  from a world coordinate frame to a camera frame 
-\begin_inset Formula $(R_{w}^{c},t^{w})$
+\begin_inset Formula $T_{c}^{w}=(R_{c}^{w},t^{w})$
 \end_inset
 
  is done by
@@ -6466,17 +6504,115 @@ b'=b-R_{2}^{T}t^{w}
 
 \end_inset
 
-Again, we need to redo the derivatives, as R is incremented from the right.
+where 
- The first argument is incremented from the left, but the result is incremented
+\begin_inset Formula $R_{1}$
- on the right:
+\end_inset
+
+ and 
+\begin_inset Formula $R_{2}$
+\end_inset
+
+ are the columns of 
+\begin_inset Formula $R$
+\end_inset
+
+ , as before.
+ 
+\end_layout
+
+\begin_layout Standard
+To find the derivatives, the transformation of a line 
+\begin_inset Formula $l^{w}=(R,a,b)$
+\end_inset
+
+ from world coordinates to a camera coordinate frame 
+\begin_inset Formula $T_{c}^{w}$
+\end_inset
+
+, specified in world coordinates, can be written as a function 
+\begin_inset Formula $f:\SEthree\times L\rightarrow L$
+\end_inset
+
+, as given above, i.e., 
 \begin_inset Formula 
-\begin{eqnarray*}
+\[
-R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\
+f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right).
-I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
+\]
-\Omega' & = & R'^{T}\Skew{S\omega}R'\\
+
-\Omega' & = & \Skew{R'^{T}S\omega}\\
+\end_inset
-\omega' & = & R'^{T}S\omega
+
-\end{eqnarray*}
+Let us find the Jacobian 
+\begin_inset Formula $J_{1}$
+\end_inset
+
+ of 
+\begin_inset Formula $f$
+\end_inset
+
+ with respect to the first argument 
+\begin_inset Formula $T_{c}^{w}$
+\end_inset
+
+, which should obey
+\begin_inset Formula 
+\begin{align*}
+f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi
+\end{align*}
+
+\end_inset
+
+Note that
+\begin_inset Formula 
+\[
+T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc}
+R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\
+0 & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+Let's write this out separately for each of 
+\begin_inset Formula $R,a,b$
+\end_inset
+
+:
+\begin_inset Formula 
+\begin{align*}
+\left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\
+a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\
+b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v
+\end{align*}
+
+\end_inset
+
+Simplifying, we get:
+\begin_inset Formula 
+\begin{align*}
+-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
+-R_{1}^{T}R_{c}^{w} & \approx J_{av}\\
+-R_{2}^{T}R_{c}^{w} & \approx J_{bv}
+\end{align*}
+
+\end_inset
+
+which gives the expressions for 
+\begin_inset Formula $J_{av}$
+\end_inset
+
+ and 
+\begin_inset Formula $J_{bv}$
+\end_inset
+
+.
+ The top line can be further simplified:
+\begin_inset Formula 
+\begin{align*}
+-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
+-R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\
+-\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\
+-R'^{T} & \approx J_{R\omega}
+\end{align*}
 
 \end_inset
 
@@ -6687,6 +6823,7 @@ Spivak
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Spivak65book"
+literal "true"
 
 \end_inset
 
@@ -6795,6 +6932,7 @@ The following is adapted from Appendix A in
 \begin_inset CommandInset citation
 LatexCommand cite
 key "Murray94book"
+literal "true"
 
 \end_inset
 
@@ -6924,6 +7062,7 @@ might be
 LatexCommand cite
 after "page 45"
 key "Hall00book"
+literal "true"
 
 \end_inset
 

examples/CMakeLists.txt

@@ -1,7 +1,4 @@
 set (excluded_examples
-    DiscreteBayesNet_FG.cpp
-    UGM_chain.cpp
-    UGM_small.cpp
     elaboratePoint2KalmanFilter.cpp
 )
 

examples/Data/Klaus3.g2o

@@ -0,0 +1,6 @@
+VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109
+VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
+VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
+EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
+EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
+EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0

examples/Data/toyExample.g2o

@@ -0,0 +1,11 @@
+VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
+VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1
+VERTEX_SE3:QUAT 2 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
+VERTEX_SE3:QUAT 3 0 0 0 -0.00499994 -0.00499994 -0.00499994 0.999963
+VERTEX_SE3:QUAT 4 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
+EDGE_SE3:QUAT 1 2 1 2 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 2 3 -3.26795e-07 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 3 4 1 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 3 1 6.9282e-07 2 0 0 0 1 1.73205e-07 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 1 4 -1 1 0 0 0 -0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
+EDGE_SE3:QUAT 0 1 0 0 0 0 0 0 1 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100

examples/DiscreteBayesNetExample.cpp

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  DiscreteBayesNetExample.cpp
+ * @brief   Discrete Bayes Net example with famous Asia Bayes Network
+ * @author  Frank Dellaert
+ * @date  JULY 10, 2020
+ */
+
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
+#include <gtsam/inference/BayesNet-inst.h>
+
+#include <iomanip>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char **argv) {
+  DiscreteBayesNet asia;
+  DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
+      Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
+  asia.add(Asia % "99/1");
+  asia.add(Smoking % "50/50");
+
+  asia.add(Tuberculosis | Asia = "99/1 95/5");
+  asia.add(LungCancer | Smoking = "99/1 90/10");
+  asia.add(Bronchitis | Smoking = "70/30 40/60");
+
+  asia.add((Either | Tuberculosis, LungCancer) = "F T T T");
+
+  asia.add(XRay | Either = "95/5 2/98");
+  asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");
+
+  // print
+  vector<string> pretty = {"Asia",    "Dyspnea", "XRay",       "Tuberculosis",
+                           "Smoking", "Either",  "LungCancer", "Bronchitis"};
+  auto formatter = [pretty](Key key) { return pretty[key]; };
+  asia.print("Asia", formatter);
+
+  // Convert to factor graph
+  DiscreteFactorGraph fg(asia);
+
+  // Create solver and eliminate
+  Ordering ordering;
+  ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
+  DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
+
+  // solve
+  DiscreteFactor::sharedValues mpe = chordal->optimize();
+  GTSAM_PRINT(*mpe);
+
+  // We can also build a Bayes tree (directed junction tree).
+  // The elimination order above will do fine:
+  auto bayesTree = fg.eliminateMultifrontal(ordering);
+  bayesTree->print("bayesTree", formatter);
+
+  // add evidence, we were in Asia and we have dyspnea
+  fg.add(Asia, "0 1");
+  fg.add(Dyspnea, "0 1");
+
+  // solve again, now with evidence
+  DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
+  DiscreteFactor::sharedValues mpe2 = chordal2->optimize();
+  GTSAM_PRINT(*mpe2);
+
+  // We can also sample from it
+  cout << "\n10 samples:" << endl;
+  for (size_t i = 0; i < 10; i++) {
+    DiscreteFactor::sharedValues sample = chordal2->sample();
+    GTSAM_PRINT(*sample);
+  }
+  return 0;
+}

examples/DiscreteBayesNet_FG.cpp

@@ -15,29 +15,38 @@
  * @author  Abhijit
  * @date  Jun 4, 2012
  *
- * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529]
+ * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009,
- * You may be familiar with other graphical model packages like BNT (available
+ * p529] You may be familiar with other graphical model packages like BNT
- * at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this is used as an
+ * (available at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this
- * example. The following demo is same as that in the above link, except that
+ * is used as an example. The following demo is same as that in the above link,
- * everything is using GTSAM.
+ * except that everything is using GTSAM.
  */
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteSequentialSolver.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
+
 #include <iomanip>
 
 using namespace std;
 using namespace gtsam;
 
 int main(int argc, char **argv) {
+  // Define keys and a print function
+  Key C(1), S(2), R(3), W(4);
+  auto print = [=](DiscreteFactor::sharedValues values) {
+    cout << boolalpha << "Cloudy = " << static_cast<bool>((*values)[C])
+         << "  Sprinkler = " << static_cast<bool>((*values)[S])
+         << "  Rain = " << boolalpha << static_cast<bool>((*values)[R])
+         << "  WetGrass = " << static_cast<bool>((*values)[W]) << endl;
+  };
 
   // We assume binary state variables
   // we have 0 == "False" and 1 == "True"
   const size_t nrStates = 2;
 
   // define variables
-  DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates),
+  DiscreteKey Cloudy(C, nrStates), Sprinkler(S, nrStates), Rain(R, nrStates),
-      WetGrass(4, nrStates);
+      WetGrass(W, nrStates);
 
   // create Factor Graph of the bayes net
   DiscreteFactorGraph graph;
@@ -49,8 +58,9 @@ int main(int argc, char **argv) {
   graph.add(Sprinkler & Rain & WetGrass,
             "1 0 0.1 0.9 0.1 0.9 0.001 0.99");  // P(WetGrass | Sprinkler, Rain)
 
-  // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional
+  // Alternatively we can also create a DiscreteBayesNet, add
-  // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp)
+  // DiscreteConditional factors and create a FactorGraph from it. (See
+  // testDiscreteBayesNet.cpp)
 
   // Since this is a relatively small distribution, we can as well print
   // the whole distribution..
@@ -63,57 +73,48 @@ int main(int argc, char **argv) {
       for (size_t h = 0; h < nrStates; h++)
         for (size_t c = 0; c < nrStates; c++) {
           DiscreteFactor::Values values;
-          values[Cloudy.first] = c;
+          values[C] = c;
-          values[Sprinkler.first] = h;
+          values[S] = h;
-          values[Rain.first] = m;
+          values[R] = m;
-          values[WetGrass.first] = a;
+          values[W] = a;
           double prodPot = graph(values);
-          cout << boolalpha << setw(8) << (bool) c << setw(14)
+          cout << setw(8) << static_cast<bool>(c) << setw(14)
-              << (bool) h << setw(12) << (bool) m << setw(13)
+               << static_cast<bool>(h) << setw(12) << static_cast<bool>(m)
-              << (bool) a << setw(16) << prodPot << endl;
+               << setw(13) << static_cast<bool>(a) << setw(16) << prodPot
+               << endl;
         }
 
-
   // "Most Probable Explanation", i.e., configuration with largest value
-  DiscreteSequentialSolver solver(graph);
+  DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize();
-  DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
   cout << "\nMost Probable Explanation (MPE):" << endl;
-  cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first]
+  print(mpe);
-                  << "  Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first]
-                  << "  Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first]
-                  << "  WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl;
 
+  // "Inference" We show an inference query like: probability that the Sprinkler
+  // was on; given that the grass is wet i.e. P( S | C=0) = ?
 
-  // "Inference" We show an inference query like: probability that the Sprinkler was on;
+  // add evidence that it is not Cloudy
-  // given that the grass is wet i.e. P( S | W=1) =?
+  graph.add(Cloudy, "1 0");
-  cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl;
 
-  // Method 1: we can compute the joint marginal P(S,W) and from that we can compute
+  // solve again, now with evidence
-  // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps..
+  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
+  DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize();
 
-  //Step1: Compute P(S,W)
+  cout << "\nMPE given C=0:" << endl;
-  DiscreteFactorGraph jointFG;
+  print(mpe_with_evidence);
-  jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices());
-  DecisionTreeFactor probSW = jointFG.product();
 
-  //Step2: Compute P(W)
+  // we can also calculate arbitrary marginals:
-  DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first);
+  DiscreteMarginals marginals(graph);
-
+  cout << "\nP(S=1|C=0):" << marginals.marginalProbabilities(Sprinkler)[1]
-  //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1)
+       << endl;
-  DiscreteFactor::Values values;
+  cout << "\nP(R=0|C=0):" << marginals.marginalProbabilities(Rain)[0] << endl;
-  values[WetGrass.first] = 1;
+  cout << "\nP(W=1|C=0):" << marginals.marginalProbabilities(WetGrass)[1]
-
+       << endl;
-  //print P(S=0|W=1)
-  values[Sprinkler.first] = 0;
-  cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl;
-
-  //print P(S=1|W=1)
-  values[Sprinkler.first] = 1;
-  cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl;
-
-  // TODO: Method 2 : One way is to modify the factor graph to
-  // incorporate the evidence node and compute the marginal
-  // TODO: graph.addEvidence(Cloudy,0);
 
+  // We can also sample from it
+  cout << "\n10 samples:" << endl;
+  for (size_t i = 0; i < 10; i++) {
+    DiscreteFactor::sharedValues sample = chordal->sample();
+    print(sample);
+  }
   return 0;
 }

examples/FisheyeExample.cpp

@@ -0,0 +1,130 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    FisheyeExample.cpp
+ * @brief   A visualSLAM example for the structure-from-motion problem on a
+ * simulated dataset. This version uses a fisheye camera model and a GaussNewton
+ * solver to solve the graph in one batch
+ * @author  ghaggin
+ * @Date    Apr 9,2020
+ */
+
+/**
+ * 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
+ */
+
+// For loading the data
+#include "SFMdata.h"
+
+// Camera observations of landmarks will be stored as Point2 (x, y).
+#include <gtsam/geometry/Point2.h>
+
+// Each variable in the system (poses and landmarks) must be identified with a
+// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
+// (X1, X2, L1). Here we will use Symbols
+#include <gtsam/inference/Symbol.h>
+
+// Use GaussNewtonOptimizer to solve graph
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+
+// In GTSAM, measurement functions are represented as 'factors'. Several common
+// factors have been provided with the library for solving robotics/SLAM/Bundle
+// Adjustment problems. Here we will use Projection factors to model the
+// camera's landmark observations. Also, we will initialize the robot at some
+// location using a Prior factor.
+#include <gtsam/geometry/Cal3Fisheye.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <fstream>
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+using symbol_shorthand::L;  // for landmarks
+using symbol_shorthand::X;  // for poses
+
+/* ************************************************************************* */
+int main(int argc, char *argv[]) {
+  // Define the camera calibration parameters
+  auto K = boost::make_shared<Cal3Fisheye>(
+      278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035,
+      0.020727425669427896, -0.012786476702685545, 0.0025242267320687625);
+
+  // Define the camera observation noise model, 1 pixel stddev
+  auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+  // Create the set of ground-truth landmarks
+  const vector<Point3> points = createPoints();
+
+  // Create the set of ground-truth poses
+  const vector<Pose3> poses = createPoses();
+
+  // Create a Factor Graph and Values to hold the new data
+  NonlinearFactorGraph graph;
+  Values initialEstimate;
+
+  // Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
+  auto posePrior = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
+  graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], posePrior);
+
+  // Add a prior on landmark l0
+  auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.emplace_shared<PriorFactor<Point3>>(L(0), points[0], pointPrior);
+
+  // Add initial guesses to all observed landmarks
+  // Intentionally initialize the variables off from the ground truth
+  static const Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+  for (size_t j = 0; j < points.size(); ++j)
+    initialEstimate.insert<Point3>(L(j), points[j] + kDeltaPoint);
+
+  // Loop over the poses, adding the observations to the graph
+  for (size_t i = 0; i < poses.size(); ++i) {
+    // Add factors for each landmark observation
+    for (size_t j = 0; j < points.size(); ++j) {
+      PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
+      Point2 measurement = camera.project(points[j]);
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
+          measurement, measurementNoise, X(i), L(j), K);
+    }
+
+    // Add an initial guess for the current pose
+    // Intentionally initialize the variables off from the ground truth
+    static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                  Point3(0.05, -0.10, 0.20));
+    initialEstimate.insert(X(i), poses[i] * kDeltaPose);
+  }
+
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION");
+  params.maxIterations = 10000;
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
+  std::cout << "final error=" << graph.error(result) << std::endl;
+
+  std::ofstream os("examples/vio_batch.dot");
+  graph.saveGraph(os, result);
+
+  return 0;
+}
+/* ************************************************************************* */

examples/HMMExample.cpp

@@ -0,0 +1,94 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file  DiscreteBayesNetExample.cpp
+ * @brief   Hidden Markov Model example, discrete.
+ * @author  Frank Dellaert
+ * @date  July 12, 2020
+ */
+
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
+#include <gtsam/inference/BayesNet-inst.h>
+
+#include <iomanip>
+#include <sstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char **argv) {
+  const int nrNodes = 4;
+  const size_t nrStates = 3;
+
+  // Define variables as well as ordering
+  Ordering ordering;
+  vector<DiscreteKey> keys;
+  for (int k = 0; k < nrNodes; k++) {
+    DiscreteKey key_i(k, nrStates);
+    keys.push_back(key_i);
+    ordering.emplace_back(k);
+  }
+
+  // Create HMM as a DiscreteBayesNet
+  DiscreteBayesNet hmm;
+
+  // Define backbone
+  const string transition = "8/1/1 1/8/1 1/1/8";
+  for (int k = 1; k < nrNodes; k++) {
+    hmm.add(keys[k] | keys[k - 1] = transition);
+  }
+
+  // Add some measurements, not needed for all time steps!
+  hmm.add(keys[0] % "7/2/1");
+  hmm.add(keys[1] % "1/9/0");
+  hmm.add(keys.back() % "5/4/1");
+
+  // print
+  hmm.print("HMM");
+
+  // Convert to factor graph
+  DiscreteFactorGraph factorGraph(hmm);
+
+  // Create solver and eliminate
+  // This will create a DAG ordered with arrow of time reversed
+  DiscreteBayesNet::shared_ptr chordal =
+      factorGraph.eliminateSequential(ordering);
+  chordal->print("Eliminated");
+
+  // solve
+  DiscreteFactor::sharedValues mpe = chordal->optimize();
+  GTSAM_PRINT(*mpe);
+
+  // We can also sample from it
+  cout << "\n10 samples:" << endl;
+  for (size_t k = 0; k < 10; k++) {
+    DiscreteFactor::sharedValues sample = chordal->sample();
+    GTSAM_PRINT(*sample);
+  }
+
+  // Or compute the marginals. This re-eliminates the FG into a Bayes tree
+  cout << "\nComputing Node Marginals .." << endl;
+  DiscreteMarginals marginals(factorGraph);
+  for (int k = 0; k < nrNodes; k++) {
+    Vector margProbs = marginals.marginalProbabilities(keys[k]);
+    stringstream ss;
+    ss << "marginal " << k;
+    print(margProbs, ss.str());
+  }
+
+  // TODO(frank): put in the glue to have DiscreteMarginals produce *arbitrary*
+  // joints efficiently, by the Bayes tree shortcut magic. All the code is there
+  // but it's not yet connected.
+
+  return 0;
+}

examples/IMUKittiExampleGPS.cpp

@@ -0,0 +1,359 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file IMUKittiExampleGPS
+ * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
+ * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
+ */
+
+// GTSAM related includes.
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/ISAM2Params.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <cstring>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+
+struct KittiCalibration {
+    double body_ptx;
+    double body_pty;
+    double body_ptz;
+    double body_prx;
+    double body_pry;
+    double body_prz;
+    double accelerometer_sigma;
+    double gyroscope_sigma;
+    double integration_sigma;
+    double accelerometer_bias_sigma;
+    double gyroscope_bias_sigma;
+    double average_delta_t;
+};
+
+struct ImuMeasurement {
+    double time;
+    double dt;
+    Vector3 accelerometer;
+    Vector3 gyroscope;  // omega
+};
+
+struct GpsMeasurement {
+    double time;
+    Vector3 position;  // x,y,z
+};
+
+const string output_filename = "IMUKittiExampleGPSResults.csv";
+
+void loadKittiData(KittiCalibration& kitti_calibration,
+                   vector<ImuMeasurement>& imu_measurements,
+                   vector<GpsMeasurement>& gps_measurements) {
+    string line;
+
+    // Read IMU metadata and compute relative sensor pose transforms
+    // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
+    // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
+    string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
+    ifstream imu_metadata(imu_metadata_file.c_str());
+
+    printf("-- Reading sensor metadata\n");
+
+    getline(imu_metadata, line, '\n');  // ignore the first line
+
+    // Load Kitti calibration
+    getline(imu_metadata, line, '\n');
+    sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
+           &kitti_calibration.body_ptx,
+           &kitti_calibration.body_pty,
+           &kitti_calibration.body_ptz,
+           &kitti_calibration.body_prx,
+           &kitti_calibration.body_pry,
+           &kitti_calibration.body_prz,
+           &kitti_calibration.accelerometer_sigma,
+           &kitti_calibration.gyroscope_sigma,
+           &kitti_calibration.integration_sigma,
+           &kitti_calibration.accelerometer_bias_sigma,
+           &kitti_calibration.gyroscope_bias_sigma,
+           &kitti_calibration.average_delta_t);
+    printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
+           kitti_calibration.body_ptx,
+           kitti_calibration.body_pty,
+           kitti_calibration.body_ptz,
+           kitti_calibration.body_prx,
+           kitti_calibration.body_pry,
+           kitti_calibration.body_prz,
+           kitti_calibration.accelerometer_sigma,
+           kitti_calibration.gyroscope_sigma,
+           kitti_calibration.integration_sigma,
+           kitti_calibration.accelerometer_bias_sigma,
+           kitti_calibration.gyroscope_bias_sigma,
+           kitti_calibration.average_delta_t);
+
+    // Read IMU data
+    // Time dt accelX accelY accelZ omegaX omegaY omegaZ
+    string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
+    printf("-- Reading IMU measurements from file\n");
+    {
+        ifstream imu_data(imu_data_file.c_str());
+        getline(imu_data, line, '\n');  // ignore the first line
+
+        double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
+        while (!imu_data.eof()) {
+            getline(imu_data, line, '\n');
+            sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
+                   &time, &dt,
+                   &acc_x, &acc_y, &acc_z,
+                   &gyro_x, &gyro_y, &gyro_z);
+
+            ImuMeasurement measurement;
+            measurement.time = time;
+            measurement.dt = dt;
+            measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
+            measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
+            imu_measurements.push_back(measurement);
+        }
+    }
+
+    // Read GPS data
+    // Time,X,Y,Z
+    string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
+    printf("-- Reading GPS measurements from file\n");
+    {
+        ifstream gps_data(gps_data_file.c_str());
+        getline(gps_data, line, '\n');  // ignore the first line
+
+        double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
+        while (!gps_data.eof()) {
+            getline(gps_data, line, '\n');
+            sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
+
+            GpsMeasurement measurement;
+            measurement.time = time;
+            measurement.position = Vector3(gps_x, gps_y, gps_z);
+            gps_measurements.push_back(measurement);
+        }
+    }
+}
+
+int main(int argc, char* argv[]) {
+    KittiCalibration kitti_calibration;
+    vector<ImuMeasurement> imu_measurements;
+    vector<GpsMeasurement> gps_measurements;
+    loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
+
+    Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
+                                  kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
+                    .finished();
+    auto body_T_imu = Pose3::Expmap(BodyP);
+    if (!body_T_imu.equals(Pose3(), 1e-5)) {
+        printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
+        exit(-1);
+    }
+
+    // Configure different variables
+    // double t_offset = gps_measurements[0].time;
+    size_t first_gps_pose = 1;
+    size_t gps_skip = 10;  // Skip this many GPS measurements each time
+    double g = 9.8;
+    auto w_coriolis = Vector3::Zero();  // zero vector
+
+    // Configure noise models
+    auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+                                                                          Vector3::Constant(1.0/0.07))
+                                                            .finished());
+
+    // Set initial conditions for the estimated trajectory
+    // initial pose is the reference frame (navigation frame)
+    auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
+    // the vehicle is stationary at the beginning at position 0,0,0
+    Vector3 current_velocity_global = Vector3::Zero();
+    auto current_bias = imuBias::ConstantBias();  // init with zero bias
+
+    auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
+                                                                       Vector3::Constant(1.0))
+                                                         .finished());
+    auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
+    auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
+                                                                   Vector3::Constant(5.00e-05))
+                                                     .finished());
+
+    // Set IMU preintegration parameters
+    Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
+    Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
+    // error committed in integrating position from velocities
+    Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
+
+    auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
+    imu_params->accelerometerCovariance = measured_acc_cov;     // acc white noise in continuous
+    imu_params->integrationCovariance = integration_error_cov;  // integration uncertainty continuous
+    imu_params->gyroscopeCovariance = measured_omega_cov;       // gyro white noise in continuous
+    imu_params->omegaCoriolis = w_coriolis;
+
+    std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
+
+    // Set ISAM2 parameters and create ISAM2 solver object
+    ISAM2Params isam_params;
+    isam_params.factorization = ISAM2Params::CHOLESKY;
+    isam_params.relinearizeSkip = 10;
+
+    ISAM2 isam(isam_params);
+
+    // Create the factor graph and values object that will store new factors and values to add to the incremental graph
+    NonlinearFactorGraph new_factors;
+    Values new_values;  // values storing the initial estimates of new nodes in the factor graph
+
+    /// Main loop:
+    /// (1) we read the measurements
+    /// (2) we create the corresponding factors in the graph
+    /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
+    printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
+    size_t j = 0;
+    for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
+        // At each non=IMU measurement we initialize a new node in the graph
+        auto current_pose_key = X(i);
+        auto current_vel_key = V(i);
+        auto current_bias_key = B(i);
+        double t = gps_measurements[i].time;
+
+        if (i == first_gps_pose) {
+            // Create initial estimate and prior on initial pose, velocity, and biases
+            new_values.insert(current_pose_key, current_pose_global);
+            new_values.insert(current_vel_key, current_velocity_global);
+            new_values.insert(current_bias_key, current_bias);
+            new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
+            new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
+            new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
+        } else {
+            double t_previous = gps_measurements[i-1].time;
+
+            // Summarize IMU data between the previous GPS measurement and now
+            current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
+            static size_t included_imu_measurement_count = 0;
+            while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
+                if (imu_measurements[j].time >= t_previous) {
+                    current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
+                                                                         imu_measurements[j].gyroscope,
+                                                                         imu_measurements[j].dt);
+                    included_imu_measurement_count++;
+                }
+                j++;
+            }
+
+            // Create IMU factor
+            auto previous_pose_key = X(i-1);
+            auto previous_vel_key = V(i-1);
+            auto previous_bias_key = B(i-1);
+
+            new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
+                                                  current_pose_key, current_vel_key,
+                                                  previous_bias_key, *current_summarized_measurement);
+
+            // Bias evolution as given in the IMU metadata
+            auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
+                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
+                   Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
+                 .finished());
+            new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
+                                                                             current_bias_key,
+                                                                             imuBias::ConstantBias(),
+                                                                             sigma_between_b);
+
+            // Create GPS factor
+            auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
+            if ((i % gps_skip) == 0) {
+                new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
+                new_values.insert(current_pose_key, gps_pose);
+
+                printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
+                gps_pose.translation().print();
+                printf("\n\n");
+            } else {
+                new_values.insert(current_pose_key, current_pose_global);
+            }
+
+            // Add initial values for velocity and bias based on the previous estimates
+            new_values.insert(current_vel_key, current_velocity_global);
+            new_values.insert(current_bias_key, current_bias);
+
+            // Update solver
+            // =======================================================================
+            // We accumulate 2*GPSskip GPS measurements before updating the solver at
+            // first so that the heading becomes observable.
+            if (i > (first_gps_pose + 2*gps_skip)) {
+                printf("################ NEW FACTORS AT TIME %lf ################\n", t);
+                new_factors.print();
+
+                isam.update(new_factors, new_values);
+
+                // Reset the newFactors and newValues list
+                new_factors.resize(0);
+                new_values.clear();
+
+                // Extract the result/current estimates
+                Values result = isam.calculateEstimate();
+
+                current_pose_global = result.at<Pose3>(current_pose_key);
+                current_velocity_global = result.at<Vector3>(current_vel_key);
+                current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
+
+                printf("\n################ POSE AT TIME %lf ################\n", t);
+                current_pose_global.print();
+                printf("\n\n");
+            }
+        }
+    }
+
+    // Save results to file
+    printf("\nWriting results to file...\n");
+    FILE* fp_out = fopen(output_filename.c_str(), "w+");
+    fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
+
+    Values result = isam.calculateEstimate();
+    for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
+        auto pose_key = X(i);
+        auto vel_key = V(i);
+        auto bias_key = B(i);
+
+        auto pose = result.at<Pose3>(pose_key);
+        auto velocity = result.at<Vector3>(vel_key);
+        auto bias = result.at<imuBias::ConstantBias>(bias_key);
+
+        auto pose_quat = pose.rotation().toQuaternion();
+        auto gps = gps_measurements[i].position;
+
+        cout << "State at #" << i << endl;
+        cout << "Pose:" << endl << pose << endl;
+        cout << "Velocity:" << endl << velocity << endl;
+        cout << "Bias:" << endl << bias << endl;
+
+        fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
+                gps_measurements[i].time,
+                pose.x(), pose.y(), pose.z(),
+                pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
+                gps(0), gps(1), gps(2));
+    }
+
+    fclose(fp_out);
+}

examples/ISAM2Example_SmartFactor.cpp

@@ -57,7 +57,7 @@ int main(int argc, char* argv[]) {
   vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
 
   // Add first pose
-  graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
+  graph.addPrior(X(0), poses[0], noise);
   initialEstimate.insert(X(0), poses[0].compose(delta));
 
   // Create smart factor with measurement from first pose only
@@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
     cout << "i = " << i << endl;
 
     // Add prior on new pose
-    graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
+    graph.addPrior(X(i), poses[i], noise);
     initialEstimate.insert(X(i), poses[i].compose(delta));
 
     // "Simulate" measurement from this pose

examples/ISAM2_SmartFactorStereo_IMU.cpp

@@ -129,18 +129,16 @@ int main(int argc, char* argv[]) {
   // Pose prior - at identity
   auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
-  graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
+  graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
-                                           priorPoseNoise);
   initialEstimate.insert(X(0), Pose3::identity());
 
   // Bias prior
-  graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
+  graph.addPrior(B(1), imu.priorImuBias,
-                                               imu.biasNoiseModel));
+                                               imu.biasNoiseModel);
   initialEstimate.insert(B(0), imu.priorImuBias);
 
   // Velocity prior - assume stationary
-  graph.add(
+  graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);
-      PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
   initialEstimate.insert(V(0), Vector3(0, 0, 0));
 
   int lastFrame = 1;

examples/ImuFactorExample2.cpp

@@ -7,7 +7,6 @@
 #include <gtsam/navigation/Scenario.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 
 #include <vector>
 
@@ -61,21 +60,18 @@ int main(int argc, char* argv[]) {
   // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
   auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
-  newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
+  newgraph.addPrior(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(),
+  newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
-                                               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.addPrior(V(0), n_velocity, velnoise);
-  newgraph.push_back(velprior);
 
   initialEstimate.insert(V(0), n_velocity);
 

examples/ImuFactorsExample.cpp

@@ -11,12 +11,14 @@
 
 /**
  * @file imuFactorsExample
- * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
+ * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
+ * navigation code.
  * @author Garrett (ghemann@gmail.com), Luca Carlone
  */
 
 /**
- * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
+ * Example of use of the imuFactors (imuFactor and combinedImuFactor) in
+ * conjunction with GPS
  *  - imuFactor is used by default. You can test combinedImuFactor by
  *  appending a `-c` flag at the end (see below for example command).
  *  - we read IMU and GPS data from a CSV file, with the following format:
@@ -26,8 +28,8 @@
  *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
  *  A row starting with "1" is a gps correction formatted with
  *  N, E, D, qX, qY, qZ, qW
- *  Note that for GPS correction, we're only using the position not the rotation. The
+ * Note that for GPS correction, we're only using the position not the
- *  rotation is provided in the file for ground truth comparison.
+ * rotation. The rotation is provided in the file for ground truth comparison.
  *
  *  Usage: ./ImuFactorsExample [data_csv_path] [-c]
  *  optional arguments:
@@ -41,13 +43,13 @@
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/GPSFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/inference/Symbol.h>
+
 #include <cstring>
 #include <fstream>
 #include <iostream>
@@ -58,19 +60,14 @@
 using namespace gtsam;
 using namespace std;
 
-using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
-using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
 using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
 
-const string output_filename = "imuFactorExampleResults.csv";
+static const char output_filename[] = "imuFactorExampleResults.csv";
-const string use_combined_imu_flag = "-c";
+static const char use_combined_imu_flag[3] = "-c";
 
-// This will either be PreintegratedImuMeasurements (for ImuFactor) or
+int main(int argc, char* argv[]) {
-// PreintegratedCombinedMeasurements (for CombinedImuFactor).
-PreintegrationType *imu_preintegrated_;
-
-int main(int argc, char* argv[])
-{
   string data_filename;
   bool use_combined_imu = false;
 
@@ -88,7 +85,7 @@ int main(int argc, char* argv[])
     printf("using default CSV file\n");
     data_filename = findExampleDataFile("imuAndGPSdata.csv");
   } else if (argc < 3) {
-    if (strcmp(argv[1], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match
+    if (strcmp(argv[1], use_combined_imu_flag) == 0) {
       printf("using CombinedImuFactor\n");
       use_combined_imu = true;
       printf("using default CSV file\n");
@@ -98,15 +95,17 @@ int main(int argc, char* argv[])
     }
   } else {
     data_filename = argv[1];
-    if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match
+    if (strcmp(argv[2], use_combined_imu_flag) == 0) {
       printf("using CombinedImuFactor\n");
       use_combined_imu = true;
     }
   }
 
   // Set up output file for plotting errors
-  FILE* fp_out = fopen(output_filename.c_str(), "w+");
+  FILE* fp_out = fopen(output_filename, "w+");
-  fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
+  fprintf(fp_out,
+          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
+          "gt_qy,gt_qz,gt_qw\n");
 
   // Begin parsing the CSV file.  Input the first line for initialization.
   // From there, we'll iterate through the file and we'll preintegrate the IMU
@@ -115,7 +114,7 @@ int main(int argc, char* argv[])
   string value;
 
   // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
-  Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
+  Vector10 initial_state;
   getline(file, value, ',');  // i
   for (int i = 0; i < 9; i++) {
     getline(file, value, ',');
@@ -125,7 +124,8 @@ int main(int argc, char* argv[])
   initial_state(9) = atof(value.c_str());
   cout << "initial state:\n" << initial_state.transpose() << "\n\n";
 
-  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
+  // Assemble initial quaternion through GTSAM constructor
+  // ::quaternion(w,x,y,z);
   Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                          initial_state(4), initial_state(5));
   Point3 prior_point(initial_state.head<3>());
@@ -139,57 +139,64 @@ int main(int argc, char* argv[])
   initial_values.insert(V(correction_count), prior_velocity);
   initial_values.insert(B(correction_count), prior_imu_bias);
 
-  // Assemble prior noise model and add it the graph.
+  // Assemble prior noise model and add it the graph.`
-  noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
+  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
-  noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
+      (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
-  noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
+          .finished());  // rad,rad,rad,m, m, m
+  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
+  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);
 
   // Add all prior factors (pose, velocity, bias) to the graph.
   NonlinearFactorGraph* graph = new NonlinearFactorGraph();
-  graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
+  graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
-  graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
+  graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
-  graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
+  graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);
 
   // We use the sensor specs to build the noise model for the IMU factor.
   double accel_noise_sigma = 0.0003924;
   double gyro_noise_sigma = 0.000205689024915;
   double accel_bias_rw_sigma = 0.004905;
   double gyro_bias_rw_sigma = 0.000001454441043;
-  Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
+  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
-  Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
+  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
-  Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
+  Matrix33 integration_error_cov =
-  Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
+      I_3x3 * 1e-8;  // error committed in integrating position from velocities
-  Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
+  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
+  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
+  Matrix66 bias_acc_omega_int =
+      I_6x6 * 1e-5;  // error in the bias used for preintegration
 
-  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
+  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
   // PreintegrationBase params:
-  p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
+  p->accelerometerCovariance =
-  p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
+      measured_acc_cov;  // acc white noise in continuous
+  p->integrationCovariance =
+      integration_error_cov;  // integration uncertainty continuous
   // should be using 2nd order integration
   // PreintegratedRotation params:
-  p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
+  p->gyroscopeCovariance =
+      measured_omega_cov;  // gyro white noise in continuous
   // PreintegrationCombinedMeasurements params:
   p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
   p->biasAccOmegaInt = bias_acc_omega_int;
 
-  std::shared_ptr<PreintegrationType> imu_preintegrated_ = nullptr;
+  std::shared_ptr<PreintegrationType> preintegrated = nullptr;
   if (use_combined_imu) {
-    imu_preintegrated_ =
+    preintegrated =
         std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
   } else {
-    imu_preintegrated_ =
+    preintegrated =
         std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
   }
-  assert(imu_preintegrated_);
+  assert(preintegrated);
 
-  // Store previous state for the imu integration and the latest predicted outcome.
+  // Store previous state for imu integration and latest predicted outcome.
   NavState prev_state(prior_pose, prior_velocity);
   NavState prop_state = prev_state;
   imuBias::ConstantBias prev_bias = prior_imu_bias;
 
-  // Keep track of the total error over the entire run for a simple performance metric.
+  // Keep track of total error over the entire run as simple performance metric.
   double current_position_error = 0.0, current_orientation_error = 0.0;
 
   double output_time = 0.0;
@@ -198,13 +205,12 @@ int main(int argc, char* argv[])
 
   // All priors have been set up, now iterate through the data file.
   while (file.good()) {
-
     // Parse out first value
     getline(file, value, ',');
     int type = atoi(value.c_str());
 
     if (type == 0) {  // IMU measurement
-      Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
+      Vector6 imu;
       for (int i = 0; i < 5; ++i) {
         getline(file, value, ',');
         imu(i) = atof(value.c_str());
@@ -213,10 +219,10 @@ int main(int argc, char* argv[])
       imu(5) = atof(value.c_str());
 
       // Adding the IMU preintegration.
-      imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
+      preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
 
     } else if (type == 1) {  // GPS measurement
-      Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
+      Vector7 gps;
       for (int i = 0; i < 6; ++i) {
         getline(file, value, ',');
         gps(i) = atof(value.c_str());
@@ -228,30 +234,28 @@ int main(int argc, char* argv[])
 
       // Adding IMU factor and GPS factor and optimizing.
       if (use_combined_imu) {
-        const PreintegratedCombinedMeasurements& preint_imu_combined =
+        auto preint_imu_combined =
             dynamic_cast<const PreintegratedCombinedMeasurements&>(
-              *imu_preintegrated_);
+                *preintegrated);
-        CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
+        CombinedImuFactor imu_factor(
-                                     X(correction_count  ), V(correction_count  ),
+            X(correction_count - 1), V(correction_count - 1),
-                                     B(correction_count-1), B(correction_count  ),
+            X(correction_count), V(correction_count), B(correction_count - 1),
-                                     preint_imu_combined);
+            B(correction_count), preint_imu_combined);
         graph->add(imu_factor);
       } else {
-        const PreintegratedImuMeasurements& preint_imu =
+        auto preint_imu =
-            dynamic_cast<const PreintegratedImuMeasurements&>(
+            dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
-              *imu_preintegrated_);
         ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
                              X(correction_count), V(correction_count),
-                             B(correction_count-1),
+                             B(correction_count - 1), preint_imu);
-                             preint_imu);
         graph->add(imu_factor);
         imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-        graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
+        graph->add(BetweenFactor<imuBias::ConstantBias>(
-                                                        B(correction_count  ),
+            B(correction_count - 1), B(correction_count), zero_bias,
-                                                        zero_bias, bias_noise_model));
+            bias_noise_model));
       }
 
-      noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
+      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
       GPSFactor gps_factor(X(correction_count),
                            Point3(gps(0),   // N,
                                   gps(1),   // E,
@@ -260,7 +264,7 @@ int main(int argc, char* argv[])
       graph->add(gps_factor);
 
       // Now optimize and compare results.
-      prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
+      prop_state = preintegrated->predict(prev_state, prev_bias);
       initial_values.insert(X(correction_count), prop_state.pose());
       initial_values.insert(V(correction_count), prop_state.v());
       initial_values.insert(B(correction_count), prev_bias);
@@ -284,7 +288,7 @@ int main(int argc, char* argv[])
       prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
 
       // Reset the preintegration object.
-      imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
+      preintegrated->resetIntegrationAndSetBias(prev_bias);
 
       // Print out the position and orientation error for comparison.
       Vector3 gtsam_position = prev_state.pose().translation();
@@ -295,19 +299,19 @@ int main(int argc, char* argv[])
       Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
       Quaternion quat_error = gtsam_quat * gps_quat.inverse();
       quat_error.normalize();
-      Vector3 euler_angle_error(quat_error.x()*2,
+      Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
-                                 quat_error.y()*2,
                                 quat_error.z() * 2);
       current_orientation_error = euler_angle_error.norm();
 
       // display statistics
-      cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
+      cout << "Position error:" << current_position_error << "\t "
+           << "Angular error:" << current_orientation_error << "\n";
 
       fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
-              output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
+              output_time, gtsam_position(0), gtsam_position(1),
-              gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
+              gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
-              gps(0), gps(1), gps(2),
+              gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
-              gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
+              gps_quat.y(), gps_quat.z(), gps_quat.w());
 
       output_time += 1.0;
 
@@ -317,6 +321,7 @@ int main(int argc, char* argv[])
     }
   }
   fclose(fp_out);
-  cout << "Complete, results written to " << output_filename << "\n\n";;
+  cout << "Complete, results written to " << output_filename << "\n\n";
+
   return 0;
 }

examples/InverseKinematicsExampleExpressions.cpp

@@ -22,7 +22,6 @@
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/expressions.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/expressions.h>
 
 #include <cmath>

examples/LocalizationExample.cpp

@@ -66,7 +66,6 @@ using namespace gtsam;
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 class UnaryFactor: public NoiseModelFactor1<Pose2> {
-
   // The factor will hold a measurement consisting of an (X,Y) location
   // We could this with a Point2 but here we just use two doubles
   double mx_, my_;
@@ -85,8 +84,8 @@ public:
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
+  Vector evaluateError(const Pose2& q,
-  {
+                       boost::optional<Matrix&> H = boost::none) const {
     // The measurement function for a GPS-like measurement is simple:
     // error_x = pose.x - measurement.x
     // error_y = pose.y - measurement.y
@@ -107,25 +106,24 @@ public:
   // Additionally, we encourage you the use of unit testing your custom factors,
   // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
   // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
-
 };  // UnaryFactor
 
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add odometry factors
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
 
   // 2b. Add "GPS-like" measurements
   // We will use our custom UnaryFactor for this.
-  noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
+  auto unaryNoise =
+      noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));  // 10cm std on x,y
   graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
   graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
   graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);

examples/METISOrderingExample.cpp

@@ -11,7 +11,8 @@
 
 /**
  * @file METISOrdering.cpp
- * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
+ * @brief Simple robot motion example, with prior and two odometry measurements,
+ * using a METIS ordering
  * @author Frank Dellaert
  * @author Andrew Melim
  */
@@ -22,12 +23,11 @@
  */
 
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -36,11 +36,11 @@ int main(int argc, char** argv) {
   NonlinearFactorGraph graph;
 
   Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
+  graph.addPrior(1, priorMean, priorNoise);
 
   Pose2 odometry(2.0, 0.0, 0.0);
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
   graph.print("\nFactor Graph:\n");  // print
@@ -53,8 +53,8 @@ int main(int argc, char** argv) {
 
   // optimize using Levenberg-Marquardt optimization
   LevenbergMarquardtParams params;
-  // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
+  // In order to specify the ordering type, we need to se the "orderingType". By
-  // By default this parameter is set to OrderingType::COLAMD
+  // default this parameter is set to OrderingType::COLAMD
   params.orderingType = Ordering::METIS;
   LevenbergMarquardtOptimizer optimizer(graph, initial, params);
   Values result = optimizer.optimize();

examples/OdometryExample.cpp

@@ -29,7 +29,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -57,20 +56,19 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create an empty nonlinear factor graph
   NonlinearFactorGraph graph;
 
   // 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)
   Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
+  graph.addPrior(1, priorMean, priorNoise);
 
   // Add odometry factors
   Pose2 odometry(2.0, 0.0, 0.0);
   // For simplicity, we will use the same noise model for each odometry factor
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);

examples/PlanarSLAMExample.cpp

@@ -40,7 +40,6 @@
 // Here we will use a RangeBearing factor for the range-bearing measurements to identified
 // landmarks, and Between factors for the relative motion described by odometry measurements.
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 
@@ -70,7 +69,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create a factor graph
   NonlinearFactorGraph graph;
 
@@ -78,27 +76,29 @@ int main(int argc, char** argv) {
   static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
   static Symbol l1('l', 1), l2('l', 2);
 
-  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
+  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and
+  // a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0);  // prior mean is at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
+  auto priorNoise = noiseModel::Diagonal::Sigmas(
-  graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
+      Vector3(0.3, 0.3, 0.1));            // 30cm std on x,y, 0.1 rad on theta
+  graph.addPrior(x1, prior, priorNoise);  // add directly to graph
 
   // Add two odometry factors
-  Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
+  Pose2 odometry(2.0, 0.0, 0.0);
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
+  // create a measurement for both factors (the same in this case)
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.2, 0.2, 0.1));  // 20cm std on x,y, 0.1 rad on theta
   graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
 
   // Add Range-Bearing measurements to two different landmarks
   // create a noise model for the landmark measurements
-  noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
+  auto measurementNoise = noiseModel::Diagonal::Sigmas(
+      Vector2(0.1, 0.2));  // 0.1 rad std on bearing, 20cm on range
   // create the measurement values - indices are (pose id, landmark id)
-  Rot2 bearing11 = Rot2::fromDegrees(45),
+  Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
-       bearing21 = Rot2::fromDegrees(90),
        bearing32 = Rot2::fromDegrees(90);
-  double range11 = std::sqrt(4.0+4.0),
+  double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
-         range21 = 2.0,
-         range32 = 2.0;
 
   // Add Bearing-Range factors
   graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
@@ -139,4 +139,3 @@ int main(int argc, char** argv) {
 
   return 0;
 }
-

examples/Pose2SLAMExample.cpp

@@ -36,7 +36,6 @@
 // Here we will use Between factors for the relative motion described by odometry measurements.
 // We will also use a Between Factor to encode the loop closure constraint
 // Also, we will initialize the robot at the origin using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -65,17 +64,16 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+  graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses

examples/Pose2SLAMExampleExpressions.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -31,7 +30,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   ExpressionFactorGraph graph;
 
@@ -39,11 +37,11 @@ int main(int argc, char** argv) {
   Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
 
-  // For simplicity, we will use the same noise model for odometry and loop closures
+  // For simplicity, we use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
@@ -55,8 +53,9 @@ int main(int argc, char** argv) {
   graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
   graph.print("\nFactor Graph:\n");  // print
 
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+  // 3. Create the data structure to hold the initialEstimate estimate to the
-  // For illustrative purposes, these have been deliberately set to incorrect values
+  // solution For illustrative purposes, these have been deliberately set to
+  // incorrect values
   Values initialEstimate;
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));

examples/Pose2SLAMExample_g2o.cpp

@@ -19,7 +19,6 @@
  */
 
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>
@@ -29,7 +28,6 @@ using namespace gtsam;
 
 // HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
 int main(const int argc, const char *argv[]) {
-
   string kernelType = "none";
   int maxIterations = 100;                                    // default
   string g2oFile = findExampleDataFile("noisyToyGraph.txt");  // default
@@ -37,7 +35,6 @@ int main(const int argc, const char *argv[]) {
   // Parse user's inputs
   if (argc > 1) {
     g2oFile = argv[1];  // input dataset filename
-    // outputFile = g2oFile = argv[2]; // done later
   }
   if (argc > 3) {
     maxIterations = atoi(argv[3]);  // user can specify either tukey or huber
@@ -55,24 +52,27 @@ int main(const int argc, const char *argv[]) {
   }
   if (kernelType.compare("huber") == 0) {
     std::cout << "Using robust kernel: huber " << std::endl;
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
+    boost::tie(graph, initial) =
+        readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
   }
   if (kernelType.compare("tukey") == 0) {
     std::cout << "Using robust kernel: tukey " << std::endl;
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
+    boost::tie(graph, initial) =
+        readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
   }
 
   // Add prior on the pose having index (key) = 0
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel =  //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graph->addPrior(0, Pose2(), priorModel);
   std::cout << "Adding prior on pose 0 " << std::endl;
 
   GaussNewtonParams params;
   params.setVerbosity("TERMINATION");
   if (argc > 3) {
     params.maxIterations = maxIterations;
-    std::cout << "User required to perform maximum  " << params.maxIterations << " iterations "<< std::endl;
+    std::cout << "User required to perform maximum  " << params.maxIterations
+              << " iterations " << std::endl;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;

examples/Pose2SLAMExample_graph.cpp

@@ -17,7 +17,6 @@
  */
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -43,7 +42,7 @@ int main (int argc, char** argv) {
   // Add a Gaussian prior on first poses
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
-  graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
+  graph -> addPrior(0, priorMean, priorNoise);
 
   // Single Step Optimization using Levenberg-Marquardt
   Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();

examples/Pose2SLAMExample_graphviz.cpp

@@ -17,7 +17,6 @@
  */
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -27,16 +26,16 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+  graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
-  // For simplicity, we will use the same noise model for odometry and loop closures
+  // For simplicity, we will use the same noise model for odometry and loop
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  // closures
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);

examples/Pose2SLAMExample_lago.cpp

@@ -21,7 +21,6 @@
 
 #include <gtsam/slam/lago.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <fstream>
 
@@ -29,7 +28,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char *argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -42,9 +40,8 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-      noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
+  graph->addPrior(0, Pose2(), priorModel);
-  graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
   graph->print();
 
   std::cout << "Computing LAGO estimate" << std::endl;

examples/Pose2SLAMStressTest.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/StereoFactor.h>
 
 #include <random>
@@ -48,7 +47,7 @@ void testGtsam(int numberNodes) {
   Pose3 first = Pose3(first_M);
 
   NonlinearFactorGraph graph;
-  graph.add(PriorFactor<Pose3>(0, first, priorModel));
+  graph.addPrior(0, first, priorModel);
 
   // vo noise model
   auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);

examples/Pose2SLAMwSPCG.cpp

@@ -17,7 +17,6 @@
  */
 
 // For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -29,29 +28,29 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
   Pose2 prior(0.0, 0.0, 0.0);  // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
+  graph.addPrior(1, prior, priorNoise);
 
   // 2b. Add odometry factors
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 
   // 2c. Add the loop closure constraint
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0),
+                                              model);
   graph.print("\nFactor Graph:\n");  // print
 
-
+  // 3. Create the data structure to hold the initialEstimate estimate to the
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+  // solution
   Values initialEstimate;
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
@@ -65,9 +64,9 @@ int main(int argc, char** argv) {
   parameters.verbosity = NonlinearOptimizerParams::ERROR;
   parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
 
-  // LM is still the outer optimization loop, but by specifying "Iterative" below
+  // LM is still the outer optimization loop, but by specifying "Iterative"
-  // We indicate that an iterative linear solver should be used.
+  // below We indicate that an iterative linear solver should be used. In
-  // In addition, the *type* of the iterativeParams decides on the type of
+  // addition, the *type* of the iterativeParams decides on the type of
   // iterative solver, in this case the SPCG (subgraph PCG)
   parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
   parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

examples/Pose3Localization.cpp

@@ -10,16 +10,15 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3Localization.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3Localization input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
 #include <fstream>
@@ -28,7 +27,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -42,19 +40,19 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
-  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  params.setVerbosity("TERMINATION");  // show info about stopping conditions
   GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;

examples/Pose3SLAMExample_changeKeys.cpp

@@ -19,7 +19,6 @@
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
 using namespace std;

examples/Pose3SLAMExample_g2o.cpp

@@ -10,16 +10,15 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_g2o.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>
 
@@ -27,7 +26,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,19 +39,19 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
-  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  params.setVerbosity("TERMINATION");  //  show info about stopping conditions
   GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_initializePose3Chordal.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -20,14 +20,12 @@
 #include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
 using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,13 +39,13 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_initializePose3Gradient.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -20,14 +20,12 @@
 #include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
 using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,23 +39,25 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
 
   std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
   bool useGradient = true;
-  Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
+  Values initialization =
+      InitializePose3::initialize(*graph, *initial, useGradient);
   std::cout << "done!" << std::endl;
 
   std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
+  std::cout << "initialization error=" << graph->error(initialization)
+            << std::endl;
 
   if (argc < 3) {
     initialization.print("initialization");

examples/RangeISAMExample_plaza2.cpp

@@ -37,7 +37,6 @@
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics SLAM problems.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/dataset.h>
@@ -124,7 +123,7 @@ int main (int argc, char** argv) {
   Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
       M_PI - 2.02108900000000);
   NonlinearFactorGraph newFactors;
-  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
+  newFactors.addPrior(0, pose0, priorNoise);
   Values initial;
   initial.insert(0, pose0);
 

examples/SFMExample.cpp

@@ -30,7 +30,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Projection factors to model the camera's landmark observations.
 // Also, we will initialize the robot at some location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
@@ -57,12 +56,12 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -74,32 +73,45 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  auto poseNoise = noiseModel::Diagonal::Sigmas(
-  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
+          .finished());  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  graph.addPrior(Symbol('x', 0), poses[0], poseNoise);  // add directly to graph
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   for (size_t i = 0; i < poses.size(); ++i) {
     PinholeCamera<Cal3_S2> camera(poses[i], *K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
-      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
+          measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
     }
   }
 
-  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
+  // Because the structure-from-motion problem has a scale ambiguity, the
-  // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
+  // problem is still under-constrained Here we add a prior on the position of
-  // between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
+  // the first landmark. This fixes the scale by indicating the distance between
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  // the first camera and the first landmark. All other landmark positions are
-  graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
+  // interpreted using this scale.
+  auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(Symbol('l', 0), points[0],
+                 pointNoise);  // add directly to graph
   graph.print("Factor Graph:\n");
 
   // Create the data structure to hold the initial estimate to the solution
   // Intentionally initialize the variables off from the ground truth
   Values initialEstimate;
-  for (size_t i = 0; i < poses.size(); ++i)
+  for (size_t i = 0; i < poses.size(); ++i) {
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    auto corrupted_pose = poses[i].compose(
-  for (size_t j = 0; j < points.size(); ++j)
+        Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
-    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+    initialEstimate.insert(
+        Symbol('x', i), corrupted_pose);
+  }
+  for (size_t j = 0; j < points.size(); ++j) {
+    Point3 corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
+    initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
+  }
   initialEstimate.print("Initial Estimates:\n");
 
   /* Optimize the graph and print results */

examples/SFMExampleExpressions_bal.cpp

@@ -40,20 +40,16 @@ using symbol_shorthand::P;
 // An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
 // and has a total of 9 free parameters
 
-/* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  if (argc > 1)
+  if (argc > 1) filename = string(argv[1]);
-    filename = string(argv[1]);
 
   // Load the SfM data from file
   SfmData mydata;
   readBAL(filename, mydata);
-  cout
+  cout << boost::format("read %1% tracks on %2% cameras\n") %
-      << boost::format("read %1% tracks on %2% cameras\n")
+              mydata.number_tracks() % mydata.number_cameras();
-          % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   ExpressionFactorGraph graph;
@@ -73,10 +69,10 @@ int main(int argc, char* argv[]) {
                             noiseModel::Isotropic::Sigma(3, 0.1));
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
-      1.0); // one pixel in u and v
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   size_t j = 0;
   for (const SfmTrack& track : mydata.tracks) {
     // Leaf expression for j^th point
@@ -98,10 +94,8 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0;
   j = 0;
-  for(const SfmCamera& camera: mydata.cameras)
+  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
-    initial.insert(C(i++), camera);
+  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
-  for(const SfmTrack& track: mydata.tracks)
-    initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;
@@ -117,5 +111,3 @@ int main(int argc, char* argv[]) {
 
   return 0;
 }
-/* ************************************************************************* */
-

examples/SFMExample_SmartFactor.cpp

@@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise =
+  auto measurementNoise =
       noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
@@ -80,15 +80,15 @@ int main(int argc, char* argv[]) {
 
   // 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
-  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
+  auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
-  graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
+  graph.addPrior(0, poses[0], noise);
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is
   // still under-constrained. Here we add a prior on the second pose x1, so this will
   // fix the scale by indicating the distance between x0 and x1.
   // Because these two are fixed, the rest of the poses will be also be fixed.
-  graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
+  graph.addPrior(1, poses[1], noise); // add directly to graph
 
   graph.print("Factor Graph:\n");
 
@@ -117,7 +117,7 @@ int main(int argc, char* argv[]) {
       // The output of point() is in boost::optional<Point3>, as sometimes
       // the triangulation operation inside smart factor will encounter degeneracy.
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return NULL
+      if (point) // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_SmartFactorPCG.cpp

@@ -35,12 +35,11 @@ typedef PinholePose<Cal3_S2> Camera;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise =
+  auto measurementNoise =
       noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
@@ -52,17 +51,16 @@ int main(int argc, char* argv[]) {
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t j = 0; j < points.size(); ++j) {
-
+    // every landmark represent a single landmark, we use shared pointer to init
-    // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
+    // the factor, and then insert measurements.
     SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
-
       // generate the 2D measurement
       Camera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
 
-      // call add() function to add measurement into a single factor, here we need to add:
+      // call add() function to add measurement into a single factor
       smartfactor->add(measurement, i);
     }
 
@@ -72,12 +70,12 @@ int main(int argc, char* argv[]) {
 
   // 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
-  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
+  auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
-  graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
+  graph.addPrior(0, poses[0], noise);
 
   // Fix the scale ambiguity by adding a prior
-  graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
+  graph.addPrior(1, poses[0], noise);
 
   // Create the initial estimate to the solution
   Values initialEstimate;
@@ -85,9 +83,9 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(i, poses[i].compose(delta));
 
-  // We will use LM in the outer optimization loop, but by specifying "Iterative" below
+  // We will use LM in the outer optimization loop, but by specifying
-  // We indicate that an iterative linear solver should be used.
+  // "Iterative" below We indicate that an iterative linear solver should be
-  // In addition, the *type* of the iterativeParams decides on the type of
+  // used. In addition, the *type* of the iterativeParams decides on the type of
   // iterative solver, in this case the SPCG (subgraph PCG)
   LevenbergMarquardtParams parameters;
   parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
@@ -110,11 +108,10 @@ int main(int argc, char* argv[]) {
   result.print("Final results:\n");
   Values landmark_result;
   for (size_t j = 0; j < points.size(); ++j) {
-    SmartFactor::shared_ptr smart = //
+    auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
-        boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return NULL
+      if (point)  // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_bal.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/dataset.h> // for loading BAL datasets !
 #include <vector>
@@ -50,7 +49,7 @@ int main (int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
+  auto noise =
       noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Add measurements to the factor graph
@@ -66,8 +65,8 @@ int main (int argc, char* argv[]) {
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
+  graph.addPrior(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.emplace_shared<PriorFactor<Point3> >    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
+  graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMExample.cpp
+ * @file    SFMExample_bal_COLAMD_METIS.cpp
  * @brief   This file is to compare the ordering performance for COLAMD vs METIS.
  * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
  * @author  Frank Dellaert, Zhaoyang Lv
@@ -21,7 +21,6 @@
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 
@@ -35,13 +34,12 @@ using symbol_shorthand::C;
 using symbol_shorthand::P;
 
 // We will be using a projection factor that ties a SFM_Camera to a 3D point.
-// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler
-// and has a total of 9 free parameters
+// calibration and has a total of 9 free parameters
 typedef GeneralSFMFactor<SfmCamera, Point3> MyFactor;
 
-/* ************************************************************************* */
-int main (int argc, char* argv[]) {
 
+int main(int argc, char* argv[]) {
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
   if (argc > 1) filename = string(argv[1]);
@@ -49,14 +47,14 @@ int main (int argc, char* argv[]) {
   // Load the SfM data from file
   SfmData mydata;
   readBAL(filename, mydata);
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+  cout << boost::format("read %1% tracks on %2% cameras\n") %
+              mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   NonlinearFactorGraph graph;
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Add measurements to the factor graph
   size_t j = 0;
@@ -64,19 +62,22 @@ int main (int argc, char* argv[]) {
     for (const SfmMeasurement& m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
-      graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
+      graph.emplace_shared<MyFactor>(
+          uv, noise, C(i), P(j));  // note use of shorthand symbols C and P
     }
     j += 1;
   }
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
+  graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
+  graph.addPrior(P(0), mydata.tracks[0].p,
+                 noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;
-  size_t i = 0; j = 0;
+  size_t i = 0;
+  j = 0;
   for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
   for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
 
@@ -121,7 +122,6 @@ int main (int argc, char* argv[]) {
     cout << e.what();
   }
 
-
   {  // printing the result
 
     cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
@@ -130,15 +130,13 @@ int main (int argc, char* argv[]) {
     cout << endl << endl;
 
     cout << "Time comparison by solving " << filename << " results:" << endl;
-    cout << boost::format("%1% point tracks and %2% cameras\n") \
+    cout << boost::format("%1% point tracks and %2% cameras\n") %
-            % mydata.number_tracks() % mydata.number_cameras() \
+                mydata.number_tracks() % mydata.number_cameras()
          << endl;
 
     tictoc_print_();
   }
 
-
   return 0;
 }
-/* ************************************************************************* */
 

examples/SelfCalibrationExample.cpp

@@ -33,7 +33,6 @@
 #include <gtsam/nonlinear/Values.h>
 
 // SFM-specific factors
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>  // does calibration !
 
 // Standard headers
@@ -42,9 +41,7 @@
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Create the set of ground-truth
   vector<Point3> points = createPoints();
   vector<Pose3> poses = createPoses();
@@ -53,38 +50,51 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  auto poseNoise = noiseModel::Diagonal::Sigmas(
-  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
+          .finished());  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);
   for (size_t i = 0; i < poses.size(); ++i) {
     for (size_t j = 0; j < points.size(); ++j) {
       PinholeCamera<Cal3_S2> camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
-      // The only real difference with the Visual SLAM example is that here we use a
+      // The only real difference with the Visual SLAM example is that here we
-      // different factor type, that also calculates the Jacobian with respect to calibration
+      // use a different factor type, that also calculates the Jacobian with
-      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
+      // respect to calibration
+      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(
+          measurement, measurementNoise, Symbol('x', i), Symbol('l', j),
+          Symbol('K', 0));
     }
   }
 
   // Add a prior on the position of the first landmark.
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  auto pointNoise =
-  graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
+      noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(Symbol('l', 0), points[0],
+                 pointNoise);  // add directly to graph
 
   // Add a prior on the calibration.
-  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
+  auto calNoise = noiseModel::Diagonal::Sigmas(
-  graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
+      (Vector(5) << 500, 500, 0.1, 100, 100).finished());
+  graph.addPrior(Symbol('K', 0), K, calNoise);
 
   // Create the initial estimate to the solution
   // now including an estimate on the camera calibration parameters
   Values initialEstimate;
   initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(
+        Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                               Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+    initialEstimate.insert<Point3>(Symbol('l', j),
+                                   points[j] + Point3(-0.25, 0.20, 0.15));
 
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
@@ -92,5 +102,4 @@ int main(int argc, char* argv[]) {
 
   return 0;
 }
-/* ************************************************************************* */
 

examples/SimpleRotation.cpp

@@ -32,8 +32,8 @@
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
-// We will apply a simple prior on the rotation
+// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience
-#include <gtsam/slam/PriorFactor.h>
+// method in NonlinearFactorGraph.
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
 // are nonlinear factors, we will need a Nonlinear Factor Graph.
@@ -59,7 +59,6 @@ using namespace gtsam;
 const double degree = M_PI / 180;
 
 int main() {
-
   /**
    *    Step 1: Create a factor to express a unary constraint
    * The "prior" in this case is the measurement from a sensor,
@@ -76,9 +75,8 @@ int main() {
    */
   Rot2 prior = Rot2::fromAngle(30 * degree);
   prior.print("goal angle");
-  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
+  auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
   Symbol key('x', 1);
-  PriorFactor<Rot2> factor(key, prior, model);
 
   /**
    *    Step 2: Create a graph container and add the factor to it
@@ -90,7 +88,7 @@ int main() {
    * many more factors would be added.
    */
   NonlinearFactorGraph graph;
-  graph.push_back(factor);
+  graph.addPrior(key, prior, model);
   graph.print("full graph");
 
   /**

examples/SolverComparer.cpp

@@ -34,7 +34,6 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -58,9 +57,8 @@
 #include <iostream>
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/tbb.h>
+#include <tbb/task_arena.h> // tbb::task_arena
-#undef max // TBB seems to include windows.h and we don't want these macros
+#include <tbb/task_group.h> // tbb::task_group
-#undef min
 #endif
 
 using namespace std;
@@ -206,10 +204,11 @@ int main(int argc, char *argv[]) {
   }
 
 #ifdef GTSAM_USE_TBB
-  std::unique_ptr<tbb::task_scheduler_init> init;
+  tbb::task_arena arena;
+  tbb::task_group tg;
   if(nThreads > 0) {
     cout << "Using " << nThreads << " threads" << endl;
-    init.reset(new tbb::task_scheduler_init(nThreads));
+    arena.initialize(nThreads);
   } else
     cout << "Using threads for all processors" << endl;
 #else
@@ -219,6 +218,10 @@ int main(int argc, char *argv[]) {
   }
 #endif
 
+#ifdef GTSAM_USE_TBB
+  arena.execute([&]{
+  tg.run_and_wait([&]{
+#endif
   // Run mode
   if(incremental)
     runIncremental();
@@ -230,6 +233,10 @@ int main(int argc, char *argv[]) {
     runPerturb();
   else if(stats)
     runStats();
+#ifdef GTSAM_USE_TBB
+  });
+  });
+#endif
 
   return 0;
 }
@@ -248,7 +255,7 @@ void runIncremental()
   cout << "Looking for first measurement from step " << firstStep << endl;
   size_t nextMeasurement = 0;
   bool havePreviousPose = false;
-  Key firstPose;
+  Key firstPose = 0;
   while(nextMeasurement < datasetMeasurements.size())
   {
     if(BetweenFactor<Pose>::shared_ptr factor =
@@ -281,7 +288,7 @@ void runIncremental()
     NonlinearFactorGraph newFactors;
     Values newVariables;
 
-    newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
+    newFactors.addPrior(firstPose, Pose(), noiseModel::Unit::Create(3));
     newVariables.insert(firstPose, Pose());
 
     isam2.update(newFactors, newVariables);
@@ -464,7 +471,7 @@ void runBatch()
   cout << "Creating batch optimizer..." << endl;
 
   NonlinearFactorGraph measurements = datasetMeasurements;
-  measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
+  measurements.addPrior(0, Pose(), noiseModel::Unit::Create(3));
 
   gttic_(Create_optimizer);
   GaussNewtonParams params;

examples/StereoVOExample.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file SteroVOExample.cpp
+ * @file StereoVOExample.cpp
  * @brief A stereo visual odometry example
  * @date May 25, 2014
  * @author Stephen Camp
@@ -35,16 +35,16 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // create graph object, add first pose at origin with key '1'
   NonlinearFactorGraph graph;
   Pose3 first_pose;
   graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
 
   // create factor noise model with 3 sigmas of value 1
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  const auto model = noiseModel::Isotropic::Sigma(3, 1);
   // create stereo camera calibration object with .2m between cameras
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
+  const Cal3_S2Stereo::shared_ptr K(
+      new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
 
   //create and add stereo factors between first pose (key value 1) and the three landmarks
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
@@ -56,7 +56,8 @@ int main(int argc, char** argv){
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
 
-  //create Values object to contain initial estimates of camera poses and landmark locations
+  // create Values object to contain initial estimates of camera poses and
+  // landmark locations
   Values initial_estimate;
 
   // create and add iniital estimates

examples/StereoVOExample_large.cpp

@@ -10,7 +10,7 @@
 * -------------------------------------------------------------------------- */
 
 /**
-* @file SteroVOExample.cpp
+* @file StereoVOExample_large.cpp
 * @brief A stereo visual odometry example
 * @date May 25, 2014
 * @author Stephen Camp
@@ -42,10 +42,9 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   Values initial_estimate;
   NonlinearFactorGraph graph;
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  const auto model = noiseModel::Isotropic::Sigma(3, 1);
 
   string calibration_loc = findExampleDataFile("VO_calibration.txt");
   string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
@@ -65,7 +64,8 @@ int main(int argc, char** argv){
   cout << "Reading camera poses" << endl;
   int pose_id;
   MatrixRowMajor m(4, 4);
-  //read camera pose parameters and use to make initial estimates of camera poses
+  // read camera pose parameters and use to make initial estimates of camera
+  // poses
   while (pose_file >> pose_id) {
     for (int i = 0; i < 16; i++) {
       pose_file >> m.data()[i];
@@ -76,27 +76,31 @@ int main(int argc, char** argv){
   // camera and landmark keys
   size_t x, l;
 
-  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
+  // pixel coordinates uL, uR, v (same for left/right images due to
-  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  // rectification) landmark coordinates X, Y, Z in camera frame, resulting from
+  // triangulation
   double uL, uR, v, X, Y, Z;
   ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
-  //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
+  // read stereo measurement details from file and use to create and add
+  // GenericStereoFactor objects to the graph representation
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
-    graph.emplace_shared<
+    graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(
-        GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
+        StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
-            Symbol('x', x), Symbol('l', l), K);
+    // if the landmark variable included in this factor has not yet been added
-    //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
+    // to the initial variable value estimate, add it
     if (!initial_estimate.exists(Symbol('l', l))) {
       Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
-      //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
+      // transformFrom() transforms the input Point3 from the camera pose space,
+      // camPose, to the global space
       Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
       initial_estimate.insert(Symbol('l', l), worldPoint);
     }
   }
 
   Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x', 1));
-  //constrain the first pose such that it cannot change from its original value during optimization
+  // constrain the first pose such that it cannot change from its original value
+  // during optimization
   // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
   // QR is much slower than Cholesky, but numerically more stable
   graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);

examples/TimeTBB.cpp

@@ -28,9 +28,12 @@ using boost::assign::list_of;
 
 #ifdef GTSAM_USE_TBB
 
-#include <tbb/tbb.h>
+#include <tbb/blocked_range.h>           // tbb::blocked_range
-#undef max // TBB seems to include windows.h and we don't want these macros
+#include <tbb/tick_count.h>              // tbb::tick_count
-#undef min
+#include <tbb/parallel_for.h>            // tbb::parallel_for
+#include <tbb/cache_aligned_allocator.h> // tbb::cache_aligned_allocator
+#include <tbb/task_arena.h>              // tbb::task_arena
+#include <tbb/task_group.h>              // tbb::task_group
 
 static const DenseIndex numberOfProblems = 1000000;
 static const DenseIndex problemSize = 4;
@@ -67,10 +70,14 @@ struct WorkerWithoutAllocation
 };
 
 /* ************************************************************************* */
-map<int, double> testWithoutMemoryAllocation()
+map<int, double> testWithoutMemoryAllocation(int num_threads)
 {
   // A function to do some matrix operations without allocating any memory
 
+  // Create task_arena and task_group
+  tbb::task_arena arena(num_threads);
+  tbb::task_group tg;
+
   // Now call it
   vector<double> results(numberOfProblems);
 
@@ -79,7 +86,14 @@ map<int, double> testWithoutMemoryAllocation()
   for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
+
+    // Run parallel code (as a task group) inside of task arena
+    arena.execute([&]{
+      tg.run_and_wait([&]{
         tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
+      });
+    });
+
     tbb::tick_count t1 = tbb::tick_count::now();
     cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
     timingResults[(int)grainSize] = (t1 - t0).seconds();
@@ -120,10 +134,14 @@ struct WorkerWithAllocation
 };
 
 /* ************************************************************************* */
-map<int, double> testWithMemoryAllocation()
+map<int, double> testWithMemoryAllocation(int num_threads)
 {
   // A function to do some matrix operations with allocating memory
 
+  // Create task_arena and task_group
+  tbb::task_arena arena(num_threads);
+  tbb::task_group tg;
+
   // Now call it
   vector<double> results(numberOfProblems);
 
@@ -132,7 +150,14 @@ map<int, double> testWithMemoryAllocation()
   for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
+
+    // Run parallel code (as a task group) inside of task arena
+    arena.execute([&]{
+      tg.run_and_wait([&]{
         tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
+      });
+    });
+
     tbb::tick_count t1 = tbb::tick_count::now();
     cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
     timingResults[(int)grainSize] = (t1 - t0).seconds();
@@ -153,9 +178,8 @@ int main(int argc, char* argv[])
   for(size_t n: numThreads)
   {
     cout << "With " << n << " threads:" << endl;
-    tbb::task_scheduler_init init((int)n);
+    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation((int)n);
-    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
+    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation((int)n);
-    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
     cout << endl;
   }
 

examples/UGM_chain.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file small.cpp
+ * @file UGM_chain.cpp
  * @brief UGM (undirected graphical model) examples: chain
  * @author Frank Dellaert
  * @author Abhijit Kundu
@@ -19,10 +19,9 @@
  * for more explanation. This code demos the same example using GTSAM.
  */
 
-#include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteSequentialSolver.h>
-#include <gtsam/discrete/DiscreteMarginals.h>
 #include <gtsam/base/timing.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
 
 #include <iomanip>
 
@@ -30,7 +29,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Set Number of Nodes in the Graph
   const int nrNodes = 60;
 
@@ -51,10 +49,10 @@ int main(int argc, char** argv) {
 
   // add node potentials
   graph.add(nodes[0], ".3 .6 .1 0 0 0 0");
-    for (int i = 1; i < nrNodes; i++)
+  for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1");
-        graph.add(nodes[i], "1 1 1 1 1 1 1");
 
-    const std::string edgePotential =   ".08 .9 .01 0 0 0 .01 "
+  const std::string edgePotential =
+      ".08 .9 .01 0 0 0 .01 "
       ".03 .95 .01 0 0 0 .01 "
       ".06 .06 .75 .05 .05 .02 .01 "
       "0 0 0 .3 .6 .09 .01 "
@@ -71,39 +69,24 @@ int main(int argc, char** argv) {
 
   // "Decoding", i.e., configuration with largest value
   // We use sequential variable elimination
-  DiscreteSequentialSolver solver(graph);
+  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
-  DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
+  DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
   optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n");
 
   // "Inference" Computing marginals for each node
-
-
-  cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl;
-  gttic_(Sequential);
-  for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
-      ++itr) {
-    //Compute the marginal
-    Vector margProbs = solver.marginalProbabilities(*itr);
-
-    //Print the marginals
-    cout << "Node#" << setw(4) << itr->first << " :  ";
-    print(margProbs);
-  }
-  gttoc_(Sequential);
-
   // Here we'll make use of DiscreteMarginals class, which makes use of
   // bayes-tree based shortcut evaluation of marginals
   DiscreteMarginals marginals(graph);
 
   cout << "\nComputing Node Marginals ..(BayesTree based)" << endl;
   gttic_(Multifrontal);
-  for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
+  for (vector<DiscreteKey>::iterator it = nodes.begin(); it != nodes.end();
-      ++itr) {
+       ++it) {
     // Compute the marginal
-    Vector margProbs = marginals.marginalProbabilities(*itr);
+    Vector margProbs = marginals.marginalProbabilities(*it);
 
     // Print the marginals
-    cout << "Node#" << setw(4) << itr->first << " :  ";
+    cout << "Node#" << setw(4) << it->first << " :  ";
     print(margProbs);
   }
   gttoc_(Multifrontal);
@@ -111,4 +94,3 @@ int main(int argc, char** argv) {
   tictoc_print_();
   return 0;
 }
-

examples/UGM_small.cpp

@@ -10,15 +10,16 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file small.cpp
+ * @file UGM_small.cpp
  * @brief UGM (undirected graphical model) examples: small
  * @author Frank Dellaert
  *
  * See http://www.di.ens.fr/~mschmidt/Software/UGM/small.html
  */
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/discrete/DiscreteSequentialSolver.h>
+#include <gtsam/discrete/DiscreteMarginals.h>
 
 using namespace std;
 using namespace gtsam;
@@ -61,24 +62,24 @@ int main(int argc, char** argv) {
 
   // "Decoding", i.e., configuration with largest value (MPE)
   // We use sequential variable elimination
-  DiscreteSequentialSolver solver(graph);
+  DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
-  DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
+  DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
   optimalDecoding->print("\noptimalDecoding");
 
   // "Inference" Computing marginals
   cout << "\nComputing Node Marginals .." << endl;
-  Vector margProbs;
+  DiscreteMarginals marginals(graph);
 
-  margProbs = solver.marginalProbabilities(Cathy);
+  Vector margProbs = marginals.marginalProbabilities(Cathy);
   print(margProbs, "Cathy's Node Marginal:");
 
-  margProbs = solver.marginalProbabilities(Heather);
+  margProbs = marginals.marginalProbabilities(Heather);
   print(margProbs, "Heather's Node Marginal");
 
-  margProbs = solver.marginalProbabilities(Mark);
+  margProbs = marginals.marginalProbabilities(Mark);
   print(margProbs, "Mark's Node Marginal");
 
-  margProbs = solver.marginalProbabilities(Allison);
+  margProbs = marginals.marginalProbabilities(Allison);
   print(margProbs, "Allison's Node Marginal");
 
   return 0;

examples/VisualISAM2Example.cpp

@@ -47,7 +47,6 @@
 // Adjustment problems. Here we will use Projection factors to model the
 // camera's landmark observations. Also, we will initialize the robot at some
 // location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 #include <vector>
@@ -110,13 +109,11 @@ int main(int argc, char* argv[]) {
       static auto kPosePrior = noiseModel::Diagonal::Sigmas(
           (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
               .finished());
-      graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0],
+      graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
-                                                kPosePrior);
 
       // Add a prior on landmark l0
       static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0],
+      graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
-                                                 kPointPrior);
 
       // Add initial guesses to all observed landmarks
       // Intentionally initialize the variables off from the ground truth

examples/VisualISAMExample.cpp

@@ -38,7 +38,6 @@
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
 // Here we will use Projection factors to model the camera's landmark observations.
 // Also, we will initialize the robot at some location using a Prior factor.
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 
 // We want to use iSAM to solve the structure-from-motion problem incrementally, so
@@ -57,13 +56,11 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr noise = //
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -82,7 +79,6 @@ int main(int argc, char* argv[]) {
 
   // Loop over the different poses, adding the observations to iSAM incrementally
   for (size_t i = 0; i < poses.size(); ++i) {
-
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
       // Create ground truth measurement
@@ -106,14 +102,14 @@ int main(int argc, char* argv[]) {
     // adding it to iSAM.
     if (i == 0) {
       // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+      auto poseNoise = noiseModel::Diagonal::Sigmas(
           (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
-      graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
+      graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
 
       // Add a prior on landmark l0
-      noiseModel::Isotropic::shared_ptr pointNoise =
+      auto pointNoise =
           noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
+      graph.addPrior(Symbol('l', 0), points[0], pointNoise);
 
       // Add initial guesses to all observed landmarks
       Point3 noise(-0.25, 0.20, 0.15);

examples/easyPoint2KalmanFilter.cpp

@@ -23,7 +23,7 @@
 
 #include <gtsam/nonlinear/ExtendedKalmanFilter.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Point2.h>
 

examples/elaboratePoint2KalmanFilter.cpp

@@ -20,7 +20,7 @@
  * @author Stephen Williams
  */
 
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 //#include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/inference/Symbol.h>

gtsam.h

@@ -281,7 +281,7 @@ virtual class Value {
 };
 
 #include <gtsam/base/GenericValue.h>
-template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {
   void serializable() const;
 };
@@ -598,6 +598,7 @@ class SOn {
   // Standard Constructors
   SOn(size_t n);
   static gtsam::SOn FromMatrix(Matrix R);
+  static gtsam::SOn Lift(size_t n, Matrix R);
 
   // Testable
   void print(string s) const;
@@ -1458,7 +1459,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@@ -1469,7 +1470,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@@ -1480,7 +1481,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
@@ -1491,7 +1492,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@@ -1502,7 +1503,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@@ -1513,7 +1514,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
@@ -1524,7 +1525,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class DCS: gtsam::noiseModel::mEstimator::Base {
@@ -1535,7 +1536,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
@@ -1546,7 +1547,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 
   double weight(double error) const;
-  double residual(double error) const;
+  double loss(double error) const;
 };
 
 }///\namespace mEstimator
@@ -1566,14 +1567,12 @@ class Sampler {
   // Constructors
   Sampler(gtsam::noiseModel::Diagonal* model, int seed);
   Sampler(Vector sigmas, int seed);
-  Sampler(int seed);
 
   // Standard Interface
   size_t dim() const;
   Vector sigmas() const;
   gtsam::noiseModel::Diagonal* model() const;
   Vector sample();
-  Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
 };
 
 #include <gtsam/linear/VectorValues.h>
@@ -1939,6 +1938,22 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
   void print() const;
 };
 
+#include <gtsam/linear/Preconditioner.h>
+virtual class PreconditionerParameters {
+  PreconditionerParameters();
+};
+
+virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
+  DummyPreconditionerParameters();
+};
+
+#include <gtsam/linear/PCGSolver.h>
+virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
+  PCGSolverParameters();
+  void print(string s);
+  void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
+};
+
 #include <gtsam/linear/SubgraphSolver.h>
 virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
   SubgraphSolverParameters();
@@ -1979,6 +1994,35 @@ size_t symbol(char chr, size_t index);
 char symbolChr(size_t key);
 size_t symbolIndex(size_t key);
 
+namespace symbol_shorthand {
+  size_t A(size_t j);
+  size_t B(size_t j);
+  size_t C(size_t j);
+  size_t D(size_t j);
+  size_t E(size_t j);
+  size_t F(size_t j);
+  size_t G(size_t j);
+  size_t H(size_t j);
+  size_t I(size_t j);
+  size_t J(size_t j);
+  size_t K(size_t j);
+  size_t L(size_t j);
+  size_t M(size_t j);
+  size_t N(size_t j);
+  size_t O(size_t j);
+  size_t P(size_t j);
+  size_t Q(size_t j);
+  size_t R(size_t j);
+  size_t S(size_t j);
+  size_t T(size_t j);
+  size_t U(size_t j);
+  size_t V(size_t j);
+  size_t W(size_t j);
+  size_t X(size_t j);
+  size_t Y(size_t j);
+  size_t Z(size_t j);
+}///\namespace symbol
+
 // Default keyformatter
 void PrintKeyList  (const gtsam::KeyList& keys);
 void PrintKeyList  (const gtsam::KeyList& keys, string s);
@@ -2052,6 +2096,9 @@ class NonlinearFactorGraph {
   gtsam::KeySet keys() const;
   gtsam::KeyVector keyVector() const;
 
+  template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
+  void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
+
   // NonlinearFactorGraph
   void printErrors(const gtsam::Values& values) const;
   double error(const gtsam::Values& values) const;
@@ -2140,6 +2187,7 @@ class Values {
   void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
   void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
+  void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, Vector vector);
   void insert(size_t j, Matrix matrix);
 
@@ -2157,10 +2205,11 @@ class Values {
   void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
+  void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
   void update(size_t j, Matrix matrix);
 
-  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
+  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
   T at(size_t j);
 
   /// version for double
@@ -2262,6 +2311,8 @@ virtual class NonlinearOptimizerParams {
 bool checkConvergence(double relativeErrorTreshold,
                       double absoluteErrorTreshold, double errorThreshold,
                       double currentError, double newError);
+bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
+                      double currentError, double newError);
 
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
@@ -2496,8 +2547,8 @@ class NonlinearISAM {
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/PriorFactor.h>
-template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
+template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2520,7 +2571,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
-template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
+template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
 virtual class NonlinearEquality : gtsam::NoiseModelFactor {
   // Constructor - forces exact evaluation
   NonlinearEquality(size_t j, const T& feasible);
@@ -2761,8 +2812,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
 // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
 class BetweenFactorPose3s
 {
+  BetweenFactorPose3s();
   size_t size() const;
   gtsam::BetweenFactorPose3* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactorPose3* factor);
 };
 
 #include <gtsam/slam/InitializePose3.h>
@@ -2799,6 +2852,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
   KarcherMeanFactor(const gtsam::KeyVector& keys);
 };
 
+#include <gtsam/slam/FrobeniusFactor.h>
+gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
+    gtsam::noiseModel::Base* model, size_t d);
+
+template<T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
+  FrobeniusFactor(size_t key1, size_t key2);
+  FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+template<T = {gtsam::SO3, gtsam::SO4}>
+virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
+  FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
+
+  Vector evaluateError(const T& R1, const T& R2);
+};
+
+virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
+  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+                          size_t p);
+  FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
+                          size_t p, gtsam::noiseModel::Base* model);
+  Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
+};
+
 //*************************************************************************
 // Navigation
 //*************************************************************************
@@ -2912,11 +2993,14 @@ class PreintegratedImuMeasurements {
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
       double deltaT);
   void resetIntegration();
+  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
+
   Matrix preintMeasCov() const;
   double deltaTij() const;
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;
@@ -2971,11 +3055,14 @@ class PreintegratedCombinedMeasurements {
   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
       double deltaT);
   void resetIntegration();
+  void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
+
   Matrix preintMeasCov() const;
   double deltaTij() const;
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
+  gtsam::imuBias::ConstantBias biasHat() const;
   Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
       const gtsam::imuBias::ConstantBias& bias) const;

gtsam/3rdparty/CMakeLists.txt

@@ -1,6 +1,6 @@
 # install CCOLAMD headers
-install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
+install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/CCOLAMD)
-install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config)
+install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/SuiteSparse_config)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	# Find plain .h files
@@ -12,12 +12,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 		get_filename_component(filename ${eigen_dir} NAME)
 		if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
 			list(APPEND eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/${filename}")
-			install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen)
+			install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/Eigen)
 		endif()
 	endforeach(eigen_dir)
 
 	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
-		message("-- Installing Eigen Unsupported modules")
+		message(STATUS "Installing Eigen Unsupported modules")
 		# do the same for the unsupported eigen folder
 		file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
 
@@ -26,7 +26,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 			get_filename_component(filename ${unsupported_eigen_dir} NAME)
 			if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES "CXX11") OR (${filename} MATCHES ".svn")))
 				list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
-				install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
+				install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/Eigen)
 			endif()
 		endforeach(unsupported_eigen_dir)
 	endif()
@@ -37,12 +37,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 
 	# install Eigen - only the headers in our 3rdparty directory
 	install(DIRECTORY Eigen/Eigen
-		DESTINATION include/gtsam/3rdparty/Eigen
+		DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen
 		FILES_MATCHING PATTERN "*.h")
 
 	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
 		install(DIRECTORY Eigen/unsupported/Eigen
-			DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
+			DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/
 			FILES_MATCHING PATTERN "*.h")
 	endif()
 

gtsam/3rdparty/Eigen/blas/CMakeLists.txt

@@ -39,9 +39,9 @@ endif()
 add_dependencies(blas eigen_blas eigen_blas_static)
 
 install(TARGETS eigen_blas eigen_blas_static
-        RUNTIME DESTINATION bin
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
-        LIBRARY DESTINATION lib
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
-        ARCHIVE DESTINATION lib)
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
 if(EIGEN_Fortran_COMPILER_WORKS)
 

gtsam/3rdparty/Eigen/lapack/CMakeLists.txt

@@ -103,9 +103,9 @@ endif()
 add_dependencies(lapack eigen_lapack eigen_lapack_static)
 
 install(TARGETS eigen_lapack eigen_lapack_static
-        RUNTIME DESTINATION bin
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
-        LIBRARY DESTINATION lib
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
-        ARCHIVE DESTINATION lib)
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
         
         

gtsam/3rdparty/metis/GKlib/CMakeLists.txt

@@ -16,6 +16,6 @@ include_directories("test")
 add_subdirectory("test")
 
 install(TARGETS GKlib
-  ARCHIVE DESTINATION lib
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
-  LIBRARY DESTINATION lib)
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR})
-install(FILES ${GKlib_includes} DESTINATION include)
+install(FILES ${GKlib_includes} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})