Merge remote-tracking branch 'origin/develop' into feature/noisemodel_rename_functions

.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)

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

@@ -115,7 +115,7 @@ IF(WIN32 AND MKL_ROOT_DIR)
         IF (MKL_INCLUDE_DIR MATCHES "10.3")
                 SET(MKL_LIBS ${MKL_LIBS} libiomp5md)
         ENDIF()
-        
+
         FOREACH (LIB ${MKL_LIBS})
                 FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH)
                 IF(${LIB}_PATH)
@@ -147,7 +147,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
                 ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
                 ${MKL_ROOT_DIR}/lib/
         )
-        
+
         # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
         IF(NOT APPLE)
             FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
@@ -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()
@@ -254,7 +255,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
         ELSE()
             SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
         ENDIF()
-        
+
         MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
                 MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
 ENDIF()
@@ -266,4 +267,4 @@ find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES)
 #        LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack
 #endif()
 
-MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)
+MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)

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_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,93 @@
+"""
+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)
+
+        # 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)
+
+    @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,50 @@
+"""
+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))
+    optimize(optimizer, check_convergence, hook)

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
 
@@ -4430,7 +4448,7 @@ In the case of
 \begin_inset Formula $\SOthree$
 \end_inset
 
- the vector space is  
+ the vector space is 
 \begin_inset Formula $\Rthree$
 \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/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/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:
@@ -36,15 +38,15 @@
  */
 
 // GTSAM related includes.
+#include <gtsam/inference/Symbol.h>
 #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/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/dataset.h>
+
 #include <cstring>
 #include <fstream>
 #include <iostream>
@@ -52,26 +54,21 @@
 using namespace gtsam;
 using namespace std;
 
-using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
+using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
-using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
-using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
+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;
   if (argc < 2) {
     printf("using default CSV file\n");
     data_filename = findExampleDataFile("imuAndGPSdata.csv");
-  } else if (argc < 3){
+  } 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");
@@ -81,15 +78,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
@@ -98,9 +97,9 @@ 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
+  getline(file, value, ',');  // i
-  for (int i=0; i<9; i++) {
+  for (int i = 0; i < 9; i++) {
     getline(file, value, ',');
     initial_state(i) = atof(value.c_str());
   }
@@ -108,13 +107,14 @@ 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>());
   Pose3 prior_pose(prior_rotation, prior_point);
   Vector3 prior_velocity(initial_state.tail<3>());
-  imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
+  imuBias::ConstantBias prior_imu_bias;  // assume zero initial bias
 
   Values initial_values;
   int correction_count = 0;
@@ -122,57 +122,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();
+  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->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
-  p->biasOmegaCovariance = bias_omega_cov; // gyro 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;
@@ -181,14 +188,13 @@ 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
+    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) {
+      for (int i = 0; i < 5; ++i) {
         getline(file, value, ',');
         imu(i) = atof(value.c_str());
       }
@@ -196,11 +202,11 @@ 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
+    } 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) {
+      for (int i = 0; i < 6; ++i) {
         getline(file, value, ',');
         gps(i) = atof(value.c_str());
       }
@@ -211,39 +217,37 @@ 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),
-        ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
+                             X(correction_count), V(correction_count),
-                             X(correction_count  ), V(correction_count  ),
+                             B(correction_count - 1), preint_imu);
-                             B(correction_count-1),
-                             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,
+                           Point3(gps(0),   // N,
-                                  gps(1),  // E,
+                                  gps(1),   // E,
-                                  gps(2)), // D,
+                                  gps(2)),  // D,
                            correction_noise);
       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);
@@ -257,7 +261,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();
@@ -268,19 +272,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);
-                                 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;
 
@@ -290,6 +294,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,12 +66,11 @@ 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_;
 
-public:
+ public:
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<UnaryFactor> shared_ptr;
 
@@ -85,15 +84,15 @@ 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
     // Consequently, the Jacobians are:
     // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
     // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
-    if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
+    if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
     return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
   }
 
@@ -107,29 +106,28 @@ 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
-}; // 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);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
@@ -137,7 +135,7 @@ int main(int argc, char** argv) {
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
   initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize using Levenberg-Marquardt optimization. The optimizer
   // accepts an optional set of configuration parameters, controlling

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;
@@ -35,26 +35,26 @@ using namespace gtsam;
 int main(int argc, char** argv) {
   NonlinearFactorGraph graph;
 
-  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
+  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
+  graph.print("\nFactor Graph:\n");  // print
 
   Values initial;
   initial.insert(1, Pose2(0.5, 0.0, 0.2));
   initial.insert(2, Pose2(2.3, 0.1, -0.2));
   initial.insert(3, Pose2(4.1, 0.1, 0.1));
-  initial.print("\nInitial Estimate:\n"); // print
+  initial.print("\nInitial Estimate:\n");  // print
 
   // 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,24 +56,23 @@ 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
+  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);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // Create the data structure to hold the initialEstimate estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
@@ -82,7 +80,7 @@ int main(int argc, char** argv) {
   initial.insert(1, Pose2(0.5, 0.0, 0.2));
   initial.insert(2, Pose2(2.3, 0.1, -0.2));
   initial.insert(3, Pose2(4.1, 0.1, 0.1));
-  initial.print("\nInitial Estimate:\n"); // print
+  initial.print("\nInitial Estimate:\n");  // print
 
   // optimize using Levenberg-Marquardt optimization
   Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

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,35 +69,36 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create a factor graph
   NonlinearFactorGraph graph;
 
   // Create the keys we need for this simple example
-  static Symbol x1('x',1), x2('x',2), x3('x',3);
+  static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
-  static Symbol l1('l',1), l2('l',2);
+  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
-  Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
+  // a noise model (covariance matrix)
-  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
+  Pose2 prior(0.0, 0.0, 0.0);  // prior mean is at origin
-  graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
+  auto priorNoise = noiseModel::Diagonal::Sigmas(
+      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);
@@ -111,7 +111,7 @@ int main(int argc, char** argv) {
   // Create (deliberately inaccurate) initial estimate
   Values initialEstimate;
   initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
+  initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));
   initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
   initialEstimate.insert(l1, Point2(1.8, 2.1));
   initialEstimate.insert(l2, Point2(4.1, 1.8));
@@ -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,21 +64,20 @@ 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
-  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@@ -89,17 +87,17 @@ int main(int argc, char** argv) {
   // these constraints may be identified in many ways, such as appearance-based techniques
   // with camera images. We will use another Between Factor to enforce this constraint:
   graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the 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(1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
-  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
-  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
   initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
   // The optimizer accepts an optional set of configuration parameters,

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,31 +37,32 @@ 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);
+  graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
-  graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
-  graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
-  graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
 
   // 2c. Add the loop closure constraint
-  graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
-  graph.print("\nFactor Graph:\n"); // print
+  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(1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
-  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
-  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
   initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
   GaussNewtonParams parameters;

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,50 +28,51 @@ 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
+  int maxIterations = 100;                                    // default
-  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt");  // default
 
   // Parse user's inputs
-  if (argc > 1){
+  if (argc > 1) {
-    g2oFile = argv[1]; // input dataset filename
+    g2oFile = argv[1];  // input dataset filename
-    // outputFile = g2oFile = argv[2]; // done later
   }
-  if (argc > 3){
+  if (argc > 3) {
-    maxIterations = atoi(argv[3]); // user can specify either tukey or huber
+    maxIterations = atoi(argv[3]);  // user can specify either tukey or huber
   }
-  if (argc > 4){
+  if (argc > 4) {
-    kernelType = argv[4]; // user can specify either tukey or huber
+    kernelType = argv[4];  // user can specify either tukey or huber
   }
 
   // reading file and creating factor graph
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = false;
-  if(kernelType.compare("none") == 0){
+  if (kernelType.compare("none") == 0) {
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D);
+    boost::tie(graph, initial) = readG2o(g2oFile, is3D);
   }
-  if(kernelType.compare("huber") == 0){
+  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){
+  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;
@@ -80,8 +80,8 @@ int main(const int argc, const char *argv[]) {
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");

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>
@@ -26,20 +25,20 @@
 using namespace std;
 using namespace gtsam;
 
-int main (int argc, char** argv) {
+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);
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@@ -50,10 +49,10 @@ int main (int argc, char** argv) {
   // 3. Create the data structure to hold the initial estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
   Values initial;
-  initial.insert(1, Pose2(0.5, 0.0,  0.2   ));
+  initial.insert(1, Pose2(0.5, 0.0, 0.2));
-  initial.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initial.insert(2, Pose2(2.3, 0.1, -0.2));
-  initial.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
-  initial.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initial.insert(4, Pose2(4.0, 2.0, M_PI));
   initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
 
   // Single Step Optimization using Levenberg-Marquardt

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,45 +28,45 @@ 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
+  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),
-  graph.print("\nFactor Graph:\n"); // print
+                                              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));
   initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
   initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
-  initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
+  initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Single Step Optimization using Levenberg-Marquardt
   LevenbergMarquardtParams parameters;
   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>
@@ -27,8 +26,7 @@
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -42,25 +40,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) {
+  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;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");
@@ -76,7 +74,7 @@ int main(const int argc, const char *argv[]) {
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for(const auto& key_value: result) {
+  for (const auto& key_value : result) {
     auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
     if (!p) continue;
     std::cout << marginals.marginalCovariance(key_value.key) << 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,24 +10,22 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @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>
 
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,25 +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) {
+  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;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");

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[]) {
+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) {
+  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;
   }
 
@@ -59,7 +57,7 @@ int main(const int argc, const char *argv[]) {
     initialization.print("initialization");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
     boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

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[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,29 +39,31 @@ 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) {
+  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 << "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");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
     boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

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) {
+    auto 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

@@ -28,7 +28,7 @@
 // Header order is close to far
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 #include <vector>
 
 using namespace std;
@@ -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;
@@ -65,23 +61,23 @@ int main(int argc, char* argv[]) {
   Pose3_ pose0_(&SfmCamera::getPose, camera0_);
   // Finally, we say it should be equal to first guess
   graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
-      noiseModel::Isotropic::Sigma(6, 0.1));
+                            noiseModel::Isotropic::Sigma(6, 0.1));
 
   // similarly, we create a prior on the first point
   Point3_ point0_(P(0));
   graph.addExpressionFactor(point0_, mydata.tracks[0].p,
-      noiseModel::Isotropic::Sigma(3, 0.1));
+                            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) {
+  for (const SfmTrack& track : mydata.tracks) {
     // Leaf expression for j^th point
     Point3_ point_('p', j);
-    for(const SfmMeasurement& m: track.measurements) {
+    for (const SfmMeasurement& m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       // Leaf expression for i^th camera
@@ -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,13 +35,12 @@ 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
+      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
   vector<Point3> points = createPoints();
@@ -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,9 +21,8 @@
 #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 !
+#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 
 #include <gtsam/base/timing.h>
 
@@ -35,50 +34,52 @@ 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;
+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]);
+  if (argc > 1) filename = string(argv[1]);
 
   // 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;
-  for(const SfmTrack& track: mydata.tracks) {
+  for (const SfmTrack& track : mydata.tracks) {
-    for(const SfmMeasurement& m: track.measurements) {
+    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;
-  for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
+  j = 0;
-  for(const SfmTrack& track: mydata.tracks)    initial.insert(P(j++), track.p);
+  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
+  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
 
   /** ---------------  COMPARISON  -----------------------**/
   /** ----------------------------------------------------**/
@@ -99,7 +100,7 @@ int main (int argc, char* argv[]) {
   }
 
   // expect they have different ordering results
-  if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
+  if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
     cout << "COLAMD and METIS produce the same ordering. "
          << "Problem here!!!" << endl;
   }
@@ -121,8 +122,7 @@ int main (int argc, char* argv[]) {
     cout << e.what();
   }
 
-
+  {  // printing the result
-  { // printing the result
 
     cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
     cout << "METIS final error: " << graph.error(result_METIS) << 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,8 +33,7 @@
 #include <gtsam/nonlinear/Values.h>
 
 // SFM-specific factors
-#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/GeneralSFMFactor.h>  // does calibration !
-#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
 
 // Standard headers
 #include <vector>
@@ -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);
+  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
@@ -34,17 +34,17 @@
 using namespace std;
 using namespace gtsam;
 
-int main(int argc, char** argv){
+int main(int argc, char** argv) {
-
+  // create graph object, add first pose at origin with key '1'
-  //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
+  // 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
+  // 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,17 +56,18 @@ 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
+  // create and add iniital estimates
   initial_estimate.insert(1, first_pose);
   initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
   initial_estimate.insert(3, Point3(1, 1, 5));
   initial_estimate.insert(4, Point3(-1, 1, 5));
   initial_estimate.insert(5, Point3(0, -0.5, 5));
 
-  //create Levenberg-Marquardt optimizer for resulting factor graph, optimize
+  // create Levenberg-Marquardt optimizer for resulting factor graph, optimize
   LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
   Values result = optimizer.optimize();
 

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
@@ -41,31 +41,31 @@
 using namespace std;
 using namespace gtsam;
 
-int main(int argc, char** argv){
+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");
   string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
 
-  //read camera calibration info from file
+  // read camera calibration info from file
   // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
   double fx, fy, s, u0, v0, b;
   ifstream calibration_file(calibration_loc.c_str());
   cout << "Reading calibration info" << endl;
   calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
 
-  //create stereo camera calibration object
+  // create stereo camera calibration object
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b));
 
   ifstream pose_file(pose_loc.c_str());
   cout << "Reading camera poses" << endl;
   int pose_id;
-  MatrixRowMajor m(4,4);
+  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,33 +76,37 @@ 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));
+  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);
+  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);
 
   cout << "Optimizing" << endl;
-  //create Levenberg-Marquardt optimizer to optimize the factor graph
+  // create Levenberg-Marquardt optimizer to optimize the factor graph
   LevenbergMarquardtParams params;
   params.orderingType = Ordering::METIS;
   LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);

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();
-    tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
+
+    // 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();
-    tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
+
+    // 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/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;
@@ -1563,17 +1564,15 @@ virtual class Robust : gtsam::noiseModel::Base {
 
 #include <gtsam/linear/Sampler.h>
 class Sampler {
-    //Constructors
+  // Constructors
   Sampler(gtsam::noiseModel::Diagonal* model, int seed);
   Sampler(Vector sigmas, int seed);
-  Sampler(int seed);
 
-    //Standard Interface
+  // 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>
@@ -1979,6 +1978,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 +2080,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 +2171,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 +2189,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
@@ -2260,8 +2293,10 @@ virtual class NonlinearOptimizerParams {
 };
 
 bool checkConvergence(double relativeErrorTreshold,
-    double absoluteErrorTreshold, double errorThreshold,
+                      double absoluteErrorTreshold, double errorThreshold,
-    double currentError, double newError);
+                      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 +2531,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 +2555,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 +2796,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 +2836,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 +2977,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 +3039,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})

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

@@ -4,23 +4,26 @@ include_directories(.)
 file(GLOB metis_sources *.c)
 # Build libmetis.
 add_definitions(-fPIC)
-add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
+add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
 if(UNIX)
-  target_link_libraries(metis m)
+  target_link_libraries(metis-gtsam m)
 endif()
 
 if(WIN32)
-		set_target_properties(metis PROPERTIES
+		set_target_properties(metis-gtsam PROPERTIES
 			PREFIX ""
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
 endif()
 
 if (APPLE)
-		set_target_properties(metis PROPERTIES
+		set_target_properties(metis-gtsam PROPERTIES
 			INSTALL_NAME_DIR
 			"${CMAKE_INSTALL_PREFIX}/lib")
  endif()
 
-install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
+install(TARGETS metis-gtsam EXPORT GTSAM-exports
-list(APPEND GTSAM_EXPORTED_TARGETS metis)
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
+list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

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

@@ -15,7 +15,7 @@ endforeach(prog)
 
 if(METIS_INSTALL)
   install(TARGETS gpmetis ndmetis mpmetis m2gmetis graphchk cmpfillin
-    RUNTIME DESTINATION bin)
+    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
 endif()
 
 # Try to find subversion revision.

gtsam/CMakeLists.txt

@@ -49,7 +49,7 @@ endif()
 
 # Common headers
 file(GLOB gtsam_core_headers "*.h")
-install(FILES ${gtsam_core_headers} DESTINATION include/gtsam)
+install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)
 
 # assemble core libaries
 foreach(subdir ${gtsam_subdirs})
@@ -86,10 +86,10 @@ configure_file(config.h.in config.h)
 set(library_name GTSAM) # For substitution in dllexport.h.in
 configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
 list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h")
-install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam)
+install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)
 
 if(GTSAM_SUPPORT_NESTED_DISSECTION)
-    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam)
 endif()
 
 # Versions
@@ -172,12 +172,16 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 	endif()
 endif()
 
+if(WIN32) # library to help with demangling variable names on Windows
+  target_link_libraries(gtsam PRIVATE Dbghelp)
+endif()
+
 install(
 	TARGETS gtsam
 	EXPORT GTSAM-exports
-	LIBRARY DESTINATION lib
+	LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
-	ARCHIVE DESTINATION lib
+	ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
-	RUNTIME DESTINATION bin)
+	RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
 
 list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

gtsam/base/GenericValue.h

@@ -20,6 +20,7 @@
 #pragma once
 
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/types.h>
 #include <gtsam/base/Value.h>
 
 #include <boost/make_shared.hpp>
@@ -83,7 +84,7 @@ public:
 
   /// Virtual print function, uses traits
   virtual void print(const std::string& str) const {
-    std::cout << "(" << typeid(T).name() << ") ";
+    std::cout << "(" << demangle(typeid(T).name()) << ") ";
     traits<T>::Print(value_, str);
   }
 
@@ -180,7 +181,7 @@ public:
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
   enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues

gtsam/base/Lie.h

@@ -170,7 +170,7 @@ namespace internal {
 /// Assumes existence of: identity, dimension, localCoordinates, retract,
 /// and additionally Logmap, Expmap, compose, between, and inverse
 template<class Class>
-struct LieGroupTraits {
+struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
   typedef lie_group_tag structure_category;
 
   /// @name Group
@@ -186,8 +186,6 @@ struct LieGroupTraits {
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
-  static int GetDimension(const Class&) {return dimension;}
-
   static TangentVector Local(const Class& origin, const Class& other,
       ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
     return origin.localCoordinates(other, Horigin, Hother);

gtsam/base/Manifold.h

@@ -72,7 +72,7 @@ struct HasManifoldPrereqs {
 
 /// Extra manifold traits for fixed-dimension types
 template<class Class, int N>
-struct ManifoldImpl {
+struct GetDimensionImpl {
   // Compile-time dimensionality
   static int GetDimension(const Class&) {
     return N;
@@ -81,7 +81,7 @@ struct ManifoldImpl {
 
 /// Extra manifold traits for variable-dimension types
 template<class Class>
-struct ManifoldImpl<Class, Eigen::Dynamic> {
+struct GetDimensionImpl<Class, Eigen::Dynamic> {
   // Run-time dimensionality
   static int GetDimension(const Class& m) {
     return m.dim();
@@ -92,7 +92,7 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
 /// To use this for your class type, define:
 /// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
 template<class Class>
-struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
+struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
 
   // Check that Class has the necessary machinery
   BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@@ -214,7 +214,7 @@ public:
   enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
   };
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Define any direct product group to be a model of the multiplicative Group concept

gtsam/base/Matrix.cpp

@@ -144,10 +144,10 @@ void print(const Matrix& A, const string &s, ostream& stream) {
       0, // flags
       ", ", // coeffSeparator
       ";\n", // rowSeparator
-      " \t",  // rowPrefix
+      "\t",  // rowPrefix
       "", // rowSuffix
       "[\n", // matPrefix
-      "\n  ]" // matSuffix
+      "\n]" // matSuffix
       );
   cout << s << A.format(matlab) << endl;
 }

gtsam/base/OptionalJacobian.h

@@ -55,7 +55,7 @@ private:
   }
 
   // Private and very dangerous constructor straight from memory
-  OptionalJacobian(double* data) : map_(NULL) {
+  OptionalJacobian(double* data) : map_(nullptr) {
     if (data) usurp(data);
   }
 
@@ -66,25 +66,25 @@ public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-      map_(NULL) {
+      map_(nullptr) {
   }
 
   /// Constructor that will usurp data of a fixed-size matrix
   OptionalJacobian(Jacobian& fixed) :
-      map_(NULL) {
+      map_(nullptr) {
     usurp(fixed.data());
   }
 
   /// Constructor that will usurp data of a fixed-size matrix, pointer version
   OptionalJacobian(Jacobian* fixedPtr) :
-      map_(NULL) {
+      map_(nullptr) {
     if (fixedPtr)
       usurp(fixedPtr->data());
   }
 
   /// Constructor that will resize a dynamic matrix (unless already correct)
   OptionalJacobian(Eigen::MatrixXd& dynamic) :
-      map_(NULL) {
+      map_(nullptr) {
     dynamic.resize(Rows, Cols); // no malloc if correct size
     usurp(dynamic.data());
   }
@@ -93,12 +93,12 @@ public:
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t /*none*/) :
-      map_(NULL) {
+      map_(nullptr) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      map_(NULL) {
+      map_(nullptr) {
     if (optional) {
       optional->resize(Rows, Cols);
       usurp(optional->data());
@@ -110,11 +110,11 @@ public:
   /// Constructor that will usurp data of a block expression
   /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
   //  template <typename Derived, bool InnerPanel>
-  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? }
+  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return map_.data() != NULL;
+    return map_.data() != nullptr;
   }
 
   /// De-reference, like boost optional
@@ -173,7 +173,7 @@ public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-    pointer_(NULL) {
+    pointer_(nullptr) {
   }
 
   /// Construct from pointer to dynamic matrix
@@ -186,12 +186,12 @@ public:
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t /*none*/) :
-    pointer_(NULL) {
+    pointer_(nullptr) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      pointer_(NULL) {
+      pointer_(nullptr) {
     if (optional) pointer_ = &(*optional);
   }
 
@@ -199,7 +199,7 @@ public:
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return pointer_!=NULL;
+    return pointer_!=nullptr;
   }
 
   /// De-reference, like boost optional

gtsam/base/SymmetricBlockMatrix.h

@@ -416,4 +416,3 @@ namespace gtsam {
   class CholeskyFailed;
 
 }
-

gtsam/base/ThreadsafeException.h

@@ -11,7 +11,7 @@
 
 /**
  * @file     ThreadSafeException.h
- * @brief    Base exception type that uses tbb_exception if GTSAM is compiled with TBB
+ * @brief    Base exception type that uses tbb_allocator if GTSAM is compiled with TBB
  * @author   Richard Roberts
  * @date     Aug 21, 2010
  * @addtogroup base
@@ -25,34 +25,28 @@
 #include <gtsam/dllexport.h>
 #include <string>
 #include <typeinfo>
+#include <exception>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/tbb_allocator.h>
-#include <tbb/tbb_exception.h>
 #include <tbb/scalable_allocator.h>
 #include <iostream>
 #endif
 
 namespace gtsam {
 
-/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
+/// Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
 template<class DERIVED>
 class ThreadsafeException:
-#ifdef GTSAM_USE_TBB
-    public tbb::tbb_exception
-#else
 public std::exception
-#endif
 {
-#ifdef GTSAM_USE_TBB
 private:
-  typedef tbb::tbb_exception Base;
+  typedef std::exception Base;
+#ifdef GTSAM_USE_TBB
 protected:
   typedef std::basic_string<char, std::char_traits<char>,
       tbb::tbb_allocator<char> > String;
 #else
-private:
-  typedef std::exception Base;
 protected:
   typedef std::string String;
 #endif
@@ -82,36 +76,6 @@ protected:
   }
 
 public:
-  // Implement functions for tbb_exception
-#ifdef GTSAM_USE_TBB
-  virtual tbb::tbb_exception* move() throw () {
-    void* cloneMemory = scalable_malloc(sizeof(DERIVED));
-    if (!cloneMemory) {
-      std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
-      exit(-1);
-    }
-    DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
-    clone->dynamic_ = true;
-    return clone;
-  }
-
-  virtual void destroy() throw () {
-    if (dynamic_) {
-      DERIVED* derivedPtr = static_cast<DERIVED*>(this);
-      derivedPtr->~DERIVED();
-      scalable_free(derivedPtr);
-    }
-  }
-
-  virtual void throw_self() {
-    throw *static_cast<DERIVED*>(this);
-  }
-
-  virtual const char* name() const throw () {
-    return typeid(DERIVED).name();
-  }
-#endif
-
   virtual const char* what() const throw () {
     return description_ ? description_->c_str() : "";
   }

gtsam/base/debug.cpp

@@ -20,7 +20,7 @@
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #ifdef GTSAM_USE_TBB
-#include <tbb/mutex.h>
+#include <mutex> // std::mutex, std::unique_lock
 #endif
 
 namespace gtsam {
@@ -28,13 +28,13 @@ namespace gtsam {
 GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool, false> > debugFlags;
 
 #ifdef GTSAM_USE_TBB
-tbb::mutex debugFlagsMutex;
+std::mutex debugFlagsMutex;
 #endif
 
 /* ************************************************************************* */
 bool guardedIsDebug(const std::string& s) {
 #ifdef GTSAM_USE_TBB
-  tbb::mutex::scoped_lock lock(debugFlagsMutex);
+  std::unique_lock<std::mutex> lock(debugFlagsMutex);
 #endif
   return gtsam::debugFlags[s];
 }
@@ -42,7 +42,7 @@ bool guardedIsDebug(const std::string& s) {
 /* ************************************************************************* */
 void guardedSetDebug(const std::string& s, const bool v) {
 #ifdef GTSAM_USE_TBB
-  tbb::mutex::scoped_lock lock(debugFlagsMutex);
+  std::unique_lock<std::mutex> lock(debugFlagsMutex);
 #endif
   gtsam::debugFlags[s] = v;
 }

gtsam/base/make_shared.h

@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 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 make_shared.h
+ * @brief make_shared trampoline function to ensure proper alignment
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/base/types.h>
+
+#include <Eigen/Core>
+
+#include <boost/make_shared.hpp>
+
+#include <type_traits>
+
+namespace gtsam {
+  /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
+  template<bool B, class T = void>
+  using enable_if_t = typename std::enable_if<B, T>::type;
+}
+
+namespace gtsam {
+
+  /**
+   * Add our own `make_shared` as a layer of wrapping on `boost::make_shared`
+   * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs
+   * at runtime, which is notoriously hard to debug.
+   *
+   * Explanation
+   * ===============
+   * The template `needs_eigen_aligned_allocator<T>::value` will evaluate to `std::true_type` if the type alias
+   * `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the
+   * `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro.
+   *
+   * This function declaration will only be taken when the above condition is true, so if some object does not need to
+   * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for
+   * `boost::make_shared`.
+   *
+   * @tparam T The type of object being constructed
+   * @tparam Args Type of the arguments of the constructor
+   * @param args Arguments of the constructor
+   * @return The object created as a boost::shared_ptr<T>
+   */
+  template<typename T, typename ... Args>
+  gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
+    return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
+  }
+
+  /// Fall back to the boost version if no need for alignment
+  template<typename T, typename ... Args>
+  gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
+    return boost::make_shared<T>(std::forward<Args>(args)...);
+  }
+
+}