Merge branch 'develop' of github.com:borglab/gtsam into feature/1dsfm

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

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/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()

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

@@ -10,24 +10,20 @@ 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
-import gtsam
+from gtsam import symbol_shorthand_X as X
 from gtsam.examples import SFMdata
 from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
                          GenericProjectionFactorCal3_S2, Marginals,
-                         NonlinearFactorGraph, Point3, Pose3,
+                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
-                         PriorFactorPoint3, PriorFactorPose3, Rot3,
+                         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():
     """
     Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
@@ -74,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
@@ -83,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')
 
@@ -99,10 +95,10 @@ def main():
     initial_estimate = Values()
     for i, pose in enumerate(poses):
         transformed_pose = pose.retract(0.1*np.random.randn(6,1))
-        initial_estimate.insert(symbol('x', i), transformed_pose)
+        initial_estimate.insert(X(i), transformed_pose)
     for j, point in enumerate(points):
         transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
-        initial_estimate.insert(symbol('l', j), transformed_point)
+        initial_estimate.insert(L(j), transformed_point)
     initial_estimate.print_('Initial Estimates:\n')
 
     # Optimize the graph and print results

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_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,79 @@
+"""
+Unit tests for optimization that logs to comet.ml.
+Author: Jing Wu and Frank Dellaert
+"""
+# pylint: disable=invalid-name
+
+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)
+
+    def test_simple_printing(self):
+        """Test with a simple hook."""
+
+        # Provide a hook that just prints
+        def hook(_, error: float):
+            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: float):
+            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,54 @@
+"""
+Optimization with logging via a hook.
+Author: Jing Wu and Frank Dellaert
+"""
+# pylint: disable=invalid-name
+
+from typing import TypeVar
+
+from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
+import gtsam
+
+T = TypeVar('T')
+
+
+def optimize(optimizer: T, 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: NonlinearOptimizer,
+                   params: NonlinearOptimizerParams,
+                   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

@@ -13,8 +13,9 @@ 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.
-    Input
+
-      ax: a matplotlib axis, e.g., as output from plt.gca().
+    Args:
+      fignum (int): An integer representing the figure number for Matplotlib.
     """
     fig = plt.figure(fignum)
     ax = fig.gca(projection='3d')
@@ -34,7 +35,21 @@ def set_axes_equal(fignum):
 
 
 def ellipsoid(xc, yc, zc, rx, ry, rz, n):
-    """Numpy equivalent of Matlab's ellipsoid function"""
+    """
+    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
@@ -51,6 +66,14 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
     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)
@@ -74,7 +97,15 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
 
 
 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()
@@ -105,22 +136,47 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
 
 
 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, 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, 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, P)
@@ -128,10 +184,16 @@ def plot_point3(fignum, point, linespec, P=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()
@@ -142,19 +204,27 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
             key = keys.at(i)
             point = values.atPoint3(key)
             if marginals is not None:
-                P = marginals.marginalCovariance(key);
+                covariance = marginals.marginalCovariance(key)
             else:
-                P = None
+                covariance = None
 
-            plot_point3(fignum, point, linespec, P)
+            plot_point3(fignum, point, linespec, covariance)
 
         except RuntimeError:
             continue
             # I guess it's not a Point3
 
 
-def plot_pose3_on_axes(axes, pose, P=None, scale=1, 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
     origin = pose.translation().vector()
@@ -181,15 +251,35 @@ def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
         plot_covariance_ellipse_3d(axes, origin, gPp)
 
 
-def plot_pose3(fignum, pose, P, 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, P=P, axis_length=axis_length)
+    plot_pose3_on_axes(axes, pose, P=P,
+                       axis_length=axis_length)
+
 
 def plot_trajectory(fignum, values, scale=1, marginals=None):
-    pose3Values = gtsam.allPose3s(values)
+    """
+    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
 
@@ -209,11 +299,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
                 pass
 
             if marginals:
-                P = marginals.marginalCovariance(lastKey)
+                covariance = marginals.marginalCovariance(lastKey)
             else:
-                P = None
+                covariance = None
 
-            plot_pose3(fignum, lastPose, P, scale)
+            plot_pose3(fignum, lastPose,  P=covariance,
+                       axis_length=scale)
 
         lastIndex = i
 
@@ -223,11 +314,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
         try:
             lastPose = pose3Values.atPose3(lastKey)
             if marginals:
-                P = marginals.marginalCovariance(lastKey)
+                covariance = marginals.marginalCovariance(lastKey)
             else:
-                P = None
+                covariance = None
 
-            plot_pose3(fignum, lastPose, P, scale)
+            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,29 +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
-
-# Makefile target name for running unit tests:
-GTSAM_TEST_TARGET = check
-
-# 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=ON -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
-
-override_dh_auto_test-arch:
-	# Tests for arch-dependent :
-	echo "[override_dh_auto_test-arch]"
-	dh_auto_build -O--buildsystem=cmake -- $(GTSAM_TEST_TARGET)

debian/source/format

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

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

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;
 };
@@ -1977,6 +1977,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);
@@ -2050,6 +2079,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;
@@ -2138,6 +2170,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);
 
@@ -2155,10 +2188,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
@@ -2258,8 +2292,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 {
@@ -2495,7 +2531,7 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.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;
@@ -2518,7 +2554,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);
@@ -2759,8 +2795,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>
@@ -2910,11 +2948,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;
@@ -2969,11 +3010,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/SymmetricBlockMatrix.h

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

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)...);
+  }
+
+}

gtsam/base/serialization.h

@@ -42,123 +42,218 @@
 
 namespace gtsam {
 
-// Serialization directly to strings in compressed format
+/** @name Standard serialization
-template<class T>
+ *  Serialization in default compressed format
-std::string serialize(const T& input) {
+ */
-  std::ostringstream out_archive_stream;
+///@{
+/// serializes to a stream
+template <class T>
+void serializeToStream(const T& input, std::ostream& out_archive_stream) {
   boost::archive::text_oarchive out_archive(out_archive_stream);
   out_archive << input;
-  return out_archive_stream.str();
 }
 
-template<class T>
+/// deserializes from a stream
-void deserialize(const std::string& serialized, T& output) {
+template <class T>
-  std::istringstream in_archive_stream(serialized);
+void deserializeFromStream(std::istream& in_archive_stream, T& output) {
   boost::archive::text_iarchive in_archive(in_archive_stream);
   in_archive >> output;
 }
 
-template<class T>
+/// serializes to a string
+template <class T>
+std::string serializeToString(const T& input) {
+  std::ostringstream out_archive_stream;
+  serializeToStream(input, out_archive_stream);
+  return out_archive_stream.str();
+}
+
+/// deserializes from a string
+template <class T>
+void deserializeFromString(const std::string& serialized, T& output) {
+  std::istringstream in_archive_stream(serialized);
+  deserializeFromStream(in_archive_stream, output);
+}
+
+/// serializes to a file
+template <class T>
 bool serializeToFile(const T& input, const std::string& filename) {
   std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
+  if (!out_archive_stream.is_open()) return false;
-    return false;
+  serializeToStream(input, out_archive_stream);
-  boost::archive::text_oarchive out_archive(out_archive_stream);
-  out_archive << input;
   out_archive_stream.close();
   return true;
 }
 
-template<class T>
+/// deserializes from a file
+template <class T>
 bool deserializeFromFile(const std::string& filename, T& output) {
   std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
+  if (!in_archive_stream.is_open()) return false;
-    return false;
+  deserializeFromStream(in_archive_stream, output);
-  boost::archive::text_iarchive in_archive(in_archive_stream);
-  in_archive >> output;
   in_archive_stream.close();
   return true;
 }
 
-// Serialization to XML format with named structures
+/// serializes to a string
-template<class T>
+template <class T>
-std::string serializeXML(const T& input, const std::string& name="data") {
+std::string serialize(const T& input) {
-  std::ostringstream out_archive_stream;
+  return serializeToString(input);
-  // braces to flush out_archive as it goes out of scope before taking str()
-  // fixes crash with boost 1.66-1.68
-  // see https://github.com/boostorg/serialization/issues/82
-  {
-    boost::archive::xml_oarchive out_archive(out_archive_stream);
-    out_archive << boost::serialization::make_nvp(name.c_str(), input);
-  }
-  return out_archive_stream.str();
 }
 
-template<class T>
+/// deserializes from a string
-void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
+template <class T>
-  std::istringstream in_archive_stream(serialized);
+void deserialize(const std::string& serialized, T& output) {
-  boost::archive::xml_iarchive in_archive(in_archive_stream);
+  deserializeFromString(serialized, output);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
 }
+///@}
 
-template<class T>
+/** @name XML Serialization
-bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") {
+ *  Serialization to XML format with named structures
-  std::ofstream out_archive_stream(filename.c_str());
+ */
-  if (!out_archive_stream.is_open())
+///@{
-    return false;
+/// serializes to a stream in XML
+template <class T>
+void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
+                  const std::string& name = "data") {
   boost::archive::xml_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);;
+  out_archive << boost::serialization::make_nvp(name.c_str(), input);
-  out_archive_stream.close();
-  return true;
 }
 
-template<class T>
+/// deserializes from a stream in XML
-bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
+template <class T>
-  std::ifstream in_archive_stream(filename.c_str());
+void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
-  if (!in_archive_stream.is_open())
+                    const std::string& name = "data") {
-    return false;
   boost::archive::xml_iarchive in_archive(in_archive_stream);
   in_archive >> boost::serialization::make_nvp(name.c_str(), output);
-  in_archive_stream.close();
-  return true;
 }
 
-// Serialization to binary format with named structures
+/// serializes to a string in XML
-template<class T>
+template <class T>
-std::string serializeBinary(const T& input, const std::string& name="data") {
+std::string serializeToXMLString(const T& input,
+                         const std::string& name = "data") {
   std::ostringstream out_archive_stream;
-  boost::archive::binary_oarchive out_archive(out_archive_stream);
+  serializeToXMLStream(input, out_archive_stream, name);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
   return out_archive_stream.str();
 }
 
-template<class T>
+/// deserializes from a string in XML
-void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
+template <class T>
+void deserializeFromXMLString(const std::string& serialized, T& output,
+                    const std::string& name = "data") {
   std::istringstream in_archive_stream(serialized);
-  boost::archive::binary_iarchive in_archive(in_archive_stream);
+  deserializeFromXMLStream(in_archive_stream, output, name);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
 }
 
-template<class T>
+/// serializes to an XML file
-bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
+template <class T>
+bool serializeToXMLFile(const T& input, const std::string& filename,
+                        const std::string& name = "data") {
   std::ofstream out_archive_stream(filename.c_str());
-  if (!out_archive_stream.is_open())
+  if (!out_archive_stream.is_open()) return false;
-    return false;
+  serializeToXMLStream(input, out_archive_stream, name);
-  boost::archive::binary_oarchive out_archive(out_archive_stream);
-  out_archive << boost::serialization::make_nvp(name.c_str(), input);
   out_archive_stream.close();
   return true;
 }
 
-template<class T>
+/// deserializes from an XML file
-bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
+template <class T>
+bool deserializeFromXMLFile(const std::string& filename, T& output,
+                            const std::string& name = "data") {
   std::ifstream in_archive_stream(filename.c_str());
-  if (!in_archive_stream.is_open())
+  if (!in_archive_stream.is_open()) return false;
-    return false;
+  deserializeFromXMLStream(in_archive_stream, output, name);
-  boost::archive::binary_iarchive in_archive(in_archive_stream);
-  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
   in_archive_stream.close();
   return true;
 }
 
-} // \namespace gtsam
+/// serializes to a string in XML
+template <class T>
+std::string serializeXML(const T& input,
+                         const std::string& name = "data") {
+  return serializeToXMLString(input, name);
+}
+
+/// deserializes from a string in XML
+template <class T>
+void deserializeXML(const std::string& serialized, T& output,
+                    const std::string& name = "data") {
+  deserializeFromXMLString(serialized, output, name);
+}
+///@}
+
+/** @name Binary Serialization
+ *  Serialization to binary format with named structures
+ */
+///@{
+/// serializes to a stream in binary
+template <class T>
+void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
+                     const std::string& name = "data") {
+  boost::archive::binary_oarchive out_archive(out_archive_stream);
+  out_archive << boost::serialization::make_nvp(name.c_str(), input);
+}
+
+/// deserializes from a stream in binary
+template <class T>
+void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
+                       const std::string& name = "data") {
+  boost::archive::binary_iarchive in_archive(in_archive_stream);
+  in_archive >> boost::serialization::make_nvp(name.c_str(), output);
+}
+
+/// serializes to a string in binary
+template <class T>
+std::string serializeToBinaryString(const T& input,
+                            const std::string& name = "data") {
+  std::ostringstream out_archive_stream;
+  serializeToBinaryStream(input, out_archive_stream, name);
+  return out_archive_stream.str();
+}
+
+/// deserializes from a string in binary
+template <class T>
+void deserializeFromBinaryString(const std::string& serialized, T& output,
+                       const std::string& name = "data") {
+  std::istringstream in_archive_stream(serialized);
+  deserializeFromBinaryStream(in_archive_stream, output, name);
+}
+
+/// serializes to a binary file
+template <class T>
+bool serializeToBinaryFile(const T& input, const std::string& filename,
+                           const std::string& name = "data") {
+  std::ofstream out_archive_stream(filename.c_str());
+  if (!out_archive_stream.is_open()) return false;
+  serializeToBinaryStream(input, out_archive_stream, name);
+  out_archive_stream.close();
+  return true;
+}
+
+/// deserializes from a binary file
+template <class T>
+bool deserializeFromBinaryFile(const std::string& filename, T& output,
+                               const std::string& name = "data") {
+  std::ifstream in_archive_stream(filename.c_str());
+  if (!in_archive_stream.is_open()) return false;
+  deserializeFromBinaryStream(in_archive_stream, output, name);
+  in_archive_stream.close();
+  return true;
+}
+
+/// serializes to a string in binary
+template <class T>
+std::string serializeBinary(const T& input,
+                            const std::string& name = "data") {
+  return serializeToBinaryString(input, name);
+}
+
+/// deserializes from a string in binary
+template <class T>
+void deserializeBinary(const std::string& serialized, T& output,
+                       const std::string& name = "data") {
+  deserializeFromBinaryString(serialized, output, name);
+}
+///@}
+
+}  // namespace gtsam

gtsam/base/serializationTestHelpers.h

@@ -26,6 +26,7 @@
 #include <gtsam/base/serialization.h>
 
 #include <boost/serialization/serialization.hpp>
+#include <boost/filesystem.hpp>
 
 
 // whether to print the serialized text to stdout
@@ -40,22 +41,37 @@ T create() {
   return T();
 }
 
+// Creates or empties a folder in the build folder and returns the relative path
+boost::filesystem::path resetFilesystem(
+    boost::filesystem::path folder = "actual") {
+  boost::filesystem::remove_all(folder);
+  boost::filesystem::create_directory(folder);
+  return folder;
+}
+
 // Templated round-trip serialization
 template<class T>
 void roundtrip(const T& input, T& output) {
-  // Serialize
   std::string serialized = serialize(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
   deserialize(serialized, output);
 }
 
-// This version requires equality operator
+// Templated round-trip serialization using a file
+template<class T>
+void roundtripFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.dat";
+  serializeToFile(input, path.string());
+  deserializeFromFile(path.string(), output);
+}
+
+// This version requires equality operator and uses string and file round-trips
 template<class T>
 bool equality(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtrip<T>(input,output);
-  return input==output;
+  roundtripFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
@@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
 // Templated round-trip serialization using XML
 template<class T>
 void roundtripXML(const T& input, T& output) {
-  // Serialize
   std::string serialized = serializeXML<T>(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
-  // De-serialize
   deserializeXML(serialized, output);
 }
 
+// Templated round-trip serialization using XML File
+template<class T>
+void roundtripXMLFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.xml";
+  serializeToXMLFile(input, path.string());
+  deserializeFromXMLFile(path.string(), output);
+}
+
 // This version requires equality operator
 template<class T>
 bool equalityXML(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtripXML<T>(input,output);
-  return input==output;
+  roundtripXMLFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable
@@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
 // Templated round-trip serialization using XML
 template<class T>
 void roundtripBinary(const T& input, T& output) {
-  // Serialize
   std::string serialized = serializeBinary<T>(input);
   if (verbose) std::cout << serialized << std::endl << std::endl;
-
-  // De-serialize
   deserializeBinary(serialized, output);
 }
 
+// Templated round-trip serialization using Binary file
+template<class T>
+void roundtripBinaryFile(const T& input, T& output) {
+  boost::filesystem::path path = resetFilesystem()/"graph.bin";
+  serializeToBinaryFile(input, path.string());
+  deserializeFromBinaryFile(path.string(), output);
+}
+
 // This version requires equality operator
 template<class T>
 bool equalityBinary(const T& input = T()) {
-  T output = create<T>();
+  T output = create<T>(), outputf = create<T>();
   roundtripBinary<T>(input,output);
-  return input==output;
+  roundtripBinaryFile<T>(input,outputf);
+  return (input==output) && (input==outputf);
 }
 
 // This version requires Testable

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -32,28 +32,6 @@ namespace gtsam {
 
     namespace internal {
 
-      /* ************************************************************************* */
-      template<typename NODE, typename DATA, typename VISITOR_POST>
-      class PostOrderTask : public tbb::task
-      {
-      public:
-        const boost::shared_ptr<NODE>& treeNode;
-        boost::shared_ptr<DATA> myData;
-        VISITOR_POST& visitorPost;
-
-        PostOrderTask(const boost::shared_ptr<NODE>& treeNode,
-                      const boost::shared_ptr<DATA>& myData, VISITOR_POST& visitorPost)
-            : treeNode(treeNode), myData(myData), visitorPost(visitorPost) {}
-
-        tbb::task* execute()
-        {
-          // Run the post-order visitor
-          (void) visitorPost(treeNode, *myData);
-
-          return nullptr;
-        }
-      };
-
       /* ************************************************************************* */
       template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
       class PreOrderTask : public tbb::task

gtsam/base/types.cpp

@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     types.cpp
+ * @brief    Functions for handling type information
+ * @author   Varun Agrawal
+ * @date     May 18, 2020
+ * @addtogroup base
+ */
+
+#include <gtsam/base/types.h>
+
+#ifdef _WIN32
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#include <DbgHelp.h>
+#endif
+
+#ifdef __GNUG__
+#include <cstdlib>
+#include <cxxabi.h>
+#include <string>
+#endif
+
+namespace gtsam {
+
+/// Pretty print Value type name
+std::string demangle(const char* name) {
+  // by default set to the original mangled name
+  std::string demangled_name = std::string(name);
+
+#ifdef __GNUG__
+  // g++ version of demangle
+  char* demangled = nullptr;
+  int status = -1; // some arbitrary value to eliminate the compiler warning
+  demangled = abi::__cxa_demangle(name, nullptr, nullptr, &status),
+  
+  demangled_name = (status == 0) ? std::string(demangled) : std::string(name);
+
+  std::free(demangled);
+
+#elif _WIN32
+    char undecorated_name[1024];
+    
+    if (UnDecorateSymbolName(
+        name, undecorated_name, sizeof(undecorated_name),
+        UNDNAME_COMPLETE))
+    {
+      // successful conversion, take value from: undecorated_name			                      
+      demangled_name = std::string(undecorated_name);               
+    }
+    // else keep using mangled name
+#endif
+
+  return demangled_name;
+}
+
+} /* namespace gtsam */

gtsam/base/types.h

@@ -53,6 +53,9 @@
 
 namespace gtsam {
 
+  /// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
+  std::string demangle(const char* name);
+
   /// Integer nonlinear key type
   typedef std::uint64_t Key;
 
@@ -227,3 +230,50 @@ namespace std {
 #ifdef ERROR
 #undef ERROR
 #endif
+
+namespace gtsam {
+
+  /// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::`
+  template<typename ...> using void_t = void;
+
+  /**
+   * A SFINAE trait to mark classes that need special alignment.
+   *
+   * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python
+   * wrappers to work properly.
+   *
+   * Explanation
+   * =============
+   * When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template
+   * will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`.
+   *
+   * Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`.
+   *
+   * Please refer to `gtsam/base/make_shared.h` for an example.
+   */
+  template<typename, typename = void_t<>>
+  struct needs_eigen_aligned_allocator : std::false_type {
+  };
+  template<typename T>
+  struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
+  };
+
+}
+
+/**
+ * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
+ * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
+ * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+  using _eigen_aligned_allocator_trait = void;
+
+/**
+ * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
+ * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
+ * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
+ */
+#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+  using _eigen_aligned_allocator_trait = void;

gtsam/config.h.in

@@ -42,6 +42,9 @@
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
 #cmakedefine GTSAM_USE_TBB
 
+// Whether we are using a TBB version higher than 2020
+#cmakedefine TBB_GREATER_EQUAL_2020
+
 // Whether we are using system-Eigen or our own patched version
 #cmakedefine GTSAM_USE_SYSTEM_EIGEN
 

gtsam/geometry/BearingRange.h

@@ -162,7 +162,7 @@ private:
     NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
   };
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 
 // Declare this to be both Testable and a Manifold

gtsam/geometry/Cal3Fisheye.cpp

@@ -24,7 +24,7 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Cal3Fisheye::Cal3Fisheye(const Vector& v)
+Cal3Fisheye::Cal3Fisheye(const Vector9& v)
     : fx_(v[0]),
       fy_(v[1]),
       s_(v[2]),
@@ -50,76 +50,73 @@ Matrix3 Cal3Fisheye::K() const {
 }
 
 /* ************************************************************************* */
-static Matrix29 D2dcalibration(const double xd, const double yd,
+double Cal3Fisheye::Scaling(double r) {
-                               const double xi, const double yi,
+  static constexpr double threshold = 1e-8;
-                               const double t3, const double t5,
+  if (r > threshold || r < -threshold) {
-                               const double t7, const double t9, const double r,
+    return atan(r) / r;
-                               Matrix2& DK) {
+  } else {
-  // order: fx, fy, s, u0, v0
+    // Taylor expansion close to 0
-  Matrix25 DR1;
+    double r2 = r * r, r4 = r2 * r2;
-  DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
+    return 1.0 - r2 / 3 + r4 / 5;
-
+  }
-  // order: k1, k2, k3, k4
-  Matrix24 DR2;
-  DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi;
-  DR2 /= r;
-  Matrix29 D;
-  D << DR1, DK * DR2;
-  return D;
-}
-
-/* ************************************************************************* */
-static Matrix2 D2dintrinsic(const double xi, const double yi, const double r,
-                            const double td, const double t, const double tt,
-                            const double t4, const double t6, const double t8,
-                            const double k1, const double k2, const double k3,
-                            const double k4, const Matrix2& DK) {
-  const double dr_dxi = xi / sqrt(xi * xi + yi * yi);
-  const double dr_dyi = yi / sqrt(xi * xi + yi * yi);
-  const double dt_dr = 1 / (1 + r * r);
-  const double dtd_dt =
-      1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8;
-  const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
-  const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
-
-  const double rinv = 1 / r;
-  const double rrinv = 1 / (r * r);
-  const double dxd_dxi =
-      dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi;
-  const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi;
-  const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi;
-  const double dyd_dyi =
-      dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi;
-
-  Matrix2 DR;
-  DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
-
-  return DK * DR;
 }
 
 /* ************************************************************************* */
 Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
                                 OptionalJacobian<2, 2> H2) const {
   const double xi = p.x(), yi = p.y();
-  const double r = sqrt(xi * xi + yi * yi);
+  const double r2 = xi * xi + yi * yi, r = sqrt(r2);
   const double t = atan(r);
-  const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
+  const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
-  const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
+  Vector5 K, T;
-  const double td_o_r = r > 1e-8 ? td / r : 1;
+  K << 1, k1_, k2_, k3_, k4_;
-  const double xd = td_o_r * xi, yd = td_o_r * yi;
+  T << 1, t2, t4, t6, t8;
+  const double scaling = Scaling(r);
+  const double s = scaling * K.dot(T);
+  const double xd = s * xi, yd = s * yi;
   Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
 
   Matrix2 DK;
   if (H1 || H2) DK << fx_, s_, 0.0, fy_;
 
   // Derivative for calibration parameters (2 by 9)
-  if (H1)
+  if (H1) {
-    *H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
+    Matrix25 DR1;
+    // order: fx, fy, s, u0, v0
+    DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
+
+    // order: k1, k2, k3, k4
+    Matrix24 DR2;
+    auto T4 = T.tail<4>().transpose();
+    DR2 << xi * T4, yi * T4;
+    *H1 << DR1, DK * scaling * DR2;
+  }
 
   // Derivative for points in intrinsic coords (2 by 2)
-  if (H2)
+  if (H2) {
-    *H2 =
+    const double dtd_dt =
-        D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
+        1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
+    const double dt_dr = 1 / (1 + r2);
+    const double rinv = 1 / r;
+    const double dr_dxi = xi * rinv;
+    const double dr_dyi = yi * rinv;
+    const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
+    const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
+
+    const double td = t * K.dot(T);
+    const double rrinv = 1 / r2;
+    const double dxd_dxi =
+        dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
+    const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
+    const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
+    const double dyd_dyi =
+        dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
+
+    Matrix2 DR;
+    DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
+
+    *H2 = DK * DR;
+  }
 
   return uv;
 }
@@ -157,39 +154,10 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
   return pi;
 }
 
-/* ************************************************************************* */
-Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const {
-  const double xi = p.x(), yi = p.y();
-  const double r = sqrt(xi * xi + yi * yi);
-  const double t = atan(r);
-  const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4;
-  const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
-
-  Matrix2 DK;
-  DK << fx_, s_, 0.0, fy_;
-
-  return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
-}
-
-/* ************************************************************************* */
-Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const {
-  const double xi = p.x(), yi = p.y();
-  const double r = sqrt(xi * xi + yi * yi);
-  const double t = atan(r);
-  const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
-  const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
-  const double xd = td / r * xi, yd = td / r * yi;
-
-  Matrix2 DK;
-  DK << fx_, s_, 0.0, fy_;
-  return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
-}
-
 /* ************************************************************************* */
 void Cal3Fisheye::print(const std::string& s_) const {
   gtsam::print((Matrix)K(), s_ + ".K");
   gtsam::print(Vector(k()), s_ + ".k");
-  ;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cal3Fisheye.h

@@ -20,6 +20,8 @@
 
 #include <gtsam/geometry/Point2.h>
 
+#include <string>
+
 namespace gtsam {
 
 /**
@@ -43,7 +45,7 @@ namespace gtsam {
  *   [u; v; 1] = K*[x_d; y_d; 1]
  */
 class GTSAM_EXPORT Cal3Fisheye {
- protected:
+ private:
   double fx_, fy_, s_, u0_, v0_;  // focal length, skew and principal point
   double k1_, k2_, k3_, k4_;      // fisheye distortion coefficients
 
@@ -78,7 +80,7 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// @name Advanced Constructors
   /// @{
 
-  Cal3Fisheye(const Vector& v);
+  explicit Cal3Fisheye(const Vector9& v);
 
   /// @}
   /// @name Standard Interface
@@ -120,6 +122,9 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// Return all parameters as a vector
   Vector9 vector() const;
 
+  /// Helper function that calculates atan(r)/r
+  static double Scaling(double r);
+
   /**
    * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
    * coordinates [u; v]
@@ -136,13 +141,6 @@ class GTSAM_EXPORT Cal3Fisheye {
   /// y_i]
   Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
 
-  /// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
-  Matrix2 D2d_intrinsic(const Point2& p) const;
-
-  /// Derivative of uncalibrate wrpt the calibration parameters
-  /// [fx, fy, s, u0, v0, k1, k2, k3, k4]
-  Matrix29 D2d_calibration(const Point2& p) const;
-
   /// @}
   /// @name Testable
   /// @{

gtsam/geometry/CameraSet.h

@@ -319,7 +319,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<class CAMERA>

gtsam/geometry/EssentialMatrix.h

@@ -212,7 +212,7 @@ class EssentialMatrix {
   /// @}
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 template<>

gtsam/geometry/PinholeCamera.h

@@ -325,7 +325,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // manifold traits

gtsam/geometry/PinholePose.h

@@ -222,7 +222,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholeBaseK
 
@@ -425,7 +425,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // end of class PinholePose
 

gtsam/geometry/Pose2.h

@@ -317,7 +317,7 @@ public:
 
 public:
   // Align for Point2, which is either derived from, or is typedef, of Vector2
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 }; // Pose2
 
 /** specialization for pose2 wedge function (generic template in Lie.h) */

gtsam/geometry/Pose3.h

@@ -355,7 +355,7 @@ public:
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 // Pose3 class

gtsam/geometry/Rot3.h

@@ -544,7 +544,7 @@ namespace gtsam {
 #ifdef GTSAM_USE_QUATERNIONS
   // only align if quaternion, Matrix3 has no alignment requirements
   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
   };
 

gtsam/geometry/SO3.cpp

@@ -261,13 +261,13 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
 
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
-  if (std::abs(tr + 1.0) < 1e-10) {
+  if (tr + 1.0 < 1e-10) {
-    if (std::abs(R33 + 1.0) > 1e-10)
+    if (std::abs(R33 + 1.0) > 1e-5)
       omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
-    else if (std::abs(R22 + 1.0) > 1e-10)
+    else if (std::abs(R22 + 1.0) > 1e-5)
       omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
     else
-      // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
+      // if(std::abs(R.r1_.x()+1.0) > 1e-5)  This is implicit
       omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
   } else {
     double magnitude;

gtsam/geometry/SO4.cpp

@@ -33,9 +33,10 @@ using namespace std;
 
 namespace gtsam {
 
+// TODO(frank): is this better than SOn::Random?
 // /* *************************************************************************
 // */ static Vector3 randomOmega(boost::mt19937 &rng) {
-//   static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
+//   static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
 //   return Unit3::Random(rng).unitVector() * randomAngle(rng);
 // }
 
@@ -58,9 +59,9 @@ Matrix4 SO4::Hat(const Vector6& xi) {
   Y(0, 1) = -xi(5);
   Y(0, 2) = +xi(4);
   Y(1, 2) = -xi(3);
-  Y(0, 3) = -xi(2);
+  Y(0, 3) = +xi(2);
-  Y(1, 3) = +xi(1);
+  Y(1, 3) = -xi(1);
-  Y(2, 3) = -xi(0);
+  Y(2, 3) = +xi(0);
   return Y - Y.transpose();
 }
 
@@ -72,9 +73,9 @@ Vector6 SO4::Vee(const Matrix4& X) {
   xi(5) = -X(0, 1);
   xi(4) = +X(0, 2);
   xi(3) = -X(1, 2);
-  xi(2) = -X(0, 3);
+  xi(2) = +X(0, 3);
-  xi(1) = +X(1, 3);
+  xi(1) = -X(1, 3);
-  xi(0) = -X(2, 3);
+  xi(0) = +X(2, 3);
   return xi;
 }
 
@@ -208,9 +209,9 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
   if (H) {
     const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
                   q = R.topRightCorner<3, 1>();
-    *H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2,  //
+    *H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2,  //
-        Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1,   //
+        Z_3x1, q, Z_3x1, m3, Z_3x1, -m1,     //
-        q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
+        -q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
   }
   return M;
 }
@@ -221,9 +222,9 @@ GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
   const Matrix43 M = R.leftCols<3>();
   if (H) {
     const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
-    *H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2,  //
+    *H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2,  //
-        Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1,   //
+        Z_4x1, q, Z_4x1, m3, Z_4x1, -m1,     //
-        q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
+        -q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
   }
   return M;
 }

gtsam/geometry/SOn.cpp

@@ -38,11 +38,12 @@ Matrix SOn::Hat(const Vector& xi) {
     const size_t dmin = (n - 1) * (n - 2) / 2;
     X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
 
+    // determine sign of last element (signs alternate)
+    double sign = pow(-1.0, xi.size());
     // Now fill last row and column
-    double sign = 1.0;
     for (size_t i = 0; i < n - 1; i++) {
       const size_t j = n - 2 - i;
-      X(n - 1, j) = sign * xi(i);
+      X(n - 1, j) = -sign * xi(i);
       X(j, n - 1) = -X(n - 1, j);
       sign = -sign;
     }
@@ -67,10 +68,10 @@ Vector SOn::Vee(const Matrix& X) {
     Vector xi(d);
 
     // Fill first n-1 spots from last row of X
-    double sign = 1.0;
+    double sign = pow(-1.0, xi.size());
     for (size_t i = 0; i < n - 1; i++) {
       const size_t j = n - 2 - i;
-      xi(i) = sign * X(n - 1, j);
+      xi(i) = -sign * X(n - 1, j);
       sign = -sign;
     }
 

gtsam/geometry/SOn.h

@@ -20,14 +20,15 @@
 
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/make_shared.h>
 #include <gtsam/dllexport.h>
-
 #include <Eigen/Core>
 
-#include <random>
+#include <iostream> // TODO(frank): how to avoid?
 #include <string>
 #include <type_traits>
 #include <vector>
+#include <random>
 
 namespace gtsam {
 
@@ -53,7 +54,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
   using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
 
  protected:
   MatrixNN matrix_;  ///< Rotation matrix
@@ -93,6 +94,16 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
     return SO(R);
   }
 
+  /// Named constructor from lower dimensional matrix
+  template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
+  static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
+    Matrix Q = Matrix::Identity(n, n);
+    size_t p = R.rows();
+    assert(p < n && R.cols() == p);
+    Q.topLeftCorner(p, p) = R;
+    return SO(Q);
+  }
+
   /// Construct dynamic SO(n) from Fixed SO<M>
   template <int M, int N_ = N, typename = IsDynamic<N_>>
   explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
@@ -207,11 +218,11 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
    * etc... For example, the vector-space isomorphic to so(5) is laid out as:
    *   a b c d | u v w | x y | z
    * where the latter elements correspond to "telescoping" sub-algebras:
-   *   0 -z  y -w  d
+   *   0 -z  y  w -d
-   *   z  0 -x  v -c
+   *   z  0 -x -v  c
-   *  -y  x  0 -u  b
+   *  -y  x  0  u -b
-   *   w -v  u  0 -a
+   *  -w  v -u  0  a
-   *  -d  c -b  a  0
+   *   d -c  b -a  0
    * This scheme behaves exactly as expected for SO(2) and SO(3).
    */
   static MatrixNN Hat(const TangentVector& xi);

gtsam/geometry/Unit3.h

@@ -214,7 +214,7 @@ private:
   /// @}
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 // Define GTSAM traits

gtsam/geometry/tests/testCal3DFisheye.cpp

@@ -10,17 +10,18 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testCal3Fisheye.cpp
+ * @file  testCal3DFisheye.cpp
  * @brief Unit tests for fisheye calibration class
  * @author ghaggin
  */
 
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Point3.h>
 
+#include <CppUnitLite/TestHarness.h>
+
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye)
@@ -30,12 +31,27 @@ static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240;
 static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035,
                      0.020727425669427896, -0.012786476702685545,
                      0.0025242267320687625);
-static Point2 p(2, 3);
+static Point2 kTestPoint2(2, 3);
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
+
+/* ************************************************************************* */
+TEST(Cal3Fisheye, retract) {
+  Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
+                       K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
+                       K.k4() + 9);
+  Vector d(9);
+  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  Cal3Fisheye actual = K.retract(d);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
+}
 
 /* ************************************************************************* */
 TEST(Cal3Fisheye, uncalibrate1) {
   // Calculate the solution
-  const double xi = p.x(), yi = p.y();
+  const double xi = kTestPoint2.x(), yi = kTestPoint2.y();
   const double r = sqrt(xi * xi + yi * yi);
   const double t = atan(r);
   const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
@@ -46,32 +62,42 @@ TEST(Cal3Fisheye, uncalibrate1) {
 
   Point2 uv_sol(v[0] / v[2], v[1] / v[2]);
 
-  Point2 uv = K.uncalibrate(p);
+  Point2 uv = K.uncalibrate(kTestPoint2);
   CHECK(assert_equal(uv, uv_sol));
 }
 
 /* ************************************************************************* */
-/**
+// For numerical derivatives
- * Check that a point at (0,0) projects to the
+Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }
- * image center.
+
- */
+/* ************************************************************************* */
-TEST(Cal3Fisheye, uncalibrate2) {
+TEST(Cal3Fisheye, Derivatives) {
-  Point2 pz(0, 0);
+  Matrix H1, H2;
-  auto uv = K.uncalibrate(pz);
+  K.uncalibrate(kTestPoint2, H1, H2);
-  CHECK(assert_equal(uv, Point2(u0, v0)));
+  CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5));
+  CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 1e-7), H2, 1e-5));
 }
 
 /* ************************************************************************* */
-/**
+// Check that a point at (0,0) projects to the image center.
- *  This test uses cv2::fisheye::projectPoints to test that uncalibrate
+TEST(Cal3Fisheye, uncalibrate2) {
- *  properly projects a point into the image plane.  One notable difference
+  Point2 pz(0, 0);
- *  between opencv and the Cal3Fisheye::uncalibrate function is the skew
+  Matrix H1, H2;
- *  parameter. The equivalence is alpha = s/fx.
+  auto uv = K.uncalibrate(pz, H1, H2);
- *
+  CHECK(assert_equal(uv, Point2(u0, v0)));
- *
+  CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5));
- * Python script to project points with fisheye model in OpenCv
+  // TODO(frank): the second jacobian is all NaN for the image center!
- * (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
+  // CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5));
- */
+}
+
+/* ************************************************************************* */
+//  This test uses cv2::fisheye::projectPoints to test that uncalibrate
+//  properly projects a point into the image plane.  One notable difference
+//  between opencv and the Cal3Fisheye::uncalibrate function is the skew
+//  parameter. The equivalence is alpha = s/fx.
+//
+// Python script to project points with fisheye model in OpenCv
+// (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
 // clang-format off
 /*
 ===========================================================
@@ -94,6 +120,7 @@ tvec = np.float64([[0.,0.,0.]]);
 imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha) 
 np.set_printoptions(precision=14) 
 print(imagePoints)
+
 ===========================================================
  * Script output: [[[457.82638130304935 408.18905848512986]]]
  */
@@ -134,21 +161,18 @@ TEST(Cal3Fisheye, calibrate1) {
 }
 
 /* ************************************************************************* */
-/**
+// Check that calibrate returns (0,0) for the image center
- * Check that calibrate returns (0,0) for the image center
- */
 TEST(Cal3Fisheye, calibrate2) {
   Point2 uv(u0, v0);
   auto xi_hat = K.calibrate(uv);
   CHECK(assert_equal(xi_hat, Point2(0, 0)))
 }
 
-/**
+/* ************************************************************************* */
- * Run calibrate on OpenCv test from uncalibrate3
+// Run calibrate on OpenCv test from uncalibrate3
- *  (script shown above)
+//  (script shown above)
- * 3d point: (23, 27, 31)
+// 3d point: (23, 27, 31)
- * 2d point in image plane: (457.82638130304935, 408.18905848512986)
+// 2d point in image plane: (457.82638130304935, 408.18905848512986)
- */
 TEST(Cal3Fisheye, calibrate3) {
   Point3 p3(23, 27, 31);
   Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
@@ -157,47 +181,6 @@ TEST(Cal3Fisheye, calibrate3) {
   CHECK(assert_equal(xi_hat, xi));
 }
 
-/* ************************************************************************* */
-// For numerical derivatives
-Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) {
-  return k.uncalibrate(pt);
-}
-
-/* ************************************************************************* */
-TEST(Cal3Fisheye, Duncalibrate1) {
-  Matrix computed;
-  K.uncalibrate(p, computed, boost::none);
-  Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical, computed, 1e-5));
-  Matrix separate = K.D2d_calibration(p);
-  CHECK(assert_equal(numerical, separate, 1e-5));
-}
-
-/* ************************************************************************* */
-TEST(Cal3Fisheye, Duncalibrate2) {
-  Matrix computed;
-  K.uncalibrate(p, boost::none, computed);
-  Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical, computed, 1e-5));
-  Matrix separate = K.D2d_intrinsic(p);
-  CHECK(assert_equal(numerical, separate, 1e-5));
-}
-
-/* ************************************************************************* */
-TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
-
-/* ************************************************************************* */
-TEST(Cal3Fisheye, retract) {
-  Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
-                       K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
-                       K.k4() + 9);
-  Vector d(9);
-  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
-  Cal3Fisheye actual = K.retract(d);
-  CHECK(assert_equal(expected, actual, 1e-7));
-  CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testLine3.cpp

@@ -2,10 +2,10 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Line3.h>
-#include <gtsam/slam/expressions.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/expressionTesting.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
 
 using namespace gtsam;
 
@@ -146,7 +146,7 @@ TEST(Line3, projection) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1);
   Unit3_ projected_(wL_, &Line3::project);
   ExpressionFactor<Unit3> f(model, expected, projected_);
-  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
 }
 
 int main() {

gtsam/geometry/tests/testRot3.cpp

@@ -181,63 +181,81 @@ TEST( Rot3, retract)
 }
 
 /* ************************************************************************* */
-TEST(Rot3, log)
+TEST(Rot3, log) {
-{
   static const double PI = boost::math::constants::pi<double>();
   Vector w;
   Rot3 R;
 
-#define CHECK_OMEGA(X,Y,Z) \
+#define CHECK_OMEGA(X, Y, Z)             \
-  w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
+  w = (Vector(3) << X, Y, Z).finished(); \
-  R = Rot3::Rodrigues(w); \
+  R = Rot3::Rodrigues(w);                \
-  EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
+  EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
 
   // Check zero
-  CHECK_OMEGA(  0,   0,   0)
+  CHECK_OMEGA(0, 0, 0)
 
   // create a random direction:
-  double norm=sqrt(1.0+16.0+4.0);
+  double norm = sqrt(1.0 + 16.0 + 4.0);
-  double x=1.0/norm, y=4.0/norm, z=2.0/norm;
+  double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
 
   // Check very small rotation for Taylor expansion
   // Note that tolerance above is 1e-12, so Taylor is pretty good !
   double d = 0.0001;
-  CHECK_OMEGA(  d,   0,   0)
+  CHECK_OMEGA(d, 0, 0)
-  CHECK_OMEGA(  0,   d,   0)
+  CHECK_OMEGA(0, d, 0)
-  CHECK_OMEGA(  0,   0,   d)
+  CHECK_OMEGA(0, 0, d)
-  CHECK_OMEGA(x*d, y*d, z*d)
+  CHECK_OMEGA(x * d, y * d, z * d)
 
   // check normal rotation
   d = 0.1;
-  CHECK_OMEGA(  d,   0,   0)
+  CHECK_OMEGA(d, 0, 0)
-  CHECK_OMEGA(  0,   d,   0)
+  CHECK_OMEGA(0, d, 0)
-  CHECK_OMEGA(  0,   0,   d)
+  CHECK_OMEGA(0, 0, d)
-  CHECK_OMEGA(x*d, y*d, z*d)
+  CHECK_OMEGA(x * d, y * d, z * d)
 
   // Check 180 degree rotations
-  CHECK_OMEGA(  PI,   0,   0)
+  CHECK_OMEGA(PI, 0, 0)
-  CHECK_OMEGA(   0,  PI,   0)
+  CHECK_OMEGA(0, PI, 0)
-  CHECK_OMEGA(   0,   0,  PI)
+  CHECK_OMEGA(0, 0, PI)
 
   // Windows and Linux have flipped sign in quaternion mode
-#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
+#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
-  w = (Vector(3) << x*PI, y*PI, z*PI).finished();
+  w = (Vector(3) << x * PI, y * PI, z * PI).finished();
   R = Rot3::Rodrigues(w);
-  EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
+  EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
 #else
-  CHECK_OMEGA(x*PI,y*PI,z*PI)
+  CHECK_OMEGA(x * PI, y * PI, z * PI)
 #endif
 
   // Check 360 degree rotations
-#define CHECK_OMEGA_ZERO(X,Y,Z) \
+#define CHECK_OMEGA_ZERO(X, Y, Z)        \
-  w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
+  w = (Vector(3) << X, Y, Z).finished(); \
-  R = Rot3::Rodrigues(w); \
+  R = Rot3::Rodrigues(w);                \
-  EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R)));
+  EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
 
-  CHECK_OMEGA_ZERO( 2.0*PI,      0,      0)
+  CHECK_OMEGA_ZERO(2.0 * PI, 0, 0)
-  CHECK_OMEGA_ZERO(      0, 2.0*PI,      0)
+  CHECK_OMEGA_ZERO(0, 2.0 * PI, 0)
-  CHECK_OMEGA_ZERO(      0,      0, 2.0*PI)
+  CHECK_OMEGA_ZERO(0, 0, 2.0 * PI)
-  CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
+  CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI)
+
+  // Check problematic case from Lund dataset vercingetorix.g2o
+  // This is an almost rotation with determinant not *quite* 1.
+  Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092,  //
+             -0.03997006, -0.88835923, 0.45740671,   //
+             -0.16293753, 0.45743998, 0.87418537);
+
+  // Rot3's Logmap returns different, but equivalent compacted
+  // axis-angle vectors depending on whether Rot3 is implemented
+  // by Quaternions or SO3.
+  #if defined(GTSAM_USE_QUATERNIONS)
+    // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
+    EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
+                        (Vector)Rot3::Logmap(Rlund), 1e-8));
+  #else
+    // SO3 does not bound angle resulting in ~180.1 degrees
+    EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
+                        (Vector)Rot3::Logmap(Rlund), 1e-8));
+  #endif
 }
 
 /* ************************************************************************* */
@@ -536,16 +554,15 @@ TEST( Rot3, logmapStability ) {
 TEST(Rot3, quaternion) {
   // NOTE: This is also verifying the ability to convert Vector to Quaternion
   Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
-  Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
+  Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
-      0.271018623057411,   0.278786459830371,   0.921318086098018,
+          0.578529366719085, 0.717799701969298, -0.387385285854279,
-      0.578529366719085,   0.717799701969298,  -0.387385285854279,
+          -0.769319620053772, 0.637998195662053, 0.033250932803219);
-     -0.769319620053772,   0.637998195662053,   0.033250932803219).finished());
 
-  Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
+  Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
-  Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
+                0.599136268678053);
-      -0.207341903877828,   0.250149415542075,   0.945745528564780,
+  Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
-       0.881304914479026,  -0.371869043667957,   0.291573424846290,
+          0.881304914479026, -0.371869043667957, 0.291573424846290,
-       0.424630407073532,   0.893945571198514,  -0.143353873763946).finished());
+          0.424630407073532, 0.893945571198514, -0.143353873763946);
 
   // Check creating Rot3 from quaternion
   EXPECT(assert_equal(R1, Rot3(q1)));

gtsam/geometry/tests/testSOn.cpp

@@ -46,7 +46,7 @@ TEST(SOn, SO0) {
   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
   EXPECT_LONGS_EQUAL(0, R.dim());
-  EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R));
+  EXPECT_LONGS_EQUAL(0, traits<SOn>::GetDimension(R));
 }
 
 //******************************************************************************
@@ -56,7 +56,7 @@ TEST(SOn, SO5) {
   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
   EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
   EXPECT_LONGS_EQUAL(10, R.dim());
-  EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R));
+  EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
 }
 
 //******************************************************************************
@@ -95,32 +95,42 @@ TEST(SOn, Random) {
 
 //******************************************************************************
 TEST(SOn, HatVee) {
-  Vector6 v;
+  Vector10 v;
-  v << 1, 2, 3, 4, 5, 6;
+  v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
 
   Matrix expected2(2, 2);
   expected2 << 0, -1, 1, 0;
   const auto actual2 = SOn::Hat(v.head<1>());
-  CHECK(assert_equal(expected2, actual2));
+  EXPECT(assert_equal(expected2, actual2));
-  CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
+  EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
 
   Matrix expected3(3, 3);
-  expected3 << 0, -3, 2,  //
+  expected3 << 0, -3,  2, //
-      3, 0, -1,           //
+               3,  0, -1, //
-      -2, 1, 0;
+              -2,  1,  0;
   const auto actual3 = SOn::Hat(v.head<3>());
-  CHECK(assert_equal(expected3, actual3));
+  EXPECT(assert_equal(expected3, actual3));
-  CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3));
+  EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
-  CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
+  EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
 
   Matrix expected4(4, 4);
-  expected4 << 0, -6, 5, -3,  //
+  expected4 << 0, -6,  5,  3, //
-      6, 0, -4, 2,            //
+               6,  0, -4, -2, //
-      -5, 4, 0, -1,           //
+              -5,  4,  0,  1, //
-      3, -2, 1, 0;
+              -3,  2, -1,  0;
-  const auto actual4 = SOn::Hat(v);
+  const auto actual4 = SOn::Hat(v.head<6>());
-  CHECK(assert_equal(expected4, actual4));
+  EXPECT(assert_equal(expected4, actual4));
-  CHECK(assert_equal((Vector)v, SOn::Vee(actual4)));
+  EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
+
+  Matrix expected5(5, 5);
+  expected5 << 0,-10,  9,  7, -4, //
+              10,  0, -8, -6,  3, //
+              -9,  8,  0,  5, -2, //
+              -7,  6, -5,  0,  1, //
+               4, -3,  2, -1,  0;
+  const auto actual5 = SOn::Hat(v);
+  EXPECT(assert_equal(expected5, actual5));
+  EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
 }
 
 //******************************************************************************

gtsam/geometry/triangulation.h

@@ -215,7 +215,7 @@ struct CameraProjectionMatrix {
 private:
   const Matrix3 K_;
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**

gtsam/linear/GaussianFactor.cpp

@@ -0,0 +1,33 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    GaussianFactor.cpp
+ * @brief   A factor with a quadratic error function - a Gaussian
+ * @brief   GaussianFactor
+ * @author  Fan Jiang
+ */
+
+// \callgraph
+
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+  VectorValues GaussianFactor::hessianDiagonal() const {
+    VectorValues d;
+    hessianDiagonalAdd(d);
+    return d;
+  }
+
+}

gtsam/linear/GaussianFactor.h

@@ -100,7 +100,10 @@ namespace gtsam {
     virtual Matrix information() const = 0;
 
     /// Return the diagonal of the Hessian for this factor
-    virtual VectorValues hessianDiagonal() const = 0;
+    VectorValues hessianDiagonal() const;
+
+    /// Add the current diagonal to a VectorValues instance
+    virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
 
     /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const = 0;

gtsam/linear/GaussianFactorGraph.cpp

@@ -255,8 +255,7 @@ namespace gtsam {
     VectorValues d;
     for (const sharedFactor& factor : *this) {
       if(factor){
-        VectorValues di = factor->hessianDiagonal();
+        factor->hessianDiagonalAdd(d);
-        d.addInPlace_(di);
       }
     }
     return d;

gtsam/linear/HessianFactor.cpp

@@ -302,12 +302,14 @@ Matrix HessianFactor::information() const {
 }
 
 /* ************************************************************************* */
-VectorValues HessianFactor::hessianDiagonal() const {
+void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
-  VectorValues d;
   for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
-    d.emplace(keys_[j], info_.diagonal(j));
+    auto result = d.emplace(keys_[j], info_.diagonal(j));
+    if(!result.second) {
+      // if emplace fails, it returns an iterator to the existing element, which we add to:
+      result.first->second += info_.diagonal(j);
+    }
   }
-  return d;
 }
 
 /* ************************************************************************* */

gtsam/linear/HessianFactor.h

@@ -293,8 +293,11 @@ namespace gtsam {
      */
     Matrix information() const override;
 
-    /// Return the diagonal of the Hessian for this factor
+    /// Add the current diagonal to a VectorValues instance
-    VectorValues hessianDiagonal() const override;
+    void hessianDiagonalAdd(VectorValues& d) const override;
+
+    /// Using the base method
+    using Base::hessianDiagonal;
 
     /// Raw memory access version of hessianDiagonal
     void hessianDiagonal(double* d) const override;

gtsam/linear/JacobianFactor.cpp

@@ -542,21 +542,31 @@ Matrix JacobianFactor::information() const {
 }
 
 /* ************************************************************************* */
-VectorValues JacobianFactor::hessianDiagonal() const {
+void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
-  VectorValues d;
   for (size_t pos = 0; pos < size(); ++pos) {
     Key j = keys_[pos];
     size_t nj = Ab_(pos).cols();
-    Vector dj(nj);
+    auto result = d.emplace(j, nj);
+
+    Vector& dj = result.first->second;
+
     for (size_t k = 0; k < nj; ++k) {
-      Vector column_k = Ab_(pos).col(k);
+      Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
-      if (model_)
+      if (model_) {
-        model_->whitenInPlace(column_k);
+        Vector column_k_copy = column_k;
-      dj(k) = dot(column_k, column_k);
+        model_->whitenInPlace(column_k_copy);
+        if(!result.second)
+          dj(k) += dot(column_k_copy, column_k_copy);
+        else
+          dj(k) = dot(column_k_copy, column_k_copy);
+      } else {
+        if (!result.second)
+          dj(k) += dot(column_k, column_k);
+        else
+          dj(k) = dot(column_k, column_k);
+      }
     }
-    d.emplace(j, dj);
   }
-  return d;
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.h

@@ -215,8 +215,11 @@ namespace gtsam {
      */
     Matrix information() const override;
 
-    /// Return the diagonal of the Hessian for this factor
+    /// Using the base method
-    VectorValues hessianDiagonal() const override;
+    using Base::hessianDiagonal;
+
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
 
     /// Raw memory access version of hessianDiagonal
     void hessianDiagonal(double* d) const override;

gtsam/linear/Preconditioner.cpp

@@ -183,21 +183,21 @@ void BlockJacobiPreconditioner::clean() {
 }
 
 /***************************************************************************************/
-boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
+boost::shared_ptr<Preconditioner> createPreconditioner(
-
+    const boost::shared_ptr<PreconditionerParameters> params) {
-  if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
+  using boost::dynamic_pointer_cast;
+  if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
     return boost::make_shared<DummyPreconditioner>();
-  }
+  } else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(
-  else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
+                 params)) {
     return boost::make_shared<BlockJacobiPreconditioner>();
-  }
+  } else if (auto subgraph =
-  else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
+                 dynamic_pointer_cast<SubgraphPreconditionerParameters>(
+                     params)) {
     return boost::make_shared<SubgraphPreconditioner>(*subgraph);
   }
 
-  throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
+  throw invalid_argument(
+      "createPreconditioner: unexpected preconditioner parameter type");
 }
-
+}  // namespace gtsam
-}
-
-

gtsam/linear/RegularJacobianFactor.h

@@ -102,10 +102,8 @@ public:
       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
   }
 
-  /// Expose base class hessianDiagonal
+  /// Using the base method
-  virtual VectorValues hessianDiagonal() const {
+  using GaussianFactor::hessianDiagonal;
-    return JacobianFactor::hessianDiagonal();
-  }
 
   /// Raw memory access version of hessianDiagonal
   void hessianDiagonal(double* d) const {

gtsam/linear/VectorValues.cpp

@@ -96,20 +96,6 @@ namespace gtsam {
     return result.first;
   }
 
-  /* ************************************************************************* */
-  VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
-#ifdef TBB_GREATER_EQUAL_2020
-    std::pair<iterator, bool> result = values_.emplace(j, value);
-#else
-    std::pair<iterator, bool> result = values_.insert(std::make_pair(j, value));
-#endif
-    if(!result.second)
-      throw std::invalid_argument(
-      "Requested to emplace variable '" + DefaultKeyFormatter(j)
-      + "' already in this VectorValues.");
-    return result.first;
-  }
-
   /* ************************************************************************* */
   void VectorValues::update(const VectorValues& values)
   {

gtsam/linear/VectorValues.h

@@ -179,7 +179,14 @@ namespace gtsam {
      *  j is already used.
      * @param value The vector to be inserted.
      * @param j The index with which the value will be associated. */
-    iterator emplace(Key j, const Vector& value);
+    template<class... Args>
+    inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
+#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
+      return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
+#else
+      return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
+#endif
+    }
 
     /** Insert a vector \c value with key \c j.  Throws an invalid_argument exception if the key \c
      *  j is already used.
@@ -197,7 +204,7 @@ namespace gtsam {
      *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
      *  returned. */
-    std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
+    inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
 #ifdef TBB_GREATER_EQUAL_2020
       return values_.emplace(j, value);
 #else

gtsam/linear/linearAlgorithms-inst.h

@@ -98,8 +98,13 @@ namespace gtsam
             // Insert solution into a VectorValues
             DenseIndex vectorPosition = 0;
             for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
-              VectorValues::const_iterator r =
+              auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
-                collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
+              if(!result.second)
+                  throw std::runtime_error(
+                      "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
+                      + "' that exists.");
+
+              VectorValues::const_iterator r = result.first;
               myData.cliqueResults.emplace(r->first, r);
               vectorPosition += c.getDim(frontal);
             }

gtsam/navigation/AttitudeFactor.h

@@ -139,7 +139,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits
@@ -219,7 +219,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /// traits

gtsam/navigation/CombinedImuFactor.h

@@ -100,7 +100,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 
@@ -210,7 +210,7 @@ public:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 /**
@@ -332,7 +332,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 // class CombinedImuFactor
 

gtsam/navigation/ImuBias.h

@@ -171,7 +171,7 @@ private:
 
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
   /// @}
 
 }; // ConstantBias class

gtsam/navigation/PreintegratedRotation.h

@@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 
@@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation {
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
   public:
-	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 

gtsam/navigation/PreintegrationBase.cpp

@@ -115,20 +115,21 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
 NavState PreintegrationBase::predict(const NavState& state_i,
     const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
     OptionalJacobian<9, 6> H2) const {
-  // TODO(frank): make sure this stuff is still correct
   Matrix96 D_biasCorrected_bias;
   Vector9 biasCorrected = biasCorrectedDelta(bias_i,
-      H2 ? &D_biasCorrected_bias : 0);
+                                             H2 ? &D_biasCorrected_bias : nullptr);
 
   // Correct for initial velocity and gravity
   Matrix9 D_delta_state, D_delta_biasCorrected;
   Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
-      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
+                                  p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr,
-      H2 ? &D_delta_biasCorrected : 0);
+                                  H2 ? &D_delta_biasCorrected : nullptr);
 
   // Use retract to get back to NavState manifold
   Matrix9 D_predict_state, D_predict_delta;
-  NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
+  NavState state_j = state_i.retract(xi,
+                                     H1 ? &D_predict_state : nullptr,
+                                     H2 || H2 ? &D_predict_delta : nullptr);
   if (H1)
     *H1 = D_predict_state + D_predict_delta * D_delta_state;
   if (H2)

gtsam/navigation/PreintegrationBase.h

@@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase {
 #endif
 
  public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 }  /// namespace gtsam

gtsam/navigation/PreintegrationParams.h

@@ -84,7 +84,7 @@ protected:
 #ifdef GTSAM_USE_QUATERNIONS
   // Align if we are using Quaternions
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 #endif
 };
 

gtsam/navigation/TangentPreintegration.h

@@ -141,7 +141,7 @@ private:
   }
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /// namespace gtsam

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -342,7 +342,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
 //******************************************************************************
 TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
   // Linearization point
-  Vector3 bias; ///< Current estimate of rotation rate bias
+  Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
 
   Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
 

gtsam/nonlinear/ExpressionFactor.h

@@ -209,7 +209,7 @@ private:
  // 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)
 };
 // ExpressionFactor
 

gtsam/nonlinear/NonlinearEquality.h

@@ -175,7 +175,7 @@ public:
 
   /// @}
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
 private:
 
@@ -265,7 +265,7 @@ public:
     traits<X>::Print(value_, "Value");
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
 private:
 
@@ -331,7 +331,7 @@ public:
     return traits<X>::Local(x1,x2);
   }
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
 private:
 

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -147,13 +147,15 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
   } else if (params.isIterative()) {
     // Conjugate Gradient -> needs params.iterativeParams
     if (!params.iterativeParams)
-      throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ...");
+      throw std::runtime_error(
+          "NonlinearOptimizer::solve: cg parameter has to be assigned ...");
 
-    if (boost::shared_ptr<PCGSolverParameters> pcg =
+    if (auto pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(
-            boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams)) {
+            params.iterativeParams)) {
       delta = PCGSolver(*pcg).optimize(gfg);
-    } else if (boost::shared_ptr<SubgraphSolverParameters> spcg =
+    } else if (auto spcg =
-                   boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)) {
+                   boost::dynamic_pointer_cast<SubgraphSolverParameters>(
+                       params.iterativeParams)) {
       if (!params.ordering)
         throw std::runtime_error("SubgraphSolver needs an ordering");
       delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();

gtsam/nonlinear/PriorFactor.h

@@ -114,7 +114,7 @@ namespace gtsam {
 	// 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)
   };
 
 } /// namespace gtsam