Merge branch 'develop' into python_examples

CMakeLists.txt

@@ -61,8 +61,8 @@ option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the defau
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
 option(GTSAM_ENABLE_CONSISTENCY_CHECKS   "Enable/Disable expensive consistency checks"       OFF)
 option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TBB) if available" ON)
-option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" ON)
+option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" OFF)
-option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
+option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module" OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
@@ -564,10 +564,10 @@ if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
 	message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled.  Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
 endif()
 if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
-	message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance.  Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.")
+	message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled.  Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.  See INSTALL.md for notes on performance.")
 endif()
 if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
-	message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
+	message(WARNING "Your compiler does not support OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.")
 endif()
 if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
 	message(WARNING "${GTSAM_PYTHON_WARNINGS}")

GTSAM-Concepts.md

@@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc
 
 For Lie groups, the `exponential map` above is the most obvious mapping: it 
 associates straight lines in the tangent space with geodesics on the manifold 
-(and it's inverse, the log map). However, there are two cases in which we deviate from this:
+(and it's inverse, the log map). However, there are several cases in which we deviate from this:
 
 However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`)
 
 Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method.
 
+A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs:
+
+* operator* : implements group operator
+* inverse: implements group inverse
+* AdjointMap: maps tangent vectors according to group element
+* Expmap/Logmap: exponential map and its inverse
+* ChartAtOrigin: struct where you define Retract/Local at origin
+
+To use, simply derive, but also say `using LieGroup<Class,N>::inverse` so you get an inverse with a derivative.
+
+Finally, to create the traits automatically you can use `internal::LieGroupTraits<Class>`
+
 Vector Space
 ------------
 

INSTALL.md

@@ -23,18 +23,22 @@ $ make install
        disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
        may be installed from the Ubuntu repositories, and for other platforms it
        may be downloaded from https://www.threadingbuildingblocks.org/
+     - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
+       `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
+       achieved with MKL disabled. We therefore advise you to benchmark your problem 
+       before using MKL.
 
     Tested compilers:
 
-    - GCC 4.2-4.7
+    - GCC 4.2-7.3
-    - OSX Clang 2.9-5.0
+    - OS X Clang 2.9-10.0
-    - OSX GCC 4.2
+    - OS X GCC 4.2
-    - MSVC 2010, 2012
+    - MSVC 2010, 2012, 2017
 
     Tested systems:
 
-    - Ubuntu 11.04 - 18.04
+    - Ubuntu 16.04 - 18.04
-    - MacOS 10.6 - 10.9
+    - MacOS 10.6 - 10.14
     - Windows 7, 8, 8.1, 10
 
     Known issues:
@@ -134,6 +138,27 @@ MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included i
 
 $MATLABROOT can be found by executing the command `matlabroot` in MATLAB
 
+## Performance
+
+Here are some tips to get the best possible performance out of GTSAM.
+
+1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
+2. Enable TBB. On modern processors with multiple cores, this can easily speed up
+    optimization by 30-50%. Please note that this may not be true for very small 
+    problems where the overhead of dispatching work to multiple threads outweighs
+    the benefit. We recommend that you benchmark your problem with/without TBB.
+3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of
+    25-30% can be expected on modern processors. Note that this affects the portability
+    of your executable. It may not run when copied to another system with older/different
+    processor architecture.
+    Also note that all dependent projects *must* be compiled with the same flag, or
+    seg-faults and other undefined behavior may result.
+4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only
+    in very limited cases, and actually hurts performance in the usual case. We therefore
+    recommend that you do *not* enable MKL, unless you have benchmarked it on
+    your problem and have verified that it improves performance.
+
+
 ## Debugging tips
 
 Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find.

THANKS

@@ -1,5 +1,6 @@
 GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
 
+* Jeremy Aguilon, Facebook
 * Sungtae An
 * Doru Balcan, Bank of America
 * Chris Beall

cmake/FindMKL.cmake

@@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
                 )
         ENDIF()
 
+        IF(NOT MKL_LAPACK_LIBRARY)
+                FIND_LIBRARY(MKL_LAPACK_LIBRARY
+                  mkl_intel_lp64
+                  PATHS
+                        ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
+                        ${MKL_ROOT_DIR}/lib/
+                )
+        ENDIF()
+
         # iomp5
         IF("${MKL_ARCH_DIR}" STREQUAL "32")
                 IF(UNIX AND NOT APPLE)

cython/CMakeLists.txt

@@ -19,22 +19,25 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
   # wrap gtsam_unstable
   if(GTSAM_BUILD_UNSTABLE)
     add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
-    set(GTSAM_UNSTABLE_IMPORT "from .gtsam_unstable import *")
     wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
                                     "from gtsam.gtsam cimport *"                  # extra imports
-                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path
+                                    "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path
                                     gtsam_unstable  # library to link with
                                     "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam"  # dependencies to be built before wrapping
                                     )
-    # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this
-    file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "")
   endif()
 
+  file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS)
+  file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
+
   # Install the custom-generated __init__.py
   # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
-  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py)
+  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY)
-  install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam")
+  configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY)
+  configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
+  install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}")
   # install scripts and tests
   install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
+  install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
 
 endif ()

cython/README.md

@@ -17,10 +17,15 @@ named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy.
 The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is
 by default: `<your CMAKE_INSTALL_PREFIX>/cython`
 
-- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
+- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
 ```bash
 export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
 ```
+- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
+    - (the same command can be used to install into a virtual environment if it is active)
+    - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory.
+    - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`.
+      Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package.
 
 UNIT TESTS
 ==========

cython/gtsam/__init__.py

@@ -0,0 +1,26 @@
+from .gtsam import *
+
+try:
+    import gtsam_unstable
+
+
+    def _deprecated_wrapper(item, name):
+        def wrapper(*args, **kwargs):
+            from warnings import warn
+            message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) +
+                       'Please import it from gtsam_unstable.')
+            warn(message)
+            return item(*args, **kwargs)
+        return wrapper
+
+
+    for name in dir(gtsam_unstable):
+        if not name.startswith('__'):
+            item = getattr(gtsam_unstable, name)
+            if callable(item):
+                item = _deprecated_wrapper(item, name)
+            globals()[name] = item
+
+except ImportError:
+    pass
+

cython/gtsam/__init__.py.in

@@ -1,2 +0,0 @@
-from .gtsam import *
-${GTSAM_UNSTABLE_IMPORT}

cython/gtsam/examples/PlanarManipulatorExample.py

@@ -24,6 +24,7 @@ from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 import gtsam
 import gtsam.utils.plot as gtsam_plot
 from gtsam import Pose2
+from gtsam.utils.test_case import GtsamTestCase
 
 
 def vector3(x, y, z):
@@ -202,7 +203,7 @@ Q1 = np.radians(vector3(-30, -45, -90))
 Q2 = np.radians(vector3(-90, 90, 0))
 
 
-class TestPose2SLAMExample(unittest.TestCase):
+class TestPose2SLAMExample(GtsamTestCase):
     """Unit tests for functions used below."""
 
     def setUp(self):

cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py

@@ -0,0 +1,35 @@
+"""
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Initialize PoseSLAM with Chordal init
+Author: Luca Carlone, Frank Dellaert (python port)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+# Read graph from file
+g2oFile = gtsam.findExampleDataFile("pose3example.txt")
+
+is3D = True
+graph, initial = gtsam.readG2o(g2oFile, is3D)
+
+# Add prior on the first key. TODO: assumes first key ios z
+priorModel = gtsam.noiseModel_Diagonal.Variances(
+    np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
+firstKey = initial.keys().at(0)
+graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
+
+# Initializing Pose3 - chordal relaxation"
+initialization = gtsam.InitializePose3.initialize(graph)
+
+print(initialization)

cython/gtsam/tests/test_Cal3Unified.py

@@ -1,9 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Cal3Unified unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
 
-class TestCal3Unified(unittest.TestCase):
+
+class TestCal3Unified(GtsamTestCase):
 
     def test_Cal3Unified(self):
         K = gtsam.Cal3Unified()
@@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase):
         self.assertEqual(K.fx(), 1.)
 
     def test_retract(self):
-        expected  = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
+        expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
-        K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
+                                     1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1)
+        K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240,
+                              1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1)
         d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F')
         actual = K.retract(d)
-        self.assertTrue(actual.equals(expected, 1e-9))
+        self.gtsamAssertEquals(actual, expected)
         np.testing.assert_allclose(d, K.localCoordinates(actual))
 
+
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_JacobianFactor.py

@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+JacobianFactor unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestJacobianFactor(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestJacobianFactor(GtsamTestCase):
 
     def test_eliminate(self):
         # Recommended way to specify a matrix (see cython/README)
@@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase):
         expectedCG = gtsam.GaussianConditional(
             x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2))
         # check if the result matches
-        self.assertTrue(actualCG.equals(expectedCG, 1e-4))
+        self.gtsamAssertEquals(actualCG, expectedCG, 1e-4)
 
         # the expected linear factor
         Bl1 = np.array([[4.47214, 0.00],
@@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase):
         expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2)
 
         # check if the result matches the combined (reduced) factor
-        self.assertTrue(lf.equals(expectedLF, 1e-4))
+        self.gtsamAssertEquals(lf, expectedLF, 1e-4)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_KalmanFilter.py

@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+KalmanFilter unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestKalmanFilter(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestKalmanFilter(GtsamTestCase):
 
     def test_KalmanFilter(self):
         F = np.eye(2)

cython/gtsam/tests/test_LocalizationExample.py

@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Localization unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestLocalizationExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestLocalizationExample(GtsamTestCase):
 
     def test_LocalizationExample(self):
         # Create the graph (defined in pose2SLAM.h, derived from
@@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase):
         P = [None] * result.size()
         for i in range(0, result.size()):
             pose_i = result.atPose2(i)
-            self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4))
+            self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4)
             P[i] = marginals.marginalCovariance(i)
 
 if __name__ == "__main__":

cython/gtsam/tests/test_NonlinearOptimizer.py

@@ -0,0 +1,72 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for IMU testing scenarios.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, no-name-in-module
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
+                   GaussNewtonParams, LevenbergMarquardtOptimizer,
+                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   Point2, PriorFactorPoint2, Values)
+from gtsam.utils.test_case import GtsamTestCase
+
+KEY1 = 1
+KEY2 = 2
+
+
+class TestScenario(GtsamTestCase):
+    def test_optimize(self):
+        """Do trivial test with three optimizer variants."""
+        fg = NonlinearFactorGraph()
+        model = gtsam.noiseModel_Unit.Create(2)
+        fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
+
+        # test error at minimum
+        xstar = Point2(0, 0)
+        optimal_values = Values()
+        optimal_values.insert(KEY1, xstar)
+        self.assertEqual(0.0, fg.error(optimal_values), 0.0)
+
+        # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
+        x0 = Point2(3, 3)
+        initial_values = Values()
+        initial_values.insert(KEY1, x0)
+        self.assertEqual(9.0, fg.error(initial_values), 1e-3)
+
+        # optimize parameters
+        ordering = Ordering()
+        ordering.push_back(KEY1)
+
+        # Gauss-Newton
+        gnParams = GaussNewtonParams()
+        gnParams.setOrdering(ordering)
+        actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual1))
+
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setOrdering(ordering)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
+        # Dogleg
+        dlParams = DoglegParams()
+        dlParams.setOrdering(ordering)
+        actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual3))
+
+
+if __name__ == "__main__":
+    unittest.main()

cython/gtsam/tests/test_OdometryExample.py

@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Odometry unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestOdometryExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestOdometryExample(GtsamTestCase):
 
     def test_OdometryExample(self):
         # Create the graph (defined in pose2SLAM.h, derived from
@@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase):
 
         # Check first pose equality
         pose_1 = result.atPose2(1)
-        self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_PlanarSLAMExample.py

@@ -1,11 +1,25 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PlanarSLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
 from math import pi
+
 import numpy as np
 
-class TestPose2SLAMExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
 
-    def test_Pose2SLAMExample(self):
+
+class TestPlanarSLAM(GtsamTestCase):
+
+    def test_PlanarSLAM(self):
         # Assumptions
         #  - All values are axis aligned
         #  - Robot poses are facing along the X axis (horizontal, to the right in images)
@@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase):
         P = marginals.marginalCovariance(1)
 
         pose_1 = result.atPose2(1)
-        self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
 
 
 

cython/gtsam/tests/test_Pose2.py

@@ -1,12 +1,23 @@
-"""Pose2 unit tests."""
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose2 unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
 
 import numpy as np
 
+import gtsam
 from gtsam import Pose2
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestPose2(unittest.TestCase):
+class TestPose2(GtsamTestCase):
     """Test selected Pose2 methods."""
 
     def test_adjoint(self):

cython/gtsam/tests/test_Pose2SLAMExample.py

@@ -1,9 +1,23 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose2SLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
 from math import pi
+
 import numpy as np
 
-class TestPose2SLAMExample(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPose2SLAMExample(GtsamTestCase):
 
     def test_Pose2SLAMExample(self):
         # Assumptions
@@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase):
         P = marginals.marginalCovariance(1)
 
         pose_1 = result.atPose2(1)
-        self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4))
+        self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_Pose3.py

@@ -1,13 +1,24 @@
-"""Pose3 unit tests."""
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Pose3 unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import math
 import unittest
 
 import numpy as np
 
+import gtsam
 from gtsam import Point3, Pose3, Rot3
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestPose3(unittest.TestCase):
+class TestPose3(GtsamTestCase):
     """Test selected Pose3 methods."""
 
     def test_between(self):
@@ -16,14 +27,14 @@ class TestPose3(unittest.TestCase):
         T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
         expected = T2.inverse().compose(T3)
         actual = T2.between(T3)
-        self.assertTrue(actual.equals(expected, 1e-6))
+        self.gtsamAssertEquals(actual, expected, 1e-6)
 
     def test_transform_to(self):
         """Test transform_to method."""
         transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
         actual = transform.transform_to(Point3(3, 2, 10))
         expected = Point3(2, 1, 10)
-        self.assertTrue(actual.equals(expected, 1e-6))
+        self.gtsamAssertEquals(actual, expected, 1e-6)
 
     def test_range(self):
         """Test range method."""

cython/gtsam/tests/test_Pose3SLAMExample.py

@@ -1,10 +1,24 @@
-import unittest
+"""
-import numpy as np
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
-import gtsam
+Atlanta, Georgia 30332-0415
-from math import pi
+All Rights Reserved
-from gtsam.utils.circlePose3 import * 
 
-class TestPose3SLAMExample(unittest.TestCase):
+See LICENSE for the license information
+
+PoseSLAM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+from gtsam.utils.circlePose3 import *
+
+
+class TestPose3SLAMExample(GtsamTestCase):
 
     def test_Pose3SLAMExample(self):
         # Create a hexagon of poses
@@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase):
         result = optimizer.optimizeSafely()
 
         pose_1 = result.atPose3(1)
-        self.assertTrue(pose_1.equals(p1, 1e-4))
+        self.gtsamAssertEquals(pose_1, p1, 1e-4)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_PriorFactor.py

@@ -1,8 +1,22 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PriorFactor unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
 import numpy as np
 
-class TestPriorFactor(unittest.TestCase):
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPriorFactor(GtsamTestCase):
 
     def test_PriorFactor(self):
         values = gtsam.Values()

cython/gtsam/tests/test_SFMExample.py

@@ -1,11 +1,24 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SFM unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
-from gtsam import symbol
 import numpy as np
+
+import gtsam
 import gtsam.utils.visual_data_generator as generator
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestSFMExample(unittest.TestCase):
+class TestSFMExample(GtsamTestCase):
 
     def test_SFMExample(self):
         options = generator.Options()
@@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase):
         # Check optimized results, should be equal to ground truth
         for i in range(len(truth.cameras)):
             pose_i = result.atPose3(symbol(ord('x'), i))
-            self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
+            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
         for j in range(len(truth.points)):
             point_j = result.atPoint3(symbol(ord('p'), j))
-            self.assertTrue(point_j.equals(truth.points[j], 1e-5))
+            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_Scenario.py

@@ -1,11 +1,27 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Scenario unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+from __future__ import print_function
+
 import math
 import unittest
+
 import numpy as np
 
 import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+# pylint: disable=invalid-name, E1101
 
 
-class TestScenario(unittest.TestCase):
+class TestScenario(GtsamTestCase):
     def setUp(self):
         pass
 
@@ -29,7 +45,8 @@ class TestScenario(unittest.TestCase):
         T30 = scenario.pose(T)
         np.testing.assert_almost_equal(
             np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
-        self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
+        self.gtsamAssertEquals(gtsam.Point3(
+            0, 0, 2.0 * R), T30.translation(), 1e-9)
 
 
 if __name__ == '__main__':

cython/gtsam/tests/test_SimpleCamera.py

@@ -1,18 +1,31 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+SimpleCamera unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import math
-import numpy as np
 import unittest
 
-from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera
+import numpy as np
+
+import gtsam
+from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
+from gtsam.utils.test_case import GtsamTestCase
 
 K = Cal3_S2(625, 625, 0, 0, 0)
 
-class TestSimpleCamera(unittest.TestCase):
+class TestSimpleCamera(GtsamTestCase):
 
     def test_constructor(self):
         pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5))
         camera = SimpleCamera(pose1, K)
-        self.assertTrue(camera.calibration().equals(K, 1e-9))
+        self.gtsamAssertEquals(camera.calibration(), K, 1e-9)
-        self.assertTrue(camera.pose().equals(pose1, 1e-9))
+        self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
 
     def test_level2(self):
         # Create a level camera, looking in Y-direction
@@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase):
         z = Point3(0,1,0)
         wRc = Rot3(x,y,z)
         expected = Pose3(wRc,Point3(0.4,0.3,0.1))
-        self.assertTrue(camera.pose().equals(expected, 1e-9))
+        self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
 
 
 if __name__ == "__main__":

cython/gtsam/tests/test_StereoVOExample.py

@@ -1,10 +1,23 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Stereo VO unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
-import gtsam
+
-from gtsam import symbol
 import numpy as np
 
+import gtsam
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
 
-class TestStereoVOExample(unittest.TestCase):
+
+class TestStereoVOExample(GtsamTestCase):
 
     def test_StereoVOExample(self):
         ## Assumptions
@@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase):
 
         ## check equality for the first pose and point
         pose_x1 = result.atPose3(x1)
-        self.assertTrue(pose_x1.equals(first_pose,1e-4))
+        self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
 
         point_l1 = result.atPoint3(l1)
-        self.assertTrue(point_l1.equals(expected_l1,1e-4))
+        self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_Values.py

@@ -1,19 +1,34 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Values unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
 import unittest
+
 import numpy as np
 
-from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3
+import gtsam
-from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias
+from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2,
+                   Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values,
+                   imuBias_ConstantBias)
+from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestValues(unittest.TestCase):
+class TestValues(GtsamTestCase):
 
     def test_values(self):
         values = Values()
         E = EssentialMatrix(Rot3(), Unit3())
         tol = 1e-9
 
-        values.insert(0, Point2(0,0))
+        values.insert(0, Point2(0, 0))
-        values.insert(1, Point3(0,0,0))
+        values.insert(1, Point3(0, 0, 0))
         values.insert(2, Rot2())
         values.insert(3, Pose2())
         values.insert(4, Rot3())
@@ -34,36 +49,38 @@ class TestValues(unittest.TestCase):
         # The wrapper will automatically fix the type and storage order for you,
         # but for performance reasons, it's recommended to specify the correct
         # type and storage order.
-        vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is
+        # for vectors, the order is not important, but dtype still is
+        vec = np.array([1., 2., 3.])
         values.insert(11, vec)
         mat = np.array([[1., 2.], [3., 4.]], order='F')
         values.insert(12, mat)
         # Test with dtype int and the default order='C'
         # This still works as the wrapper converts to the correct type and order for you
         # but is nornally not recommended!
-        mat2 = np.array([[1,2,],[3,5]])
+        mat2 = np.array([[1, 2, ], [3, 5]])
         values.insert(13, mat2)
 
-        self.assertTrue(values.atPoint2(0).equals(Point2(), tol))
+        self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol)
-        self.assertTrue(values.atPoint3(1).equals(Point3(), tol))
+        self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol)
-        self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
+        self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol)
-        self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
+        self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol)
-        self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
+        self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol)
-        self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
+        self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol)
-        self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
+        self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol)
-        self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
+        self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol)
-        self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
+        self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol)
-        self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
+        self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol)
-        self.assertTrue(values.atimuBias_ConstantBias(
+        self.gtsamAssertEquals(values.atimuBias_ConstantBias(
-            10).equals(imuBias_ConstantBias(), tol))
+            10), imuBias_ConstantBias(), tol)
 
         # special cases for Vector and Matrix:
         actualVector = values.atVector(11)
-        self.assertTrue(np.allclose(vec, actualVector, tol))
+        np.testing.assert_allclose(vec, actualVector, tol)
         actualMatrix = values.atMatrix(12)
-        self.assertTrue(np.allclose(mat, actualMatrix, tol))
+        np.testing.assert_allclose(mat, actualMatrix, tol)
         actualMatrix2 = values.atMatrix(13)
-        self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
+        np.testing.assert_allclose(mat2, actualMatrix2, tol)
+
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_VisualISAMExample.py

@@ -1,10 +1,25 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+visual_isam unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
 import unittest
+
 import numpy as np
-from gtsam import symbol
+
+import gtsam
 import gtsam.utils.visual_data_generator as generator
 import gtsam.utils.visual_isam as visual_isam
+from gtsam import symbol
+from gtsam.utils.test_case import GtsamTestCase
 
-class TestVisualISAMExample(unittest.TestCase):
+
+class TestVisualISAMExample(GtsamTestCase):
 
     def test_VisualISAMExample(self):
         # Data Options
@@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase):
 
         for i in range(len(truth.cameras)):
             pose_i = result.atPose3(symbol(ord('x'), i))
-            self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
+            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
         for j in range(len(truth.points)):
             point_j = result.atPoint3(symbol(ord('l'), j))
-            self.assertTrue(point_j.equals(truth.points[j], 1e-5))
+            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
 if __name__ == "__main__":
     unittest.main()

cython/gtsam/tests/test_dataset.py

@@ -0,0 +1,45 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import BetweenFactorPose3, BetweenFactorPose3s
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestDataset(GtsamTestCase):
+    """Tests for datasets.h wrapper."""
+
+    def setUp(self):
+        """Get some common paths."""
+        self.pose3_example_g2o_file = gtsam.findExampleDataFile(
+            "pose3example.txt")
+
+    def test_readG2o3D(self):
+        """Test reading directly into factor graph."""
+        is3D = True
+        graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
+        self.assertEqual(graph.size(), 6)
+        self.assertEqual(initial.size(), 5)
+
+    def test_parse3Dfactors(self):
+        """Test parsing into data structure."""
+        factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
+        self.assertEqual(factors.size(), 6)
+        self.assertIsInstance(factors.at(0), BetweenFactorPose3)
+
+
+if __name__ == '__main__':
+    unittest.main()

cython/gtsam/tests/test_initialize_pose3.py

@@ -0,0 +1,89 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for 3D SLAM initialization, using rotation relaxation.
+Author: Luca Carlone and Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
+from gtsam.utils.test_case import GtsamTestCase
+
+x0, x1, x2, x3 = 0, 1, 2, 3
+
+
+class TestValues(GtsamTestCase):
+
+    def setUp(self):
+
+        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+
+        # We consider a small graph:
+        #                            symbolic FG
+        #               x2               0  1
+        #             / | \              1  2
+        #            /  |  \             2  3
+        #          x3   |   x1           2  0
+        #           \   |   /            0  3
+        #            \  |  /
+        #               x0
+        #
+        p0 = Point3(0, 0, 0)
+        self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
+        p1 = Point3(1, 2, 0)
+        self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
+        p2 = Point3(0, 2, 0)
+        self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
+        p3 = Point3(-1, 1, 0)
+        self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
+
+        pose0 = Pose3(self.R0, p0)
+        pose1 = Pose3(self.R1, p1)
+        pose2 = Pose3(self.R2, p2)
+        pose3 = Pose3(self.R3, p3)
+
+        g = NonlinearFactorGraph()
+        g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
+        g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
+        g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
+        g.add(gtsam.PriorFactorPose3(x0, pose0, model))
+        self.graph = g
+
+    def test_buildPose3graph(self):
+        pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+    def test_orientations(self):
+        pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+        initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
+
+        # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+        self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6)
+        self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6)
+
+    def test_initializePoses(self):
+        g2oFile = gtsam.findExampleDataFile("pose3example-grid")
+        is3D = True
+        inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
+        priorModel = gtsam.noiseModel_Unit.Create(6)
+        inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
+
+        initial = gtsam.InitializePose3.initialize(inputGraph)
+        # TODO(frank): very loose !!
+        self.gtsamAssertEquals(initial, expectedValues, 0.1)
+
+
+if __name__ == "__main__":
+    unittest.main()

cython/gtsam/utils/test_case.py

@@ -0,0 +1,27 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+TestCase class with GTSAM assert utils.
+Author: Frank Dellaert
+"""
+import unittest
+
+
+class GtsamTestCase(unittest.TestCase):
+    """TestCase class with GTSAM assert utils."""
+
+    def gtsamAssertEquals(self, actual, expected, tol=1e-9):
+        """ AssertEqual function that prints out actual and expected if not equal.
+            Usage:
+                self.gtsamAssertEqual(actual,expected)
+            Keyword Arguments:
+                tol {float} -- tolerance passed to 'equals', default 1e-9
+        """
+        equal = actual.equals(expected, tol)
+        if not equal:
+            raise self.failureException(
+                "Values are not equal:\n{}!={}".format(actual, expected))

cython/gtsam_unstable/__init__.py

@@ -0,0 +1 @@
+from .gtsam_unstable import *

cython/gtsam_unstable/examples/FixedLagSmootherExample.py

@@ -0,0 +1,102 @@
+"""
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Demonstration of the fixed-lag smoothers using a planar robot example
+and multiple odometry-like sensors
+Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
+"""
+
+import numpy as np
+
+import gtsam
+import gtsam_unstable
+
+
+def _timestamp_key_value(key, value):
+    """
+
+    """
+    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
+        key, value
+    )
+
+
+def BatchFixedLagSmootherExample():
+    """
+    Runs a batch fixed smoother on an agent with two odometry
+    sensors that is simply moving to the
+    """
+
+    # Define a batch fixed lag smoother, which uses
+    # Levenberg-Marquardt to perform the nonlinear optimization
+    lag = 2.0
+    smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
+
+    # Create containers to store the factors and linearization points
+    # that will be sent to the smoothers
+    new_factors = gtsam.NonlinearFactorGraph()
+    new_values = gtsam.Values()
+    new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
+
+    # Create  a prior on the first pose, placing it at the origin
+    prior_mean = gtsam.Pose2(0, 0, 0)
+    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
+    X1 = 0
+    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
+    new_values.insert(X1, prior_mean)
+    new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+
+    delta_time = 0.25
+    time = 0.25
+
+    while time <= 3.0:
+        previous_key = 1000 * (time - delta_time)
+        current_key = 1000 * time
+
+        # assign current key to the current timestamp
+        new_timestamps.insert(_timestamp_key_value(current_key, time))
+
+        # Add a guess for this pose to the new values
+        # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
+        current_pose = gtsam.Pose2(time * 2, 0, 0)
+        new_values.insert(current_key, current_pose)
+
+        # Add odometry factors from two different sources with different error
+        # stats
+        odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
+        odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.1, 0.1, 0.05]))
+        new_factors.push_back(gtsam.BetweenFactorPose2(
+            previous_key, current_key, odometry_measurement_1, odometry_noise_1
+        ))
+
+        odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
+        odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.05, 0.05, 0.05]))
+        new_factors.push_back(gtsam.BetweenFactorPose2(
+            previous_key, current_key, odometry_measurement_2, odometry_noise_2
+        ))
+
+        # Update the smoothers with the new factors. In this case,
+        # one iteration must pass for Levenberg-Marquardt to accurately
+        # estimate
+        if time >= 0.50:
+            smoother_batch.update(new_factors, new_values, new_timestamps)
+            print("Timestamp = " + str(time) + ", Key = " + str(current_key))
+            print(smoother_batch.calculateEstimatePose2(current_key))
+
+            new_timestamps.clear()
+            new_values.clear()
+            new_factors.resize(0)
+
+        time += delta_time
+
+
+if __name__ == '__main__':
+    BatchFixedLagSmootherExample()
+    print("Example complete")

cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py

@@ -0,0 +1,135 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Cal3Unified unit tests.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+import unittest
+
+import numpy as np
+
+import gtsam
+import gtsam_unstable
+from gtsam.utils.test_case import GtsamTestCase
+
+
+def _timestamp_key_value(key, value):
+    return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
+        key, value
+    )
+
+
+class TestFixedLagSmootherExample(GtsamTestCase):
+    '''
+    Tests the fixed lag smoother wrapper
+    '''
+
+    def test_FixedLagSmootherExample(self):
+        '''
+        Simple test that checks for equality between C++ example
+        file and the Python implementation. See
+        gtsam_unstable/examples/FixedLagSmootherExample.cpp
+        '''
+        # Define a batch fixed lag smoother, which uses
+        # Levenberg-Marquardt to perform the nonlinear optimization
+        lag = 2.0
+        smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
+
+        # Create containers to store the factors and linearization points
+        # that will be sent to the smoothers
+        new_factors = gtsam.NonlinearFactorGraph()
+        new_values = gtsam.Values()
+        new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
+
+        # Create  a prior on the first pose, placing it at the origin
+        prior_mean = gtsam.Pose2(0, 0, 0)
+        prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
+            np.array([0.3, 0.3, 0.1]))
+        X1 = 0
+        new_factors.push_back(
+            gtsam.PriorFactorPose2(
+                X1, prior_mean, prior_noise))
+        new_values.insert(X1, prior_mean)
+        new_timestamps.insert(_timestamp_key_value(X1, 0.0))
+
+        delta_time = 0.25
+        time = 0.25
+
+        i = 0
+
+        ground_truth = [
+            gtsam.Pose2(0.995821, 0.0231012, 0.0300001),
+            gtsam.Pose2(1.49284, 0.0457247, 0.045),
+            gtsam.Pose2(1.98981, 0.0758879, 0.06),
+            gtsam.Pose2(2.48627, 0.113502, 0.075),
+            gtsam.Pose2(2.98211, 0.158558, 0.09),
+            gtsam.Pose2(3.47722, 0.211047, 0.105),
+            gtsam.Pose2(3.97149, 0.270956, 0.12),
+            gtsam.Pose2(4.4648, 0.338272, 0.135),
+            gtsam.Pose2(4.95705, 0.41298, 0.15),
+            gtsam.Pose2(5.44812, 0.495063, 0.165),
+            gtsam.Pose2(5.9379, 0.584503, 0.18),
+        ]
+
+        # Iterates from 0.25s to 3.0s, adding 0.25s each loop
+        # In each iteration, the agent moves at a constant speed
+        # and its two odometers measure the change. The smoothed
+        # result is then compared to the ground truth
+        while time <= 3.0:
+            previous_key = 1000 * (time - delta_time)
+            current_key = 1000 * time
+
+            # assign current key to the current timestamp
+            new_timestamps.insert(_timestamp_key_value(current_key, time))
+
+            # Add a guess for this pose to the new values
+            # Assume that the robot moves at 2 m/s. Position is time[s] *
+            # 2[m/s]
+            current_pose = gtsam.Pose2(time * 2, 0, 0)
+            new_values.insert(current_key, current_pose)
+
+            # Add odometry factors from two different sources with different
+            # error stats
+            odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
+            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
+                np.array([0.1, 0.1, 0.05]))
+            new_factors.push_back(
+                gtsam.BetweenFactorPose2(
+                    previous_key,
+                    current_key,
+                    odometry_measurement_1,
+                    odometry_noise_1))
+
+            odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
+            odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
+                np.array([0.05, 0.05, 0.05]))
+            new_factors.push_back(
+                gtsam.BetweenFactorPose2(
+                    previous_key,
+                    current_key,
+                    odometry_measurement_2,
+                    odometry_noise_2))
+
+            # Update the smoothers with the new factors. In this case,
+            # one iteration must pass for Levenberg-Marquardt to accurately
+            # estimate
+            if time >= 0.50:
+                smoother_batch.update(new_factors, new_values, new_timestamps)
+
+                estimate = smoother_batch.calculateEstimatePose2(current_key)
+                self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+                i += 1
+
+                new_timestamps.clear()
+                new_values.clear()
+                new_factors.resize(0)
+
+            time += delta_time
+
+
+if __name__ == "__main__":
+    unittest.main()

cython/setup.py.in

@@ -0,0 +1,49 @@
+import os
+import sys
+try:
+    from setuptools import setup, find_packages
+except ImportError:
+    from distutils.core import setup, find_packages
+
+if 'SETUP_PY_NO_CHECK' not in os.environ:
+    script_path = os.path.abspath(os.path.realpath(__file__))
+    install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py')))
+    if script_path != install_path:
+        print('setup.py is being run from an unexpected location: "{}"'.format(script_path))
+        print('please run `make install` and run the script from there')
+        sys.exit(1)
+
+
+packages = find_packages()
+
+setup(
+    name='gtsam',
+    description='Georgia Tech Smoothing And Mapping library',
+    url='https://bitbucket.org/gtborg/gtsam',
+    version='${GTSAM_VERSION_STRING}',  # https://www.python.org/dev/peps/pep-0440/
+    license='Simplified BSD license',
+    keywords='slam sam robotics localization mapping optimization',
+    long_description='''${README_CONTENTS}''',
+    # https://pypi.org/pypi?%3Aaction=list_classifiers
+    classifiers=[
+        'Development Status :: 5 - Production/Stable',
+        'Intended Audience :: Education',
+        'Intended Audience :: Developers',
+        'Intended Audience :: Science/Research',
+        'Operating System :: MacOS',
+        'Operating System :: Microsoft :: Windows',
+        'Operating System :: POSIX',
+        'License :: OSI Approved :: BSD License',
+        'Programming Language :: Python :: 2',
+        'Programming Language :: Python :: 3',
+    ],
+
+    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')]
+        for package in packages
+    },
+    install_requires=[line.strip() for line in '''
+${CYTHON_INSTALL_REQUIREMENTS}
+'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')]
+)

gtsam.h

@@ -1218,14 +1218,30 @@ class VariableIndex {
 namespace noiseModel {
 #include <gtsam/linear/NoiseModel.h>
 virtual class Base {
+  void print(string s) const;
+  // Methods below are available for all noise models. However, can't add them 
+  // because wrap (incorrectly) thinks robust classes derive from this Base as well.
+  // bool isConstrained() const;
+  // bool isUnit() const;
+  // size_t dim() const;
+  // Vector sigmas() const;
 };
 
 virtual class Gaussian : gtsam::noiseModel::Base {
   static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
   static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
-  Matrix R() const;
+
   bool equals(gtsam::noiseModel::Base& expected, double tol);
-  void print(string s) const;
+
+  // access to noise model
+  Matrix R() const;
+  Matrix information() const;
+  Matrix covariance() const;
+
+  // Whitening operations
+  Vector whiten(Vector v) const;
+  Vector unwhiten(Vector v) const;
+  Matrix Whiten(Matrix H) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1236,8 +1252,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   static gtsam::noiseModel::Diagonal* Variances(Vector variances);
   static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
   Matrix R() const;
-  void print(string s) const;
 
+  // access to noise model
+  Vector sigmas() const;
+  Vector invsigmas() const;
+  Vector precisions() const;
+    
   // enabling serialization functionality
   void serializable() const;
 };
@@ -1263,7 +1283,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
   static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
   static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
   static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
-  void print(string s) const;
+
+  // access to noise model
+  double sigma() const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1271,7 +1293,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
 
 virtual class Unit : gtsam::noiseModel::Isotropic {
   static gtsam::noiseModel::Unit* Create(size_t dim);
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -1279,11 +1300,11 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
 
 namespace mEstimator {
 virtual class Base {
+  void print(string s) const;
 };
 
 virtual class Null: gtsam::noiseModel::mEstimator::Base {
   Null();
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Null* Create();
 
   // enabling serialization functionality
@@ -1292,7 +1313,6 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Fair: gtsam::noiseModel::mEstimator::Base {
   Fair(double c);
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Fair* Create(double c);
 
   // enabling serialization functionality
@@ -1301,7 +1321,6 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Huber: gtsam::noiseModel::mEstimator::Base {
   Huber(double k);
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Huber* Create(double k);
 
   // enabling serialization functionality
@@ -1310,7 +1329,6 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
 
 virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   Tukey(double k);
-  void print(string s) const;
   static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
 
   // enabling serialization functionality
@@ -1322,7 +1340,6 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
 virtual class Robust : gtsam::noiseModel::Base {
   Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
   static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
-  void print(string s) const;
 
   // enabling serialization functionality
   void serializable() const;
@@ -2002,10 +2019,12 @@ virtual class NonlinearOptimizerParams {
   void setVerbosity(string s);
 
   string getLinearSolverType() const;
-
   void setLinearSolverType(string solver);
-  void setOrdering(const gtsam::Ordering& ordering);
+
   void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
+  void setOrdering(const gtsam::Ordering& ordering);
+  string getOrderingType() const;
+  void setOrderingType(string ordering);
 
   bool isMultifrontal() const;
   bool isSequential() const;
@@ -2026,15 +2045,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
 virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
   LevenbergMarquardtParams();
 
-  double getlambdaInitial() const;
+  bool getDiagonalDamping() const;
   double getlambdaFactor() const;
+  double getlambdaInitial() const;
+  double getlambdaLowerBound() const;
   double getlambdaUpperBound() const;
+  bool getUseFixedLambdaFactor();
+  string getLogFile() const;
   string getVerbosityLM() const;
-
+  
-  void setlambdaInitial(double value);
+  void setDiagonalDamping(bool flag);
   void setlambdaFactor(double value);
+  void setlambdaInitial(double value);
+  void setlambdaLowerBound(double value);
   void setlambdaUpperBound(double value);
+  void setUseFixedLambdaFactor(bool flag);
+  void setLogFile(string s);
   void setVerbosityLM(string s);
+
+  static gtsam::LevenbergMarquardtParams LegacyDefaults();
+  static gtsam::LevenbergMarquardtParams CeresDefaults();
+
+  static gtsam::LevenbergMarquardtParams EnsureHasOrdering(
+      gtsam::LevenbergMarquardtParams params,
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::LevenbergMarquardtParams ReplaceOrdering(
+      gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering);
 };
 
 #include <gtsam/nonlinear/DoglegOptimizer.h>
@@ -2480,6 +2516,36 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
 
+// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+class BetweenFactorPose3s
+{
+  size_t size() const;
+  gtsam::BetweenFactorPose3* at(size_t i) const;
+};
+
+#include <gtsam/slam/InitializePose3.h>
+class InitializePose3 {
+  static gtsam::Values computeOrientationsChordal(
+      const gtsam::NonlinearFactorGraph& pose3Graph);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess);
+  static gtsam::NonlinearFactorGraph buildPose3graph(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initializeOrientations(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
+                                  const gtsam::Values& givenGuess,
+                                  bool useGradient);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
+};
+
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,

gtsam/nonlinear/LevenbergMarquardtParams.h

@@ -122,24 +122,35 @@ public:
   virtual ~LevenbergMarquardtParams() {}
   void print(const std::string& str = "") const override;
 
-  /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++.
+  /// @name Getters/Setters, mainly for wrappers. Use fields above in C++.
   /// @{
   bool getDiagonalDamping() const { return diagonalDamping; }
   double getlambdaFactor() const { return lambdaFactor; }
   double getlambdaInitial() const { return lambdaInitial; }
   double getlambdaLowerBound() const { return lambdaLowerBound; }
   double getlambdaUpperBound() const { return lambdaUpperBound; }
+  bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; }
   std::string getLogFile() const { return logFile; }
   std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);}
+  
   void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
   void setlambdaFactor(double value) { lambdaFactor = value; }
   void setlambdaInitial(double value) { lambdaInitial = value; }
   void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
   void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
-  void setLogFile(const std::string& s) { logFile = s; }
   void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;}
+  void setLogFile(const std::string& s) { logFile = s; }
   void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
   // @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  boost::shared_ptr<NonlinearOptimizerParams> clone() const {
+    return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this));
+  }
+
+  /// @}
 };
 
 }

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -54,47 +54,24 @@ public:
   }
   virtual void print(const std::string& str = "") const;
 
-  size_t getMaxIterations() const {
+  size_t getMaxIterations() const { return maxIterations; }
-    return maxIterations;
+  double getRelativeErrorTol() const { return relativeErrorTol; }
-  }
+  double getAbsoluteErrorTol() const { return absoluteErrorTol; }
-  double getRelativeErrorTol() const {
+  double getErrorTol() const { return errorTol; }
-    return relativeErrorTol;
+  std::string getVerbosity() const { return verbosityTranslator(verbosity); }
-  }
-  double getAbsoluteErrorTol() const {
-    return absoluteErrorTol;
-  }
-  double getErrorTol() const {
-    return errorTol;
-  }
-  std::string getVerbosity() const {
-    return verbosityTranslator(verbosity);
-  }
 
-  void setMaxIterations(int value) {
+  void setMaxIterations(int value) { maxIterations = value; }
-    maxIterations = value;
+  void setRelativeErrorTol(double value) { relativeErrorTol = value; }
-  }
+  void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
-  void setRelativeErrorTol(double value) {
+  void setErrorTol(double value) { errorTol = value; }
-    relativeErrorTol = value;
+  void setVerbosity(const std::string& src) {
-  }
-  void setAbsoluteErrorTol(double value) {
-    absoluteErrorTol = value;
-  }
-  void setErrorTol(double value) {
-    errorTol = value;
-  }
-  void setVerbosity(const std::string &src) {
     verbosity = verbosityTranslator(src);
   }
 
   static Verbosity verbosityTranslator(const std::string &s) ;
   static std::string verbosityTranslator(Verbosity value) ;
 
-  // Successive Linearization Parameters
-
-public:
-
   /** See NonlinearOptimizerParams::linearSolverType */
-
   enum LinearSolverType {
     MULTIFRONTAL_CHOLESKY,
     MULTIFRONTAL_QR,
@@ -168,13 +145,9 @@ public:
 
 private:
   std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
-
   LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
-
   std::string orderingTypeTranslator(Ordering::OrderingType type) const;
-
   Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
-
 };
 
 // For backward compatibility:

gtsam/slam/InitializePose3.cpp

@@ -16,7 +16,7 @@
  *  @date   August, 2014
  */
 
-#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/InitializePose3.h> 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -29,69 +29,65 @@
 using namespace std;
 
 namespace gtsam {
-namespace InitializePose3 {
 
-static const Matrix I9 = I_9x9;
+static const Key kAnchorKey = symbol('Z', 9999999);
-static const Vector zero9 = Vector::Zero(9);
-static const Matrix zero33 = Z_3x3;
-
-static const Key keyAnchor = symbol('Z', 9999999);
 
 /* ************************************************************************* */
-GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
+GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
   GaussianFactorGraph linearGraph;
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
 
-  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
+  for(const auto& factor: g) {
     Matrix3 Rij;
+    double rotationPrecision = 1.0;
 
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    if (pose3Between){
-    if (pose3Between)
       Rij = pose3Between->measured().rotation().matrix();
-    else
+      Vector precisions = Vector::Zero(6);
-      std::cout << "Error in buildLinearOrientationGraph" << std::endl;
+      precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
+      pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
+      rotationPrecision = precisions[0]; // rotations first
+    }else{
+      cout << "Error in buildLinearOrientationGraph" << endl;
+    }
 
-    const KeyVector& keys = factor->keys();
+    const auto& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
     Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = Rij;
     M9.block(3,3,3,3) = Rij;
     M9.block(6,6,3,3) = Rij;
-    linearGraph.add(key1, -I9, key2, M9, zero9, model);
+    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
   }
   // prior on the anchor orientation
-  linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0).finished(), model);
+  linearGraph.add(
+      kAnchorKey, I_9x9,
+      (Vector(9) << 1.0, 0.0, 0.0, /*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0)
+          .finished(),
+          noiseModel::Isotropic::Precision(9, 1));
   return linearGraph;
 }
 
 /* ************************************************************************* */
 // Transform VectorValues into valid Rot3
-Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
+Values InitializePose3::normalizeRelaxedRotations(
+    const VectorValues& relaxedRot3) {
   gttic(InitializePose3_computeOrientationsChordal);
 
   Matrix ppm = Z_3x3; // plus plus minus
   ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
 
   Values validRot3;
-  for(const VectorValues::value_type& it: relaxedRot3) {
+  for(const auto& it: relaxedRot3) {
     Key key = it.first;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
-      const Vector& rotVector = it.second;
+      Matrix3 R; R << it.second;
-      Matrix3 rotMat;
-      rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
-      rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
-      rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
 
       Matrix U, V; Vector s;
-      svd(rotMat, U, s, V);
+      svd(R.transpose(), U, s, V);
       Matrix3 normalizedRotMat = U * V.transpose();
 
-      //      std::cout << "rotMat \n" << rotMat << std::endl;
-      //      std::cout << "U V' \n" << U * V.transpose() << std::endl;
-      //      std::cout << "V \n" << V << std::endl;
-
       if(normalizedRotMat.determinant() < 0)
         normalizedRotMat = U * ppm * V.transpose();
 
@@ -103,32 +99,29 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
 }
 
 /* ************************************************************************* */
-// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
+NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
-NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
   gttic(InitializePose3_buildPose3graph);
   NonlinearFactorGraph pose3Graph;
 
-  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
+  for(const auto& factor: graph) {
 
     // recast to a between on Pose3
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between)
       pose3Graph.add(pose3Between);
 
     // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
-    boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
+    const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
     if (pose3Prior)
-      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
+      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
               pose3Prior->prior(), pose3Prior->noiseModel());
   }
   return pose3Graph;
 }
 
 /* ************************************************************************* */
-// Return the orientations of a graph including only BetweenFactors<Pose3>
+Values InitializePose3::computeOrientationsChordal(
-Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
+    const NonlinearFactorGraph& pose3Graph) {
   gttic(InitializePose3_computeOrientationsChordal);
 
   // regularize measurements and plug everything in a factor graph
@@ -142,14 +135,15 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
 }
 
 /* ************************************************************************* */
-// Return the orientations of a graph including only BetweenFactors<Pose3>
+Values InitializePose3::computeOrientationsGradient(
-Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
+    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+    const size_t maxIter, const bool setRefFrame) {
   gttic(InitializePose3_computeOrientationsGradient);
 
   // this works on the inverse rotations, according to Tron&Vidal,2011
   Values inverseRot;
-  inverseRot.insert(keyAnchor, Rot3());
+  inverseRot.insert(kAnchorKey, Rot3());
-  for(const Values::ConstKeyValuePair& key_value: givenGuess) {
+  for(const auto& key_value: givenGuess) {
     Key key = key_value.key;
     const Pose3& pose = givenGuess.at<Pose3>(key);
     inverseRot.insert(key, pose.rotation().inverse());
@@ -164,9 +158,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   // calculate max node degree & allocate gradient
   size_t maxNodeDeg = 0;
   VectorValues grad;
-  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+  for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    grad.insert(key,Vector3::Zero());
+    grad.insert(key,Z_3x1);
     size_t currNodeDeg = (adjEdgesMap.at(key)).size();
     if(currNodeDeg > maxNodeDeg)
       maxNodeDeg = currNodeDeg;
@@ -180,42 +174,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   double mu_max = maxNodeDeg * rho;
   double stepsize = 2/mu_max; // = 1/(a b dG)
 
-  std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
   double maxGrad;
   // gradient iterations
   size_t it;
-  for(it=0; it < maxIter; it++){
+  for (it = 0; it < maxIter; it++) {
     //////////////////////////////////////////////////////////////////////////
     // compute the gradient at each node
-    //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
-    //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
     maxGrad = 0;
-    for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+    for (const auto& key_value : inverseRot) {
       Key key = key_value.key;
-      //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
+      Vector gradKey = Z_3x1;
-      Vector gradKey = Vector3::Zero();
       // collect the gradient for each edge incident on key
-      for(const size_t& factorId: adjEdgesMap.at(key)){
+      for (const size_t& factorId : adjEdgesMap.at(key)) {
         Rot3 Rij = factorId2RotMap.at(factorId);
         Rot3 Ri = inverseRot.at<Rot3>(key);
-        if( key == (pose3Graph.at(factorId))->keys()[0] ){
+        auto factor = pose3Graph.at(factorId);
-          Key key1 = (pose3Graph.at(factorId))->keys()[1];
+        const auto& keys = factor->keys();
+        if (key == keys[0]) {
+          Key key1 = keys[1];
           Rot3 Rj = inverseRot.at<Rot3>(key1);
-          gradKey = gradKey + gradientTron(Ri, Rij  * Rj, a, b);
+          gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
-          //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij  * Rj, a, b) \n " << gradientTron(Ri, Rij  * Rj, a, b) << std::endl;
+        } else if (key == keys[1]) {
-        }else if( key == (pose3Graph.at(factorId))->keys()[1] ){
+          Key key0 = keys[0];
-          Key key0 = (pose3Graph.at(factorId))->keys()[0];
           Rot3 Rj = inverseRot.at<Rot3>(key0);
           gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
-          //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse()  * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
+        } else {
-        }else{
+          cout << "Error in gradient computation" << endl;
-          std::cout << "Error in gradient computation" << std::endl;
         }
-      } // end of i-th gradient computation
+      }  // end of i-th gradient computation
-      grad.at(key) = stepsize *  gradKey;
+      grad.at(key) = stepsize * gradKey;
 
       double normGradKey = (gradKey).norm();
-      //std::cout << "key  " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
       if(normGradKey>maxGrad)
         maxGrad = normGradKey;
     } // end of loop over nodes
@@ -230,14 +219,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
       break;
   } // enf of gradient iterations
 
-  std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl;
-
   // Return correct rotations
-  const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
+  const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
   Values estimateRot;
-  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+  for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
       const Rot3& R = inverseRot.at<Rot3>(key);
       if(setRefFrame)
         estimateRot.insert(key, Rref.compose(R.inverse()));
@@ -249,11 +236,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
 }
 
 /* ************************************************************************* */
-void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
+void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+                         const NonlinearFactorGraph& pose3Graph) {
   size_t factorId = 0;
-  for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
+  for(const auto& factor: pose3Graph) {
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between){
       Rot3 Rij = pose3Between->measured().rotation();
       factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
@@ -275,14 +262,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
         adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
       }
     }else{
-      std::cout << "Error in computeOrientationsGradient" << std::endl;
+      cout << "Error in createSymbolicGraph" << endl;
     }
     factorId++;
   }
 }
 
 /* ************************************************************************* */
-Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
+Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
   Vector3 logRot = Rot3::Logmap(R1.between(R2));
 
   double th = logRot.norm();
@@ -292,10 +279,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
     th = logRot.norm();
   }
   // exclude small or invalid rotations
-  if (th > 1e-5 && th == th){ // nonzero valid rotations
+  if (th > 1e-5 && th == th) {  // nonzero valid rotations
     logRot = logRot / th;
-  }else{
+  } else {
-    logRot = Vector3::Zero();
+    logRot = Z_3x1;
     th = 0.0;
   }
 
@@ -304,8 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
 }
 
 /* ************************************************************************* */
-Values initializeOrientations(const NonlinearFactorGraph& graph) {
+Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) {
-
   // We "extract" the Pose3 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
   NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
@@ -315,29 +301,30 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
 }
 
 ///* ************************************************************************* */
-Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
+Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
   gttic(InitializePose3_computePoses);
 
   // put into Values structure
   Values initialPose;
-  for(const Values::ConstKeyValuePair& key_value: initialRot){
+  for (const auto& key_value : initialRot) {
     Key key = key_value.key;
     const Rot3& rot = initialRot.at<Rot3>(key);
     Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
     initialPose.insert(key, initializedPose);
   }
+
   // add prior
   noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
-  initialPose.insert(keyAnchor, Pose3());
+  initialPose.insert(kAnchorKey, Pose3());
-  pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);
+  pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
 
   // Create optimizer
   GaussNewtonParams params;
   bool singleIter = true;
-  if(singleIter){
+  if (singleIter) {
     params.maxIterations = 1;
-  }else{
+  } else {
-    std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl;
+    cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" << endl;
     params.setVerbosity("TERMINATION");
   }
   GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
@@ -345,9 +332,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // put into Values structure
   Values estimate;
-  for(const Values::ConstKeyValuePair& key_value: GNresult) {
+  for (const auto& key_value : GNresult) {
     Key key = key_value.key;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
       const Pose3& pose = GNresult.at<Pose3>(key);
       estimate.insert(key, pose);
     }
@@ -356,22 +343,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 }
 
 /* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph) {
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
+                  bool useGradient) {
   gttic(InitializePose3_initialize);
-
-  // We "extract" the Pose3 subgraph of the original graph: this
-  // is done to properly model priors and avoiding operating on a larger graph
-  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
-
-  // Get orientations from relative orientation measurements
-  Values valueRot3 = computeOrientationsChordal(pose3Graph);
-
-  // Compute the full poses (1 GN iteration on full poses)
-  return computePoses(pose3Graph, valueRot3);
-}
-
-/* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
   Values initialValues;
 
   // We "extract" the Pose3 subgraph of the original graph: this
@@ -380,27 +354,18 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
 
   // Get orientations from relative orientation measurements
   Values orientations;
-  if(useGradient)
+  if (useGradient)
     orientations = computeOrientationsGradient(pose3Graph, givenGuess);
   else
     orientations = computeOrientationsChordal(pose3Graph);
 
-//  orientations.print("orientations\n");
-
   // Compute the full poses (1 GN iteration on full poses)
   return computePoses(pose3Graph, orientations);
-
-  //  for(const Values::ConstKeyValuePair& key_value: orientations) {
-  //    Key key = key_value.key;
-  //    if (key != keyAnchor) {
-  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
-  //      const Rot3& rot = orientations.at<Rot3>(key);
-  //      Pose3 initializedPoses = Pose3(rot, pos);
-  //      initialValues.insert(key, initializedPoses);
-  //    }
-  //  }
-  //  return initialValues;
 }
 
-} // end of namespace lago
+/* ************************************************************************* */
-} // end of namespace gtsam
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph) {
+  return initialize(graph, Values(), false);
+}
+
+} // namespace gtsam

gtsam/slam/InitializePose3.h

@@ -20,40 +20,68 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/graph.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/graph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/geometry/Rot3.h>
 
 namespace gtsam {
 
 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
-typedef std::map<Key, Rot3 > KeyRotMap;
+typedef std::map<Key, Rot3> KeyRotMap;
 
-namespace InitializePose3 {
+struct GTSAM_EXPORT InitializePose3 {
+  static GaussianFactorGraph buildLinearOrientationGraph(
+      const NonlinearFactorGraph& g);
 
-GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g);
+  static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
 
-GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
+  /**
+   * Return the orientations of a graph including only BetweenFactors<Pose3>
+   */
+  static Values computeOrientationsChordal(
+      const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
+  /**
+   * Return the orientations of a graph including only BetweenFactors<Pose3>
+   */
+  static Values computeOrientationsGradient(
+      const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+      size_t maxIter = 10000, const bool setRefFrame = true);
 
-GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
+  static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
-    const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
+                                  KeyRotMap& factorId2RotMap,
+                                  const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+  static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
-    const NonlinearFactorGraph& pose3Graph);
+                              const double b);
 
-GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b);
+  /**
+   * Select the subgraph of betweenFactors and transforms priors into between
+   * wrt a fictitious node
+   */
+  static NonlinearFactorGraph buildPose3graph(
+      const NonlinearFactorGraph& graph);
 
-GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
+  static Values computePoses(NonlinearFactorGraph& pose3graph,
+                             Values& initialRot);
 
-GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot);
+  /**
+   * "extract" the Pose3 subgraph of the original graph, get orientations from
+   * relative orientation measurements using chordal method.
+   */
+  static Values initializeOrientations(const NonlinearFactorGraph& graph);
 
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
+  /**
+   * "extract" the Pose3 subgraph of the original graph, get orientations from
+   * relative orientation measurements (using either gradient or chordal
+   * method), and finish up with 1 GN iteration on full poses.
+   */
+  static Values initialize(const NonlinearFactorGraph& graph,
+                           const Values& givenGuess, bool useGradient = false);
 
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
+  /// Calls initialize above using Chordal method.
-
+  static Values initialize(const NonlinearFactorGraph& graph);
-} // end of namespace lago
+};
-} // end of namespace gtsam
+}  // end of namespace gtsam

gtsam/slam/dataset.cpp

@@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
 
 /* ************************************************************************* */
 GraphAndValues readG2o(const string& g2oFile, const bool is3D,
-    KernelFunctionType kernelFunctionType) {
+                       KernelFunctionType kernelFunctionType) {
-  // just call load2D
+  if (is3D) {
-  int maxID = 0;
-  bool addNoise = false;
-  bool smart = true;
-
-  if(is3D)
     return load3D(g2oFile);
-
+  } else {
-  return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+    // just call load2D
-      NoiseFormatG2O, kernelFunctionType);
+    int maxID = 0;
+    bool addNoise = false;
+    bool smart = true;
+    return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+                  NoiseFormatG2O, kernelFunctionType);
+  }
 }
 
 /* ************************************************************************* */
@@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
 }
 
 /* ************************************************************************* */
-GraphAndValues load3D(const string& filename) {
+std::map<Key, Pose3> parse3DPoses(const string& filename) {
-
   ifstream is(filename.c_str());
   if (!is)
-    throw invalid_argument("load3D: can not find file " + filename);
+    throw invalid_argument("parse3DPoses: can not find file " + filename);
-
-  Values::shared_ptr initial(new Values);
-  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
 
+  std::map<Key, Pose3> poses;
   while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
@@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) {
       Key id;
       double x, y, z, roll, pitch, yaw;
       ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
+      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
     }
     if (tag == "VERTEX_SE3:QUAT") {
       Key id;
       double x, y, z, qx, qy, qz, qw;
       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
+      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
     }
   }
-  is.clear(); /* clears the end-of-file and error flags */
+  return poses;
-  is.seekg(0, ios::beg);
+}
 
+/* ************************************************************************* */
+BetweenFactorPose3s parse3DFactors(const string& filename) {
+  ifstream is(filename.c_str());
+  if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
+
+  std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
@@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) {
       Key id1, id2;
       double x, y, z, roll, pitch, yaw;
       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
+      Matrix m(6, 6);
-      Point3 t = Point3(x, y, z);
+      for (size_t i = 0; i < 6; i++)
-      Matrix m = I_6x6;
+        for (size_t j = i; j < 6; j++) ls >> m(i, j);
-      for (int i = 0; i < 6; i++)
-        for (int j = i; j < 6; j++)
-          ls >> m(i, j);
       SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-      NonlinearFactor::shared_ptr factor(
+      factors.emplace_back(new BetweenFactor<Pose3>(
-          new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
+          id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
-      graph->push_back(factor);
     }
     if (tag == "EDGE_SE3:QUAT") {
-      Matrix m = I_6x6;
       Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
+      Matrix m(6, 6);
-      Point3 t = Point3(x, y, z);
+      for (size_t i = 0; i < 6; i++) {
-      for (int i = 0; i < 6; i++){
+        for (size_t j = i; j < 6; j++) {
-        for (int j = i; j < 6; j++){
           double mij;
           ls >> mij;
           m(i, j) = mij;
           m(j, i) = mij;
         }
       }
-      Matrix mgtsam = I_6x6;
+      Matrix mgtsam(6, 6);
 
-      mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
+      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
-      mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
+      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
-      mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
+      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
-      mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
+      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
-      NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
+      factors.emplace_back(new BetweenFactor<Pose3>(
-      graph->push_back(factor);
+          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
     }
   }
+  return factors;
+}
+
+/* ************************************************************************* */
+GraphAndValues load3D(const string& filename) {
+  const auto factors = parse3DFactors(filename);
+  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
+  for (const auto& factor : factors) {
+    graph->push_back(factor);
+  }
+
+  const auto poses = parse3DPoses(filename);
+  Values::shared_ptr initial(new Values);
+  for (const auto& key_pose : poses) {
+    initial->insert(key_pose.first, key_pose.second);
+  }
+
   return make_pair(graph, initial);
 }
 

gtsam/slam/dataset.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
@@ -34,6 +35,7 @@
 #include <utility> // for pair
 #include <vector>
 #include <iosfwd>
+#include <map>
 
 namespace gtsam {
 
@@ -153,9 +155,14 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
 GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
     const Values& estimate, const std::string& filename);
 
-/**
+/// Parse edges in 3D TORO graph file into a set of BetweenFactors.
- * Load TORO 3D Graph
+using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
- */
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
+
+/// Parse vertices in 3D TORO graph file into a map of Pose3s.
+GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
+
+/// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
 /// A measurement with its camera index

gtsam/slam/tests/testDataset.cpp

@@ -16,8 +16,8 @@
  */
 
 
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -162,65 +162,74 @@ TEST( dataSet, readG2o)
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2o3D)
+TEST(dataSet, readG2o3D) {
-{
   const string g2oFile = findExampleDataFile("pose3example");
+  auto model = noiseModel::Isotropic::Precision(6, 10000);
+
+  // Initialize 6 relative measurements with quaternion/point3 values:
+  const std::vector<Pose3> relative_poses = {
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.105373, 0.311512, 0.656877, -0.678505},
+       {0.523923, 0.776654, 0.326659}},
+      {{0.568551, 0.595795, -0.561677, 0.079353},
+       {0.910927, 0.055169, -0.411761}},
+      {{0.542221, -0.592077, 0.303380, -0.513226},
+       {0.775288, 0.228798, -0.596923}},
+      {{0.327419, -0.125250, -0.534379, 0.769122},
+       {-0.577841, 0.628016, -0.543592}},
+      {{0.083672, 0.104639, 0.627755, 0.766795},
+       {-0.623267, 0.086928, 0.773222}},
+  };
+
+  // Initialize 5 poses with quaternion/point3 values:
+  const std::vector<Pose3> poses = {
+      {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.421446, -0.351729, -0.597838, 0.584174},
+       {1.993500, 0.023275, 0.003793}},
+      {{0.067024, 0.331798, -0.200659, 0.919323},
+       {2.004291, 1.024305, 0.018047}},
+      {{0.765488, -0.035697, -0.462490, 0.445933},
+       {0.999908, 1.055073, 0.020212}},
+  };
+
+  // Specify connectivity:
+  using KeyPair = pair<Key, Key>;
+  std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
+
+  // Created expected factor graph
+  size_t i = 0;
+  NonlinearFactorGraph expectedGraph;
+  for (const auto& keys : edges) {
+    expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
+        keys.first, keys.second, relative_poses[i++], model);
+  }
+
+  // Check factor parsing
+  const auto actualFactors = parse3DFactors(g2oFile);
+  for (size_t i : {0, 1, 2, 3, 4, 5}) {
+    EXPECT(assert_equal(
+        *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
+        *actualFactors[i], 1e-5));
+  }
+
+  // Check pose parsing
+  const auto actualPoses = parse3DPoses(g2oFile);
+  for (size_t j : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
+  }
+
+  // Check graph version
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
   bool is3D = true;
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
-
+  EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
-  Values expectedValues;
+  for (size_t j : {0, 1, 2, 3, 4}) {
-  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
+    EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
-  Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
+  }
-  expectedValues.insert(0, Pose3(R0, p0));
-
-  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
-  expectedValues.insert(1, Pose3(R1, p1));
-
-  Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
-  Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
-  expectedValues.insert(2, Pose3(R2, p2));
-
-  Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
-  Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
-  expectedValues.insert(3, Pose3(R3, p3));
-
-  Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
-  Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
-  expectedValues.insert(4, Pose3(R4, p4));
-
-  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
-
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished());
-  NonlinearFactorGraph expectedGraph;
-
-  Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
-  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
-
-  Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
-  Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, Pose3(R12,p12), model);
-
-  Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
-  Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, Pose3(R23,p23), model);
-
-  Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
-  Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, Pose3(R34,p34), model);
-
-  Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
-  Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 4, Pose3(R14,p14), model);
-
-  Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
-  Rot3 R30 = Rot3::Quaternion(0.083672 ,  0.104639, 0.627755, 0.766795  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 0, Pose3(R30,p30), model);
-
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */

gtsam/slam/tests/testInitializePose3.cpp

@@ -70,6 +70,17 @@ NonlinearFactorGraph graph() {
   g.add(PriorFactor<Pose3>(x0, pose0, model));
   return g;
 }
+
+NonlinearFactorGraph graph2() {
+  NonlinearFactorGraph g;
+  g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
+  g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
+  g.add(PriorFactor<Pose3>(x0, pose0, model));
+  return g;
+}
 }
 
 /* *************************************************************************** */
@@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) {
   EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
 }
 
+/* *************************************************************************** */
+TEST( InitializePose3, orientationsPrecisions ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2());
+
+  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
+  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
+  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
+  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
+}
+
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
   NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());

gtsam_unstable/examples/FixedLagSmootherExample.cpp

@@ -111,23 +111,27 @@ int main(int argc, char** argv) {
     noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
     newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
 
-    // Update the smoothers with the new factors
+    // Update the smoothers with the new factors. In this example, batch smoother needs one iteration
-    smootherBatch.update(newFactors, newValues, newTimestamps);
+    // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when
-    smootherISAM2.update(newFactors, newValues, newTimestamps);
+    // both are ready for simplicity.
-    for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
+    if (time >= 0.50) {
-      smootherISAM2.update();
+      smootherBatch.update(newFactors, newValues, newTimestamps);
+      smootherISAM2.update(newFactors, newValues, newTimestamps);
+      for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
+          smootherISAM2.update();
+      }
+
+      // Print the optimized current pose
+      cout << setprecision(5) << "Timestamp = " << time << endl;
+      smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
+      smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
+      cout << endl;
+
+      // Clear contains for the next iteration
+      newTimestamps.clear();
+      newValues.clear();
+      newFactors.resize(0);
     }
-
-    // Print the optimized current pose
-    cout << setprecision(5) << "Timestamp = " << time << endl;
-    smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
-    smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
-    cout << endl;
-
-    // Clear contains for the next iteration
-    newTimestamps.clear();
-    newValues.clear();
-    newFactors.resize(0);
   }
 
   // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds

gtsam_unstable/gtsam_unstable.h

@@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
 //*************************************************************************
 // nonlinear
 //*************************************************************************
-// #include <gtsam_unstable/nonlinear/sequentialSummarization.h>
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-// gtsam::GaussianFactorGraph* summarizeGraphSequential(
+class FixedLagSmootherKeyTimestampMapValue {
-//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
+  FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
-// gtsam::GaussianFactorGraph* summarizeGraphSequential(
+  FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
-//     const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
+};
 
-// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+class FixedLagSmootherKeyTimestampMap {
-// class FixedLagSmootherKeyTimestampMapValue {
+  FixedLagSmootherKeyTimestampMap();
-//   FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);
+  FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
-//   FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
+
-// };
+  // Note: no print function
-// 
+
-// class FixedLagSmootherKeyTimestampMap {
+  // common STL methods
-//   FixedLagSmootherKeyTimestampMap();
+  size_t size() const;
-//   FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
+  bool empty() const;
-// 
+  void clear();
-//   // Note: no print function
+
-// 
+  double at(const size_t key) const;
-//   // common STL methods
+  void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
-//   size_t size() const;
+};
-//   bool empty() const;
+
-//   void clear();
+class FixedLagSmootherResult {
-// 
+  size_t getIterations() const;
-//   double at(const gtsam::Key& key) const;
+  size_t getNonlinearVariables() const;
-//   void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
+  size_t getLinearVariables() const;
-// };
+  double getError() const;
-// 
+};
-// class FixedLagSmootherResult {
+
-//   size_t getIterations() const;
+#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
-//   size_t getNonlinearVariables() const;
+virtual class FixedLagSmoother {
-//   size_t getLinearVariables() const;
+  void print(string s) const;
-//   double getError() const;
+  bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
-// };
+
-// 
+  gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
-// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
+  double smootherLag() const;
-// virtual class FixedLagSmoother {
+
-//   void print(string s) const;
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
-//   bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
+  gtsam::Values calculateEstimate() const;
-// 
+};
-//   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
+
-//   double smootherLag() const;
+#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
-// 
+virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
-//   gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
+  BatchFixedLagSmoother();
-//   gtsam::Values calculateEstimate() const;
+  BatchFixedLagSmoother(double smootherLag);
-// };
+  BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
-// 
+
-// #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+  gtsam::LevenbergMarquardtParams params() const;
-// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
+  template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-//   BatchFixedLagSmoother();
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-//   BatchFixedLagSmoother(double smootherLag);
+                     Vector, Matrix}>
-//   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
+  VALUE calculateEstimate(size_t key) const;
-// 
+};
-//   gtsam::LevenbergMarquardtParams params() const;
+
-// };
+#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-// 
+virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
-// #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+  IncrementalFixedLagSmoother();
-// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
+  IncrementalFixedLagSmoother(double smootherLag);
-//   IncrementalFixedLagSmoother();
+  IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
-//   IncrementalFixedLagSmoother(double smootherLag);
+
-//   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
+  gtsam::ISAM2Params params() const;
-// 
+};
-//   gtsam::ISAM2Params params() const;
+
-// };
+#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
-// 
+virtual class ConcurrentFilter {
-// #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
+  void print(string s) const;
-// virtual class ConcurrentFilter {
+  bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
-//   void print(string s) const;
+};
-//   bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
+
-// };
+virtual class ConcurrentSmoother {
-// 
+  void print(string s) const;
-// virtual class ConcurrentSmoother {
+  bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
-//   void print(string s) const;
+};
-//   bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
+
-// };
+// Synchronize function
-// 
+void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
-// // Synchronize function
+
-// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
+#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
-// 
+class ConcurrentBatchFilterResult {
-// #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
+  size_t getIterations() const;
-// class ConcurrentBatchFilterResult {
+  size_t getLambdas() const;
-//   size_t getIterations() const;
+  size_t getNonlinearVariables() const;
-//   size_t getLambdas() const;
+  size_t getLinearVariables() const;
-//   size_t getNonlinearVariables() const;
+  double getError() const;
-//   size_t getLinearVariables() const;
+};
-//   double getError() const;
+
-// };
+virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
-// 
+  ConcurrentBatchFilter();
-// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
+  ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
-//   ConcurrentBatchFilter();
+
-//   ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
+  gtsam::NonlinearFactorGraph getFactors() const;
-// 
+  gtsam::Values getLinearizationPoint() const;
-//   gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::Ordering getOrdering() const;
-//   gtsam::Values getLinearizationPoint() const;
+  gtsam::VectorValues getDelta() const;
-//   gtsam::Ordering getOrdering() const;
+
-//   gtsam::VectorValues getDelta() const;
+  gtsam::ConcurrentBatchFilterResult update();
-// 
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
-//   gtsam::ConcurrentBatchFilterResult update();
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
+  gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+  gtsam::Values calculateEstimate() const;
-//   gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
+};
-//   gtsam::Values calculateEstimate() const;
+
-// };
+#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
-// 
+class ConcurrentBatchSmootherResult {
-// #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
+  size_t getIterations() const;
-// class ConcurrentBatchSmootherResult {
+  size_t getLambdas() const;
-//   size_t getIterations() const;
+  size_t getNonlinearVariables() const;
-//   size_t getLambdas() const;
+  size_t getLinearVariables() const;
-//   size_t getNonlinearVariables() const;
+  double getError() const;
-//   size_t getLinearVariables() const;
+};
-//   double getError() const;
+
-// };
+virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
-// 
+  ConcurrentBatchSmoother();
-// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
+  ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
-//   ConcurrentBatchSmoother();
+
-//   ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
+  gtsam::NonlinearFactorGraph getFactors() const;
-// 
+  gtsam::Values getLinearizationPoint() const;
-//   gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::Ordering getOrdering() const;
-//   gtsam::Values getLinearizationPoint() const;
+  gtsam::VectorValues getDelta() const;
-//   gtsam::Ordering getOrdering() const;
+
-//   gtsam::VectorValues getDelta() const;
+  gtsam::ConcurrentBatchSmootherResult update();
-// 
+  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
-//   gtsam::ConcurrentBatchSmootherResult update();
+  gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
+  gtsam::Values calculateEstimate() const;
-//   gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
+};
-//   gtsam::Values calculateEstimate() const;
-// };
 
 //*************************************************************************
 // slam