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

.gitignore

@@ -1,4 +1,5 @@
 /build*
+/debug*
 .idea
 *.pyc
 *.DS_Store

CMakeLists.txt

@@ -84,6 +84,12 @@ set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build
 if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP)
 	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
 endif()
+if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
+    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE}_POSTFIX})
+    if(NOT "${CURRENT_POSTFIX}" STREQUAL "")
+        message(FATAL_ERROR "Cannot use executable postfixes with the matlab or cython wrappers. Please disable GTSAM_BUILD_TYPE_POSTFIXES")
+    endif()
+endif()
 if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
 	message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
 endif()
@@ -130,6 +136,14 @@ if(MSVC)
 	endif()
 endif()
 
+# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() 
+# or explicit instantiation will generate build errors. 
+# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
+#
+if(MSVC AND BUILD_SHARED_LIBS)
+	list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
+endif()
+
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
 set(BOOST_FIND_MINIMUM_VERSION 1.43)
 set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
@@ -142,22 +156,43 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE
   message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
 endif()
 
-option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
 # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
+option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
+
+# JLBC: This was once updated to target-based names (Boost::xxx), but it caused
+# problems with Boost versions newer than FindBoost.cmake was prepared to handle,
+# so we downgraded this to classic filenames-based variables, and manually adding
+# the target_include_directories(xxx ${Boost_INCLUDE_DIR})
 set(GTSAM_BOOST_LIBRARIES
-  Boost::serialization
+  optimized
-  Boost::system
+    ${Boost_SERIALIZATION_LIBRARY_RELEASE}
-  Boost::filesystem
+    ${Boost_SYSTEM_LIBRARY_RELEASE}
-  Boost::thread
+    ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-  Boost::date_time
+    ${Boost_THREAD_LIBRARY_RELEASE}
-  Boost::regex
+    ${Boost_DATE_TIME_LIBRARY_RELEASE}
+    ${Boost_REGEX_LIBRARY_RELEASE}
+  debug
+    ${Boost_SERIALIZATION_LIBRARY_DEBUG}
+    ${Boost_SYSTEM_LIBRARY_DEBUG}
+    ${Boost_FILESYSTEM_LIBRARY_DEBUG}
+    ${Boost_THREAD_LIBRARY_DEBUG}
+    ${Boost_DATE_TIME_LIBRARY_DEBUG}
+    ${Boost_REGEX_LIBRARY_DEBUG}
 )
+message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}")
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
     list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
-      list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
+      list(APPEND GTSAM_BOOST_LIBRARIES
+        optimized
+          ${Boost_TIMER_LIBRARY_RELEASE}
+          ${Boost_CHRONO_LIBRARY_RELEASE}
+        debug
+          ${Boost_TIMER_LIBRARY_DEBUG}
+          ${Boost_CHRONO_LIBRARY_DEBUG}
+        )
     else()
       list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
       message("WARNING:  GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")

CppUnitLite/CMakeLists.txt

@@ -6,7 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
 add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
+target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h
 
 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 

CppUnitLite/Failure.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

CppUnitLite/Test.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -36,7 +36,7 @@ Test *Test::getNext() const
 }
 
 void Test::setNext(Test *test)
-{  
+{
   next_ = test;
 }
 
@@ -46,9 +46,9 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri
     return true;
   result.addFailure (
     Failure (
-      name_, 
+      name_,
       boost::lexical_cast<std::string> (__FILE__),
-      __LINE__, 
+      __LINE__,
       boost::lexical_cast<std::string> (expected),
       boost::lexical_cast<std::string> (actual)));
 
@@ -63,10 +63,10 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes
     return true;
   result.addFailure (
     Failure (
-      name_, 
+      name_,
       boost::lexical_cast<std::string> (__FILE__),
-      __LINE__, 
+      __LINE__,
-      expected, 
+      expected,
       actual));
 
   return false;

CppUnitLite/Test.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -12,7 +12,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 //
 // TEST.H
-// 
+//
 // This file contains the Test class along with the macros which make effective
 // in the harness.
 //
@@ -66,7 +66,7 @@ protected:
             virtual ~testGroup##testName##Test () {};\
             void run (TestResult& result_);} \
     testGroup##testName##Instance; \
-  void testGroup##testName##Test::run (TestResult& result_) 
+  void testGroup##testName##Test::run (TestResult& result_)
 
 /**
  * Declare friend in a class to test its private methods

CppUnitLite/TestHarness.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -12,7 +12,7 @@
 ///////////////////////////////////////////////////////////////////////////////
 //
 // TESTHARNESS.H
-// 
+//
 // The primary include file for the framework.
 //
 ///////////////////////////////////////////////////////////////////////////////

CppUnitLite/TestRegistry.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -18,26 +18,26 @@
 #include "TestResult.h"
 #include "TestRegistry.h"
 
-void TestRegistry::addTest (Test *test) 
+void TestRegistry::addTest (Test *test)
 {
   instance ().add (test);
 }
 
 
-int TestRegistry::runAllTests (TestResult& result) 
+int TestRegistry::runAllTests (TestResult& result)
 {
   return instance ().run (result);
 }
 
 
-TestRegistry& TestRegistry::instance () 
+TestRegistry& TestRegistry::instance ()
 {
   static TestRegistry registry;
   return registry;
 }
 
 
-void TestRegistry::add (Test *test) 
+void TestRegistry::add (Test *test)
 {
   if (tests == 0) {
     test->setNext(0);
@@ -52,7 +52,7 @@ void TestRegistry::add (Test *test)
 }
 
 
-int TestRegistry::run (TestResult& result) 
+int TestRegistry::run (TestResult& result)
 {
   result.testsStarted ();
 

CppUnitLite/TestRegistry.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -12,9 +12,9 @@
 ///////////////////////////////////////////////////////////////////////////////
 //
 // TESTREGISTRY.H
-// 
+//
-// TestRegistry is a singleton collection of all the tests to run in a system.  
+// TestRegistry is a singleton collection of all the tests to run in a system.
-// 
+//
 ///////////////////////////////////////////////////////////////////////////////
 
 #ifndef TESTREGISTRY_H
@@ -38,7 +38,7 @@ private:
   void          add (Test *test);
   int           run (TestResult& result);
 
-  
+
   Test          *tests;
   Test          *lastTest;
 

CppUnitLite/TestResult.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -22,12 +22,12 @@ TestResult::TestResult ()
 }
 
 
-void TestResult::testsStarted () 
+void TestResult::testsStarted ()
 {
 }
 
 
-void TestResult::addFailure (const Failure& failure) 
+void TestResult::addFailure (const Failure& failure)
 {
   if (failure.lineNumber < 0) // allow for no line number
     fprintf (stdout, "%s%s%s%s\n",
@@ -48,7 +48,7 @@ void TestResult::addFailure (const Failure& failure)
 }
 
 
-void TestResult::testsEnded () 
+void TestResult::testsEnded ()
 {
   if (failureCount > 0)
     fprintf (stdout, "There were %d failures\n", failureCount);

CppUnitLite/TestResult.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -12,10 +12,10 @@
 ///////////////////////////////////////////////////////////////////////////////
 //
 // TESTRESULT.H
-// 
+//
 // A TestResult is a collection of the history of some test runs.  Right now
 // it just collects failures.
-// 
+//
 ///////////////////////////////////////////////////////////////////////////////
 
 #ifndef TESTRESULT_H
@@ -26,12 +26,12 @@ class Failure;
 class TestResult
 {
 public:
-          TestResult (); 
+          TestResult ();
   virtual ~TestResult() {};
   virtual void  testsStarted ();
   virtual void  addFailure (const Failure& failure);
   virtual void  testsEnded ();
-  
+
   int getFailureCount() {return failureCount;}
 
 private:

INSTALL.md

@@ -166,3 +166,30 @@ Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks an
 NOTE:  The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG.  MacPorts g++ compilers do work with it though.
 
 NOTE:  If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen.
+
+
+## Installing MKL on Linux
+
+Intel has a guide for installing MKL on Linux through APT repositories at <https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo>.
+
+After following the instructions, add the following to your `~/.bashrc` (and afterwards, open a new terminal before compiling GTSAM):
+`LD_PRELOAD` need only be set if you are building the cython wrapper to use GTSAM from python.
+```sh
+source /opt/intel/mkl/bin/mklvars.sh intel64
+export LD_PRELOAD="$LD_PRELOAD:/opt/intel/mkl/lib/intel64/libmkl_core.so:/opt/intel/mkl/lib/intel64/libmkl_sequential.so"
+```
+To use MKL in GTSAM pass the flag `-DGTSAM_WITH_EIGEN_MKL=ON` to cmake.
+
+
+The `LD_PRELOAD` fix seems to be related to a well known problem with MKL which leads to lots of undefined symbol errors, for example:
+- <https://software.intel.com/en-us/forums/intel-math-kernel-library/topic/300857>
+- <https://software.intel.com/en-us/forums/intel-distribution-for-python/topic/628976>
+- <https://groups.google.com/a/continuum.io/forum/#!topic/anaconda/J3YGoef64z8>
+
+Failing to specify `LD_PRELOAD` may lead to errors such as:
+`ImportError: /opt/intel/mkl/lib/intel64/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv`
+or
+`Intel MKL FATAL ERROR: Cannot load libmkl_avx2.so or libmkl_def.so.`
+when importing GTSAM using the cython wrapper in python.
+
+

README.md

@@ -36,7 +36,9 @@ Prerequisites:
 Optional prerequisites - used automatically if findable by CMake:
 
 - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
-- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
+- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo))
+    - See [INSTALL.md](INSTALL.md) for more installation information
+    - Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL.
 
 GTSAM 4 Compatibility
 ---------------------

cmake/example_project/CMakeLists.txt

@@ -30,9 +30,12 @@ find_package(GTSAM REQUIRED) # Uses installed package
 ###################################################################################
 # Build static library from common sources
 set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
-add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp)
+add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp)
 target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
 
+# Install library
+install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
+
 ###################################################################################
 # Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
 gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")

cmake/example_project/README.md

@@ -0,0 +1,32 @@
+# MATLAB Wrapper Example Project
+
+This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM.
+
+## Compiling
+
+We follow the regular build procedure inside the `example_project` directory:
+
+```sh
+mkdir build && cd build
+cmake ..
+make -j8
+sudo make install
+
+sudo ldconfig  # ensures the shared object file generated is correctly loaded
+```
+
+## Usage
+
+Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path
+
+```matlab
+addpath('/usr/local/gtsam_toolbox')
+```
+
+At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g.
+
+```matlab
+pe = example.PrintExamples();
+pe.sayHello();
+pe.sayGoodbye();
+```

cmake/example_project/example.h

@@ -18,11 +18,14 @@
 // This is an interface file for automatic MATLAB wrapper generation.  See
 // gtsam.h for full documentation and more examples.
 
+#include <example/PrintExamples.h>
+
 namespace example {
 
 class PrintExamples {
-  void sayHello() const;
+    PrintExamples();
-  void sayGoodbye() const;
+    void sayHello() const;
+    void sayGoodbye() const;
 };
 
 }

cmake/example_project/example/PrintExamples.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include <iostream>
 #include <string>
 
 namespace example {

cython/README.md

@@ -62,7 +62,7 @@ Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey
 - Casting from a base class to a derive class must be done explicitly.
 Examples:
 ```Python
-      noiseBase = factor.get_noiseModel()
+      noiseBase = factor.noiseModel()
       noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
 ```
 

cython/gtsam/examples/DogLegOptimizerExample.py

@@ -0,0 +1,118 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Example comparing DoglegOptimizer with Levenberg-Marquardt.
+Author: Frank Dellaert
+"""
+# pylint: disable=no-member, invalid-name
+
+import math
+import argparse
+
+import gtsam
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def run(args):
+    """Test Dogleg vs LM, inspired by issue #452."""
+
+    # print parameters
+    print("num samples = {}, deltaInitial = {}".format(
+        args.num_samples, args.delta))
+
+    # Ground truth solution
+    T11 = gtsam.Pose2(0, 0, 0)
+    T12 = gtsam.Pose2(1, 0, 0)
+    T21 = gtsam.Pose2(0, 1, 0)
+    T22 = gtsam.Pose2(1, 1, 0)
+
+    # Factor graph
+    graph = gtsam.NonlinearFactorGraph()
+
+    # Priors
+    prior = gtsam.noiseModel_Isotropic.Sigma(3, 1)
+    graph.add(gtsam.PriorFactorPose2(11, T11, prior))
+    graph.add(gtsam.PriorFactorPose2(21, T21, prior))
+
+    # Odometry
+    model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3]))
+    graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model))
+    graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model))
+
+    # Range
+    model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01)
+    graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho))
+
+    params = gtsam.DoglegParams()
+    params.setDeltaInitial(args.delta)  # default is 10
+
+    # Add progressively more noise to ground truth
+    sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20]
+    n = len(sigmas)
+    p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n
+    for i, sigma in enumerate(sigmas):
+        dl_fails, lm_fails = 0, 0
+        # Attempt num_samples optimizations for both DL and LM
+        for _attempt in range(args.num_samples):
+            initial = gtsam.Values()
+            initial.insert(11, T11.retract(np.random.normal(0, sigma, 3)))
+            initial.insert(12, T12.retract(np.random.normal(0, sigma, 3)))
+            initial.insert(21, T21.retract(np.random.normal(0, sigma, 3)))
+            initial.insert(22, T22.retract(np.random.normal(0, sigma, 3)))
+
+            # Run dogleg optimizer
+            dl = gtsam.DoglegOptimizer(graph, initial, params)
+            result = dl.optimize()
+            dl_fails += graph.error(result) > 1e-9
+
+            # Run
+            lm = gtsam.LevenbergMarquardtOptimizer(graph, initial)
+            result = lm.optimize()
+            lm_fails += graph.error(result) > 1e-9
+
+        # Calculate Bayes estimate of success probability
+        # using a beta prior of alpha=0.5, beta=0.5
+        alpha, beta = 0.5, 0.5
+        v = args.num_samples+alpha+beta
+        p_dl[i] = (args.num_samples-dl_fails+alpha)/v
+        p_lm[i] = (args.num_samples-lm_fails+alpha)/v
+
+        def stddev(p):
+            """Calculate standard deviation."""
+            return math.sqrt(p*(1-p)/(1+v))
+
+        s_dl[i] = stddev(p_dl[i])
+        s_lm[i] = stddev(p_lm[i])
+
+        fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%"
+        print(fmt.format(sigma,
+                         100*p_dl[i], 100*s_dl[i],
+                         100*p_lm[i], 100*s_lm[i]))
+
+    if args.plot:
+        fig, ax = plt.subplots()
+        dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg")
+        lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM")
+        plt.title("Dogleg emprical success vs. LM")
+        plt.legend(handles=[dl_plot, lm_plot])
+        ax.set_xlim(0, sigmas[-1]+1)
+        ax.set_ylim(0, 1)
+        plt.show()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description="Compare Dogleg and LM success rates")
+    parser.add_argument("-n", "--num_samples", type=int, default=1000,
+                        help="Number of samples for each sigma")
+    parser.add_argument("-d", "--delta", type=float, default=10.0,
+                        help="Initial delta for dogleg")
+    parser.add_argument("-p", "--plot", action="store_true",
+                        help="Flag to plot results")
+    args = parser.parse_args()
+    run(args)

cython/gtsam/examples/PlanarManipulatorExample.py

@@ -115,7 +115,7 @@ class ThreeLinkArm(object):
 
         l1Zl1 = expmap(0.0, 0.0, q[0])
         l2Zl2 = expmap(0.0, self.L1, q[1])
-        l3Zl3 = expmap(0.0, 7.0, q[2])
+        l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
         return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
 
     def ik(self, sTt_desired, e=1e-9):
@@ -297,12 +297,18 @@ def run_example():
     """ Use trajectory interpolation and then trajectory tracking a la Murray
         to move a 3-link arm on a straight line.
     """
+    # Create arm
     arm = ThreeLinkArm()
+
+    # Get initial pose using forward kinematics
     q = np.radians(vector3(30, -30, 45))
     sTt_initial = arm.fk(q)
+
+    # Create interpolated trajectory in task space to desired goal pose
     sTt_goal = Pose2(2.4, 4.3, math.radians(0))
     poses = trajectory(sTt_initial, sTt_goal, 50)
 
+    # Setup figure and plot initial pose
     fignum = 0
     fig = plt.figure(fignum)
     axes = fig.gca()
@@ -310,6 +316,8 @@ def run_example():
     axes.set_ylim(0, 10)
     gtsam_plot.plot_pose2(fignum, arm.fk(q))
 
+    # For all poses in interpolated trajectory, calculate dq to move to next pose.
+    # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
     for pose in poses:
         sTt = arm.fk(q)
         error = delta(sTt, pose)

cython/gtsam/tests/test_Pose3.py

@@ -30,9 +30,9 @@ class TestPose3(GtsamTestCase):
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
     def test_transform_to(self):
-        """Test transform_to method."""
+        """Test transformTo method."""
         transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
-        actual = transform.transform_to(Point3(3, 2, 10))
+        actual = transform.transformTo(Point3(3, 2, 10))
         expected = Point3(2, 1, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 

cython/gtsam/tests/test_dsf_map.py

@@ -0,0 +1,38 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Disjoint Set Forest.
+Author: Frank Dellaert & Varun Agrawal
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestDSFMap(GtsamTestCase):
+    """Tests for DSFMap."""
+
+    def test_all(self):
+        """Test everything in DFSMap."""
+        def key(index_pair):
+            return index_pair.i(), index_pair.j()
+
+        dsf = gtsam.DSFMapIndexPair()
+        pair1 = gtsam.IndexPair(1, 18)
+        self.assertEqual(key(dsf.find(pair1)), key(pair1))
+        pair2 = gtsam.IndexPair(2, 2)
+        dsf.merge(pair1, pair2)
+        self.assertTrue(dsf.find(pair1), dsf.find(pair1))
+
+
+if __name__ == '__main__':
+    unittest.main()

cython/gtsam_eigency/eigency_cpp.h

@@ -35,7 +35,7 @@ inline PyArrayObject *_ndarray_view<double>(double *data, long rows, long cols,
         return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1);
     } else {
         // Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride
-        // If no stride is given, the cow_stride is set to the number of rows.        
+        // If no stride is given, the cow_stride is set to the number of rows.
         return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows);
     }
 }
@@ -355,7 +355,7 @@ public:
     FlattenedMap()
         : Base(NULL, 0, 0),
           object_(NULL) {}
-    
+
     FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
         : Base(data, rows, cols,
                Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
@@ -363,7 +363,7 @@ public:
     }
 
     FlattenedMap(PyArrayObject *object)
-        : Base((Scalar *)((PyArrayObject*)object)->data,        
+        : Base((Scalar *)((PyArrayObject*)object)->data,
         // : Base(_from_numpy<Scalar>((PyArrayObject*)object),
                (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
                (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
@@ -390,7 +390,7 @@ public:
 
         return *this;
     }
-    
+
     operator Base() const {
         return static_cast<Base>(*this);
     }
@@ -429,7 +429,7 @@ public:
                (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
           object_(NULL) {
     }
-    
+
     Map(Scalar *data, long rows, long cols)
         : Base(data, rows, cols),
           object_(NULL) {}
@@ -456,7 +456,7 @@ public:
             throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
         Py_XINCREF(object_);
     }
-    
+
     Map &operator=(const Map &other) {
         if (other.object_) {
             new (this) Map(other.object_);
@@ -474,7 +474,7 @@ public:
         MapBase<MatrixType>::operator=(other);
         return *this;
     }
-    
+
     operator Base() const {
         return static_cast<Base>(*this);
     }

doc/Code/LocalizationExample2.cpp

@@ -1,5 +1,5 @@
 // add unary measurement factors, like GPS, on all three poses
-noiseModel::Diagonal::shared_ptr unaryNoise = 
+noiseModel::Diagonal::shared_ptr unaryNoise =
  noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
 graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
 graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));

doc/Code/LocalizationFactor.cpp

@@ -5,7 +5,7 @@ public:
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
-  Vector evaluateError(const Pose2& q, 
+  Vector evaluateError(const Pose2& q,
                        boost::optional<Matrix&> H = boost::none) const
   {
     if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();

doc/Code/OdometryExample.cpp

@@ -3,13 +3,13 @@ NonlinearFactorGraph graph;
 
 // Add a Gaussian prior on pose x_1
 Pose2 priorMean(0.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr priorNoise = 
+noiseModel::Diagonal::shared_ptr priorNoise =
   noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
 graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
 
 // Add two odometry factors
 Pose2 odometry(2.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr odometryNoise = 
+noiseModel::Diagonal::shared_ptr odometryNoise =
   noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

doc/Code/Pose2SLAMExample.cpp

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

examples/CameraResectioning.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/Data/HS21.QPS

@@ -0,0 +1,20 @@
+NAME          HS21
+ROWS
+  N OBJ.FUNC
+  G R------1
+COLUMNS
+    C------1  R------1  0.100000e+02
+    C------2  R------1  -.100000e+01
+RHS
+    RHS       OBJ.FUNC  0.100000e+03
+    RHS       R------1  0.100000e+02
+RANGES
+BOUNDS
+ LO BOUNDS    C------1  0.200000e+01
+ UP BOUNDS    C------1  0.500000e+02
+ LO BOUNDS    C------2  -.500000e+02
+ UP BOUNDS    C------2  0.500000e+02
+QUADOBJ
+    C------1  C------1  0.200000e-01
+    C------2  C------2  0.200000e+01
+ENDATA

examples/Data/HS268.QPS

@@ -0,0 +1,55 @@
+NAME          HS268
+ROWS
+  N OBJ.FUNC
+  G R------1
+  G R------2
+  G R------3
+  G R------4
+  G R------5
+COLUMNS
+    C------1  OBJ.FUNC  0.183400e+05   R------1  -.100000e+01
+    C------1  R------2  0.100000e+02   R------3  -.800000e+01
+    C------1  R------4  0.800000e+01   R------5  -.400000e+01
+    C------2  OBJ.FUNC  -.341980e+05   R------1  -.100000e+01
+    C------2  R------2  0.100000e+02   R------3  0.100000e+01
+    C------2  R------4  -.100000e+01   R------5  -.200000e+01
+    C------3  OBJ.FUNC  0.454200e+04   R------1  -.100000e+01
+    C------3  R------2  -.300000e+01   R------3  -.200000e+01
+    C------3  R------4  0.200000e+01   R------5  0.300000e+01
+    C------4  OBJ.FUNC  0.867200e+04   R------1  -.100000e+01
+    C------4  R------2  0.500000e+01   R------3  -.500000e+01
+    C------4  R------4  0.500000e+01   R------5  -.500000e+01
+    C------5  OBJ.FUNC  0.860000e+02   R------1  -.100000e+01
+    C------5  R------2  0.400000e+01   R------3  0.300000e+01
+    C------5  R------4  -.300000e+01   R------5  0.100000e+01
+RHS
+    RHS       OBJ.FUNC  -.144630e+05
+    RHS       R------1  -.500000e+01
+    RHS       R------2  0.200000e+02
+    RHS       R------3  -.400000e+02
+    RHS       R------4  0.110000e+02
+    RHS       R------5  -.300000e+02
+RANGES
+BOUNDS
+ FR BOUNDS    C------1
+ FR BOUNDS    C------2
+ FR BOUNDS    C------3
+ FR BOUNDS    C------4
+ FR BOUNDS    C------5
+QUADOBJ
+    C------1  C------1  0.203940e+05
+    C------1  C------2  -.249080e+05
+    C------1  C------3  -.202600e+04
+    C------1  C------4  0.389600e+04
+    C------1  C------5  0.658000e+03
+    C------2  C------2  0.418180e+05
+    C------2  C------3  -.346600e+04
+    C------2  C------4  -.982800e+04
+    C------2  C------5  -.372000e+03
+    C------3  C------3  0.351000e+04
+    C------3  C------4  0.217800e+04
+    C------3  C------5  -.348000e+03
+    C------4  C------4  0.303000e+04
+    C------4  C------5  -.440000e+02
+    C------5  C------5  0.540000e+02
+ENDATA

examples/Data/HS35.QPS

@@ -0,0 +1,20 @@
+NAME          HS35
+ROWS
+  N OBJ.FUNC
+  G R------1
+COLUMNS
+    C------1  OBJ.FUNC  -.800000e+01   R------1  -.100000e+01
+    C------2  OBJ.FUNC  -.600000e+01   R------1  -.100000e+01
+    C------3  OBJ.FUNC  -.400000e+01   R------1  -.200000e+01
+RHS
+    RHS       OBJ.FUNC  -.900000e+01
+    RHS       R------1  -.300000e+01
+RANGES
+BOUNDS
+QUADOBJ
+    C------1  C------1  0.400000e+01
+    C------1  C------2  0.200000e+01
+    C------1  C------3  0.200000e+01
+    C------2  C------2  0.400000e+01
+    C------3  C------3  0.200000e+01
+ENDATA

examples/Data/HS35MOD.QPS

@@ -0,0 +1,21 @@
+NAME          HS35MOD
+ROWS
+  N OBJ.FUNC
+  G R------1
+COLUMNS
+    C------1  OBJ.FUNC  -.800000e+01   R------1  -.100000e+01
+    C------2  OBJ.FUNC  -.600000e+01   R------1  -.100000e+01
+    C------3  OBJ.FUNC  -.400000e+01   R------1  -.200000e+01
+RHS
+    RHS       OBJ.FUNC  -.900000e+01
+    RHS       R------1  -.300000e+01
+RANGES
+BOUNDS
+ FX BOUNDS    C------2  0.500000e+00
+QUADOBJ
+    C------1  C------1  0.400000e+01
+    C------1  C------2  0.200000e+01
+    C------1  C------3  0.200000e+01
+    C------2  C------2  0.400000e+01
+    C------3  C------3  0.200000e+01
+ENDATA

examples/Data/HS51.QPS

@@ -0,0 +1,33 @@
+NAME          HS51
+ROWS
+  N OBJ.FUNC
+  E R------1
+  E R------2
+  E R------3
+COLUMNS
+    C------1  R------1  0.100000e+01
+    C------2  OBJ.FUNC  -.400000e+01   R------1  0.300000e+01
+    C------2  R------3  0.100000e+01
+    C------3  OBJ.FUNC  -.400000e+01   R------2  0.100000e+01
+    C------4  OBJ.FUNC  -.200000e+01   R------2  0.100000e+01
+    C------5  OBJ.FUNC  -.200000e+01   R------2  -.200000e+01
+    C------5  R------3  -.100000e+01
+RHS
+    RHS       OBJ.FUNC  -.600000e+01
+    RHS       R------1  0.400000e+01
+RANGES
+BOUNDS
+ FR BOUNDS    C------1
+ FR BOUNDS    C------2
+ FR BOUNDS    C------3
+ FR BOUNDS    C------4
+ FR BOUNDS    C------5
+QUADOBJ
+    C------1  C------1  0.200000e+01
+    C------1  C------2  -.200000e+01
+    C------2  C------2  0.400000e+01
+    C------2  C------3  0.200000e+01
+    C------3  C------3  0.200000e+01
+    C------4  C------4  0.200000e+01
+    C------5  C------5  0.200000e+01
+ENDATA

examples/Data/HS52.QPS

@@ -0,0 +1,32 @@
+NAME          HS52
+ROWS
+  N OBJ.FUNC
+  E R------1
+  E R------2
+  E R------3
+COLUMNS
+    C------1  R------1  0.100000e+01
+    C------2  OBJ.FUNC  -.400000e+01   R------1  0.300000e+01
+    C------2  R------3  0.100000e+01
+    C------3  OBJ.FUNC  -.400000e+01   R------2  0.100000e+01
+    C------4  OBJ.FUNC  -.200000e+01   R------2  0.100000e+01
+    C------5  OBJ.FUNC  -.200000e+01   R------2  -.200000e+01
+    C------5  R------3  -.100000e+01
+RHS
+    RHS       OBJ.FUNC  -.600000e+01
+RANGES
+BOUNDS
+ FR BOUNDS    C------1
+ FR BOUNDS    C------2
+ FR BOUNDS    C------3
+ FR BOUNDS    C------4
+ FR BOUNDS    C------5
+QUADOBJ
+    C------1  C------1  0.320000e+02
+    C------1  C------2  -.800000e+01
+    C------2  C------2  0.400000e+01
+    C------2  C------3  0.200000e+01
+    C------3  C------3  0.200000e+01
+    C------4  C------4  0.200000e+01
+    C------5  C------5  0.200000e+01
+ENDATA

examples/Data/QPTEST.QPS

@@ -0,0 +1,19 @@
+NAME          QP example
+ROWS
+ N  obj
+ G  r1
+ L  r2
+COLUMNS
+    c1        r1                 2.0   r2                -1.0
+    c1        obj                1.5
+    c2        r1                 1.0   r2                 2.0
+    c2        obj               -2.0
+RHS
+    rhs1      r1                 2.0   r2                 6.0
+BOUNDS
+ UP bnd1      c1                20.0
+QUADOBJ
+    c1        c1                 8.0
+    c1        c2                 2.0
+    c2        c2                10.0
+ENDATA

examples/Data/toy3D.xml

@@ -0,0 +1,169 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<!DOCTYPE boost_serialization>
+<boost_serialization signature="serialization::archive" version="17">
+<graph class_id="0" tracking_level="0" version="0">
+	<Base class_id="1" tracking_level="0" version="0">
+		<factors_ class_id="2" tracking_level="0" version="0">
+			<count>2</count>
+			<item_version>1</item_version>
+			<item class_id="3" tracking_level="0" version="1">
+				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+					<Base class_id="5" tracking_level="0" version="0">
+						<Base class_id="6" tracking_level="0" version="0">
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>0</item>
+								<item>1</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_ class_id="8" tracking_level="0" version="0">
+						<matrix_ class_id="9" tracking_level="0" version="0">
+							<rows>9</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.22427709071730959e+01</item>
+								<item>1.51514613104920048e+01</item>
+								<item>3.60934366857813060e+00</item>
+								<item>-1.18407259026383116e+01</item>
+								<item>7.84826117220921216e+00</item>
+								<item>1.23509165494819051e+01</item>
+								<item>-6.09875015991639735e+00</item>
+								<item>6.16547190708139126e-01</item>
+								<item>3.94972084922329048e+00</item>
+								<item>-4.89208482920378174e+00</item>
+								<item>3.02091647632478866e+00</item>
+								<item>-8.95328692238917512e+00</item>
+								<item>7.89831607220345955e+00</item>
+								<item>-2.36793602009719084e+00</item>
+								<item>1.48517612051941725e+01</item>
+								<item>-3.97284286249233731e-01</item>
+								<item>-1.95744531643153863e+01</item>
+								<item>-3.85954855417462017e+00</item>
+								<item>4.79268277145419042e+00</item>
+								<item>-9.01707953629520453e+00</item>
+								<item>1.37848069005841385e+01</item>
+								<item>1.04829326688375950e+01</item>
+								<item>-5.00630568442241675e+00</item>
+								<item>4.70463561852773182e+00</item>
+								<item>-1.59179134598689274e+01</item>
+								<item>-2.04767784956723942e+00</item>
+								<item>9.54135497908261954e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>-1.76201757312015372e+00</item>
+								<item>1.98634190821282672e+01</item>
+								<item>1.52966546661624236e+00</item>
+								<item>1.94817649567575373e+01</item>
+								<item>1.39684693294110307e+00</item>
+								<item>4.30228460420588288e+00</item>
+								<item>1.76201757312015372e+00</item>
+								<item>-1.98634190821282672e+01</item>
+								<item>-1.52966546661624236e+00</item>
+								<item>-0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>4.16606867942304948e+00</item>
+								<item>1.86906420801308037e+00</item>
+								<item>-1.94717865319198360e+01</item>
+								<item>-1.94817649567575373e+01</item>
+								<item>-1.39684693294110307e+00</item>
+								<item>-4.30228460420588288e+00</item>
+								<item>-4.16606867942304948e+00</item>
+								<item>-1.86906420801308037e+00</item>
+								<item>1.94717865319198360e+01</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00891462904330531e+01</item>
+								<item>-1.08132497987816869e+01</item>
+								<item>8.66487736568128497e+00</item>
+								<item>2.88370015604634311e+01</item>
+								<item>1.89391698948574643e+01</item>
+								<item>2.12398960190661290e+00</item>
+								<item>1.22150946112124039e+01</item>
+								<item>-2.33658532501596561e+01</item>
+								<item>1.51576204760307363e+01</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>9</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_ class_id="11" tracking_level="0" version="1">
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+			<item>
+				<px class_id_reference="4" object_id="_1">
+					<Base>
+						<Base>
+							<keys_>
+								<count>2</count>
+								<item_version>0</item_version>
+								<item>0</item>
+								<item>1</item>
+							</keys_>
+						</Base>
+					</Base>
+					<Ab_>
+						<matrix_>
+							<rows>3</rows>
+							<cols>7</cols>
+							<data>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>1.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+								<item>0.00000000000000000e+00</item>
+							</data>
+						</matrix_>
+						<variableColOffsets_>
+							<count>4</count>
+							<item_version>0</item_version>
+							<item>0</item>
+							<item>3</item>
+							<item>6</item>
+							<item>7</item>
+						</variableColOffsets_>
+						<rowStart_>0</rowStart_>
+						<rowEnd_>3</rowEnd_>
+						<blockStart_>0</blockStart_>
+					</Ab_>
+					<model_>
+						<px class_id="-1"></px>
+					</model_>
+				</px>
+			</item>
+		</factors_>
+	</Base>
+</graph>
+</boost_serialization>
+

examples/DiscreteBayesNet_FG.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/ImuFactorsExample.cpp

@@ -91,7 +91,7 @@ int main(int argc, char* argv[])
   cout << "initial state:\n" << initial_state.transpose() << "\n\n";
 
   // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
-  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), 
+  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
                                          initial_state(4), initial_state(5));
   Point3 prior_point(initial_state.head<3>());
   Pose3 prior_pose(prior_rotation, prior_point);
@@ -102,7 +102,7 @@ int main(int argc, char* argv[])
   int correction_count = 0;
   initial_values.insert(X(correction_count), prior_pose);
   initial_values.insert(V(correction_count), prior_velocity);
-  initial_values.insert(B(correction_count), prior_imu_bias);  
+  initial_values.insert(B(correction_count), prior_imu_bias);
 
   // Assemble prior noise model and add it the graph.
   noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
@@ -138,7 +138,7 @@ int main(int argc, char* argv[])
   p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
   p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
   p->biasAccOmegaInt = bias_acc_omega_int;
-  
+
 #ifdef USE_COMBINED
   imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
 #else
@@ -154,7 +154,7 @@ int main(int argc, char* argv[])
   double current_position_error = 0.0, current_orientation_error = 0.0;
 
   double output_time = 0.0;
-  double dt = 0.005;  // The real system has noise, but here, results are nearly 
+  double dt = 0.005;  // The real system has noise, but here, results are nearly
                       // exactly the same, so keeping this for simplicity.
 
   // All priors have been set up, now iterate through the data file.
@@ -203,8 +203,8 @@ int main(int argc, char* argv[])
                            *preint_imu);
       graph->add(imu_factor);
       imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-      graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1), 
+      graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
-                                                      B(correction_count  ), 
+                                                      B(correction_count  ),
                                                       zero_bias, bias_noise_model));
 #endif
 
@@ -215,7 +215,7 @@ int main(int argc, char* argv[])
                                   gps(2)), // D,
                            correction_noise);
       graph->add(gps_factor);
-      
+
       // Now optimize and compare results.
       prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
       initial_values.insert(X(correction_count), prop_state.pose());
@@ -250,10 +250,10 @@ int main(int argc, char* argv[])
       // display statistics
       cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
 
-      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", 
+      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
               output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
               gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
-              gps(0), gps(1), gps(2), 
+              gps(0), gps(1), gps(2),
               gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
 
       output_time += 1.0;

examples/InverseKinematicsExampleExpressions.cpp

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file InverseKinematicsExampleExpressions.cpp
+ * @brief Implement inverse kinematics on a three-link arm using expressions.
+ * @date April 15, 2019
+ * @author Frank Dellaert
+ */
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
+
+#include <cmath>
+
+using namespace std;
+using namespace gtsam;
+
+// Scalar multiplication of a vector, with derivatives.
+inline Vector3 scalarMultiply(const double& s, const Vector3& v,
+                              OptionalJacobian<3, 1> Hs,
+                              OptionalJacobian<3, 3> Hv) {
+  if (Hs) *Hs = v;
+  if (Hv) *Hv = s * I_3x3;
+  return s * v;
+}
+
+// Expression version of scalar product, using above function.
+inline Vector3_ operator*(const Double_& s, const Vector3_& v) {
+  return Vector3_(&scalarMultiply, s, v);
+}
+
+// Expression version of Pose2::Expmap
+inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); }
+
+// Main function
+int main(int argc, char** argv) {
+  // Three-link planar manipulator specification.
+  const double L1 = 3.5, L2 = 3.5, L3 = 2.5;    // link lengths
+  const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2);  // end-effector pose at rest
+  const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1),
+      xi3(L1 + L2, 0, 1);  // screw axes at rest
+
+  // Create Expressions for unknowns
+  using symbol_shorthand::Q;
+  Double_ q1(Q(1)), q2(Q(2)), q3(Q(3));
+
+  // Forward kinematics expression as product of exponentials
+  Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1));
+  Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2));
+  Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3));
+  Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0)));
+
+  // Create a factor graph with a a single expression factor.
+  ExpressionFactorGraph graph;
+  Pose2 desiredEndEffectorPose(3, 2, 0);
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  graph.addExpressionFactor(forward, desiredEndEffectorPose, model);
+
+  // Create initial estimate
+  Values initial;
+  initial.insert(Q(1), 0.1);
+  initial.insert(Q(2), 0.2);
+  initial.insert(Q(3), 0.3);
+  initial.print("\nInitial Estimate:\n");  // print
+  GTSAM_PRINT(forward.value(initial));
+
+  // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer
+  LevenbergMarquardtParams params;
+  params.setlambdaInitial(1e6);
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+
+  GTSAM_PRINT(forward.value(result));
+
+  return 0;
+}

examples/LocalizationExample.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/METISOrderingExample.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/OdometryExample.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/PlanarSLAMExample.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp

@@ -34,47 +34,47 @@ int main(int argc, char* argv[]) {
   // Concatenate poses to create trajectory
   poses.insert( poses.end(), poses2.begin(), poses2.end() );
   poses.insert( poses.end(), poses3.begin(), poses3.end() );  // std::vector of Pose3
-  auto points = createPoints();                               // std::vector of Point3 
+  auto points = createPoints();                               // std::vector of Point3
 
   // (ground-truth) sensor pose in body frame, further an unknown variable
   Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
-  
+
   // The graph
   ExpressionFactorGraph graph;
-  
+
   // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
   auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
-  
+
   // Uncertainty bearing range measurement;
   auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
-  
+
   // Expressions for body-frame at key 0 and sensor-tf
   Pose3_ x_('x', 0);
   Pose3_ body_T_sensor_('T', 0);
-  
+
   // Add a prior on the body-pose
-  graph.addExpressionFactor(x_, poses[0], poseNoise); 
+  graph.addExpressionFactor(x_, poses[0], poseNoise);
-  
+
   // Simulated measurements from pose
   for (size_t i = 0; i < poses.size(); ++i) {
     auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
     for (size_t j = 0; j < points.size(); ++j) {
-      
+
       // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
       auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
-      
+
       // Create a *perfect* measurement
       auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
-      
+
       // Add factor
       graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
     }
-    
+
     // and add a between factor to the graph
     if (i > 0)
     {
       // And also we have a *perfect* measurement for the between factor.
-      graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); 
+      graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise);
     }
   }
 
@@ -82,12 +82,12 @@ int main(int argc, char* argv[]) {
   Values initial;
   Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initial.insert(Symbol('x', i), poses[i].compose(delta)); 
+    initial.insert(Symbol('x', i), poses[i].compose(delta));
   for (size_t j = 0; j < points.size(); ++j)
     initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
-  
+
   // Initialize body_T_sensor wrongly (because we do not know!)
-  initial.insert<Pose3>(Symbol('T',0), Pose3()); 
+  initial.insert<Pose3>(Symbol('T',0), Pose3());
 
   std::cout << "initial error: " << graph.error(initial) << std::endl;
   Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

examples/Pose3SLAMExample_changeKeys.cpp

@@ -79,7 +79,7 @@ int main(const int argc, const char *argv[]) {
           key2 = pose3Between->key2() - firstKey;
         }
         NonlinearFactor::shared_ptr simpleFactor(
-            new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
+            new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));
         simpleGraph.add(simpleFactor);
       }
     }

examples/SFMExampleExpressions.cpp

@@ -72,7 +72,7 @@ int main(int argc, char* argv[]) {
       Point2 measurement = camera.project(points[j]);
       // Below an expression for the prediction of the measurement:
       Point3_ p('l', j);
-      Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
+      Point2_ prediction = uncalibrate(cK, project(transformTo(x, p)));
       // Again, here we use an ExpressionFactor
       graph.addExpressionFactor(prediction, measurement, measurementNoise);
     }

examples/SFMdata.h

@@ -52,9 +52,9 @@ std::vector<gtsam::Point3> createPoints() {
 /* ************************************************************************* */
 std::vector<gtsam::Pose3> createPoses(
             const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
-            const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), 
+            const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))),
             int steps = 8) {
-  
+
   // Create the set of ground-truth poses
   // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
   std::vector<gtsam::Pose3> poses;
@@ -63,6 +63,6 @@ std::vector<gtsam::Pose3> createPoses(
   for(; i < steps; ++i) {
     poses.push_back(poses[i-1].compose(delta));
   }
-  
+
   return poses;
 }

examples/SimpleRotation.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/SolverComparer.cpp

@@ -361,8 +361,8 @@ void runIncremental()
           const auto& measured = factor->measured();
           Rot2 measuredBearing = measured.bearing();
           double measuredRange = measured.range();
-          newVariables.insert(lmKey, 
+          newVariables.insert(lmKey,
-            pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
+            pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0))));
         }
       }
       else

examples/StereoVOExample.cpp

@@ -22,7 +22,7 @@
  *  -moves forward 1 meter
  *  -takes stereo readings on three landmarks
  */
- 
+
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/nonlinear/Values.h>
@@ -40,7 +40,7 @@ int main(int argc, char** argv){
   NonlinearFactorGraph graph;
   Pose3 first_pose;
   graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
-  
+
   //create factor noise model with 3 sigmas of value 1
   const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
   //create stereo camera calibration object with .2m between cameras

examples/StereoVOExample_large.cpp

@@ -50,7 +50,7 @@ int main(int argc, char** argv){
   string calibration_loc = findExampleDataFile("VO_calibration.txt");
   string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
   string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
-  
+
   //read camera calibration info from file
   // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
   double fx, fy, s, u0, v0, b;
@@ -60,7 +60,7 @@ int main(int argc, char** argv){
 
   //create stereo camera calibration object
   const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
-  
+
   ifstream pose_file(pose_loc.c_str());
   cout << "Reading camera poses" << endl;
   int pose_id;
@@ -72,7 +72,7 @@ int main(int argc, char** argv){
     }
     initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
   }
-  
+
   // camera and landmark keys
   size_t x, l;
 
@@ -89,8 +89,8 @@ int main(int argc, char** argv){
     //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
     if (!initial_estimate.exists(Symbol('l', l))) {
       Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
-      //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
+      //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
-      Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
+      Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
       initial_estimate.insert(Symbol('l', l), worldPoint);
     }
   }

examples/UGM_chain.cpp

@@ -36,7 +36,7 @@ int main(int argc, char** argv) {
 
   // Each node takes 1 of 7 possible states denoted by 0-6 in following order:
   // ["VideoGames"  "Industry"  "GradSchool"  "VideoGames(with PhD)"
-  // "Industry(with PhD)"  "Academia"  "Deceased"] 
+  // "Industry(with PhD)"  "Academia"  "Deceased"]
   const size_t nrStates = 7;
 
   // define variables

examples/UGM_small.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/easyPoint2KalmanFilter.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

examples/elaboratePoint2KalmanFilter.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -140,7 +140,7 @@ int main() {
   const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
   assert(cg0->nrFrontals() == 1);
   assert(cg0->nrParents() == 0);
-  linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
+  linearFactorGraph->add(0, cg0->R(), cg0->d() - cg0->R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
 
   // Create the desired ordering
   ordering = Ordering::shared_ptr(new Ordering);

gtsam.h

@@ -209,11 +209,60 @@ class KeyGroupMap {
   bool insert2(size_t key, int val);
 };
 
+// Actually a FastSet<FactorIndex>
+class FactorIndexSet {
+  FactorIndexSet();
+  FactorIndexSet(const gtsam::FactorIndexSet& set);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(size_t factorIndex);
+  bool erase(size_t factorIndex); // returns true if value was removed
+  bool count(size_t factorIndex) const; // returns true if value exists
+};
+
+// Actually a vector<FactorIndex>
+class FactorIndices {
+  FactorIndices();
+  FactorIndices(const gtsam::FactorIndices& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  size_t at(size_t i) const;
+  size_t front() const;
+  size_t back() const;
+  void push_back(size_t factorIndex) const;
+};
+
 //*************************************************************************
 // base
 //*************************************************************************
 
 /** gtsam namespace functions */
+
+#include <gtsam/base/DSFMap.h>
+class IndexPair { 
+  IndexPair(); 
+  IndexPair(size_t i, size_t j); 
+  size_t i() const;
+  size_t j() const;
+};
+
+template<KEY = {gtsam::IndexPair}>
+class DSFMap {
+  DSFMap();
+  KEY find(const KEY& key) const;
+  void merge(const KEY& x, const KEY& y);
+};
+
 #include <gtsam/base/Matrix.h>
 bool linear_independent(Matrix A, Matrix B, double tol);
 
@@ -583,8 +632,8 @@ class Pose2 {
   static Matrix wedge(double vx, double vy, double w);
 
   // Group Actions on Point2
-  gtsam::Point2 transform_from(const gtsam::Point2& p) const;
+  gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
-  gtsam::Point2 transform_to(const gtsam::Point2& p) const;
+  gtsam::Point2 transformTo(const gtsam::Point2& p) const;
 
   // Standard Interface
   double x() const;
@@ -636,8 +685,8 @@ class Pose3 {
   static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
 
   // Group Action on Point3
-  gtsam::Point3 transform_from(const gtsam::Point3& point) const;
+  gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
-  gtsam::Point3 transform_to(const gtsam::Point3& point) const;
+  gtsam::Point3 transformTo(const gtsam::Point3& point) const;
 
   // Standard Interface
   gtsam::Rot3 rotation() const;
@@ -646,7 +695,8 @@ class Pose3 {
   double y() const;
   double z() const;
   Matrix matrix() const;
-  gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
+  gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
   double range(const gtsam::Point3& point);
   double range(const gtsam::Pose3& pose);
 
@@ -1219,7 +1269,7 @@ 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 
+  // 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;
@@ -1257,7 +1307,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
   Vector sigmas() const;
   Vector invsigmas() const;
   Vector precisions() const;
-    
+
   // enabling serialization functionality
   void serializable() const;
 };
@@ -1730,7 +1780,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
 
 virtual class SubgraphSolver  {
   SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
-  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   gtsam::VectorValues optimize() const;
 };
 
@@ -1867,7 +1917,6 @@ virtual class NonlinearFactor {
 #include <gtsam/nonlinear/NonlinearFactor.h>
 virtual class NoiseModelFactor: gtsam::NonlinearFactor {
   bool equals(const gtsam::NoiseModelFactor& other, double tol) const;
-  gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
   gtsam::noiseModel::Base* noiseModel() const;
   Vector unwhitenedError(const gtsam::Values& x) const;
   Vector whitenedError(const gtsam::Values& x) const;
@@ -2053,7 +2102,7 @@ virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
   bool getUseFixedLambdaFactor();
   string getLogFile() const;
   string getVerbosityLM() const;
-  
+
   void setDiagonalDamping(bool flag);
   void setlambdaFactor(double value);
   void setlambdaInitial(double value);
@@ -2091,7 +2140,7 @@ virtual class NonlinearOptimizer {
   double error() const;
   int iterations() const;
   gtsam::Values values() const;
-  void iterate() const;
+  gtsam::GaussianFactorGraph* iterate() const;
 };
 
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -2209,8 +2258,6 @@ class ISAM2Result {
   size_t getCliques() const;
 };
 
-class FactorIndices {};
-
 class ISAM2 {
   ISAM2();
   ISAM2(const gtsam::ISAM2Params& params);

gtsam/CMakeLists.txt

@@ -101,7 +101,10 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 
 # BUILD_SHARED_LIBS automatically defines static/shared libs:
 add_library(gtsam ${gtsam_srcs})
+# Boost:
 target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
+target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR})
+# Other deps:
 target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
 target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
 target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})

gtsam/base/DSFMap.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -18,9 +18,9 @@
 
 #pragma once
 
+#include <cstdlib>  // Provides size_t
 #include <map>
 #include <set>
-#include <cstdlib>        // Provides size_t
 
 namespace gtsam {
 
@@ -29,11 +29,9 @@ namespace gtsam {
  * Uses rank compression and union by rank, iterator version
  * @addtogroup base
  */
-template<class KEY>
+template <class KEY>
 class DSFMap {
-
+ protected:
-protected:
-
   /// We store the forest in an STL map, but parents are done with pointers
   struct Entry {
     typename std::map<KEY, Entry>::iterator parent_;
@@ -41,7 +39,7 @@ protected:
     Entry() {}
   };
 
-    typedef typename std::map<KEY, Entry> Map;
+  typedef typename std::map<KEY, Entry> Map;
   typedef typename Map::iterator iterator;
   mutable Map entries_;
 
@@ -62,8 +60,7 @@ protected:
   iterator find_(const iterator& it) const {
     // follow parent pointers until we reach set representative
     iterator& parent = it->second.parent_;
-    if (parent != it)
+    if (parent != it) parent = find_(parent);  // not yet, recurse!
-      parent = find_(parent); // not yet, recurse!
     return parent;
   }
 
@@ -73,13 +70,11 @@ protected:
     return find_(initial);
   }
 
-public:
+ public:
-
   typedef std::set<KEY> Set;
 
   /// constructor
-  DSFMap() {
+  DSFMap() {}
-  }
 
   /// Given key, find the representative key for the set in which it lives
   inline KEY find(const KEY& key) const {
@@ -89,12 +84,10 @@ public:
 
   /// Merge two sets
   void merge(const KEY& x, const KEY& y) {
-
     // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
     iterator xRoot = find_(x);
     iterator yRoot = find_(y);
-    if (xRoot == yRoot)
+    if (xRoot == yRoot) return;
-      return;
 
     // Merge sets
     if (xRoot->second.rank_ < yRoot->second.rank_)
@@ -117,7 +110,14 @@ public:
     }
     return sets;
   }
-
 };
 
-}
+/// Small utility class for representing a wrappable pairs of ints.
+class IndexPair : public std::pair<size_t,size_t> {
+ public:
+  IndexPair(): std::pair<size_t,size_t>(0,0) {}
+  IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
+  inline size_t i() const { return first; };
+  inline size_t j() const { return second; };
+};
+}  // namespace gtsam

gtsam/base/DSFVector.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/DSFVector.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/LieMatrix.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/LieScalar.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/LieVector.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/Manifold.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -170,7 +170,7 @@ struct FixedDimension {
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 /// Helper class to construct the product manifold of two other manifolds, M1 and M2
-/// Deprecated because of limited usefulness, maximum obfuscation 
+/// Deprecated because of limited usefulness, maximum obfuscation
 template<typename M1, typename M2>
 class ProductManifold: public std::pair<M1, M2> {
   BOOST_CONCEPT_ASSERT((IsManifold<M1>));

gtsam/base/Matrix.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -540,7 +540,7 @@ Matrix cholesky_inverse(const Matrix &A)
 }
 
 /* ************************************************************************* */
-// Semantics: 
+// Semantics:
 // if B = inverse_square_root(A), then all of the following are true:
 // inv(B) * inv(B)' == A
 // inv(B' * B) == A

gtsam/base/OptionalJacobian.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/ProductLieGroup.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/SymmetricBlockMatrix.cpp

@@ -65,8 +65,9 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const {
 void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) {
   gttic(VerticalBlockMatrix_choleskyPartial);
   DenseIndex topleft = variableColOffsets_[blockStart_];
-  if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft))
+  if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) {
     throw CholeskyFailed();
+  }
 }
 
 /* ************************************************************************* */

gtsam/base/TestableAssertions.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/ThreadsafeException.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/Value.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/Vector.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/VerticalBlockMatrix.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/VerticalBlockMatrix.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/deprecated/LieMatrix.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/deprecated/LieScalar.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/deprecated/LieVector.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -36,7 +36,7 @@ struct GTSAM_EXPORT LieVector : public Vector {
 
   /** initialize from a normal vector */
   LieVector(const Vector& v) : Vector(v) {}
-  
+
   template <class V>
   LieVector(const V& v) : Vector(v) {}
 

gtsam/base/lieProxies.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/numericalDerivative.h

@@ -67,28 +67,29 @@ struct FixedSizeMatrix {
 }
 
 /**
- * Numerically compute gradient of scalar function
+ * @brief Numerically compute gradient of scalar function
+ * @return n-dimensional gradient computed via central differencing
  * Class X is the input argument
  * The class X needs to have dim, expmap, logmap
+  * int N is the dimension of the X input value if variable dimension type but known at test time
  */
-template<class X>
+
-typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
+template <class X, int N = traits<X>::dimension>
-    double delta = 1e-5) {
+typename Eigen::Matrix<double, N, 1> numericalGradient(
+    boost::function<double(const X&)> h, const X& x, double delta = 1e-5) {
   double factor = 1.0 / (2.0 * delta);
 
   BOOST_STATIC_ASSERT_MSG(
       (boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
-  static const int N = traits<X>::dimension;
+  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
-  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
-
-  typedef typename traits<X>::TangentVector TangentX;
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
-  TangentX d;
+  typename traits<X>::TangentVector d;
   d.setZero();
 
-  Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
+  Eigen::Matrix<double,N,1> g; 
+  g.setZero();
   for (int j = 0; j < N; j++) {
     d(j) = delta;
     double hxplus = h(traits<X>::Retract(x, d));
@@ -108,37 +109,34 @@ typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<do
  * @param delta increment for numerical derivative
  * Class Y is the output argument
  * Class X is the input argument
+ * int N is the dimension of the X input value if variable dimension type but known at test time
  * @return m*n Jacobian computed via central differencing
  */
 
-template<class Y, class X>
+template <class Y, class X, int N = traits<X>::dimension>
 // TODO Should compute fixed-size matrix
-typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
+typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
-    double delta = 1e-5) {
+    boost::function<Y(const X&)> h, const X& x, double delta = 1e-5) {
-
   typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
       "Template argument Y must be a manifold type.");
   typedef traits<Y> TraitsY;
-  typedef typename TraitsY::TangentVector TangentY;
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
-  static const int N = traits<X>::dimension;
+  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified.");
-  BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
   typedef traits<X> TraitsX;
-  typedef typename TraitsX::TangentVector TangentX;
 
   // get value at x, and corresponding chart
   const Y hx = h(x);
 
   // Bit of a hack for now to find number of rows
-  const TangentY zeroY = TraitsY::Local(hx, hx);
+  const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx);
   const size_t m = zeroY.size();
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
-  TangentX dx;
+  Eigen::Matrix<double, N, 1> dx;
   dx.setZero();
 
   // Fill in Jacobian H
@@ -146,9 +144,9 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
   const double factor = 1.0 / (2.0 * delta);
   for (int j = 0; j < N; j++) {
     dx(j) = delta;
-    const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
+    const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = -delta;
-    const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
+    const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = 0;
     H.col(j) << (dy1 - dy2) * factor;
   }

gtsam/base/serialization.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testDSFMap.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -16,7 +16,7 @@
  * @brief unit tests for DSFMap
  */
 
-#include <gtsam_unstable/base/DSFMap.h>
+#include <gtsam/base/DSFMap.h>
 
 #include <boost/assign/std/list.hpp>
 #include <boost/assign/std/set.hpp>
@@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) {
 TEST(DSFMap, sets){
   // Create some "matches"
   typedef pair<size_t,size_t> Match;
-  typedef pair<size_t, set<size_t> > key_pair;
   list<Match> matches;
   matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6);
 
@@ -128,18 +127,24 @@ TEST(DSFMap, sets){
   set<size_t> s1, s2;
   s1 += 1,2,3;
   s2 += 4,5,6;
-  
+
   /*for(key_pair st: sets){
     cout << "Set " << st.first << " :{";
     for(const size_t s: st.second)
       cout << s << ", ";
     cout << "}" << endl;
   }*/
-    
+
   EXPECT(s1 == sets[1]);
   EXPECT(s2 == sets[4]);
 }
 
+/* ************************************************************************* */
+TEST(DSFMap, findIndexPair) {
+  DSFMap<IndexPair> dsf;
+  EXPECT(dsf.find(IndexPair(1,2))==IndexPair(1,2));
+  EXPECT(dsf.find(IndexPair(1,2)) != dsf.find(IndexPair(1,3)));
+}
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam/base/tests/testDSFVector.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testGroup.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testLieMatrix.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testLieScalar.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testLieVector.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testMatrix.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -275,7 +275,7 @@ TEST(Matrix, stream_read ) {
     -0.3, -8e-2, 5.1, 9.0,
     1.2, 3.4, 4.5, 6.7).finished();
 
-  string matrixAsString = 
+  string matrixAsString =
     "1.1 2.3 4.2 7.6\n"
     "-0.3 -8e-2 5.1    9.0\n\r" // Test extra spaces and windows newlines
     "1.2 \t 3.4 4.5 6.7"; // Test tab as separator

gtsam/base/tests/testOptionalJacobian.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/tests/testVector.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/timing.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/timing.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/base/types.h

@@ -56,6 +56,9 @@ namespace gtsam {
   /// Integer nonlinear key type
   typedef std::uint64_t Key;
 
+  /// Integer nonlinear factor index type
+  typedef std::uint64_t FactorIndex;
+
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 

gtsam/discrete/DecisionTree-inl.h

@@ -355,7 +355,7 @@ namespace gtsam {
     NodePtr choose(const L& label, size_t index) const {
       if (label_ == label)
         return branches_[index]; // choose branch
-    
+
       // second case, not label of interest, just recurse
       boost::shared_ptr<Choice> r(new Choice(label_, branches_.size()));
       for(const NodePtr& branch: branches_)

gtsam/discrete/DiscreteBayesTree.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/discrete/DiscreteEliminationTree.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/discrete/DiscreteEliminationTree.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -31,7 +31,7 @@ namespace gtsam {
     typedef EliminationTree<DiscreteBayesNet, DiscreteFactorGraph> Base; ///< Base class
     typedef DiscreteEliminationTree This; ///< This class
     typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
-    
+
     /**
     * Build the elimination tree of a factor graph using pre-computed column structure.
     * @param factorGraph The factor graph for which to build the elimination tree

gtsam/discrete/DiscreteJunctionTree.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

gtsam/discrete/DiscreteJunctionTree.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -51,7 +51,7 @@ namespace gtsam {
     typedef JunctionTree<DiscreteBayesTree, DiscreteFactorGraph> Base; ///< Base class
     typedef DiscreteJunctionTree This; ///< This class
     typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
-    
+
     /**
     * Build the elimination tree of a factor graph using precomputed column structure.
     * @param factorGraph The factor graph for which to build the elimination tree

gtsam/discrete/DiscreteMarginals.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)