Merge branch 'develop' into feature/fixExpressionFactorKeys

.gitignore

@@ -1,4 +1,5 @@
 /build*
+.idea
 *.pyc
 *.DS_Store
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt

CMakeLists.txt

@@ -66,8 +66,9 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                "Enable/Disable building & installation of Python module" OFF)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
-option(GTSAM_USE_VECTOR3_POINTS          "Simply typdef Point3 to eigen::Vector3d" OFF)
+option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
+option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
 
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
@@ -90,8 +91,8 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
     message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
 endif()
 
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
-    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet.  Please turn one of the two options off.")
+    message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet.  Please turn one of the two options off.")
 endif()
 
 # Flags for choosing default packaging tools
@@ -115,11 +116,11 @@ if(MSVC)
 	endif()
 endif()
 
-find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono)
+find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
 
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
-    NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY OR NOT Boost_REGEX_LIBRARY)
+    NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
   message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
 endif()
 
@@ -127,7 +128,7 @@ 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)
 set(GTSAM_BOOST_LIBRARIES
     ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
-	${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
+	${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY})
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
     add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
@@ -305,7 +306,7 @@ endif()
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.
 include_directories(BEFORE
-  gtsam/3rdparty/UFconfig
+  gtsam/3rdparty/SuiteSparse_config
   gtsam/3rdparty/CCOLAMD/Include
   ${METIS_INCLUDE_DIRECTORIES}
   ${PROJECT_SOURCE_DIR}
@@ -485,13 +486,14 @@ message(STATUS "  CPack Source Generator         : ${CPACK_SOURCE_GENERATOR}")
 message(STATUS "  CPack Generator                : ${CPACK_GENERATOR}")
 
 message(STATUS "GTSAM flags                                               ")
-print_config_flag(${GTSAM_USE_QUATERNIONS}             "Quaternions as default Rot3    ")
+print_config_flag(${GTSAM_USE_QUATERNIONS}             "Quaternions as default Rot3     ")
-print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency checking   ")
+print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency checking    ")
-print_config_flag(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap    ")
+print_config_flag(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
-print_config_flag(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap   ")
+print_config_flag(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap    ")
-print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4}   "Deprecated in GTSAM 4 allowed  ")
+print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4}   "Deprecated in GTSAM 4 allowed   ")
-print_config_flag(${GTSAM_USE_VECTOR3_POINTS}          "Point3 is typedef to Vector3   ")
+print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS}          "Point3 is typedef to Vector3    ")
-print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection  ")
+print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
+print_config_flag(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")
 
 message(STATUS "MATLAB toolbox flags                                      ")
 print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install matlab toolbox         ")

CppUnitLite/Test.h

@@ -68,6 +68,12 @@ protected:
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_) 
 
+/**
+ * Declare friend in a class to test its private methods
+ */
+#define FRIEND_TEST(testGroup, testName) \
+    friend class testGroup##testName##Test;
+
 /**
  * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this
  * will not wrap execution with a try/catch block

CppUnitLite/TestResult.h

@@ -18,8 +18,6 @@
 // 
 ///////////////////////////////////////////////////////////////////////////////
 
-
-
 #ifndef TESTRESULT_H
 #define TESTRESULT_H
 

LICENSE

@@ -4,11 +4,11 @@ LICENSE.BSD in this directory.
 GTSAM contains two third party libraries, with documentation of licensing and 
 modifications as follows:
 
-- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree 
+- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree 
 ordering library
     - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
-    - http://www.cise.ufl.edu/research/sparse
+    - http://faculty.cse.tamu.edu/davis/suitesparse.html
-    - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
+    - Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt
 - Eigen 3.2:  General C++ matrix and linear algebra library
     - Modified with 3 patches that have been contributed back to the Eigen team:
         - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)

README.md

@@ -1,59 +1,77 @@
-README - Georgia Tech Smoothing and Mapping library
+README - Georgia Tech Smoothing and Mapping library
-===================================================
+===================================================
-
+
-What is GTSAM?
+What is GTSAM?
---------------
+--------------
-
+
-GTSAM is a library of C++ classes that implement smoothing and
+GTSAM is a library of C++ classes that implement smoothing and
-mapping (SAM) in robotics and vision, using factor graphs and Bayes
+mapping (SAM) in robotics and vision, using factor graphs and Bayes
-networks as the underlying computing paradigm rather than sparse
+networks as the underlying computing paradigm rather than sparse
-matrices.
+matrices.
-
+
-On top of the C++ library, GTSAM includes a MATLAB interface (enable
+On top of the C++ library, GTSAM includes a MATLAB interface (enable
-GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
+GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
-is under development.
+is under development.
-
+
-Quickstart
+Quickstart
-----------
+----------
-
+
-In the root library folder execute:
+In the root library folder execute:
-
+
-```
+```
-#!bash
+#!bash
-$ mkdir build
+$ mkdir build
-$ cd build
+$ cd build
-$ cmake ..
+$ cmake ..
-$ make check (optional, runs unit tests)
+$ make check (optional, runs unit tests)
-$ make install
+$ make install
-```
+```
-
+
-Prerequisites:
+Prerequisites:
-
+
-- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
+- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
-- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
+- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
-- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
+- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
-
+
-Optional prerequisites - used automatically if findable by CMake:
+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 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)
-
+
-GTSAM 4 Compatibility
+GTSAM 4 Compatibility
----------------------
+---------------------
-
+
-GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
+GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
-
+
-Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
+Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
-
+
-Additional Information
+The Preintegrated IMU Factor
-----------------------
+----------------------------
-
+
-Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
+GTSAM includes a state of the art IMU handling scheme based on
-
+
-See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
+- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
-
+
-GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
+Our implementation improves on this using integration on the manifold, as detailed in
-
+
-Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. 
-
+- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015.
+
+If you are using the factor in academic work, please cite the publications above.
+
+In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
+
+
+
+Additional Information
+----------------------
+
+Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
+
+See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
+
+GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
+
+Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+
 GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

bitbucket-pipelines.yml

@@ -0,0 +1,16 @@
+# This is a sample build configuration for C++ – Make.
+# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples.
+# Only use spaces to indent your .yml configuration.
+# -----
+# You can specify a custom docker image from Docker Hub as your build environment.
+image: ilssim/cmake-boost-qt5
+
+pipelines:
+  default:
+    - step:
+        script: # Modify the commands below to build your repository.
+          - mkdir build
+          - cd build
+          - cmake .. 
+          - make
+          - make check

cmake/GtsamBuildTypes.cmake

@@ -6,7 +6,8 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
 if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
     set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
       "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
-    set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
+    set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE} CACHE STRING
+      "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE)
 endif()
 
 # Add option for using build type postfixes to allow installing multiple build modes
@@ -60,18 +61,18 @@ mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_C
 
 # Apply the gtsam specific build flags as normal variables. This makes it so that they only
 # apply to the gtsam part of the build if gtsam is built as a subproject
-set(CMAKE_C_FLAGS   ${GTSAM_CMAKE_C_FLAGS})
+set(CMAKE_C_FLAGS                  "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}")
-set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS})
+set(CMAKE_CXX_FLAGS                "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}")
-set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG})
+set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}")
-set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG})
+set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}")
-set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO})
+set(CMAKE_C_FLAGS_RELWITHDEBINFO   "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}")
-set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO})
+set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
-set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE})
+set(CMAKE_C_FLAGS_RELEASE          "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}")
-set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE})
+set(CMAKE_CXX_FLAGS_RELEASE        "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}")
-set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING})
+set(CMAKE_C_FLAGS_PROFILING        "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}")
-set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING})
+set(CMAKE_CXX_FLAGS_PROFILING      "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}")
-set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING})
+set(CMAKE_C_FLAGS_TIMING           "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}")
-set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING})
+set(CMAKE_CXX_FLAGS_TIMING         "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}")
 
 set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
 set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})

cmake/GtsamMatlabWrap.cmake

@@ -128,8 +128,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
     ## This needs to be fixed!!
     if(UNIX AND NOT APPLE)
       list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-        ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}
+        ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE})
-        ${Boost_REGEX_LIBRARY_RELEASE})
       if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
         list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
         if(GTSAM_MEX_BUILD_STATIC_MODULE)

cmake/GtsamTesting.cmake

@@ -101,6 +101,9 @@ mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
 # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
 if(GTSAM_BUILD_TESTS)
     add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+    
+    # Add target to build tests without running
+	add_custom_target(all.tests)
 endif()
 
 # Add examples target
@@ -109,8 +112,6 @@ add_custom_target(examples)
 # Add timing target
 add_custom_target(timing)
 
-# Add target to build tests without running
-add_custom_target(all.tests)
 
 # Implementations of this file's macros:
 
@@ -179,11 +180,17 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 				set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
 			endforeach()
 		else()
+
+			#skip folders which don't have any tests
+			if(NOT script_srcs)
+				return()
+			endif()
+
 			# Default on MSVC and XCode - combine test group into a single exectuable
 			set(target_name check_${groupName}_program)
 		
 			# Add executable
-			add_executable(${target_name} ${script_srcs} ${script_headers})
+			add_executable(${target_name} "${script_srcs}" ${script_headers})
 			target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
 		
 			# Only have a main function in one script - use preprocessor
@@ -196,7 +203,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			add_dependencies(check.${groupName} ${target_name})
 			add_dependencies(check ${target_name})
 			if(NOT XCODE_VERSION)
-				add_dependencies(all.tests ${script_name})
+				add_dependencies(all.tests ${target_name})
 			endif()
 		
 			# Add TOPSRCDIR

examples/CameraResectioning.cpp

@@ -48,8 +48,7 @@ public:
   virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
       boost::none) const {
     SimpleCamera camera(pose, *K_);
-    Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
+    return camera.project(P_, H, boost::none, boost::none) - p_;
-    return reprojectionError.vector();
   }
 };
 
@@ -72,18 +71,14 @@ int main(int argc, char* argv[]) {
   // add measurement factors
   SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
   boost::shared_ptr<ResectioningFactor> factor;
-  graph.push_back(
+  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
-      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
+          Point2(55, 45), Point3(10, 10, 0));
-          Point2(55, 45), Point3(10, 10, 0)));
+  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
-  graph.push_back(
+          Point2(45, 45), Point3(-10, 10, 0));
-      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
+  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
-          Point2(45, 45), Point3(-10, 10, 0)));
+          Point2(45, 55), Point3(-10, -10, 0));
-  graph.push_back(
+  graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
-      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
+          Point2(55, 55), Point3(10, -10, 0));
-          Point2(45, 55), Point3(-10, -10, 0)));
-  graph.push_back(
-      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
-          Point2(55, 55), Point3(10, -10, 0)));
 
   /* 3. Create an initial estimate for the camera pose */
   Values initial;

examples/CreateSFMExampleData.cpp

@@ -18,7 +18,6 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 
-#include <boost/foreach.hpp>
 #include <boost/assign/std/vector.hpp>
 
 using namespace boost::assign;
@@ -41,7 +40,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
   data.cameras.push_back(SfM_Camera(pose1, K));
   data.cameras.push_back(SfM_Camera(pose2, K));
 
-  BOOST_FOREACH(const Point3& p, P) {
+  for(const Point3& p: P) {
 
     // Create the track
     SfM_Track track;

examples/Data/QPExample.QPS

@@ -0,0 +1,21 @@
+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      obj               -4.0
+    rhs1      r1                 2.0   r2                 6.0
+RANGES
+BOUNDS
+    UP BOUNDS      c1                20.0
+QUADOBJ
+    c1        c1                 8.0
+    c1        c2                 2.0
+    c2        c2                10.0
+ENDATA

examples/ImuFactorsExample.cpp

@@ -0,0 +1,269 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 imuFactorsExample
+ * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
+ * @author Garrett (ghemann@gmail.com), Luca Carlone
+ */
+
+/**
+ * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
+ *  - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
+ *  the line #define USE_COMBINED (few lines below)
+ *  - we read IMU and GPS data from a CSV file, with the following format:
+ *  A row starting with "i" is the first initial position formatted with
+ *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
+ *  A row starting with "0" is an imu measurement
+ *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  A row starting with "1" is a gps correction formatted with
+ *  N, E, D, qX, qY, qZ, qW
+ *  Note that for GPS correction, we're only using the position not the rotation. The
+ *  rotation is provided in the file for ground truth comparison.
+ */
+
+// GTSAM related includes.
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
+#include <fstream>
+#include <iostream>
+
+// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
+// #define USE_COMBINED
+
+using namespace gtsam;
+using namespace std;
+
+using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
+using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
+
+const string output_filename = "imuFactorExampleResults.csv";
+
+// This will either be PreintegratedImuMeasurements (for ImuFactor) or
+// PreintegratedCombinedMeasurements (for CombinedImuFactor).
+PreintegrationType *imu_preintegrated_;
+
+int main(int argc, char* argv[])
+{
+  string data_filename;
+  if (argc < 2) {
+    printf("using default CSV file\n");
+    data_filename = findExampleDataFile("imuAndGPSdata.csv");
+  } else {
+    data_filename = argv[1];
+  }
+
+  // Set up output file for plotting errors
+  FILE* fp_out = fopen(output_filename.c_str(), "w+");
+  fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
+
+  // Begin parsing the CSV file.  Input the first line for initialization.
+  // From there, we'll iterate through the file and we'll preintegrate the IMU
+  // or add in the GPS given the input.
+  ifstream file(data_filename.c_str());
+  string value;
+
+  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
+  Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
+  getline(file, value, ','); // i
+  for (int i=0; i<9; i++) {
+    getline(file, value, ',');
+    initial_state(i) = atof(value.c_str());
+  }
+  getline(file, value, '\n');
+  initial_state(9) = atof(value.c_str());
+  cout << "initial state:\n" << initial_state.transpose() << "\n\n";
+
+  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
+  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), 
+                                         initial_state(4), initial_state(5));
+  Point3 prior_point(initial_state.head<3>());
+  Pose3 prior_pose(prior_rotation, prior_point);
+  Vector3 prior_velocity(initial_state.tail<3>());
+  imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
+
+  Values initial_values;
+  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);  
+
+  // 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
+  noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
+  noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
+
+  // Add all prior factors (pose, velocity, bias) to the graph.
+  NonlinearFactorGraph *graph = new NonlinearFactorGraph();
+  graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
+  graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
+  graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
+
+  // We use the sensor specs to build the noise model for the IMU factor.
+  double accel_noise_sigma = 0.0003924;
+  double gyro_noise_sigma = 0.000205689024915;
+  double accel_bias_rw_sigma = 0.004905;
+  double gyro_bias_rw_sigma = 0.000001454441043;
+  Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
+  Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
+  Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
+  Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
+  Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
+  Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
+
+  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
+  // PreintegrationBase params:
+  p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
+  p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
+  // should be using 2nd order integration
+  // PreintegratedRotation params:
+  p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
+  // PreintegrationCombinedMeasurements params:
+  p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
+  p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
+  p->biasAccOmegaInt = bias_acc_omega_int;
+  
+#ifdef USE_COMBINED
+  imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
+#else
+  imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
+#endif
+
+  // Store previous state for the imu integration and the latest predicted outcome.
+  NavState prev_state(prior_pose, prior_velocity);
+  NavState prop_state = prev_state;
+  imuBias::ConstantBias prev_bias = prior_imu_bias;
+
+  // Keep track of the total error over the entire run for a simple performance metric.
+  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 
+                      // exactly the same, so keeping this for simplicity.
+
+  // All priors have been set up, now iterate through the data file.
+  while (file.good()) {
+
+    // Parse out first value
+    getline(file, value, ',');
+    int type = atoi(value.c_str());
+
+    if (type == 0) { // IMU measurement
+      Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
+      for (int i=0; i<5; ++i) {
+        getline(file, value, ',');
+        imu(i) = atof(value.c_str());
+      }
+      getline(file, value, '\n');
+      imu(5) = atof(value.c_str());
+
+      // Adding the IMU preintegration.
+      imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
+
+    } else if (type == 1) { // GPS measurement
+      Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
+      for (int i=0; i<6; ++i) {
+        getline(file, value, ',');
+        gps(i) = atof(value.c_str());
+      }
+      getline(file, value, '\n');
+      gps(6) = atof(value.c_str());
+
+      correction_count++;
+
+      // Adding IMU factor and GPS factor and optimizing.
+#ifdef USE_COMBINED
+      PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
+      CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
+                                   X(correction_count  ), V(correction_count  ),
+                                   B(correction_count-1), B(correction_count  ),
+                                   *preint_imu_combined);
+      graph->add(imu_factor);
+#else
+      PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
+      ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
+                           X(correction_count  ), V(correction_count  ),
+                           B(correction_count-1),
+                           *preint_imu);
+      graph->add(imu_factor);
+      imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
+      graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1), 
+                                                      B(correction_count  ), 
+                                                      zero_bias, bias_noise_model));
+#endif
+
+      noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
+      GPSFactor gps_factor(X(correction_count),
+                           Point3(gps(0),  // N,
+                                  gps(1),  // E,
+                                  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());
+      initial_values.insert(V(correction_count), prop_state.v());
+      initial_values.insert(B(correction_count), prev_bias);
+
+      LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
+      Values result = optimizer.optimize();
+
+      // Overwrite the beginning of the preintegration for the next step.
+      prev_state = NavState(result.at<Pose3>(X(correction_count)),
+                            result.at<Vector3>(V(correction_count)));
+      prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
+
+      // Reset the preintegration object.
+      imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
+
+      // Print out the position and orientation error for comparison.
+      Vector3 gtsam_position = prev_state.pose().translation();
+      Vector3 position_error = gtsam_position - gps.head<3>();
+      current_position_error = position_error.norm();
+
+      Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
+      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
+      Quaternion quat_error = gtsam_quat * gps_quat.inverse();
+      quat_error.normalize();
+      Vector3 euler_angle_error(quat_error.x()*2,
+                                 quat_error.y()*2,
+                                 quat_error.z()*2);
+      current_orientation_error = euler_angle_error.norm();
+
+      // display statistics
+      cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
+
+      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_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
+
+      output_time += 1.0;
+
+    } else {
+      cerr << "ERROR parsing file\n";
+      return 1;
+    }
+  }
+  fclose(fp_out);
+  cout << "Complete, results written to " << output_filename << "\n\n";;
+  return 0;
+}

examples/LocalizationExample.cpp

@@ -120,15 +120,15 @@ int main(int argc, char** argv) {
   // For simplicity, we will use the same noise model for each odometry factor
   noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
-  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
 
   // 2b. Add "GPS-like" measurements
   // We will use our custom UnaryFactor for this.
   noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
-  graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
+  graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
-  graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
+  graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
-  graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
+  graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
   graph.print("\nFactor Graph:\n"); // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution

examples/METISOrderingExample.cpp

@@ -37,12 +37,12 @@ int main(int argc, char** argv) {
 
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+  graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
 
   Pose2 odometry(2.0, 0.0, 0.0);
   noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
-  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
   graph.print("\nFactor Graph:\n"); // print
 
   Values initial;

examples/OdometryExample.cpp

@@ -65,15 +65,15 @@ int main(int argc, char** argv) {
   // A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
+  graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
 
   // Add odometry factors
   Pose2 odometry(2.0, 0.0, 0.0);
   // For simplicity, we will use the same noise model for each odometry factor
   noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
-  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
   graph.print("\nFactor Graph:\n"); // print
 
   // Create the data structure to hold the initialEstimate estimate to the solution

examples/PlanarSLAMExample.cpp

@@ -81,13 +81,13 @@ int main(int argc, char** argv) {
   // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
-  graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
+  graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
 
   // Add two odometry factors
   Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
   noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
-  graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
-  graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
 
   // Add Range-Bearing measurements to two different landmarks
   // create a noise model for the landmark measurements
@@ -101,9 +101,9 @@ int main(int argc, char** argv) {
          range32 = 2.0;
 
   // Add Bearing-Range factors
-  graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
+  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
-  graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
+  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
-  graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
+  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
 
   // Print
   graph.print("Factor Graph:\n");

examples/Pose2SLAMExample.cpp

@@ -72,23 +72,23 @@ int main(int argc, char** argv) {
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
+  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses
-  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
-  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
-  graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
-  graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
 
   // 2c. Add the loop closure constraint
   // This factor encodes the fact that we have returned to the same pose. In real systems,
   // these constraints may be identified in many ways, such as appearance-based techniques
   // with camera images. We will use another Between Factor to enforce this constraint:
-  graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
   graph.print("\nFactor Graph:\n"); // print
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution

examples/Pose2SLAMExample_graphviz.cpp

@@ -33,19 +33,19 @@ int main (int argc, char** argv) {
 
   // 2a. Add a prior on the first pose, setting it to the origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
+  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
-  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
-  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
-  graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
-  graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
 
   // 2c. Add the loop closure constraint
-  graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
 
   // 3. Create the data structure to hold the initial estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values

examples/Pose2SLAMwSPCG.cpp

@@ -36,18 +36,18 @@ int main(int argc, char** argv) {
   // 2a. Add a prior on the first pose, setting it to the origin
   Pose2 prior(0.0, 0.0, 0.0); // prior at origin
   noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
+  graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
 
   // 2b. Add odometry factors
   noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
-  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
-  graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
-  graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
+  graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 
   // 2c. Add the loop closure constraint
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
   graph.print("\nFactor Graph:\n"); // print
 
 

examples/Pose3SLAMExample_changeKeys.cpp

@@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    for(const Values::ConstKeyValuePair& key_value: *initial) {
       Key key;
       if(add)
         key = key_value.key + firstKey;
@@ -66,7 +66,7 @@ int main(const int argc, const char *argv[]) {
       simpleInitial.insert(key, initial->at(key_value.key));
     }
     NonlinearFactorGraph simpleGraph;
-    BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
+    for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
       boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
           boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
       if (pose3Between){

examples/Pose3SLAMExample_g2o.cpp

@@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

examples/README

@@ -1,37 +0,0 @@
-This directory contains a number of examples that illustrate the use of GTSAM:
-
-SimpleRotation:  a super-simple example of optimizing a single rotation according to a single prior
-
-Kalman Filter Examples
-======================
-elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs
-easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same
-fullStateKalmanFilter: simple 1D example with a full-state filter
-errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
-
-2D Pose SLAM 
-============
-LocalizationExample.cpp: modeling robot motion
-LocalizationExample2.cpp: example with GPS like measurements
-Pose2SLAMExample: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
-Pose2SLAMExample_advanced: same, but uses an Optimizer object
-Pose2SLAMwSPCG: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
-
-Planar SLAM with landmarks
-==========================
-PlanarSLAMExample: simple robotics example using the pre-built planar SLAM domain
-PlanarSLAMExample_selfcontained: simple robotics example with all typedefs internal to this script.
-
-Visual SLAM
-===========
-CameraResectioning.cpp: An example of gtsam for solving the camera resectioning problem
-The directory vSLAMexample includes 2 simple examples using GTSAM:
-- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
-- vISAMexample using visualSLAM and ISAM for incremental SLAM updates
-See the separate README file there.
-
-Undirected Graphical Models (UGM)
-=================================
-The best representation for a Markov Random Field is a factor graph :-)
-This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
-can be found at http://www.di.ens.fr/~mschmidt/Software/UGM

examples/README.md

@@ -0,0 +1,96 @@
+# GTSAM Examples
+
+This directory contains all GTSAM C++ examples GTSAM pertaining to SFM
+
+
+## Basic Examples:
+
+* **SimpleRotation**:  a simple example of optimizing a single rotation according to a single prior
+* **CameraResectioning**: resection camera from some known points
+* **SFMExample**: basic structure from motion
+* **SFMExample_bal**: same, but read data from read from BAL file
+* **SelfCalibrationExample**: Do SFM while also optimizing for calibration
+
+## Stereo Visual Odometry Examples
+Visual odometry using a stereo rig:
+
+* **StereoVOExample**: basic example of stereo VO
+* **StereoVOExample_large**: larger, with a snippet of Kitti data
+
+## More Advanced Examples
+The following examples illustrate some concepts from Georgia Tech's research papers, listed in the references section at the end:
+
+* **VisualISAMExample**: uses iSAM [TRO08]
+* **VisualISAM2Example**: uses iSAM2 [IJRR12]
+* **SFMExample_SmartFactor**: uses smartFactors [ICRA14]
+
+## Kalman Filter Examples
+* **elaboratePoint2KalmanFilter**: simple linear Kalman filter on a moving 2D point, but done using factor graphs
+* **easyPoint2KalmanFilter**: uses the generic templated Kalman filter class to do the same
+* **fullStateKalmanFilter**: simple 1D example with a full-state filter
+* **errorStateKalmanFilter**: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
+
+## 2D Pose SLAM 
+
+* **LocalizationExample.cpp**: modeling robot motion
+* **LocalizationExample2.cpp**: example with GPS like measurements
+* **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
+* **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
+* **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
+
+## Planar SLAM with landmarks
+* **PlanarSLAMExample**: simple robotics example using the pre-built planar SLAM domain
+* **PlanarSLAMExample_selfcontained**: simple robotics example with all typedefs internal to this script.
+
+## Visual SLAM
+
+The directory **vSLAMexample** includes 2 simple examples using GTSAM:
+
+- **vSFMexample** using visual SLAM for structure-from-motion (SFM)
+- **vISAMexample** using visual SLAM and ISAM for incremental SLAM updates
+
+See the separate README file there.
+
+##Undirected Graphical Models (UGM)
+The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
+can be found at <http://www.di.ens.fr/~mschmidt/Software/UGM>
+
+
+##Building and Running
+To build, cd into the directory and do:
+
+```
+mkdir build
+cd build
+cmake ..
+```
+
+For each .cpp file in this directory two make targets are created, one to build the executable, and one to build and run it. For example, the file `CameraResectioning.cpp` contains simple example to resection a camera from 4 known points. You can build it using
+
+```
+make CameraResectioning
+```
+or build and run it immediately with
+
+```
+make CameraResectioning.run
+```
+which should output:
+
+```
+Final result:
+Values with 1 values:
+Value x1: R:
+[
+           1,	         0.0,	         0.0,	
+         0.0,	          -1,	         0.0,	
+         0.0,	         0.0,	          -1,	
+];
+t: [0, 0, 2]';
+```
+
+
+## References
+- [TRO08]: [iSAM: Incremental Smoothing and Mapping, Michael Kaess](http://frank.dellaert.com/pub/Kaess08tro.pdf), Michael Kaess, Ananth Ranganathan, and Frank Dellaert, IEEE Transactions on Robotics, 2008
+- [IJRR12]: [iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree](http://www.cc.gatech.edu/~dellaert/pub/Kaess12ijrr.pdf), Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert, International Journal of Robotics Research, 2012
+- [ICRA14]: [Eliminating Conditionally Independent Sets in Factor Graphs: A Unifying Perspective based on Smart Factors](http://frank.dellaert.com/pub/Carlone14icra.pdf), Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, IEEE International Conference on Robotics and Automation (ICRA), 2014

examples/RangeISAMExample_plaza2.cpp

@@ -43,7 +43,6 @@
 #include <gtsam/slam/dataset.h>
 
 // Standard headers, added last, so we know headers above work on their own
-#include <boost/foreach.hpp>
 #include <fstream>
 #include <iostream>
 
@@ -151,7 +150,7 @@ int main (int argc, char** argv) {
 
   // Loop over odometry
   gttic_(iSAM);
-  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
+  for(const TimedOdometry& timedOdometry: odometry) {
     //--------------------------------- odometry loop -----------------------------------------
     double t;
     Pose2 odometry;
@@ -196,7 +195,7 @@ int main (int argc, char** argv) {
     }
     i += 1;
     //--------------------------------- odometry loop -----------------------------------------
-  } // BOOST_FOREACH
+  } // end for
   gttoc_(iSAM);
 
   // Print timings

examples/SFMExample.cpp

@@ -75,14 +75,14 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
+  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t i = 0; i < poses.size(); ++i) {
     SimpleCamera camera(poses[i], *K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
-      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
     }
   }
 
@@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
   // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
   // between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
   noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-  graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+  graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
   graph.print("Factor Graph:\n");
 
   // Create the data structure to hold the initial estimate to the solution

examples/SFMExampleExpressions_bal.cpp

@@ -78,10 +78,10 @@ int main(int argc, char* argv[]) {
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   size_t j = 0;
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+  for(const SfM_Track& track: mydata.tracks) {
     // Leaf expression for j^th point
     Point3_ point_('p', j);
-    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+    for(const SfM_Measurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       // Leaf expression for i^th camera
@@ -98,9 +98,9 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0;
   j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
+  for(const SfM_Camera& camera: mydata.cameras)
     initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
+  for(const SfM_Track& track: mydata.tracks)
     initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */

examples/SFMExample_SmartFactor.cpp

@@ -82,13 +82,13 @@ int main(int argc, char* argv[]) {
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
+  graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is
   // still under-constrained. Here we add a prior on the second pose x1, so this will
   // fix the scale by indicating the distance between x0 and x1.
   // Because these two are fixed, the rest of the poses will be also be fixed.
-  graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
+  graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
 
   graph.print("Factor Graph:\n");
 
@@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
   Values initialEstimate;
   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)
-    initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
+    initialEstimate.insert(i, poses[i].compose(delta));
   initialEstimate.print("Initial Estimates:\n");
 
   // Optimize the graph and print results

examples/SFMExample_SmartFactorPCG.cpp

@@ -74,16 +74,16 @@ int main(int argc, char* argv[]) {
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
+  graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
 
   // Fix the scale ambiguity by adding a prior
-  graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
+  graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
 
   // Create the initial estimate to the solution
   Values initialEstimate;
   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)
-    initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
+    initialEstimate.insert(i, poses[i].compose(delta));
 
   // We will use LM in the outer optimization loop, but by specifying "Iterative" below
   // We indicate that an iterative linear solver should be used.

examples/SFMExample_bal.cpp

@@ -55,25 +55,25 @@ int main (int argc, char* argv[]) {
 
   // Add measurements to the factor graph
   size_t j = 0;
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+  for(const SfM_Track& track: mydata.tracks) {
-    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+    for(const SfM_Measurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
-      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
+      graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
     }
     j += 1;
   }
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
+  graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
+  graph.emplace_shared<PriorFactor<Point3> >    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;
   size_t i = 0; j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+  for(const SfM_Track& track: mydata.tracks)    initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -60,25 +60,25 @@ int main (int argc, char* argv[]) {
 
   // Add measurements to the factor graph
   size_t j = 0;
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+  for(const SfM_Track& track: mydata.tracks) {
-    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+    for(const SfM_Measurement& m: track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
-      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
+      graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
     }
     j += 1;
   }
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
+  graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
+  graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;
   size_t i = 0; j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+  for(const SfM_Track& track: mydata.tracks)    initial.insert(P(j++), track.p);
 
   /** ---------------  COMPARISON  -----------------------**/
   /** ----------------------------------------------------**/

examples/SelfCalibrationExample.cpp

@@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x1.
   noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+  graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
@@ -65,17 +65,17 @@ int main(int argc, char* argv[]) {
       Point2 measurement = camera.project(points[j]);
       // The only real difference with the Visual SLAM example is that here we use a
       // different factor type, that also calculates the Jacobian with respect to calibration
-      graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
+      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
     }
   }
 
   // Add a prior on the position of the first landmark.
   noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-  graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+  graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
 
   // Add a prior on the calibration.
   noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
-  graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
+  graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
 
   // Create the initial estimate to the solution
   // now including an estimate on the camera calibration parameters

examples/SolverComparer.cpp

@@ -50,6 +50,7 @@
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
+#include <boost/range/adaptor/reversed.hpp>
 #include <boost/random.hpp>
 #include <boost/serialization/export.hpp>
 
@@ -80,7 +81,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c
   // the factor graph already includes a factor for the prior/equality constraint.
   //  double dof = graph.size() - config.size();
   int graph_dim = 0;
-  BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
+  for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
     graph_dim += (int)nlf->dim();
   }
   double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
@@ -421,9 +422,9 @@ void runIncremental()
   //try {
   //  Marginals marginals(graph, values);
   //  int i=0;
-  //  BOOST_REVERSE_FOREACH(Key key1, values.keys()) {
+  //  for (Key key1: boost::adaptors::reverse(values.keys())) {
   //    int j=0;
-  //    BOOST_REVERSE_FOREACH(Key key2, values.keys()) {
+  //    for (Key key12: boost::adaptors::reverse(values.keys())) {
   //      if(i != j) {
   //        gttic_(jointMarginalInformation);
   //        std::vector<Key> keys(2);
@@ -442,7 +443,7 @@ void runIncremental()
   //      break;
   //  }
   //  tictoc_print_();
-  //  BOOST_FOREACH(Key key, values.keys()) {
+  //  for(Key key: values.keys()) {
   //    gttic_(marginalInformation);
   //    Matrix info = marginals.marginalInformation(key);
   //    gttoc_(marginalInformation);
@@ -535,7 +536,7 @@ void runCompare()
   vector<Key> commonKeys;
   br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
   double maxDiff = 0.0;
-  BOOST_FOREACH(Key j, commonKeys)
+  for(Key j: commonKeys)
     maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
   cout << "  Maximum solution difference (norm of logmap): " << maxDiff << endl;
 }
@@ -549,7 +550,7 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
+  for(const Values::KeyValuePair& key_val: initial)
   {
     Vector noisev(key_val.value.dim());
     for(Vector::Index i = 0; i < noisev.size(); ++i)

examples/StereoVOExample.cpp

@@ -39,7 +39,7 @@ int main(int argc, char** argv){
   //create graph object, add first pose at origin with key '1'
   NonlinearFactorGraph graph;
   Pose3 first_pose;
-  graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
+  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);
@@ -47,14 +47,14 @@ int main(int argc, char** argv){
   const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
 
   //create and add stereo factors between first pose (key value 1) and the three landmarks
-  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
+  graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
-  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
+  graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(120, 80, 440), model, 1, 4, K);
-  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
+  graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 280, 140), model, 1, 5, K);
 
   //create and add stereo factors between second pose and the three landmarks
-  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
+  graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(570, 520, 490), model, 2, 3, K);
-  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
+  graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
-  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
+  graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
 
   //create Values object to contain initial estimates of camera poses and landmark locations
   Values initial_estimate;
@@ -67,7 +67,7 @@ int main(int argc, char** argv){
   initial_estimate.insert(5, Point3(0, -0.5, 5));
 
   //create Levenberg-Marquardt optimizer for resulting factor graph, optimize
-  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
   Values result = optimizer.optimize();
 
   result.print("Final result:\n");

examples/StereoVOExample_large.cpp

@@ -83,9 +83,9 @@ int main(int argc, char** argv){
   cout << "Reading stereo factors" << endl;
   //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
-    graph.push_back(
+    graph.emplace_shared<
-        GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
+        GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
-            Symbol('x', x), Symbol('l', l), K));
+            Symbol('x', x), Symbol('l', l), K);
     //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
     if (!initial_estimate.exists(Symbol('l', l))) {
       Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
@@ -99,13 +99,13 @@ int main(int argc, char** argv){
   //constrain the first pose such that it cannot change from its original value during optimization
   // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
   // QR is much slower than Cholesky, but numerically more stable
-  graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
+  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
 
   cout << "Optimizing" << endl;
   //create Levenberg-Marquardt optimizer to optimize the factor graph
   LevenbergMarquardtParams params;
   params.orderingType = Ordering::METIS;
-  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
+  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;

examples/TimeTBB.cpp

@@ -19,8 +19,8 @@
 #include <gtsam/base/Matrix.h>
 
 #include <boost/assign/list_of.hpp>
-#include <boost/foreach.hpp>
 #include <map>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;
@@ -76,7 +76,7 @@ map<int, double> testWithoutMemoryAllocation()
 
   const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
   map<int, double> timingResults;
-  BOOST_FOREACH(size_t grainSize, grainSizes)
+  for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
     tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
@@ -129,7 +129,7 @@ map<int, double> testWithMemoryAllocation()
 
   const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
   map<int, double> timingResults;
-  BOOST_FOREACH(size_t grainSize, grainSizes)
+  for(size_t grainSize: grainSizes)
   {
     tbb::tick_count t0 = tbb::tick_count::now();
     tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
@@ -150,7 +150,7 @@ int main(int argc, char* argv[])
   const vector<int> numThreads = list_of(1)(4)(8);
   Results results;
 
-  BOOST_FOREACH(size_t n, numThreads)
+  for(size_t n: numThreads)
   {
     cout << "With " << n << " threads:" << endl;
     tbb::task_scheduler_init init((int)n);
@@ -160,19 +160,19 @@ int main(int argc, char* argv[])
   }
 
   cout << "Summary of results:" << endl;
-  BOOST_FOREACH(const Results::value_type& threads_result, results)
+  for(const Results::value_type& threads_result: results)
   {
     const int threads = threads_result.first;
     const ResultWithThreads& result = threads_result.second;
     if(threads != 1)
     {
-      BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation)
+      for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation)
       {
         const int grainsize = grainsize_time.first;
         const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
         cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
       }
-      BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation)
+      for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation)
       {
         const int grainsize = grainsize_time.first;
         const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;

examples/VisualISAM2Example.cpp

@@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
     for (size_t j = 0; j < points.size(); ++j) {
       SimpleCamera camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
-      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
     }
 
     // Add an initial guess for the current pose
@@ -104,11 +104,11 @@ int main(int argc, char* argv[]) {
     if( i == 0) {
       // Add a prior on pose x0
       noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+      graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
 
       // Add a prior on landmark l0
       noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+      graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
 
       // Add initial guesses to all observed landmarks
       // Intentionally initialize the variables off from the ground truth

examples/VisualISAMExample.cpp

@@ -89,9 +89,8 @@ int main(int argc, char* argv[]) {
       SimpleCamera camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
       // Add measurement
-      graph.add(
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,
-          GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
+              Symbol('x', i), Symbol('l', j), K);
-              Symbol('x', i), Symbol('l', j), K));
     }
 
     // Intentionally initialize the variables off from the ground truth
@@ -109,12 +108,12 @@ int main(int argc, char* argv[]) {
       // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
       noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
           (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-      graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+      graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
 
       // Add a prior on landmark l0
       noiseModel::Isotropic::shared_ptr pointNoise =
           noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
+      graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
 
       // Add initial guesses to all observed landmarks
       Point3 noise(-0.25, 0.20, 0.15);

examples/easyPoint2KalmanFilter.cpp

@@ -70,7 +70,7 @@ int main() {
 
   // Predict the new value with the EKF class
   Point2 x1_predict = ekf.predict(factor1);
-  x1_predict.print("X1 Predict");
+  traits<Point2>::Print(x1_predict, "X1 Predict");
 
 
 
@@ -91,7 +91,7 @@ int main() {
 
   // Update the Kalman Filter with the measurement
   Point2 x1_update = ekf.update(factor2);
-  x1_update.print("X1 Update");
+  traits<Point2>::Print(x1_update, "X1 Update");
 
 
 
@@ -101,13 +101,13 @@ int main() {
   difference = Point2(1,0);
   BetweenFactor<Point2> factor3(x1, x2, difference, Q);
   Point2 x2_predict = ekf.predict(factor1);
-  x2_predict.print("X2 Predict");
+  traits<Point2>::Print(x2_predict, "X2 Predict");
 
   // Update
   Point2 z2(2.0, 0.0);
   PriorFactor<Point2> factor4(x2, z2, R);
   Point2 x2_update = ekf.update(factor4);
-  x2_update.print("X2 Update");
+  traits<Point2>::Print(x2_update, "X2 Update");
 
 
 
@@ -117,13 +117,13 @@ int main() {
   difference = Point2(1,0);
   BetweenFactor<Point2> factor5(x2, x3, difference, Q);
   Point2 x3_predict = ekf.predict(factor5);
-  x3_predict.print("X3 Predict");
+  traits<Point2>::Print(x3_predict, "X3 Predict");
 
   // Update
   Point2 z3(3.0, 0.0);
   PriorFactor<Point2> factor6(x3, z3, R);
   Point2 x3_update = ekf.update(factor6);
-  x3_update.print("X3 Update");
+  traits<Point2>::Print(x3_update, "X3 Update");
 
   return 0;
 }

gtsam.h

@@ -266,23 +266,12 @@ class Point2 {
 
   // Group
   static gtsam::Point2 identity();
-  gtsam::Point2 inverse() const;
-  gtsam::Point2 compose(const gtsam::Point2& p2) const;
-  gtsam::Point2 between(const gtsam::Point2& p2) const;
-
-  // Manifold
-  gtsam::Point2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Point2& p) const;
-
-  // Lie Group
-  static gtsam::Point2 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Point2& p);
 
   // Standard Interface
   double x() const;
   double y() const;
   Vector vector() const;
-  double dist(const gtsam::Point2& p2) const;
+  double distance(const gtsam::Point2& p2) const;
   double norm() const;
 
   // enabling serialization functionality
@@ -1368,7 +1357,7 @@ virtual class HessianFactor : gtsam::GaussianFactor {
 
   //Standard Interface
   size_t rows() const;
-  Matrix info() const;
+  Matrix information() const;
   double constantTerm() const;
   Vector linearTerm() const;
 
@@ -1941,10 +1930,10 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
   gtsam::JacobianFactor* toJacobian() const;
   gtsam::HessianFactor* toHessian() const;
 
-  static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
+  static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
       const gtsam::Values& linearizationPoint);
 
-  static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph);
+  static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph);
 
   // enabling serialization functionality
   void serializable() const;
@@ -2506,30 +2495,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   bool   getUse2ndOrderCoriolis()     const;
 };
 
-#include <gtsam/navigation/PreintegrationBase.h>
-virtual class PreintegrationBase {
-  // Constructors
-  PreintegrationBase(const gtsam::PreintegrationParams* params);
-  PreintegrationBase(const gtsam::PreintegrationParams* params,
-      const gtsam::imuBias::ConstantBias& bias);
-
-  // Testable
-  void print(string s) const;
-  bool equals(const gtsam::PreintegrationBase& expected, double tol);
-
-  double deltaTij() const;
-  gtsam::Rot3 deltaRij() const;
-  Vector deltaPij() const;
-  Vector deltaVij() const;
-  Vector biasHatVector() const;
-
-  // Standard Interface
-  gtsam::NavState predict(const gtsam::NavState& state_i,
-      const gtsam::imuBias::ConstantBias& bias) const;
-};
-
 #include <gtsam/navigation/ImuFactor.h>
-virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
+class PreintegratedImuMeasurements {
   // Constructors
   PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
   PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
@@ -2544,6 +2511,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
       double deltaT);
   void resetIntegration();
   Matrix preintMeasCov() const;
+  double deltaTij() const;
+  gtsam::Rot3 deltaRij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  Vector biasHatVector() const;
+  gtsam::NavState predict(const gtsam::NavState& state_i,
+      const gtsam::imuBias::ConstantBias& bias) const;
 };
 
 virtual class ImuFactor: gtsam::NonlinearFactor {
@@ -2559,7 +2533,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
-virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
+class PreintegratedCombinedMeasurements {
   // Testable
   void print(string s) const;
   bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
@@ -2570,6 +2544,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
       double deltaT);
   void resetIntegration();
   Matrix preintMeasCov() const;
+  double deltaTij() const;
+  gtsam::Rot3 deltaRij() const;
+  Vector deltaPij() const;
+  Vector deltaVij() const;
+  Vector biasHatVector() const;
+  gtsam::NavState predict(const gtsam::NavState& state_i,
+      const gtsam::imuBias::ConstantBias& bias) const;
 };
 
 virtual class CombinedImuFactor: gtsam::NonlinearFactor {

gtsam/3rdparty/CCOLAMD/Demo/Makefile

@@ -0,0 +1,49 @@
+#-----------------------------------------------------------------------------
+# compile the CCOLAMD demo
+#-----------------------------------------------------------------------------
+
+default: all
+
+include ../../SuiteSparse_config/SuiteSparse_config.mk
+
+I = -I../../include
+
+C = $(CC) $(CF) $(I)
+
+LIB2 = $(LDFLAGS) -L../../lib -lccolamd -lsuitesparseconfig $(LDLIBS)
+
+all: library ccolamd_example ccolamd_l_example
+
+library:
+	( cd ../../SuiteSparse_config ; $(MAKE) )
+	( cd ../Lib ; $(MAKE) )
+
+#------------------------------------------------------------------------------
+# Create the demo program, run it, and compare the output
+#------------------------------------------------------------------------------
+
+dist:
+
+ccolamd_example: ccolamd_example.c
+	$(C) -o ccolamd_example ccolamd_example.c $(LIB2)
+	- ./ccolamd_example > my_ccolamd_example.out
+	- diff ccolamd_example.out my_ccolamd_example.out
+
+ccolamd_l_example: ccolamd_l_example.c
+	$(C) -o ccolamd_l_example ccolamd_l_example.c $(LIB2)
+	- ./ccolamd_l_example > my_ccolamd_l_example.out
+	- diff ccolamd_l_example.out my_ccolamd_l_example.out
+
+#------------------------------------------------------------------------------
+# Remove all but the files in the original distribution
+#------------------------------------------------------------------------------
+
+clean:
+	- $(RM) -r $(CLEAN)
+
+purge: distclean
+
+distclean: clean
+	- $(RM) ccolamd_example ccolamd_l_example
+	- $(RM) my_ccolamd_example.out my_ccolamd_l_example.out
+	- $(RM) -r $(PURGE)

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /*

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out

@@ -15,7 +15,7 @@ Column 3, with 2 entries:
     row 1
     row 3
 
-ccolamd version 2.7, Jan 25, 2011: OK.  
+ccolamd version 2.9, May 4, 2016: OK.  
 ccolamd: number of dense or empty rows ignored:    0
 ccolamd: number of dense or empty columns ignored: 0
 ccolamd: number of garbage collections performed:  0
@@ -38,7 +38,7 @@ Column 3, with 1 entries:
     row 4
 Column 4, with 0 entries:
 
-csymamd version 2.7, Jan 25, 2011: OK.  
+csymamd version 2.9, May 4, 2016: OK.  
 csymamd: number of dense or empty rows ignored:    0
 csymamd: number of dense or empty columns ignored: 0
 csymamd: number of garbage collections performed:  0

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c

@@ -1,12 +1,10 @@
 /* ========================================================================== */
-/* === ccolamd and csymamd example (UF_long integer version) ================ */
+/* === ccolamd and csymamd example (long integer version) =================== */
 /* ========================================================================== */
 
 /* ----------------------------------------------------------------------------
  * CCOLAMD Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /*
@@ -46,9 +44,6 @@
 #define B_NNZ 4
 #define B_N 5
 
-/* define UF_long */
-#include "UFconfig.h"
-
 int main (void)
 {
 
@@ -56,14 +51,14 @@ int main (void)
     /* input matrix A definition */
     /* ====================================================================== */
 
-    UF_long A [ALEN] = {
+    SuiteSparse_long A [ALEN] = {
 
     	0, 1, 4,		/* row indices of nonzeros in column 0 */
 	2, 4,			/* row indices of nonzeros in column 1 */
 	0, 1, 2, 3,		/* row indices of nonzeros in column 2 */
 	1, 3} ;			/* row indices of nonzeros in column 3 */
 
-    UF_long p [ ] = {
+    SuiteSparse_long p [ ] = {
 
     	0,			/* column 0 is in A [0..2] */
 	3,			/* column 1 is in A [3..4] */ 
@@ -75,7 +70,7 @@ int main (void)
     /* input matrix B definition */
     /* ====================================================================== */
 
-    UF_long B [ ] = {		/* Note: only strictly lower triangular part */
+    SuiteSparse_long B [ ] = {  /* Note: only strictly lower triangular part */
     				/* is included, since symamd ignores the */
 				/* diagonal and upper triangular part of B. */
 
@@ -85,7 +80,7 @@ int main (void)
     	4			/* row indices of nonzeros in column 3 */
     	} ;			/* row indices of nonzeros in column 4 (none) */
 
-    UF_long q [ ] = {
+    SuiteSparse_long q [ ] = {
 
     	0,			/* column 0 is in B [0] */
 	1,			/* column 1 is in B [1..2] */ 
@@ -98,10 +93,9 @@ int main (void)
     /* other variable definitions */
     /* ====================================================================== */
 
-    UF_long perm [B_N+1] ;	    /* note the size is N+1 */
+    SuiteSparse_long perm [B_N+1] ;	    /* note the size is N+1 */
-    UF_long stats [CCOLAMD_STATS] ; /* for ccolamd and csymamd output stats */
+    SuiteSparse_long stats [CCOLAMD_STATS] ;  /* ccolamd/csymamd output stats */
-
+    SuiteSparse_long row, col, pp, length, ok ;
-    UF_long row, col, pp, length, ok ;
 
     /* ====================================================================== */
     /* dump the input matrix A */

gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out

@@ -15,7 +15,7 @@ Column 3, with 2 entries:
     row 1
     row 3
 
-ccolamd version 2.7, Jan 25, 2011: OK.  
+ccolamd version 2.9, May 4, 2016: OK.  
 ccolamd: number of dense or empty rows ignored:    0
 ccolamd: number of dense or empty columns ignored: 0
 ccolamd: number of garbage collections performed:  0
@@ -38,7 +38,7 @@ Column 3, with 1 entries:
     row 4
 Column 4, with 0 entries:
 
-csymamd version 2.7, Jan 25, 2011: OK.  
+csymamd version 2.9, May 4, 2016: OK.  
 csymamd: number of dense or empty rows ignored:    0
 csymamd: number of dense or empty columns ignored: 0
 csymamd: number of garbage collections performed:  0

gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_example.out

@@ -1,50 +0,0 @@
-ccolamd 5-by-4 input matrix:
-Column 0, with 3 entries:
-    row 0
-    row 1
-    row 4
-Column 1, with 2 entries:
-    row 2
-    row 4
-Column 2, with 4 entries:
-    row 0
-    row 1
-    row 2
-    row 3
-Column 3, with 2 entries:
-    row 1
-    row 3
-
-ccolamd version 2.7, Jan 25, 2011: OK.  
-ccolamd: number of dense or empty rows ignored:    0
-ccolamd: number of dense or empty columns ignored: 0
-ccolamd: number of garbage collections performed:  0
-ccolamd column ordering:
-1st column: 1
-2nd column: 0
-3rd column: 3
-4th column: 2
-
-
-csymamd 5-by-5 input matrix:
-Entries in strictly lower triangular part:
-Column 0, with 1 entries:
-    row 1
-Column 1, with 2 entries:
-    row 2
-    row 3
-Column 2, with 0 entries:
-Column 3, with 1 entries:
-    row 4
-Column 4, with 0 entries:
-
-csymamd version 2.7, Jan 25, 2011: OK.  
-csymamd: number of dense or empty rows ignored:    0
-csymamd: number of dense or empty columns ignored: 0
-csymamd: number of garbage collections performed:  0
-csymamd column ordering:
-1st row/column: 0
-2nd row/column: 2
-3rd row/column: 1
-4th row/column: 3
-5th row/column: 4

gtsam/3rdparty/CCOLAMD/Demo/my_ccolamd_l_example.out

@@ -1,50 +0,0 @@
-ccolamd 5-by-4 input matrix:
-Column 0, with 3 entries:
-    row 0
-    row 1
-    row 4
-Column 1, with 2 entries:
-    row 2
-    row 4
-Column 2, with 4 entries:
-    row 0
-    row 1
-    row 2
-    row 3
-Column 3, with 2 entries:
-    row 1
-    row 3
-
-ccolamd version 2.7, Jan 25, 2011: OK.  
-ccolamd: number of dense or empty rows ignored:    0
-ccolamd: number of dense or empty columns ignored: 0
-ccolamd: number of garbage collections performed:  0
-ccolamd_l column ordering:
-1st column: 1
-2nd column: 0
-3rd column: 3
-4th column: 2
-
-
-csymamd_l 5-by-5 input matrix:
-Entries in strictly lower triangular part:
-Column 0, with 1 entries:
-    row 1
-Column 1, with 2 entries:
-    row 2
-    row 3
-Column 2, with 0 entries:
-Column 3, with 1 entries:
-    row 4
-Column 4, with 0 entries:
-
-csymamd version 2.7, Jan 25, 2011: OK.  
-csymamd: number of dense or empty rows ignored:    0
-csymamd: number of dense or empty columns ignored: 0
-csymamd: number of garbage collections performed:  0
-csymamd_l column ordering:
-1st row/column: 0
-2nd row/column: 2
-3rd row/column: 1
-4th row/column: 3
-5th row/column: 4

gtsam/3rdparty/CCOLAMD/Doc/ChangeLog

@@ -1,3 +1,41 @@
+May 4, 2016: version 2.9.6
+
+    * minor changes to Makefile
+
+Apr 1, 2016: version 2.9.5
+
+    * licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt
+
+Feb 1, 2016: version 2.9.4
+
+    * update to Makefiles
+
+Jan 30, 2016: version 2.9.3
+
+    * modifications to Makefiles
+
+Jan 1, 2016: version 2.9.2
+
+    * modified Makefile to create shared libraries
+        No change to C code except version number.
+        The empty file ccolamd_global.c removed.
+
+Oct 10, 2014: version 2.9.1
+
+    modified MATLAB/ccolamd_make.m.  No change to C code except version number.
+
+July 31, 2013: version 2.9.0
+
+    * changed malloc and printf pointers to use SuiteSparse_config
+
+Jun 1, 2012: version 2.8.0
+
+    * changed from UFconfig to SuiteSparse_config
+
+Dec 7, 2011: version 2.7.4
+
+    * fixed the Makefile to better align with CFLAGS and other standards
+
 Jan 25, 2011: version 2.7.3
 
     * minor fix to "make install"

gtsam/3rdparty/CCOLAMD/Doc/License.txt

@@ -0,0 +1,33 @@
+CCOLAMD: constrained column approximate minimum degree ordering
+Copyright (C) 2005-2016, Univ. of Florida.  Authors: Timothy A. Davis,
+Sivasankaran Rajamanickam, and Stefan Larimore.  Closely based on COLAMD by
+Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert.
+http://www.suitesparse.com
+
+--------------------------------------------------------------------------------
+
+CCOLAMD license: BSD 3-clause:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+        * Redistributions of source code must retain the above copyright
+          notice, this list of conditions and the following disclaimer.
+        * Redistributions in binary form must reproduce the above copyright
+          notice, this list of conditions and the following disclaimer in the
+          documentation and/or other materials provided with the distribution.
+        * Neither the name of the organizations to which the authors are
+          affiliated, nor the names of its contributors may be used to endorse
+          or promote products derived from this software without specific prior
+          written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+    OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+    DAMAGE.

gtsam/3rdparty/CCOLAMD/Include/ccolamd.h

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /*
@@ -32,24 +30,24 @@ extern "C" {
 /* All versions of CCOLAMD will include the following definitions.
  * As an example, to test if the version you are using is 1.3 or later:
  *
- *  if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ...
+ *	if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ...
  *
  * This also works during compile-time:
  *
- *  #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)
+ *	#if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)
- *      printf ("This is version 1.3 or later\n") ;
+ *	    printf ("This is version 1.3 or later\n") ;
- *  #else
+ *	#else
- *      printf ("This is an early version\n") ;
+ *	    printf ("This is an early version\n") ;
- *  #endif
+ *	#endif
  */
 
-#define CCOLAMD_DATE "Jan 25, 2011"
+#define CCOLAMD_DATE "May 4, 2016"
 #define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub))
 #define CCOLAMD_MAIN_VERSION 2
-#define CCOLAMD_SUB_VERSION 7
+#define CCOLAMD_SUB_VERSION 9
-#define CCOLAMD_SUBSUB_VERSION 3
+#define CCOLAMD_SUBSUB_VERSION 6
 #define CCOLAMD_VERSION \
-  CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION)
+	CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION)
 
 /* ========================================================================== */
 /* === Knob and statistics definitions ====================================== */
@@ -94,106 +92,105 @@ extern "C" {
 #define CCOLAMD_NEWLY_EMPTY_COL 10
 
 /* error codes returned in stats [3]: */
-#define CCOLAMD_OK        (0)
+#define CCOLAMD_OK				(0)
-#define CCOLAMD_OK_BUT_JUMBLED      (1)
+#define CCOLAMD_OK_BUT_JUMBLED			(1)
-#define CCOLAMD_ERROR_A_not_present    (-1)
+#define CCOLAMD_ERROR_A_not_present		(-1)
-#define CCOLAMD_ERROR_p_not_present    (-2)
+#define CCOLAMD_ERROR_p_not_present		(-2)
-#define CCOLAMD_ERROR_nrow_negative    (-3)
+#define CCOLAMD_ERROR_nrow_negative		(-3)
-#define CCOLAMD_ERROR_ncol_negative    (-4)
+#define CCOLAMD_ERROR_ncol_negative		(-4)
-#define CCOLAMD_ERROR_nnz_negative    (-5)
+#define CCOLAMD_ERROR_nnz_negative		(-5)
-#define CCOLAMD_ERROR_p0_nonzero    (-6)
+#define CCOLAMD_ERROR_p0_nonzero		(-6)
-#define CCOLAMD_ERROR_A_too_small    (-7)
+#define CCOLAMD_ERROR_A_too_small		(-7)
-#define CCOLAMD_ERROR_col_length_negative  (-8)
+#define CCOLAMD_ERROR_col_length_negative	(-8)
-#define CCOLAMD_ERROR_row_index_out_of_bounds  (-9)
+#define CCOLAMD_ERROR_row_index_out_of_bounds	(-9)
-#define CCOLAMD_ERROR_out_of_memory    (-10)
+#define CCOLAMD_ERROR_out_of_memory		(-10)
-#define CCOLAMD_ERROR_invalid_cmember    (-11)
+#define CCOLAMD_ERROR_invalid_cmember		(-11)
-#define CCOLAMD_ERROR_internal_error    (-999)
+#define CCOLAMD_ERROR_internal_error		(-999)
 
 /* ========================================================================== */
 /* === Prototypes of user-callable routines ================================= */
 /* ========================================================================== */
 
-/* define UF_long */
+#include "SuiteSparse_config.h"
-#include "UFconfig.h"
 
-size_t ccolamd_recommended  /* returns recommended value of Alen, */
+size_t ccolamd_recommended	/* returns recommended value of Alen, */
-        /* or 0 if input arguments are erroneous */
+				/* or 0 if input arguments are erroneous */
 (
-    int nnz,      /* nonzeros in A */
+    int nnz,			/* nonzeros in A */
-    int n_row,      /* number of rows in A */
+    int n_row,			/* number of rows in A */
-    int n_col      /* number of columns in A */
+    int n_col			/* number of columns in A */
 ) ;
 
-size_t ccolamd_l_recommended  /* returns recommended value of Alen, */
+size_t ccolamd_l_recommended	/* returns recommended value of Alen, */
-        /* or 0 if input arguments are erroneous */
+				/* or 0 if input arguments are erroneous */
 (
-    UF_long nnz,    /* nonzeros in A */
+    SuiteSparse_long nnz,		/* nonzeros in A */
-    UF_long n_row,    /* number of rows in A */
+    SuiteSparse_long n_row,		/* number of rows in A */
-    UF_long n_col    /* number of columns in A */
+    SuiteSparse_long n_col		/* number of columns in A */
 ) ;
 
-void ccolamd_set_defaults  /* sets default parameters */
+void ccolamd_set_defaults	/* sets default parameters */
-(        /* knobs argument is modified on output */
+(				/* knobs argument is modified on output */
-    double knobs [CCOLAMD_KNOBS]  /* parameter settings for ccolamd */
+    double knobs [CCOLAMD_KNOBS]	/* parameter settings for ccolamd */
 ) ;
 
-void ccolamd_l_set_defaults  /* sets default parameters */
+void ccolamd_l_set_defaults	/* sets default parameters */
-(        /* knobs argument is modified on output */
+(				/* knobs argument is modified on output */
-    double knobs [CCOLAMD_KNOBS]  /* parameter settings for ccolamd */
+    double knobs [CCOLAMD_KNOBS]	/* parameter settings for ccolamd */
 ) ;
 
-int ccolamd      /* returns (1) if successful, (0) otherwise*/
+int ccolamd			/* returns (1) if successful, (0) otherwise*/
-(        /* A and p arguments are modified on output */
+(				/* A and p arguments are modified on output */
-    int n_row,      /* number of rows in A */
+    int n_row,			/* number of rows in A */
-    int n_col,      /* number of columns in A */
+    int n_col,			/* number of columns in A */
-    int Alen,      /* size of the array A */
+    int Alen,			/* size of the array A */
-    int A [ ],      /* row indices of A, of size Alen */
+    int A [ ],			/* row indices of A, of size Alen */
-    int p [ ],      /* column pointers of A, of size n_col+1 */
+    int p [ ],			/* column pointers of A, of size n_col+1 */
     double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */
-    int stats [CCOLAMD_STATS],  /* ccolamd output statistics and error codes */
+    int stats [CCOLAMD_STATS],	/* ccolamd output statistics and error codes */
-    int cmember [ ]    /* Constraint set of A, of size n_col */
+    int cmember [ ]		/* Constraint set of A, of size n_col */
 ) ;
 
-UF_long ccolamd_l    /* same as ccolamd, but with UF_long integers */
+SuiteSparse_long ccolamd_l      /* as ccolamd w/ SuiteSparse_long integers */
 (
-    UF_long n_row,
+    SuiteSparse_long n_row,
-    UF_long n_col,
+    SuiteSparse_long n_col,
-    UF_long Alen,
+    SuiteSparse_long Alen,
-    UF_long A [ ],
+    SuiteSparse_long A [ ],
-    UF_long p [ ],
+    SuiteSparse_long p [ ],
     double knobs [CCOLAMD_KNOBS],
-    UF_long stats [CCOLAMD_STATS],
+    SuiteSparse_long stats [CCOLAMD_STATS],
-    UF_long cmember [ ]
+    SuiteSparse_long cmember [ ]
 ) ;
 
-int csymamd      /* return (1) if OK, (0) otherwise */
+int csymamd			/* return (1) if OK, (0) otherwise */
 (
-    int n,      /* number of rows and columns of A */
+    int n,			/* number of rows and columns of A */
-    int A [ ],      /* row indices of A */
+    int A [ ],			/* row indices of A */
-    int p [ ],      /* column pointers of A */
+    int p [ ],			/* column pointers of A */
-    int perm [ ],    /* output permutation, size n_col+1 */
+    int perm [ ],		/* output permutation, size n_col+1 */
     double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */
-    int stats [CCOLAMD_STATS],  /* output statistics and error codes */
+    int stats [CCOLAMD_STATS],	/* output statistics and error codes */
     void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */
-        /* mxCalloc (for MATLAB mexFunction) */
+				/* mxCalloc (for MATLAB mexFunction) */
-    void (*release) (void *),  /* pointer to free (ANSI C) or */
+    void (*release) (void *),	/* pointer to free (ANSI C) or */
-            /* mxFree (for MATLAB mexFunction) */
+    				/* mxFree (for MATLAB mexFunction) */
-    int cmember [ ],    /* Constraint set of A */
+    int cmember [ ],		/* Constraint set of A */
-    int stype      /* 0: use both parts, >0: upper, <0: lower */
+    int stype			/* 0: use both parts, >0: upper, <0: lower */
 ) ;
 
-UF_long csymamd_l    /* same as csymamd, but with UF_long integers */
+SuiteSparse_long csymamd_l      /* as csymamd, w/ SuiteSparse_long integers */
 (
-    UF_long n,
+    SuiteSparse_long n,
-    UF_long A [ ],
+    SuiteSparse_long A [ ],
-    UF_long p [ ],
+    SuiteSparse_long p [ ],
-    UF_long perm [ ],
+    SuiteSparse_long perm [ ],
     double knobs [CCOLAMD_KNOBS],
-    UF_long stats [CCOLAMD_STATS],
+    SuiteSparse_long stats [CCOLAMD_STATS],
     void * (*allocate) (size_t, size_t),
     void (*release) (void *),
-    UF_long cmember [ ],
+    SuiteSparse_long cmember [ ],
-    UF_long stype
+    SuiteSparse_long stype
 ) ;
 
 void ccolamd_report
@@ -203,7 +200,7 @@ void ccolamd_report
 
 void ccolamd_l_report
 (
-    UF_long stats [CCOLAMD_STATS]
+    SuiteSparse_long stats [CCOLAMD_STATS]
 ) ;
 
 void csymamd_report
@@ -213,7 +210,7 @@ void csymamd_report
 
 void csymamd_l_report
 (
-    UF_long stats [CCOLAMD_STATS]
+    SuiteSparse_long stats [CCOLAMD_STATS]
 ) ;
 
 
@@ -227,42 +224,42 @@ void csymamd_l_report
  */
 
 int ccolamd2
-(        /* A and p arguments are modified on output */
+(				/* A and p arguments are modified on output */
-    int n_row,      /* number of rows in A */
+    int n_row,			/* number of rows in A */
-    int n_col,      /* number of columns in A */
+    int n_col,			/* number of columns in A */
-    int Alen,      /* size of the array A */
+    int Alen,			/* size of the array A */
-    int A [ ],      /* row indices of A, of size Alen */
+    int A [ ],			/* row indices of A, of size Alen */
-    int p [ ],      /* column pointers of A, of size n_col+1 */
+    int p [ ],			/* column pointers of A, of size n_col+1 */
     double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */
-    int stats [CCOLAMD_STATS],  /* ccolamd output statistics and error codes */
+    int stats [CCOLAMD_STATS],	/* ccolamd output statistics and error codes */
     /* each Front_ array is of size n_col+1: */
-    int Front_npivcol [ ],  /* # pivot cols in each front */
+    int Front_npivcol [ ],	/* # pivot cols in each front */
-    int Front_nrows [ ],  /* # of rows in each front (incl. pivot rows) */
+    int Front_nrows [ ],	/* # of rows in each front (incl. pivot rows) */
-    int Front_ncols [ ],  /* # of cols in each front (incl. pivot cols) */
+    int Front_ncols [ ],	/* # of cols in each front (incl. pivot cols) */
-    int Front_parent [ ],  /* parent of each front */
+    int Front_parent [ ],	/* parent of each front */
-    int Front_cols [ ],    /* link list of pivot columns for each front */
+    int Front_cols [ ],		/* link list of pivot columns for each front */
-    int *p_nfr,      /* total number of frontal matrices */
+    int *p_nfr,			/* total number of frontal matrices */
-    int InFront [ ],    /* InFront [row] = f if row in front f */
+    int InFront [ ],		/* InFront [row] = f if row in front f */
-    int cmember [ ]    /* Constraint set of A */
+    int cmember [ ]		/* Constraint set of A */
 ) ;
 
-UF_long ccolamd2_l      /* same as ccolamd2, but with UF_long integers */
+SuiteSparse_long ccolamd2_l     /* as ccolamd2, w/ SuiteSparse_long integers */
 (
-    UF_long n_row,
+    SuiteSparse_long n_row,
-    UF_long n_col,
+    SuiteSparse_long n_col,
-    UF_long Alen,
+    SuiteSparse_long Alen,
-    UF_long A [ ],
+    SuiteSparse_long A [ ],
-    UF_long p [ ],
+    SuiteSparse_long p [ ],
     double knobs [CCOLAMD_KNOBS],
-    UF_long stats [CCOLAMD_STATS],
+    SuiteSparse_long stats [CCOLAMD_STATS],
-    UF_long Front_npivcol [ ],
+    SuiteSparse_long Front_npivcol [ ],
-    UF_long Front_nrows [ ],
+    SuiteSparse_long Front_nrows [ ],
-    UF_long Front_ncols [ ],
+    SuiteSparse_long Front_ncols [ ],
-    UF_long Front_parent [ ],
+    SuiteSparse_long Front_parent [ ],
-    UF_long Front_cols [ ],
+    SuiteSparse_long Front_cols [ ],
-    UF_long *p_nfr,
+    SuiteSparse_long *p_nfr,
-    UF_long InFront [ ],
+    SuiteSparse_long InFront [ ],
-    UF_long cmember [ ]
+    SuiteSparse_long cmember [ ]
 ) ;
 
 void ccolamd_apply_order
@@ -276,11 +273,11 @@ void ccolamd_apply_order
 
 void ccolamd_l_apply_order
 (
-    UF_long Front [ ],
+    SuiteSparse_long Front [ ],
-    const UF_long Order [ ],
+    const SuiteSparse_long Order [ ],
-    UF_long Temp [ ],
+    SuiteSparse_long Temp [ ],
-    UF_long nn,
+    SuiteSparse_long nn,
-    UF_long nfr
+    SuiteSparse_long nfr
 ) ;
 
 
@@ -296,12 +293,12 @@ void ccolamd_fsize
 
 void ccolamd_l_fsize
 (
-    UF_long nn,
+    SuiteSparse_long nn,
-    UF_long MaxFsize [ ],
+    SuiteSparse_long MaxFsize [ ],
-    UF_long Fnrows [ ],
+    SuiteSparse_long Fnrows [ ],
-    UF_long Fncols [ ],
+    SuiteSparse_long Fncols [ ],
-    UF_long Parent [ ],
+    SuiteSparse_long Parent [ ],
-    UF_long Npiv [ ]
+    SuiteSparse_long Npiv [ ]
 ) ;
 
 void ccolamd_postorder
@@ -320,16 +317,16 @@ void ccolamd_postorder
 
 void ccolamd_l_postorder
 (
-    UF_long nn,
+    SuiteSparse_long nn,
-    UF_long Parent [ ],
+    SuiteSparse_long Parent [ ],
-    UF_long Npiv [ ],
+    SuiteSparse_long Npiv [ ],
-    UF_long Fsize [ ],
+    SuiteSparse_long Fsize [ ],
-    UF_long Order [ ],
+    SuiteSparse_long Order [ ],
-    UF_long Child [ ],
+    SuiteSparse_long Child [ ],
-    UF_long Sibling [ ],
+    SuiteSparse_long Sibling [ ],
-    UF_long Stack [ ],
+    SuiteSparse_long Stack [ ],
-    UF_long Front_cols [ ],
+    SuiteSparse_long Front_cols [ ],
-    UF_long cmember [ ]
+    SuiteSparse_long cmember [ ]
 ) ;
 
 int ccolamd_post_tree
@@ -342,22 +339,16 @@ int ccolamd_post_tree
     int Stack [ ]
 ) ;
 
-UF_long ccolamd_l_post_tree
+SuiteSparse_long ccolamd_l_post_tree
 (
-    UF_long root,
+    SuiteSparse_long root,
-    UF_long k,
+    SuiteSparse_long k,
-    UF_long Child [ ],
+    SuiteSparse_long Child [ ],
-    const UF_long Sibling [ ],
+    const SuiteSparse_long Sibling [ ],
-    UF_long Order [ ],
+    SuiteSparse_long Order [ ],
-    UF_long Stack [ ]
+    SuiteSparse_long Stack [ ]
 ) ;
 
-#ifndef EXTERN
-#define EXTERN extern
-#endif
-
-EXTERN int (*ccolamd_printf) (const char *, ...) ;
-
 #ifdef __cplusplus
 }
 #endif

gtsam/3rdparty/CCOLAMD/Lib/Makefile

@@ -0,0 +1,73 @@
+#-------------------------------------------------------------------------------
+# CCOLAMD Lib/Makefile
+#-------------------------------------------------------------------------------
+
+LIBRARY = libccolamd
+VERSION = 2.9.6
+SO_VERSION = 2
+
+default: library
+
+include ../../SuiteSparse_config/SuiteSparse_config.mk
+
+# CCOLAMD depends on SuiteSparse_config
+LDLIBS += -lsuitesparseconfig
+
+# compile and install in SuiteSparse/lib
+library:
+	$(MAKE) install INSTALL=$(SUITESPARSE)
+
+I = -I../Include -I../../SuiteSparse_config
+
+INC = ../Include/ccolamd.h ../../SuiteSparse_config/SuiteSparse_config.h
+
+SRC = ../Source/ccolamd.c
+
+OBJ = ccolamd.o ccolamd_l.o
+
+ccolamd.o: $(SRC) $(INC)
+	$(CC) $(CF) $(I) -c ../Source/ccolamd.c
+
+ccolamd_l.o: $(SRC) $(INC)
+	$(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o
+
+# creates libccolamd.a, a C-callable CCOLAMD library
+static: $(AR_TARGET)
+
+$(AR_TARGET):  $(OBJ)
+	$(ARCHIVE)  $@ $^
+	- $(RANLIB) $@ 
+
+ccode: library
+
+clean:
+	- $(RM) -r $(CLEAN)
+
+purge: distclean
+
+distclean: clean
+	- $(RM) -r $(PURGE)
+
+# install CCOLAMD
+install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET)
+
+$(INSTALL_LIB)/$(SO_TARGET): $(OBJ)
+	@mkdir -p $(INSTALL_LIB)
+	@mkdir -p $(INSTALL_INCLUDE)
+	@mkdir -p $(INSTALL_DOC)
+	$(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS)
+	( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) )
+	( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) )
+	$(CP) ../Include/ccolamd.h $(INSTALL_INCLUDE)
+	$(CP) ../README.txt $(INSTALL_DOC)/CCOLAMD_README.txt
+	chmod 755 $(INSTALL_LIB)/$(SO_TARGET)
+	chmod 644 $(INSTALL_INCLUDE)/ccolamd.h
+	chmod 644 $(INSTALL_DOC)/CCOLAMD_README.txt
+
+uninstall:
+	$(RM) $(INSTALL_LIB)/$(SO_TARGET)
+	$(RM) $(INSTALL_LIB)/$(SO_PLAIN)
+	$(RM) $(INSTALL_LIB)/$(SO_MAIN)
+	$(RM) $(INSTALL_INCLUDE)/ccolamd.h
+	$(RM) $(INSTALL_DOC)/CCOLAMD_README.txt
+

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m

@@ -47,9 +47,8 @@ spparms ('default') ;
 A = sprandn (n, n, 2/n) + speye (n) ;
 b = (1:n)' ;
 
-figure (1)
 clf ;
-subplot (2,2,1)
+subplot (3,4,1)
 spy (A)
 title ('original matrix')
 
@@ -62,7 +61,7 @@ fl = luflops (L, U) ;
 x = Q * (U \ (L \ (P * b))) ;
 fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q):          %d\n', fl) ;
 fprintf (1, 'residual:                                     %e\n', norm (A*x-b));
-subplot (2,2,2) ;
+subplot (3,4,2) ;
 spy (L|U) ;
 title ('LU with ccolamd') ;
 
@@ -76,7 +75,7 @@ fl = luflops (L, U) ;
 x = Q * (U \ (L \ (P * b))) ;
 fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q):          %d\n', fl) ;
 fprintf (1, 'residual:                                     %e\n', norm (A*x-b));
-subplot (2,2,3) ;
+subplot (3,4,3) ;
 spy (L|U) ;
 title ('LU with colamd') ;
 catch
@@ -89,7 +88,7 @@ fl = luflops (L, U) ;
 x = U \ (L \ (P * b)) ;
 fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q):          %d\n', fl) ;
 fprintf (1, 'residual:                                     %e\n', norm (A*x-b));
-subplot (2,2,4) ;
+subplot (3,4,4) ;
 spy (L|U) ;
 title ('LU with no ordering') ;
 
@@ -111,9 +110,7 @@ n = 1000 ;
 fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ;
 A = sprandn (n, n, 2/n) + speye (n) ;
 
-figure (2)
+subplot (3,4,5)
-clf ;
-subplot (2,2,1)
 spy (A)
 title ('original matrix')
 
@@ -121,7 +118,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ;
 [lnz,h,parent,post,R] = symbfact (A, 'col') ;
 fprintf (1, 'nz in Cholesky factors of A''A:            %d\n', sum (lnz)) ;
 fprintf (1, 'flop count for Cholesky of A''A:           %d\n', sum (lnz.^2)) ;
-subplot (2,2,4) ;
+subplot (3,4,6) ;
 spy (R) ;
 title ('Cholesky with no ordering') ;
 
@@ -133,7 +130,7 @@ fprintf (1, '\n\nccolamd run time:                         %f\n', t) ;
 fprintf (1, 'ccolamd ordering quality: \n') ;
 fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p):  %d\n', sum (lnz)) ;
 fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ;
-subplot (2,2,2) ;
+subplot (3,4,7) ;
 spy (R) ;
 title ('Cholesky with ccolamd') ;
 
@@ -146,7 +143,7 @@ fprintf (1, '\n\ncolamd run time:                          %f\n', t) ;
 fprintf (1, 'colamd ordering quality: \n') ;
 fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p):  %d\n', sum (lnz)) ;
 fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ;
-subplot (2,2,3) ;
+subplot (3,4,8) ;
 spy (R) ;
 title ('Cholesky with colamd') ;
 catch
@@ -164,9 +161,7 @@ fprintf (1, '\n-----------------------------------------------------------\n') ;
 fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ;
 A = A+A' ;
 
-figure (3)
+subplot (3,4,9) ;
-clf ;
-subplot (2,2,1)
 spy (A)
 title ('original matrix')
 
@@ -174,7 +169,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ;
 [lnz,h,parent,post,R] = symbfact (A, 'sym') ;
 fprintf (1, 'nz in Cholesky factors of A:       %d\n', sum (lnz)) ;
 fprintf (1, 'flop count for Cholesky of A:      %d\n', sum (lnz.^2)) ;
-subplot (2,2,4) ;
+subplot (3,4,10) ;
 spy (R) ;
 title ('Cholesky with no ordering') ;
 
@@ -186,7 +181,7 @@ fprintf (1, '\n\ncsymamd run time:                  %f\n', t) ;
 fprintf (1, 'csymamd ordering quality: \n') ;
 fprintf (1, 'nz in Cholesky factors of A(p,p):  %d\n', sum (lnz)) ;
 fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ;
-subplot (2,2,2) ;
+subplot (3,4,11) ;
 spy (R) ;
 title ('Cholesky with csymamd') ;
 
@@ -199,7 +194,7 @@ fprintf (1, '\n\nsymamd run time:                   %f\n', t) ;
 fprintf (1, 'symamd ordering quality: \n') ;
 fprintf (1, 'nz in Cholesky factors of A(p,p):  %d\n', sum (lnz)) ;
 fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ;
-subplot (2,2,3) ;
+subplot (3,4,12) ;
 spy (R) ;
 title ('Cholesky with symamd') ;
 catch

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m

@@ -14,14 +14,33 @@ d = '' ;
 if (~isempty (strfind (computer, '64')))
     d = '-largeArrayDims' ;
 end
-src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ;
+
-cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include -output ', d) ;
+% MATLAB 8.3.0 now has a -silent option to keep 'mex' from burbling too much
+if (~verLessThan ('matlab', '8.3.0'))
+    d = ['-silent ' d] ;
+end
+
+src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ;
+cmd = sprintf ( ...
+    'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ;
 s = [cmd 'ccolamd ccolamdmex.c ' src] ;
+
+if (~(ispc || ismac))
+    % for POSIX timing routine
+    s = [s ' -lrt'] ;
+end
 if (details)
     fprintf ('%s\n', s) ;
 end
 eval (s) ;
+
 s = [cmd 'csymamd csymamdmex.c ' src] ;
+
+if (~(ispc || ismac))
+    % for POSIX timing routine
+    s = [s ' -lrt'] ;
+end
+
 if (details)
     fprintf ('%s\n', s) ;
 end

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m

@@ -22,8 +22,13 @@ csymamd_default_knobs = [10 1 0] ;
     if (~isempty (strfind (computer, '64')))
 	d = '-largeArrayDims' ;
     end
-    src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ;
+    cmd = sprintf ( ...
-    cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include ', d) ;
+        'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ;
+    src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ;
+    if (~(ispc || ismac))
+        % for POSIX timing routine
+        src = [src ' -lrt'] ;
+    end
     eval ([cmd 'ccolamdtestmex.c ' src]) ;
     eval ([cmd 'csymamdtestmex.c ' src]) ;
     fprintf ('Done compiling.\n') ; 

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD, Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /* 
@@ -26,7 +24,7 @@
 #include "matrix.h"
 #include <stdlib.h>
 #include <string.h>
-#include "UFconfig.h"
+#define Long SuiteSparse_long
 
 /* ========================================================================== */
 /* === ccolamd mexFunction ================================================== */
@@ -44,24 +42,24 @@ void mexFunction
 {
     /* === Local variables ================================================== */
 
-    UF_long *A ;		/* ccolamd's copy of the matrix and workspace */
+    Long *A ;                   /* ccolamd's copy of the matrix and workspace */
-    UF_long *cmember ;		/* ccolamd's copy of the constraint set */
+    Long *cmember ;             /* ccolamd's copy of the constraint set */
-    double *in_cmember ;	/* input constraint set */
+    double *in_cmember ;        /* input constraint set */
-    UF_long *p ;		/* ccolamd's copy of the column pointers */
+    Long *p ;                   /* ccolamd's copy of the column pointers */
-    UF_long Alen ;		/* size of A */
+    Long Alen ;                 /* size of A */
-    UF_long cslen ;		/* size of CS  */
+    Long cslen ;                /* size of CS  */
-    UF_long n_col ;		/* number of columns of A */
+    Long n_col ;                /* number of columns of A */
-    UF_long n_row ;		/* number of rows of A */
+    Long n_row ;                /* number of rows of A */
-    UF_long nnz ;		/* number of entries in A */
+    Long nnz ;                  /* number of entries in A */
-    UF_long full ;		/* TRUE if input matrix full, FALSE if sparse */
+    Long full ;                 /* TRUE if input matrix full, FALSE if sparse */
     double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
-    double *out_perm ;		/* output permutation vector */
+    double *out_perm ;          /* output permutation vector */
-    double *out_stats ;		/* output stats vector */
+    double *out_stats ;         /* output stats vector */
-    double *in_knobs ;		/* input knobs vector */
+    double *in_knobs ;          /* input knobs vector */
-    UF_long i ;			/* loop counter */
+    Long i ;                    /* loop counter */
-    mxArray *Ainput ;		/* input matrix handle */
+    mxArray *Ainput ;           /* input matrix handle */
-    UF_long spumoni ;		/* verbosity variable */
+    Long spumoni ;              /* verbosity variable */
-    UF_long stats [CCOLAMD_STATS] ;	/* stats for ccolamd */
+    Long stats [CCOLAMD_STATS] ;/* stats for ccolamd */
 
     /* === Check inputs ===================================================== */
 
@@ -80,11 +78,11 @@ void mexFunction
 	cslen = mxGetNumberOfElements (pargin [2]) ;
 	if (cslen != 0)
 	{
-	    cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ;
+	    cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ;
 	    for (i = 0 ; i < cslen ; i++)
 	    {
 		/* convert cmember from 1-based to 0-based */
-		cmember[i] = ((UF_long) in_cmember [i] - 1) ;
+		cmember[i] = ((Long) in_cmember [i] - 1) ;
 	    }
 	}
     }
@@ -157,10 +155,10 @@ void mexFunction
     n_col = mxGetN (Ainput) ;
 
     /* get column pointer vector */
-    p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
+    p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
-    (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
+    (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
     nnz = p [n_col] ;
-    Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ;
+    Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ;
     if (Alen == 0)
     {
     	mexErrMsgTxt ("ccolamd: problem too large") ;
@@ -168,8 +166,8 @@ void mexFunction
 
     /* === Copy input matrix into workspace ================================= */
 
-    A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ;
+    A = (Long *) mxCalloc (Alen, sizeof (Long)) ;
-    (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
+    (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
 
     if (full)
     {

gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /*
@@ -43,7 +41,7 @@
 #include "matrix.h"
 #include <stdlib.h>
 #include <string.h>
-#include "UFconfig.h"
+#define Long SuiteSparse_long
 
 /* Here only for testing */
 #undef MIN
@@ -61,15 +59,15 @@
 
 static void dump_matrix
 (
-    UF_long A [ ],
+    Long A [ ],
-    UF_long p [ ],
+    Long p [ ],
-    UF_long n_row,
+    Long n_row,
-    UF_long n_col,
+    Long n_col,
-    UF_long Alen,
+    Long Alen,
-    UF_long limit
+    Long limit
 )
 {
-    UF_long col, k, row ;
+    Long col, k, row ;
 
     mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ;
 
@@ -102,24 +100,24 @@ void mexFunction
 {
     /* === Local variables ================================================== */
 
-    UF_long *A ;		/* ccolamd's copy of the matrix and workspace */
+    Long *A ;                   /* ccolamd's copy of the matrix and workspace */
-    UF_long *p ;		/* ccolamd's copy of the column pointers */
+    Long *p ;                   /* ccolamd's copy of the column pointers */
-    UF_long Alen ;		/* size of A */
+    Long Alen ;                 /* size of A */
-    UF_long n_col ;		/* number of columns of A */
+    Long n_col ;                /* number of columns of A */
-    UF_long n_row ;		/* number of rows of A */
+    Long n_row ;                /* number of rows of A */
-    UF_long nnz ;		/* number of entries in A */
+    Long nnz ;                  /* number of entries in A */
-    UF_long full ;		/* TRUE if input matrix full, FALSE if sparse */
+    Long full ;                 /* TRUE if input matrix full, FALSE if sparse */
     double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
-    double *out_perm ;		/* output permutation vector */
+    double *out_perm ;          /* output permutation vector */
-    double *out_stats ;		/* output stats vector */
+    double *out_stats ;         /* output stats vector */
-    double *in_knobs ;		/* input knobs vector */
+    double *in_knobs ;          /* input knobs vector */
-    UF_long i ;			/* loop counter */
+    Long i ;                    /* loop counter */
-    mxArray *Ainput ;		/* input matrix handle */
+    mxArray *Ainput ;           /* input matrix handle */
-    UF_long spumoni ;		/* verbosity variable */
+    Long spumoni ;              /* verbosity variable */
-    UF_long stats2 [CCOLAMD_STATS] ;	/* stats for ccolamd */
+    Long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */
 
-    UF_long *cp, *cp_end, result, col, length, ok ;
+    Long *cp, *cp_end, result, col, length, ok ;
-    UF_long *stats ;
+    Long *stats ;
     stats = stats2 ;
 
     /* === Check inputs ===================================================== */
@@ -199,10 +197,10 @@ void mexFunction
     n_col = mxGetN (Ainput) ;
 
     /* get column pointer vector so we can find nnz */
-    p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
+    p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
-    (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
+    (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
     nnz = p [n_col] ;
-    Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ;
+    Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ;
     if (Alen == 0)
     {
     	mexErrMsgTxt ("ccolamd: problem too large") ;
@@ -230,8 +228,8 @@ void mexFunction
 
     /* === Copy input matrix into workspace ================================= */
 
-    A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ;
+    A = (Long *) mxCalloc (Alen, sizeof (Long)) ;
-    (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
+    (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
 
     if (full)
     {
@@ -261,7 +259,7 @@ void mexFunction
     */
 
     /* jumble appropriately */
-    switch ((UF_long) in_knobs [6])
+    switch ((Long) in_knobs [6])
     {
 
 	case 0 :
@@ -359,7 +357,7 @@ void mexFunction
 		mexPrintf ("ccolamdtest: A not present\n") ;
 	    }
 	    result = 0 ;		/* A not present */
-	    A = (UF_long *) NULL ;
+	    A = (Long *) NULL ;
 	    break ;
 
 	case 8 :
@@ -368,7 +366,7 @@ void mexFunction
 		mexPrintf ("ccolamdtest: p not present\n") ;
 	    }
 	    result = 0 ;		/* p not present */
-	    p = (UF_long *) NULL ;
+	    p = (Long *) NULL ;
 	    break ;
 
 	case 9 :
@@ -456,7 +454,7 @@ void mexFunction
 		mexPrintf ("ccolamdtest: stats not present\n") ;
 	    }
 	    result = 0 ;		/* stats not present */
-	    stats = (UF_long *) NULL ;
+	    stats = (Long *) NULL ;
 	    break ;
 
 	case 13 :

gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m

@@ -34,10 +34,10 @@ function [p, stats] = csymamd (S, knobs, cmember)			    %#ok
 %    p = csymamd(S) is about the same as p = symamd(S).  knobs and its default
 %    values differ.
 %
-%    Authors: S. Larimore, T. Davis (Univ of Florida), and S. Rajamanickam, in
+%    Authors: S. Larimore, T. Davis, and S. Rajamanickam, in
 %    collaboration with J. Gilbert and E. Ng.  Supported by the National
 %    Science Foundation (DMS-9504974, DMS-9803599, CCR-0203270), and a grant
-%    from Sandia National Lab.  See http://www.cise.ufl.edu/research/sparse
+%    from Sandia National Lab.  See http://www.suitesparse.com
 %    for ccolamd, csymamd, amd, colamd, symamd, and other related orderings.
 %
 %    See also AMD, CCOLAMD, COLAMD, SYMAMD, SYMRCM.

gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD, Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /* 
@@ -25,7 +23,7 @@
 #include "mex.h"
 #include "matrix.h"
 #include <stdlib.h>
-#include "UFconfig.h"
+#define Long SuiteSparse_long
 
 /* ========================================================================== */
 /* === csymamd mexFunction ================================================== */
@@ -43,23 +41,23 @@ void mexFunction
 {
     /* === Local variables ================================================== */
 
-    UF_long *A ;		/* row indices of input matrix A */
+    Long *A ;                   /* row indices of input matrix A */
-    UF_long *perm ;		/* column ordering of M and ordering of A */
+    Long *perm ;                /* column ordering of M and ordering of A */
-    UF_long *cmember ;		/* csymamd's copy of the constraint set */
+    Long *cmember ;             /* csymamd's copy of the constraint set */
-    double *in_cmember ;	/* input constraint set */
+    double *in_cmember ;        /* input constraint set */
-    UF_long *p ;		/* column pointers of input matrix A */
+    Long *p ;                   /* column pointers of input matrix A */
-    UF_long cslen ;		/* size of constraint set */
+    Long cslen ;                /* size of constraint set */
-    UF_long n_col ;		/* number of columns of A */
+    Long n_col ;                /* number of columns of A */
-    UF_long n_row ;		/* number of rows of A */
+    Long n_row ;                /* number of rows of A */
-    UF_long full ;		/* TRUE if input matrix full, FALSE if sparse */
+    Long full ;                 /* TRUE if input matrix full, FALSE if sparse */
     double knobs [CCOLAMD_KNOBS] ; /* csymamd user-controllable parameters */
-    double *out_perm ;		/* output permutation vector */
+    double *out_perm ;          /* output permutation vector */
-    double *out_stats ;		/* output stats vector */
+    double *out_stats ;         /* output stats vector */
-    double *in_knobs ;		/* input knobs vector */
+    double *in_knobs ;          /* input knobs vector */
-    UF_long i ;			/* loop counter */
+    Long i ;                    /* loop counter */
-    mxArray *Ainput ;		/* input matrix handle */
+    mxArray *Ainput ;           /* input matrix handle */
-    UF_long spumoni ;		/* verbosity variable */
+    Long spumoni ;              /* verbosity variable */
-    UF_long stats [CCOLAMD_STATS] ;	/* stats for symamd */
+    Long stats [CCOLAMD_STATS] ;/* stats for symamd */
 
     /* === Check inputs ===================================================== */
 
@@ -78,11 +76,11 @@ void mexFunction
 	cslen = mxGetNumberOfElements (pargin [2]) ;
 	if (cslen != 0)
 	{
-	    cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ;
+	    cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ;
 	    for (i = 0 ; i < cslen ; i++)
 	    {
 		/* convert cmember from 1-based to 0-based */
-		cmember[i] = ((UF_long) in_cmember [i] - 1) ;
+		cmember[i] = ((Long) in_cmember [i] - 1) ;
 	    }
 	}
     }
@@ -153,9 +151,9 @@ void mexFunction
     	mexErrMsgTxt ("csymamd: cmember must be of length equal to #cols of A");
     }
 
-    A = (UF_long *) mxGetIr (Ainput) ;
+    A = (Long *) mxGetIr (Ainput) ;
-    p = (UF_long *) mxGetJc (Ainput) ;
+    p = (Long *) mxGetJc (Ainput) ;
-    perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
+    perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
 
     /* === Order the rows and columns of A (does not destroy A) ============= */
 

gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /*
@@ -37,7 +35,7 @@
 #include "matrix.h"
 #include <stdlib.h>
 #include <string.h>
-#include "UFconfig.h"
+#define Long SuiteSparse_long
 
 #ifdef MIN
 #undef MIN
@@ -47,15 +45,15 @@
 
 static void dump_matrix
 (
-    UF_long A [ ],
+    Long A [ ],
-    UF_long p [ ],
+    Long p [ ],
-    UF_long n_row,
+    Long n_row,
-    UF_long n_col,
+    Long n_col,
-    UF_long Alen,
+    Long Alen,
-    UF_long limit
+    Long limit
 )
 {
-    UF_long col, k, row ;
+    Long col, k, row ;
 
     mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ;
 
@@ -100,23 +98,23 @@ void mexFunction
 {
     /* === Local variables ================================================== */
 
-    UF_long *perm ;		/* column ordering of M and ordering of A */
+    Long *perm ;                /* column ordering of M and ordering of A */
-    UF_long *A ;		/* row indices of input matrix A */
+    Long *A ;                   /* row indices of input matrix A */
-    UF_long *p ;		/* column pointers of input matrix A */
+    Long *p ;                   /* column pointers of input matrix A */
-    UF_long n_col ;		/* number of columns of A */
+    Long n_col ;                /* number of columns of A */
-    UF_long n_row ;		/* number of rows of A */
+    Long n_row ;                /* number of rows of A */
-    UF_long full ;		/* TRUE if input matrix full, FALSE if sparse */
+    Long full ;                 /* TRUE if input matrix full, FALSE if sparse */
     double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
-    double *out_perm ;		/* output permutation vector */
+    double *out_perm ;          /* output permutation vector */
-    double *out_stats ;		/* output stats vector */
+    double *out_stats ;         /* output stats vector */
-    double *in_knobs ;		/* input knobs vector */
+    double *in_knobs ;          /* input knobs vector */
-    UF_long i ;			/* loop counter */
+    Long i ;                    /* loop counter */
-    mxArray *Ainput ;		/* input matrix handle */
+    mxArray *Ainput ;           /* input matrix handle */
-    UF_long spumoni ;		/* verbosity variable */
+    Long spumoni ;              /* verbosity variable */
-    UF_long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */
+    Long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */
 
-    UF_long *cp, *cp_end, result, nnz, col, length, ok ;
+    Long *cp, *cp_end, result, nnz, col, length, ok ;
-    UF_long *stats ;
+    Long *stats ;
     stats = stats2 ;
 
     /* === Check inputs ===================================================== */
@@ -192,8 +190,8 @@ void mexFunction
     }
 
     /* p = mxGetJc (Ainput) ; */
-    p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
+    p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
-    (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
+    (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
 
     nnz = p [n_col] ;
     if (spumoni)
@@ -202,10 +200,10 @@ void mexFunction
     }
 
     /* A = mxGetIr (Ainput) ; */
-    A = (UF_long *) mxCalloc (nnz+1, sizeof (UF_long)) ;
+    A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ;
-    (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
+    (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
 
-    perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
+    perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
 
     /* === Jumble matrix ==================================================== */
 
@@ -230,7 +228,7 @@ void mexFunction
     */
 
     /* jumble appropriately */
-    switch ((UF_long) in_knobs [3])
+    switch ((Long) in_knobs [3])
     {
 
 	case 0 :
@@ -321,7 +319,7 @@ void mexFunction
 		mexPrintf ("csymamdtest: A not present\n") ;
 	    }
 	    result = 0 ;		/* A not present */
-	    A = (UF_long *) NULL ;
+	    A = (Long *) NULL ;
 	    break ;
 
 	case 8 :
@@ -330,7 +328,7 @@ void mexFunction
 		mexPrintf ("csymamdtest: p not present\n") ;
 	    }
 	    result = 0 ;		/* p not present */
-	    p = (UF_long *) NULL ;
+	    p = (Long *) NULL ;
 	    break ;
 
 	case 9 :
@@ -418,7 +416,7 @@ void mexFunction
 		mexPrintf ("csymamdtest: stats not present\n") ;
 	    }
 	    result = 0 ;		/* stats not present */
-	    stats = (UF_long *) NULL ;
+	    stats = (Long *) NULL ;
 	    break ;
 
 	case 13 :

gtsam/3rdparty/CCOLAMD/Makefile

@@ -0,0 +1,55 @@
+#------------------------------------------------------------------------------
+# CCOLAMD Makefile
+#------------------------------------------------------------------------------
+
+SUITESPARSE ?= $(realpath $(CURDIR)/..)
+export SUITESPARSE
+
+default: all
+
+include ../SuiteSparse_config/SuiteSparse_config.mk
+
+demos: all
+
+# Compile all C code
+all:
+	( cd Lib    ; $(MAKE) )
+	( cd Demo   ; $(MAKE) )
+
+# compile just the C-callable libraries (not Demos)
+library:
+	( cd Lib    ; $(MAKE) )
+
+# compile the static libraries only
+static:
+	( cd Lib    ; $(MAKE) static )
+
+# remove object files, but keep the compiled programs and library archives
+clean:
+	( cd Lib    ; $(MAKE) clean )
+	( cd Demo   ; $(MAKE) clean )
+	( cd MATLAB ; $(RM) $(CLEAN) )
+
+# clean, and then remove compiled programs and library archives
+purge:
+	( cd Lib    ; $(MAKE) purge )
+	( cd Demo   ; $(MAKE) purge )
+	( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* )
+
+distclean: purge
+
+# get ready for distribution
+dist: purge
+	( cd Demo   ; $(MAKE) dist )
+
+ccode: library
+
+lib: library
+
+# install CCOLAMD
+install:
+	( cd Lib  ; $(MAKE) install )
+
+# uninstall CCOLAMD
+uninstall:
+	( cd Lib  ; $(MAKE) uninstall )

gtsam/3rdparty/CCOLAMD/README.txt

@@ -1,8 +1,8 @@
 CCOLAMD: constrained column approximate minimum degree ordering
-Copyright (C) 2005-2011, Univ. of Florida.  Authors: Timothy A. Davis,
+Copyright (C) 2005-2016, Univ. of Florida.  Authors: Timothy A. Davis,
 Sivasankaran Rajamanickam, and Stefan Larimore.  Closely based on COLAMD by
 Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert.
-http://www.cise.ufl.edu/research/sparse
+http://www.suitesparse.com
 -------------------------------------------------------------------------------
 
 The CCOLAMD column approximate minimum degree ordering algorithm computes
@@ -14,7 +14,8 @@ available as a MATLAB-callable function.  It constructs a matrix M such
 that M'*M has the same pattern as A, and then uses CCOLAMD to compute a column
 ordering of M.
 
-Requires UFconfig, in the ../UFconfig directory relative to this directory.
+Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to
+this directory.
 
 To compile and install the ccolamd m-files and mexFunctions, just cd to
 CCOLAMD/MATLAB and type ccolamd_install in the MATLAB command window.
@@ -22,47 +23,27 @@ A short demo will run.  Optionally, type ccolamd_test to run an extensive tests.
 Type "make" in Unix in the CCOLAMD directory to compile the C-callable
 library and to run a short demo.
 
-If you have MATLAB 7.2 or earlier, you must first edit UFconfig/UFconfig.h to
-remove the "-largeArrayDims" option from the MEX command (or just use
-ccolamd_install.m inside MATLAB).
-
 Other "make" targets:
 
-    make mex		compiles MATLAB mexFunctions only
+    make library	compiles a C-callable library containing ccolamd
-    make libccolamd.a	compiles a C-callable library containing ccolamd
+    make clean		removes all files not in the distribution
-    make clean		removes all files not in the distribution, except for
+                            but keeps the compiled libraries.
-			libccolamd.a
     make distclean	removes all files not in the distribution
+    make install        installs the library in /usr/local/lib and
+                            /usr/local/include
+    make uninstall      uninstalls the library from /usr/local/lib and
+                            /usr/local/include
 
 To use ccolamd and csymamd within an application written in C, all you need are
 ccolamd.c and ccolamd.h, which are the C-callable ccolamd/csymamd codes.
 See ccolamd.c for more information on how to call ccolamd from a C program.
 It contains a complete description of the C-interface to CCOLAMD and CSYMAMD.
 
-	Copyright (c) 1998-2007 by the University of Florida.
-	All Rights Reserved.
 
-	Licensed under the GNU LESSER GENERAL PUBLIC LICENSE.
+See CCOLAMD/Doc/License.txt for the license.
 
 -------------------------------------------------------------------------------
 
-This library is free software; you can redistribute it and/or
-modify it under the terms of the GNU Lesser General Public
-License as published by the Free Software Foundation; either
-version 2.1 of the License, or (at your option) any later version.
-
-This library is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-Lesser General Public License for more details.
-
-You should have received a copy of the GNU Lesser General Public
-License along with this library; if not, write to the Free Software
-Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-
--------------------------------------------------------------------------------
-
-
 Related papers:
 
 	T. A. Davis and W. W. Hager, Rajamanickam, Multiple-rank updates
@@ -86,21 +67,18 @@ Related papers:
 	"An approximate minimum degree column ordering algorithm",
 	S. I. Larimore, MS Thesis, Dept. of Computer and Information
 	Science and Engineering, University of Florida, Gainesville, FL,
-	1998.  CISE Tech Report TR-98-016.  Available at 
+	1998.  CISE Tech Report TR-98-016.
-	ftp://ftp.cise.ufl.edu/cis/tech-reports/tr98/tr98-016.ps
-	via anonymous ftp.
 
 	Approximate Deficiency for Ordering the Columns of a Matrix,
 	J. L. Kern, Senior Thesis, Dept. of Computer and Information
 	Science and Engineering, University of Florida, Gainesville, FL,
-	1999.  Available at http://www.cise.ufl.edu/~davis/Kern/kern.ps 
+	1999.
 
 Authors:  Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore.
 	Closely based on COLAMD by Stefan I. Larimore and Timothy A. Davis,
-	University of Florida, in collaboration with John Gilbert, Xerox PARC
+        in collaboration with John Gilbert, Xerox PARC (now at UC Santa
-	(now at UC Santa Barbara), and Esmong Ng, Lawrence Berkeley National
+        Barbara), and Esmong Ng, Lawrence Berkeley National Laboratory (much of
-	Laboratory (much of this work he did while at Oak Ridge National
+        this work he did while at Oak Ridge National Laboratory). 
-	Laboratory). 
 
 CCOLAMD files:
 
@@ -122,7 +100,6 @@ CCOLAMD files:
 
     ./Doc:
     ChangeLog	    change log
-    lesser.txt	    license
 
     ./Include:
     ccolamd.h	    include file
@@ -147,4 +124,3 @@ CCOLAMD files:
 
     ./Source:
     ccolamd.c		primary source code
-    ccolamd_global.c	globally defined function pointers (malloc, free, ...)

gtsam/3rdparty/CCOLAMD/Source/ccolamd.c

@@ -5,8 +5,6 @@
 /* ----------------------------------------------------------------------------
  * CCOLAMD, Copyright (C) Univ. of Florida.  Authors: Timothy A. Davis,
  * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
  * -------------------------------------------------------------------------- */
 
 /*
@@ -58,39 +56,13 @@
  *	COLAMD is also available under alternate licenses, contact T. Davis
  *	for details.
  *
- *	This library is free software; you can redistribute it and/or
+ *	See CCOLAMD/Doc/License.txt for the license.
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	This library is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with this library; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301
- *	USA
- *
- *	Permission is hereby granted to use or copy this program under the
- *	terms of the GNU LGPL, provided that the Copyright, this License,
- *	and the Availability of the original version is retained on all copies.
- *	User documentation of any code that uses this code or any modified
- *	version of this code must cite the Copyright, this License, the
- *	Availability note, and "Used by permission." Permission to modify
- *	the code and to distribute modified code is granted, provided the
- *	Copyright, this License, and the Availability note are retained,
- *	and a notice that the code was modified is included.
  *
  *  Availability:
  *
  *	The CCOLAMD/CSYMAMD library is available at
  *
- *	    http://www.cise.ufl.edu/research/sparse/ccolamd/
+ *	    http://www.suitesparse.com
- *
- *	This is the http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd.c
- *	file.
  *
  *   See the ChangeLog file for changes since Version 1.0.
  */
@@ -99,10 +71,10 @@
 /* === Description of user-callable routines ================================ */
 /* ========================================================================== */
 
-/* CCOLAMD includes both int and UF_long versions of all its routines.  The
+/* CCOLAMD includes both int and SuiteSparse_long versions of all its routines.
- * description below is for the int version.   For UF_long, all int arguments
+ * The description below is for the int version.   For SuiteSparse_long, all
- * become UF_long integers.  UF_long is normally defined as long, except for
+ * int arguments become SuiteSparse_long integers.  SuiteSparse_long is
- * WIN64 */
+ * normally defined as long, except for WIN64 */
 
 /*  ----------------------------------------------------------------------------
  *  ccolamd_recommended:
@@ -112,8 +84,8 @@
  *
  *	    #include "ccolamd.h"
  *	    size_t ccolamd_recommended (int nnz, int n_row, int n_col) ;
- *	    size_t ccolamd_l_recommended (UF_long nnz, UF_long n_row,
+ *	    size_t ccolamd_l_recommended (SuiteSparse_long nnz,
- *		UF_long n_col) ;
+ *              SuiteSparse_long n_row, SuiteSparse_long n_col) ;
  *
  *	Purpose:
  *
@@ -209,9 +181,12 @@
  *	    	double knobs [CCOLAMD_KNOBS], int stats [CCOLAMD_STATS],
  *		int *cmember) ;
  *
- *	    UF_long ccolamd_l (UF_long n_row, UF_long n_col, UF_long Alen,
+ *	    SuiteSparse_long ccolamd_l (SuiteSparse_long n_row,
- *		UF_long *A, UF_long *p, double knobs [CCOLAMD_KNOBS],
+ *	        SuiteSparse_long n_col, SuiteSparse_long Alen,
- *		UF_long stats [CCOLAMD_STATS], UF_long *cmember) ;
+ *              SuiteSparse_long *A, SuiteSparse_long *p,
+ *              double knobs [CCOLAMD_KNOBS],
+ *              SuiteSparse_long stats [CCOLAMD_STATS],
+ *              SuiteSparse_long *cmember) ;
  *
  *	Purpose:
  *
@@ -385,9 +360,7 @@
  *
  *	Example:
  *
- *	    See
+ *	    See ccolamd_example.c for a complete example.
- *	    http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd_example.c
- *	    for a complete example.
  *
  *	    To order the columns of a 5-by-4 matrix with 11 nonzero entries in
  *	    the following nonzero pattern
@@ -423,10 +396,12 @@
  *		void (*allocate) (size_t, size_t), void (*release) (void *),
  *		int *cmember, int stype) ;
  *
- *	    UF_long csymamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm,
+ *	    SuiteSparse_long csymamd_l (SuiteSparse_long n,
- *	    	double knobs [CCOLAMD_KNOBS], UF_long stats [CCOLAMD_STATS],
+ *              SuiteSparse_long *A, SuiteSparse_long *p,
- *		void (*allocate) (size_t, size_t), void (*release) (void *),
+ *              SuiteSparse_long *perm, double knobs [CCOLAMD_KNOBS],
- *		UF_long *cmember, UF_long stype) ;
+ *              SuiteSparse_long stats [CCOLAMD_STATS], void (*allocate)
+ *              (size_t, size_t), void (*release) (void *),
+ *              SuiteSparse_long *cmember, SuiteSparse_long stype) ;
  *
  *	Purpose:
  *
@@ -562,7 +537,7 @@
  *
  *	    #include "ccolamd.h"
  *	    ccolamd_report (int stats [CCOLAMD_STATS]) ;
- *	    ccolamd_l_report (UF_long stats [CCOLAMD_STATS]) ;
+ *	    ccolamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ;
  *
  *	Purpose:
  *
@@ -583,7 +558,7 @@
  *
  *	    #include "ccolamd.h"
  *	    csymamd_report (int stats [CCOLAMD_STATS]) ;
- *	    csymamd_l_report (UF_long stats [CCOLAMD_STATS]) ;
+ *	    csymamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ;
  *
  *	Purpose:
  *
@@ -617,12 +592,11 @@
 
 #include "ccolamd.h"
 
+#include <stdlib.h>
 #include <math.h>
 #include <limits.h>
 
 #ifdef MATLAB_MEX_FILE
-#include <stdint.h>
-typedef uint16_t char16_t;
 #include "mex.h"
 #include "matrix.h"
 #endif
@@ -636,17 +610,14 @@ typedef uint16_t char16_t;
 #endif
 
 /* ========================================================================== */
-/* === int or UF_long ======================================================= */
+/* === int or SuiteSparse_long ============================================== */
 /* ========================================================================== */
 
-/* define UF_long */
-#include "UFconfig.h"
-
 #ifdef DLONG
 
-#define Int UF_long
+#define Int SuiteSparse_long
-#define ID  UF_long_id
+#define ID  SuiteSparse_long_id
-#define Int_MAX UF_long_max
+#define Int_MAX SuiteSparse_long_max
 
 #define CCOLAMD_recommended ccolamd_l_recommended
 #define CCOLAMD_set_defaults ccolamd_l_set_defaults
@@ -811,9 +782,6 @@ typedef struct CColamd_Row_struct
 #define INDEX(i) (i)
 #endif
 
-/* All output goes through the PRINTF macro.  */
-#define PRINTF(params) { if (ccolamd_printf != NULL) (void) ccolamd_printf params ; }
-
 
 /* ========================================================================== */
 /* === Debugging prototypes and definitions ================================= */
@@ -827,11 +795,11 @@ typedef struct CColamd_Row_struct
 PRIVATE Int ccolamd_debug ;
 
 /* debug print statements */
-#define DEBUG0(params) { PRINTF (params) ; }
+#define DEBUG0(params) { SUITESPARSE_PRINTF (params) ; }
-#define DEBUG1(params) { if (ccolamd_debug >= 1) PRINTF (params) ; }
+#define DEBUG1(params) { if (ccolamd_debug >= 1) SUITESPARSE_PRINTF (params) ; }
-#define DEBUG2(params) { if (ccolamd_debug >= 2) PRINTF (params) ; }
+#define DEBUG2(params) { if (ccolamd_debug >= 2) SUITESPARSE_PRINTF (params) ; }
-#define DEBUG3(params) { if (ccolamd_debug >= 3) PRINTF (params) ; }
+#define DEBUG3(params) { if (ccolamd_debug >= 3) SUITESPARSE_PRINTF (params) ; }
-#define DEBUG4(params) { if (ccolamd_debug >= 4) PRINTF (params) ; }
+#define DEBUG4(params) { if (ccolamd_debug >= 4) SUITESPARSE_PRINTF (params) ; }
 
 #ifdef MATLAB_MEX_FILE
 #define ASSERT(expression) (mxAssert ((expression), ""))
@@ -3752,12 +3720,12 @@ PRIVATE void print_report
 
     Int i1, i2, i3 ;
 
-    PRINTF (("\n%s version %d.%d, %s: ", method,
+    SUITESPARSE_PRINTF (("\n%s version %d.%d, %s: ", method,
 	    CCOLAMD_MAIN_VERSION, CCOLAMD_SUB_VERSION, CCOLAMD_DATE)) ;
 
     if (!stats)
     {
-    	PRINTF (("No statistics available.\n")) ;
+    	SUITESPARSE_PRINTF (("No statistics available.\n")) ;
 	return ;
     }
 
@@ -3767,11 +3735,11 @@ PRIVATE void print_report
 
     if (stats [CCOLAMD_STATUS] >= 0)
     {
-    	PRINTF(("OK.  ")) ;
+    	SUITESPARSE_PRINTF(("OK.  ")) ;
     }
     else
     {
-    	PRINTF(("ERROR.  ")) ;
+    	SUITESPARSE_PRINTF(("ERROR.  ")) ;
     }
 
     switch (stats [CCOLAMD_STATUS])
@@ -3779,91 +3747,105 @@ PRIVATE void print_report
 
 	case CCOLAMD_OK_BUT_JUMBLED:
 
-	    PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ;
+            SUITESPARSE_PRINTF((
+                    "Matrix has unsorted or duplicate row indices.\n")) ;
 
-	    PRINTF(("%s: duplicate or out-of-order row indices:    "ID"\n",
+            SUITESPARSE_PRINTF((
-		    method, i3)) ;
+                    "%s: duplicate or out-of-order row indices:    "ID"\n",
+                    method, i3)) ;
 
-	    PRINTF(("%s: last seen duplicate or out-of-order row:  "ID"\n",
+            SUITESPARSE_PRINTF((
-		    method, INDEX (i2))) ;
+                    "%s: last seen duplicate or out-of-order row:  "ID"\n",
+                    method, INDEX (i2))) ;
 
-	    PRINTF(("%s: last seen in column:                      "ID"",
+            SUITESPARSE_PRINTF((
-		    method, INDEX (i1))) ;
+                    "%s: last seen in column:                      "ID"",
+                    method, INDEX (i1))) ;
 
 	    /* no break - fall through to next case instead */
 
 	case CCOLAMD_OK:
 
-	    PRINTF(("\n")) ;
+            SUITESPARSE_PRINTF(("\n")) ;
 
- 	    PRINTF(("%s: number of dense or empty rows ignored:    "ID"\n",
+            SUITESPARSE_PRINTF((
-		    method, stats [CCOLAMD_DENSE_ROW])) ;
+                    "%s: number of dense or empty rows ignored:    "ID"\n",
+                    method, stats [CCOLAMD_DENSE_ROW])) ;
 
-	    PRINTF(("%s: number of dense or empty columns ignored: "ID"\n",
+            SUITESPARSE_PRINTF((
-		    method, stats [CCOLAMD_DENSE_COL])) ;
+                    "%s: number of dense or empty columns ignored: "ID"\n",
+                    method, stats [CCOLAMD_DENSE_COL])) ;
 
-	    PRINTF(("%s: number of garbage collections performed:  "ID"\n",
+            SUITESPARSE_PRINTF((
-		    method, stats [CCOLAMD_DEFRAG_COUNT])) ;
+                    "%s: number of garbage collections performed:  "ID"\n",
+                    method, stats [CCOLAMD_DEFRAG_COUNT])) ;
 	    break ;
 
 	case CCOLAMD_ERROR_A_not_present:
 
-	    PRINTF(("Array A (row indices of matrix) not present.\n")) ;
+            SUITESPARSE_PRINTF((
+                    "Array A (row indices of matrix) not present.\n")) ;
 	    break ;
 
 	case CCOLAMD_ERROR_p_not_present:
 
-	    PRINTF(("Array p (column pointers for matrix) not present.\n")) ;
+            SUITESPARSE_PRINTF((
+                    "Array p (column pointers for matrix) not present.\n")) ;
 	    break ;
 
 	case CCOLAMD_ERROR_nrow_negative:
 
-	    PRINTF(("Invalid number of rows ("ID").\n", i1)) ;
+            SUITESPARSE_PRINTF(("Invalid number of rows ("ID").\n", i1)) ;
 	    break ;
 
 	case CCOLAMD_ERROR_ncol_negative:
 
-	    PRINTF(("Invalid number of columns ("ID").\n", i1)) ;
+            SUITESPARSE_PRINTF(("Invalid number of columns ("ID").\n", i1)) ;
 	    break ;
 
 	case CCOLAMD_ERROR_nnz_negative:
 
-	    PRINTF(("Invalid number of nonzero entries ("ID").\n", i1)) ;
+            SUITESPARSE_PRINTF((
+                    "Invalid number of nonzero entries ("ID").\n", i1)) ;
 	    break ;
 
 	case CCOLAMD_ERROR_p0_nonzero:
 
-	    PRINTF(("Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ;
+            SUITESPARSE_PRINTF((
+                    "Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ;
 	    break ;
 
 	case CCOLAMD_ERROR_A_too_small:
 
-	    PRINTF(("Array A too small.\n")) ;
+            SUITESPARSE_PRINTF(("Array A too small.\n")) ;
-	    PRINTF(("        Need Alen >= "ID", but given only Alen = "ID".\n",
+            SUITESPARSE_PRINTF((
-		    i1, i2)) ;
+                    "        Need Alen >= "ID", but given only Alen = "ID".\n",
+                    i1, i2)) ;
 	    break ;
 
 	case CCOLAMD_ERROR_col_length_negative:
 
-	    PRINTF(("Column "ID" has a negative number of entries ("ID").\n",
+            SUITESPARSE_PRINTF((
-		    INDEX (i1), i2)) ;
+                    "Column "ID" has a negative number of entries ("ID").\n",
+                    INDEX (i1), i2)) ;
 	    break ;
 
 	case CCOLAMD_ERROR_row_index_out_of_bounds:
 
-	    PRINTF(("Row index (row "ID") out of bounds ("ID" to "ID") in"
+            SUITESPARSE_PRINTF((
-		    "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1),
+                    "Row index (row "ID") out of bounds ("ID" to "ID") in"
-		    INDEX (i1))) ;
+                    "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1),
+                    INDEX (i1))) ;
 	    break ;
 
 	case CCOLAMD_ERROR_out_of_memory:
 
-	    PRINTF(("Out of memory.\n")) ;
+            SUITESPARSE_PRINTF(("Out of memory.\n")) ;
 	    break ;
 
 	case CCOLAMD_ERROR_invalid_cmember:
 
-	    PRINTF(("cmember invalid\n")) ;
+            SUITESPARSE_PRINTF(("cmember invalid\n")) ;
 	    break ;
     }
 }

gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c

@@ -1,28 +0,0 @@
-/* ========================================================================== */
-/* === ccolamd_global.c ===================================================== */
-/* ========================================================================== */
-
-/* ----------------------------------------------------------------------------
- * CCOLAMD Copyright (C), Univ. of Florida.  Authors: Timothy A. Davis,
- * Sivasankaran Rajamanickam, and Stefan Larimore
- * See License.txt for the Version 2.1 of the GNU Lesser General Public License
- * http://www.cise.ufl.edu/research/sparse
- * -------------------------------------------------------------------------- */
-
-/* Global variables for CCOLAMD */
-
-#ifndef NPRINT
-#ifdef MATLAB_MEX_FILE
-#include <stdlib.h>
-#include <stdint.h>
-typedef uint16_t char16_t;
-#include "mex.h"
-int (*ccolamd_printf) (const char *, ...) = mexPrintf ;
-#else
-#include <stdio.h>
-int (*ccolamd_printf) (const char *, ...) = printf ;
-#endif
-#else
-int (*ccolamd_printf) (const char *, ...) = ((void *) 0) ;
-#endif
-

gtsam/3rdparty/CMakeLists.txt

@@ -1,6 +1,6 @@
 # install CCOLAMD headers
 install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
-install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig)
+install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	# Find plain .h files

gtsam/3rdparty/Eigen/.hg_archival.txt

@@ -1,4 +1,4 @@
 repo: 8a21fd850624c931e448cbcfb38168cb2717c790
-node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15
+node: b9cd8366d4e8f49471c7afafc4c2a1b00e54a54d
 branch: 3.2
-tag: 3.2.8
+tag: 3.2.10

gtsam/3rdparty/Eigen/.hgtags

@@ -31,3 +31,5 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
 bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
 c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
 b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7
+07105f7124f9aef00a68c85e0fc606e65d3d6c15 3.2.8
+dc6cfdf9bcec5efc7b6593bddbbb3d675de53524 3.2.9

gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h

@@ -66,9 +66,8 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprTyp
                          : ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime)
                          : int(traits<XprType>::MaxColsAtCompileTime),
     XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0,
-    IsDense = is_same<StorageKind,Dense>::value,
+    IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
-    IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
+               : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
-               : (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
                : XprTypeIsRowMajor,
     HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor),
     InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),

gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h

@@ -76,9 +76,7 @@ struct CommaInitializer
   template<typename OtherDerived>
   CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
   {
-    if(other.cols()==0 || other.rows()==0)
+    if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows))
-      return *this;
-    if (m_col==m_xpr.cols())
     {
       m_row+=m_currentBlockRows;
       m_col = 0;
@@ -86,24 +84,18 @@ struct CommaInitializer
       eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
         && "Too many rows passed to comma initializer (operator<<)");
     }
-    eigen_assert(m_col<m_xpr.cols()
+    eigen_assert((m_col + other.cols() <= m_xpr.cols())
       && "Too many coefficients passed to comma initializer (operator<<)");
     eigen_assert(m_currentBlockRows==other.rows());
-    if (OtherDerived::SizeAtCompileTime != Dynamic)
+    m_xpr.template block<OtherDerived::RowsAtCompileTime, OtherDerived::ColsAtCompileTime>
-      m_xpr.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
+                    (m_row, m_col, other.rows(), other.cols()) = other;
-                              OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
-                    (m_row, m_col) = other;
-    else
-      m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other;
     m_col += other.cols();
     return *this;
   }
 
   inline ~CommaInitializer()
   {
-    eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows()
+      finished();
-         && m_col == m_xpr.cols()
-         && "Too few coefficients passed to comma initializer (operator<<)");
   }
 
   /** \returns the built matrix once all its coefficients have been set.
@@ -113,7 +105,12 @@ struct CommaInitializer
     * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
     * \endcode
     */
-  inline XprType& finished() { return m_xpr; }
+  inline XprType& finished() {
+      eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0)
+           && m_col == m_xpr.cols()
+           && "Too few coefficients passed to comma initializer (operator<<)");
+      return m_xpr;
+  }
 
   XprType& m_xpr;   // target expression
   Index m_row;              // current row id

gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h

@@ -44,10 +44,10 @@ class DiagonalBase : public EigenBase<Derived>
     template<typename DenseDerived>
     void evalTo(MatrixBase<DenseDerived> &other) const;
     template<typename DenseDerived>
-    void addTo(MatrixBase<DenseDerived> &other) const
+    inline void addTo(MatrixBase<DenseDerived> &other) const
     { other.diagonal() += diagonal(); }
     template<typename DenseDerived>
-    void subTo(MatrixBase<DenseDerived> &other) const
+    inline void subTo(MatrixBase<DenseDerived> &other) const
     { other.diagonal() -= diagonal(); }
 
     inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); }
@@ -98,7 +98,7 @@ class DiagonalBase : public EigenBase<Derived>
 
 template<typename Derived>
 template<typename DenseDerived>
-void DiagonalBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
+inline void DiagonalBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
 {
   other.setZero();
   other.diagonal() = diagonal();

gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h

@@ -59,7 +59,7 @@ struct dot_nocheck<T, U, true>
   */
 template<typename Derived>
 template<typename OtherDerived>
-typename internal::scalar_product_traits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
+inline typename internal::scalar_product_traits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
 MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)

gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h

@@ -969,6 +969,8 @@ template<typename T>
 struct functor_traits<std::not_equal_to<T> >
 { enum { Cost = 1, PacketAccess = false }; };
 
+#if(__cplusplus < 201103L)
+// std::binder* are deprecated since c++11 and will be removed in c++17
 template<typename T>
 struct functor_traits<std::binder2nd<T> >
 { enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
@@ -976,6 +978,7 @@ struct functor_traits<std::binder2nd<T> >
 template<typename T>
 struct functor_traits<std::binder1st<T> >
 { enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
+#endif
 
 template<typename T>
 struct functor_traits<std::unary_negate<T> >

gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h

@@ -205,9 +205,6 @@ class GeneralProduct<Lhs, Rhs, InnerProduct>
   public:
     GeneralProduct(const Lhs& lhs, const Rhs& rhs)
     {
-      EIGEN_STATIC_ASSERT((internal::is_same<typename Lhs::RealScalar, typename Rhs::RealScalar>::value),
-        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
-
       Base::coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum();
     }
 
@@ -264,8 +261,6 @@ class GeneralProduct<Lhs, Rhs, OuterProduct>
 
     GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs)
     {
-      EIGEN_STATIC_ASSERT((internal::is_same<typename Lhs::RealScalar, typename Rhs::RealScalar>::value),
-        YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
     }
     
     struct set  { template<typename Dst, typename Src> void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived()  = src; } };

gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h

@@ -183,8 +183,8 @@ template<typename Scalar, typename Packet> inline void pstoreu(Scalar* to, const
 /** \internal tries to do cache prefetching of \a addr */
 template<typename Scalar> inline void prefetch(const Scalar* addr)
 {
-#if !defined(_MSC_VER)
+#if (!EIGEN_COMP_MSVC) && (EIGEN_COMP_GNUC || EIGEN_COMP_CLANG || EIGEN_COMP_ICC)
-__builtin_prefetch(addr);
+  __builtin_prefetch(addr);
 #endif
 }
 

gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h

@@ -218,8 +218,8 @@ struct conj_retval
 * Implementation of abs2                                                 *
 ****************************************************************************/
 
-template<typename Scalar>
+template<typename Scalar,bool IsComplex>
-struct abs2_impl
+struct abs2_impl_default
 {
   typedef typename NumTraits<Scalar>::Real RealScalar;
   static inline RealScalar run(const Scalar& x)
@@ -228,15 +228,26 @@ struct abs2_impl
   }
 };
 
-template<typename RealScalar>
+template<typename Scalar>
-struct abs2_impl<std::complex<RealScalar> >
+struct abs2_impl_default<Scalar, true> // IsComplex
 {
-  static inline RealScalar run(const std::complex<RealScalar>& x)
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  static inline RealScalar run(const Scalar& x)
   {
     return real(x)*real(x) + imag(x)*imag(x);
   }
 };
 
+template<typename Scalar>
+struct abs2_impl
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  static inline RealScalar run(const Scalar& x)
+  {
+    return abs2_impl_default<Scalar,NumTraits<Scalar>::IsComplex>::run(x);
+  }
+};
+
 template<typename Scalar>
 struct abs2_retval
 {
@@ -496,11 +507,24 @@ struct floor_log2<n, lower, upper, floor_log2_bogus>
 template<typename Scalar>
 struct random_default_impl<Scalar, false, true>
 {
-  typedef typename NumTraits<Scalar>::NonInteger NonInteger;
-
   static inline Scalar run(const Scalar& x, const Scalar& y)
   {
-    return x + Scalar((NonInteger(y)-x+1) * std::rand() / (RAND_MAX + NonInteger(1)));
+    typedef typename conditional<NumTraits<Scalar>::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX;
+    if(y<x)
+      return x;
+    // the following difference might overflow on a 32 bits system,
+    // but since y>=x the result converted to an unsigned long is still correct.
+    std::size_t range = ScalarX(y)-ScalarX(x);
+    std::size_t offset = 0;
+    // rejection sampling
+    std::size_t divisor = 1;
+    std::size_t multiplier = 1;
+    if(range<RAND_MAX) divisor = (std::size_t(RAND_MAX)+1)/(range+1);
+    else               multiplier = 1 + range/(std::size_t(RAND_MAX)+1);
+    do {
+      offset = (std::size_t(std::rand()) * multiplier) / divisor;
+    } while (offset > range);
+    return Scalar(ScalarX(x) + offset);
   }
 
   static inline Scalar run()

gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h

@@ -584,10 +584,11 @@ struct permut_matrix_product_retval
       const Index n = Side==OnTheLeft ? rows() : cols();
       // FIXME we need an is_same for expression that is not sensitive to constness. For instance
       // is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
+      const typename Dest::Scalar *dst_data = internal::extract_data(dst);
       if(    is_same<MatrixTypeNestedCleaned,Dest>::value
           && blas_traits<MatrixTypeNestedCleaned>::HasUsableDirectAccess
           && blas_traits<Dest>::HasUsableDirectAccess
-          && extract_data(dst) == extract_data(m_matrix))
+          && dst_data!=0 && dst_data == extract_data(m_matrix))
       {
         // apply the permutation inplace
         Matrix<bool,PermutationType::RowsAtCompileTime,1,0,PermutationType::MaxRowsAtCompileTime> mask(m_permutation.size());

gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h

@@ -315,8 +315,8 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
     EIGEN_STRONG_INLINE void resizeLike(const EigenBase<OtherDerived>& _other)
     {
       const OtherDerived& other = _other.derived();
-      internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(other.rows(), other.cols());
+      internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(Index(other.rows()), Index(other.cols()));
-      const Index othersize = other.rows()*other.cols();
+      const Index othersize = Index(other.rows())*Index(other.cols());
       if(RowsAtCompileTime == 1)
       {
         eigen_assert(other.rows() == 1 || other.cols() == 1);
@@ -487,7 +487,7 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
     /** \sa MatrixBase::operator=(const EigenBase<OtherDerived>&) */
     template<typename OtherDerived>
     EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase<OtherDerived> &other)
-      : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
+      : m_storage(Index(other.derived().rows()) * Index(other.derived().cols()), other.derived().rows(), other.derived().cols())
     {
       _check_template_params();
       internal::check_rows_cols_for_overflow<MaxSizeAtCompileTime>::run(other.derived().rows(), other.derived().cols());

gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h

@@ -76,9 +76,23 @@ template<typename MatrixType, int Direction> class Reverse
     EIGEN_DENSE_PUBLIC_INTERFACE(Reverse)
     using Base::IsRowMajor;
 
-    // next line is necessary because otherwise const version of operator()
+    // The following two operators are provided to worarkound
-    // is hidden by non-const version defined in this file
+    // a MSVC 2013 issue. In theory, we could simply do:
-    using Base::operator(); 
+    //   using Base::operator(); 
+    // to make const version of operator() visible.
+    // Otheriwse, they would be hidden by the non-const versions defined in this file
+    
+    inline CoeffReturnType operator()(Index row, Index col) const
+    {
+      eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols());
+      return coeff(row, col);
+    }
+
+    inline CoeffReturnType operator()(Index index) const
+    {
+      eigen_assert(index >= 0 && index < m_matrix.size());
+      return coeff(index);
+    }
 
   protected:
     enum {

gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h

@@ -243,7 +243,8 @@ template<int Side, typename TriangularType, typename Rhs> struct triangular_solv
 
   template<typename Dest> inline void evalTo(Dest& dst) const
   {
-    if(!(is_same<RhsNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_rhs)))
+    const typename Dest::Scalar *dst_data = internal::extract_data(dst);
+    if(!(is_same<RhsNestedCleaned,Dest>::value && dst_data!=0 && extract_data(dst) == extract_data(m_rhs)))
       dst = m_rhs;
     m_triangularMatrix.template solveInPlace<Side>(dst);
   }

gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h

@@ -331,11 +331,11 @@ inline void MatrixBase<Derived>::adjointInPlace()
 
 namespace internal {
 
-template<typename BinOp,typename NestedXpr,typename Rhs>
+template<typename BinOp,typename Xpr,typename Rhs>
-struct blas_traits<SelfCwiseBinaryOp<BinOp,NestedXpr,Rhs> >
+struct blas_traits<SelfCwiseBinaryOp<BinOp,Xpr,Rhs> >
- : blas_traits<NestedXpr>
+ : blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
 {
-  typedef SelfCwiseBinaryOp<BinOp,NestedXpr,Rhs> XprType;
+  typedef SelfCwiseBinaryOp<BinOp,Xpr,Rhs> XprType;
   static inline const XprType extract(const XprType& x) { return x; }
 };
 
@@ -392,7 +392,6 @@ struct checkTransposeAliasing_impl
                       ::run(extract_data(dst), other))
           && "aliasing detected during transposition, use transposeInPlace() "
              "or evaluate the rhs into a temporary using .eval()");
-
     }
 };
 

gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h

@@ -376,7 +376,8 @@ struct transposition_matrix_product_retval
       const int size = m_transpositions.size();
       Index j = 0;
 
-      if(!(is_same<MatrixTypeNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_matrix)))
+      const typename Dest::Scalar *dst_data = internal::extract_data(dst);
+      if(!(is_same<MatrixTypeNestedCleaned,Dest>::value && dst_data!=0 && dst_data == extract_data(m_matrix)))
         dst = m_matrix;
 
       for(int k=(Transposed?size-1:0) ; Transposed?k>=0:k<size ; Transposed?--k:++k)

gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h

@@ -81,7 +81,7 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
     // coherence when accessing the rhs elements
     std::ptrdiff_t l1, l2;
     manage_caching_sizes(GetAction, &l1, &l2);
-    Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) * otherStride) : 0;
+    Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) *  std::max<Index>(otherStride,size)) : 0;
     subcols = std::max<Index>((subcols/Traits::nr)*Traits::nr, Traits::nr);
 
     for(Index k2=IsLower ? 0 : size;

gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h

@@ -42,16 +42,29 @@ template<bool Conjugate> struct conj_if;
 
 template<> struct conj_if<true> {
   template<typename T>
-  inline T operator()(const T& x) { return numext::conj(x); }
+  inline T operator()(const T& x) const { return numext::conj(x); }
   template<typename T>
-  inline T pconj(const T& x) { return internal::pconj(x); }
+  inline T pconj(const T& x) const { return internal::pconj(x); }
 };
 
 template<> struct conj_if<false> {
   template<typename T>
-  inline const T& operator()(const T& x) { return x; }
+  inline const T& operator()(const T& x) const { return x; }
   template<typename T>
-  inline const T& pconj(const T& x) { return x; }
+  inline const T& pconj(const T& x) const { return x; }
+};
+
+// Generic implementation for custom complex types.
+template<typename LhsScalar, typename RhsScalar, bool ConjLhs, bool ConjRhs>
+struct conj_helper
+{
+  typedef typename scalar_product_traits<LhsScalar,RhsScalar>::ReturnType Scalar;
+
+  EIGEN_STRONG_INLINE Scalar pmadd(const LhsScalar& x, const RhsScalar& y, const Scalar& c) const
+  { return padd(c, pmul(x,y)); }
+
+  EIGEN_STRONG_INLINE Scalar pmul(const LhsScalar& x, const RhsScalar& y) const
+  { return conj_if<ConjLhs>()(x) *  conj_if<ConjRhs>()(y); }
 };
 
 template<typename Scalar> struct conj_helper<Scalar,Scalar,false,false>
@@ -171,12 +184,13 @@ template<typename XprType> struct blas_traits
 };
 
 // pop conjugate
-template<typename Scalar, typename NestedXpr>
+template<typename Scalar, typename Xpr>
-struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> >
+struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, Xpr> >
- : blas_traits<NestedXpr>
+ : blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
 {
+  typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
   typedef blas_traits<NestedXpr> Base;
-  typedef CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> XprType;
+  typedef CwiseUnaryOp<scalar_conjugate_op<Scalar>, Xpr> XprType;
   typedef typename Base::ExtractType ExtractType;
 
   enum {
@@ -188,12 +202,13 @@ struct blas_traits<CwiseUnaryOp<scalar_conjugate_op<Scalar>, NestedXpr> >
 };
 
 // pop scalar multiple
-template<typename Scalar, typename NestedXpr>
+template<typename Scalar, typename Xpr>
-struct blas_traits<CwiseUnaryOp<scalar_multiple_op<Scalar>, NestedXpr> >
+struct blas_traits<CwiseUnaryOp<scalar_multiple_op<Scalar>, Xpr> >
- : blas_traits<NestedXpr>
+ : blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
 {
+  typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
   typedef blas_traits<NestedXpr> Base;
-  typedef CwiseUnaryOp<scalar_multiple_op<Scalar>, NestedXpr> XprType;
+  typedef CwiseUnaryOp<scalar_multiple_op<Scalar>, Xpr> XprType;
   typedef typename Base::ExtractType ExtractType;
   static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
   static inline Scalar extractScalarFactor(const XprType& x)
@@ -201,12 +216,13 @@ struct blas_traits<CwiseUnaryOp<scalar_multiple_op<Scalar>, NestedXpr> >
 };
 
 // pop opposite
-template<typename Scalar, typename NestedXpr>
+template<typename Scalar, typename Xpr>
-struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> >
+struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, Xpr> >
- : blas_traits<NestedXpr>
+ : blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
 {
+  typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
   typedef blas_traits<NestedXpr> Base;
-  typedef CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> XprType;
+  typedef CwiseUnaryOp<scalar_opposite_op<Scalar>, Xpr> XprType;
   typedef typename Base::ExtractType ExtractType;
   static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); }
   static inline Scalar extractScalarFactor(const XprType& x)
@@ -214,13 +230,14 @@ struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> >
 };
 
 // pop/push transpose
-template<typename NestedXpr>
+template<typename Xpr>
-struct blas_traits<Transpose<NestedXpr> >
+struct blas_traits<Transpose<Xpr> >
- : blas_traits<NestedXpr>
+ : blas_traits<typename internal::remove_all<typename Xpr::Nested>::type>
 {
+  typedef typename internal::remove_all<typename Xpr::Nested>::type NestedXpr;
   typedef typename NestedXpr::Scalar Scalar;
   typedef blas_traits<NestedXpr> Base;
-  typedef Transpose<NestedXpr> XprType;
+  typedef Transpose<Xpr> XprType;
   typedef Transpose<const typename Base::_ExtractType>  ExtractType; // const to get rid of a compile error; anyway blas traits are only used on the RHS
   typedef Transpose<const typename Base::_ExtractType> _ExtractType;
   typedef typename conditional<bool(Base::HasUsableDirectAccess),

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h

@@ -162,7 +162,7 @@ const unsigned int HereditaryBits = RowMajorBit
 /** \ingroup enums
   * Enum containing possible values for the \p Mode parameter of 
   * MatrixBase::selfadjointView() and MatrixBase::triangularView(). */
-enum {
+enum UpLoType {
   /** View matrix as a lower triangular matrix. */
   Lower=0x1,                      
   /** View matrix as an upper triangular matrix. */
@@ -187,7 +187,7 @@ enum {
 
 /** \ingroup enums
   * Enum for indicating whether an object is aligned or not. */
-enum { 
+enum AlignmentType {
   /** Object is not correctly aligned for vectorization. */
   Unaligned=0, 
   /** Object is aligned for vectorization. */
@@ -217,7 +217,7 @@ enum DirectionType {
 
 /** \internal \ingroup enums
   * Enum to specify how to traverse the entries of a matrix. */
-enum {
+enum TraversalType {
   /** \internal Default traversal, no vectorization, no index-based access */
   DefaultTraversal,
   /** \internal No vectorization, use index-based access to have only one for loop instead of 2 nested loops */
@@ -239,7 +239,7 @@ enum {
 
 /** \internal \ingroup enums
   * Enum to specify whether to unroll loops when traversing over the entries of a matrix. */
-enum {
+enum UnrollingType {
   /** \internal Do not unroll loops. */
   NoUnrolling,
   /** \internal Unroll only the inner loop, but not the outer loop. */
@@ -251,7 +251,7 @@ enum {
 
 /** \internal \ingroup enums
   * Enum to specify whether to use the default (built-in) implementation or the specialization. */
-enum {
+enum SpecializedType {
   Specialized,
   BuiltIn
 };
@@ -259,7 +259,7 @@ enum {
 /** \ingroup enums
   * Enum containing possible values for the \p _Options template parameter of
   * Matrix, Array and BandMatrix. */
-enum {
+enum StorageOptions {
   /** Storage order is column major (see \ref TopicStorageOrders). */
   ColMajor = 0,
   /** Storage order is row major (see \ref TopicStorageOrders). */
@@ -272,7 +272,7 @@ enum {
 
 /** \ingroup enums
   * Enum for specifying whether to apply or solve on the left or right. */
-enum {
+enum SideType {
   /** Apply transformation on the left. */
   OnTheLeft = 1,  
   /** Apply transformation on the right. */
@@ -418,7 +418,7 @@ namespace Architecture
 
 /** \internal \ingroup enums
   * Enum used as template parameter in GeneralProduct. */
-enum { CoeffBasedProductMode, LazyCoeffBasedProductMode, OuterProduct, InnerProduct, GemvProduct, GemmProduct };
+enum ProductImplType { CoeffBasedProductMode, LazyCoeffBasedProductMode, OuterProduct, InnerProduct, GemvProduct, GemmProduct };
 
 /** \internal \ingroup enums
   * Enum used in experimental parallel implementation. */

gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h

@@ -35,6 +35,14 @@
     #pragma clang diagnostic push
   #endif
   #pragma clang diagnostic ignored "-Wconstant-logical-operand"
+
+#elif defined __GNUC__ && __GNUC__>=6
+
+  #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
+    #pragma GCC diagnostic push
+  #endif
+  #pragma GCC diagnostic ignored "-Wignored-attributes"
+
 #endif
 
 #endif // not EIGEN_WARNINGS_DISABLED

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h

@@ -13,7 +13,7 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 2
-#define EIGEN_MINOR_VERSION 8
+#define EIGEN_MINOR_VERSION 10
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \

gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h

@@ -507,7 +507,12 @@ template<typename T> void smart_copy(const T* start, const T* end, T* target)
 
 template<typename T> struct smart_copy_helper<T,true> {
   static inline void run(const T* start, const T* end, T* target)
-  { memcpy(target, start, std::ptrdiff_t(end)-std::ptrdiff_t(start)); }
+  {
+    std::ptrdiff_t size = std::ptrdiff_t(end)-std::ptrdiff_t(start);
+    if(size==0) return;
+    eigen_internal_assert(start!=0 && end!=0 && target!=0);
+    memcpy(target, start, size);
+  }
 };
 
 template<typename T> struct smart_copy_helper<T,false> {
@@ -515,7 +520,6 @@ template<typename T> struct smart_copy_helper<T,false> {
   { std::copy(start, end, target); }
 };
 
-
 /*****************************************************************************
 *** Implementation of runtime stack allocation (falling back to malloc)    ***
 *****************************************************************************/
@@ -655,99 +659,60 @@ template<typename T> class aligned_stack_memory_handler
 
 /****************************************************************************/
 
+
 /** \class aligned_allocator
-* \ingroup Core_Module
+  * \ingroup Core_Module
-*
+  *
-* \brief STL compatible allocator to use with with 16 byte aligned types
+  * \brief STL compatible allocator to use with with 16 byte aligned types
-*
+  *
-* Example:
+  * Example:
-* \code
+  * \code
-* // Matrix4f requires 16 bytes alignment:
+  * // Matrix4f requires 16 bytes alignment:
-* std::map< int, Matrix4f, std::less<int>, 
+  * std::map< int, Matrix4f, std::less<int>,
-*           aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
+  *           aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
-* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
+  * // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
-* std::map< int, Vector3f > my_map_vec3;
+  * std::map< int, Vector3f > my_map_vec3;
-* \endcode
+  * \endcode
-*
+  *
-* \sa \ref TopicStlContainers.
+  * \sa \blank \ref TopicStlContainers.
-*/
+  */
 template<class T>
-class aligned_allocator
+class aligned_allocator : public std::allocator<T>
 {
 public:
-    typedef size_t    size_type;
+  typedef size_t          size_type;
-    typedef std::ptrdiff_t difference_type;
+  typedef std::ptrdiff_t  difference_type;
-    typedef T*        pointer;
+  typedef T*              pointer;
-    typedef const T*  const_pointer;
+  typedef const T*        const_pointer;
-    typedef T&        reference;
+  typedef T&              reference;
-    typedef const T&  const_reference;
+  typedef const T&        const_reference;
-    typedef T         value_type;
+  typedef T               value_type;
 
-    template<class U>
+  template<class U>
-    struct rebind
+  struct rebind
-    {
+  {
-        typedef aligned_allocator<U> other;
+    typedef aligned_allocator<U> other;
-    };
+  };
 
-    pointer address( reference value ) const
+  aligned_allocator() : std::allocator<T>() {}
-    {
-        return &value;
-    }
 
-    const_pointer address( const_reference value ) const
+  aligned_allocator(const aligned_allocator& other) : std::allocator<T>(other) {}
-    {
-        return &value;
-    }
 
-    aligned_allocator()
+  template<class U>
-    {
+  aligned_allocator(const aligned_allocator<U>& other) : std::allocator<T>(other) {}
-    }
 
-    aligned_allocator( const aligned_allocator& )
+  ~aligned_allocator() {}
-    {
-    }
 
-    template<class U>
+  pointer allocate(size_type num, const void* /*hint*/ = 0)
-    aligned_allocator( const aligned_allocator<U>& )
+  {
-    {
+    internal::check_size_for_overflow<T>(num);
-    }
+    return static_cast<pointer>( internal::aligned_malloc(num * sizeof(T)) );
+  }
 
-    ~aligned_allocator()
+  void deallocate(pointer p, size_type /*num*/)
-    {
+  {
-    }
+    internal::aligned_free(p);
-
+  }
-    size_type max_size() const
-    {
-        return (std::numeric_limits<size_type>::max)();
-    }
-
-    pointer allocate( size_type num, const void* hint = 0 )
-    {
-        EIGEN_UNUSED_VARIABLE(hint);
-        internal::check_size_for_overflow<T>(num);
-        return static_cast<pointer>( internal::aligned_malloc( num * sizeof(T) ) );
-    }
-
-    void construct( pointer p, const T& value )
-    {
-        ::new( p ) T( value );
-    }
-
-    void destroy( pointer p )
-    {
-        p->~T();
-    }
-
-    void deallocate( pointer p, size_type /*num*/ )
-    {
-        internal::aligned_free( p );
-    }
-
-    bool operator!=(const aligned_allocator<T>& ) const
-    { return false; }
-
-    bool operator==(const aligned_allocator<T>& ) const
-    { return true; }
 };
 
 //---------- Cache sizes ----------

gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h

@@ -8,7 +8,10 @@
     #pragma warning pop
   #elif defined __clang__
     #pragma clang diagnostic pop
+  #elif defined __GNUC__ && __GNUC__>=6
+    #pragma GCC diagnostic pop
   #endif
+
 #endif
 
 #endif // EIGEN_WARNINGS_DISABLED

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h

@@ -327,13 +327,33 @@ GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixTyp
       }
       else
       {
-        Scalar p = Scalar(0.5) * (m_matS.coeff(i, i) - m_matS.coeff(i+1, i+1));
+        // We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a triangular 2x2 block T
-        Scalar z = sqrt(abs(p * p + m_matS.coeff(i+1, i) * m_matS.coeff(i, i+1)));
+        // From the eigen decomposition of T = U * E * U^-1,
-        m_alphas.coeffRef(i)   = ComplexScalar(m_matS.coeff(i+1, i+1) + p, z);
+        // we can extract the eigenvalues of (U^-1 * S * U) / E
-        m_alphas.coeffRef(i+1) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, -z);
+        // Here, we can take advantage that E = diag(T), and U = [ 1 T_01 ; 0 T_11-T_00], and U^-1 = [1 -T_11/(T_11-T_00) ; 0 1/(T_11-T_00)].
+        // Then taking beta=T_00*T_11*(T_11-T_00), we can avoid any division, and alpha is the eigenvalues of A = (U^-1 * S * U) * diag(T_11,T_00) * (T_11-T_00):
+
+        // T =  [a b ; 0 c]
+        // S =  [e f ; g h]
+        RealScalar a = m_realQZ.matrixT().coeff(i, i), b = m_realQZ.matrixT().coeff(i, i+1), c = m_realQZ.matrixT().coeff(i+1, i+1);
+        RealScalar e = m_matS.coeff(i, i), f = m_matS.coeff(i, i+1), g = m_matS.coeff(i+1, i), h = m_matS.coeff(i+1, i+1);
+        RealScalar d = c-a;
+        RealScalar gb = g*b;
+        Matrix<RealScalar,2,2> A;
+        A <<  (e*d-gb)*c,  ((e*b+f*d-h*b)*d-gb*b)*a,
+              g*c       ,  (gb+h*d)*a;
+
+        // NOTE, we could also compute the SVD of T's block during the QZ factorization so that the respective T block is guaranteed to be diagonal,
+        // and then we could directly apply the formula below (while taking care of scaling S columns by T11,T00):
+
+        Scalar p = Scalar(0.5) * (A.coeff(i, i) - A.coeff(i+1, i+1));
+        Scalar z = sqrt(abs(p * p + A.coeff(i+1, i) * A.coeff(i, i+1)));
+        m_alphas.coeffRef(i)   = ComplexScalar(A.coeff(i+1, i+1) + p, z);
+        m_alphas.coeffRef(i+1) = ComplexScalar(A.coeff(i+1, i+1) + p, -z);
+
+        m_betas.coeffRef(i)   =
+        m_betas.coeffRef(i+1) = a*c*d;
 
-        m_betas.coeffRef(i)   = m_realQZ.matrixT().coeff(i,i);
-        m_betas.coeffRef(i+1) = m_realQZ.matrixT().coeff(i,i);
         i += 2;
       }
     }

gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h

@@ -367,10 +367,10 @@ void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs)
     hCoeffs.tail(n-i-1).noalias() = (matA.bottomRightCorner(remainingSize,remainingSize).template selfadjointView<Lower>()
                                   * (conj(h) * matA.col(i).tail(remainingSize)));
 
-    hCoeffs.tail(n-i-1) += (conj(h)*Scalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1);
+    hCoeffs.tail(n-i-1) += (conj(h)*RealScalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1);
 
     matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView<Lower>()
-      .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), -1);
+      .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1));
 
     matA.col(i).coeffRef(i+1) = beta;
     hCoeffs.coeffRef(i) = h;

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h

@@ -75,7 +75,7 @@ template<typename MatrixType,int _Direction> class Homogeneous
     inline Index rows() const { return m_matrix.rows() + (int(Direction)==Vertical   ? 1 : 0); }
     inline Index cols() const { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); }
 
-    inline Scalar coeff(Index row, Index col) const
+    inline Scalar coeff(Index row, Index col=0) const
     {
       if(  (int(Direction)==Vertical   && row==m_matrix.rows())
         || (int(Direction)==Horizontal && col==m_matrix.cols()))

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h

@@ -276,7 +276,7 @@ public:
   inline Coefficients& coeffs() { return m_coeffs;}
   inline const Coefficients& coeffs() const { return m_coeffs;}
 
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned))
 
 protected:
   Coefficients m_coeffs;

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h

@@ -443,7 +443,7 @@ public:
     operator * (const DiagonalBase<DiagonalDerived> &b) const
   {
     TransformTimeDiagonalReturnType res(*this);
-    res.linear() *= b;
+    res.linearExt() *= b;
     return res;
   }
 
@@ -557,7 +557,7 @@ public:
     return res;
   }
 
-  inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linear() *= s; return *this; }
+  inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linearExt() *= s; return *this; }
 
   template<typename Derived>
   inline Transform& operator=(const RotationBase<Derived,Dim>& r);
@@ -828,7 +828,7 @@ Transform<Scalar,Dim,Mode,Options>::prescale(const MatrixBase<OtherDerived> &oth
 {
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
   EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
-  m_matrix.template block<Dim,HDim>(0,0).noalias() = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0));
+  affine().noalias() = (other.asDiagonal() * affine());
   return *this;
 }
 
@@ -1048,7 +1048,7 @@ void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixTy
   }
 }
 
-/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
+/** decomposes the linear part of the transformation as a product scaling x rotation, the scaling being
   * not necessarily positive.
   *
   * If either pointer is zero, the corresponding computation is skipped.

gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h

@@ -130,8 +130,10 @@ public:
   }
 
   /** Applies translation to vector */
-  inline VectorType operator* (const VectorType& other) const
+  template<typename Derived>
-  { return m_coeffs + other; }
+  inline typename internal::enable_if<Derived::IsVectorAtCompileTime,VectorType>::type
+  operator* (const MatrixBase<Derived>& vec) const
+  { return m_coeffs + vec.derived(); }
 
   /** \returns the inverse translation (opposite) */
   Translation inverse() const { return Translation(-m_coeffs); }