Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues

Conflicts:
	gtsam.h
	python/handwritten/nonlinear/Values.cpp

CMakeLists.txt

@@ -64,8 +64,10 @@ option(GTSAM_WITH_TBB                    "Use Intel Threaded Building Blocks (TB
 option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available" ON)
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
 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" 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_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
@@ -85,7 +87,11 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
 endif()
 
 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.")
+    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)
+    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.")
 endif()
 
 # Flags for choosing default packaging tools
@@ -185,10 +191,10 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
     set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
     include_directories(${MKL_INCLUDE_DIR})
     list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
-    
+
     # --no-as-needed is required with gcc according to the MKL link advisor
     if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
-        set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")  
+        set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
     endif()
 else()
     set(GTSAM_USE_EIGEN_MKL 0)
@@ -219,16 +225,17 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us
 if(GTSAM_USE_SYSTEM_EIGEN)
 	find_package(Eigen3 REQUIRED)
 	include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
-	
+
 	# Use generic Eigen include paths e.g. <Eigen/Core>
     set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
-	
+
 	# check if MKL is also enabled - can have one or the other, but not both!
-	if(EIGEN_USE_MKL_ALL)
+	# Note: Eigen >= v3.2.5 includes our patches
-	  message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL")
+	if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
+	  message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
 	endif()
 else()
-	# Use bundled Eigen include path. 
+	# Use bundled Eigen include path.
 	# Clear any variables set by FindEigen3
 	if(EIGEN3_INCLUDE_DIR)
 		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
@@ -236,11 +243,11 @@ else()
 	# Add the bundled version of eigen to the include path so that it can still be included
 	# with  #include <Eigen/Core>
 	include_directories(BEFORE "gtsam/3rdparty/Eigen/")
-	
+
 	# set full path to be used by external projects
 	# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
 	set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
-	
+
 endif()
 
 ###############################################################################
@@ -283,18 +290,24 @@ endif()
 
 # Include boost - use 'BEFORE' so that a specific boost specified to CMake
 # takes precedence over a system-installed one.
-# Use 'SYSTEM' to ignore compiler warnings in Boost headers
 include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
 
+if(GTSAM_SUPPORT_NESTED_DISSECTION)
+  set(METIS_INCLUDE_DIRECTORIES
+    gtsam/3rdparty/metis/include
+    gtsam/3rdparty/metis/libmetis
+    gtsam/3rdparty/metis/GKlib)
+else()
+  set(METIS_INCLUDE_DIRECTORIES)
+endif()
+
 # Add includes for source directories 'BEFORE' boost and any system include
 # 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/CCOLAMD/Include
-  gtsam/3rdparty/metis/include
+  ${METIS_INCLUDE_DIRECTORIES}
-  gtsam/3rdparty/metis/libmetis
-  gtsam/3rdparty/metis/GKlib
   ${PROJECT_SOURCE_DIR}
   ${PROJECT_BINARY_DIR} # So we can include generated config header files
   CppUnitLite)
@@ -322,10 +335,6 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
   add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()
 
-if(GTSAM_ALLOW_DEPRECATED_SINCE_V4)
-	add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4)
-endif()
-
 ###############################################################################
 # Add components
 
@@ -360,9 +369,9 @@ if (GTSAM_BUILD_PYTHON)
 
 	# NOTE: The automatic generation of python wrapper from the gtsampy.h interface is
 	#       not working yet, so we're using a handwritten wrapper files on python/handwritten.
-	#       Once the python wrapping from the interface file is working, you can _swap_ the 
+	#       Once the python wrapping from the interface file is working, you can _swap_ the
 	#       comments on the next lines
-	
+
 	# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
 	add_subdirectory(python)
 
@@ -481,6 +490,8 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency chec
 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_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_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection  ")
 
 message(STATUS "MATLAB toolbox flags                                      ")
 print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install matlab toolbox         ")

GTSAM-Concepts.md

@@ -18,14 +18,14 @@ To optimize over continuous types, we assume they are manifolds. This is central
 
 [Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space.
 
-In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct  `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
+In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
-In detail, we ask the following are defined in the traits object:
+In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
 
 * values:
-   * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1.
+   * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
-* types: 
+* types:
     * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
-    * `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.  
+    * `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
     * `ManifoldType`, a pointer back to the type.
     * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: 
         * `gtsam::traits::manifold_tag` -- Everything in this list is expected
@@ -33,7 +33,7 @@ In detail, we ask the following are defined in the traits object:
         * `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
         * `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
 * valid expressions:
-    * `size_t dim = traits<T>::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
+    * `size_t dim = traits<T>::GetDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
     * `v = traits<T>::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space.
     * `p = traits<T>::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space.
 * invariants

README.md

@@ -38,6 +38,13 @@ Optional prerequisites - used automatically if findable by CMake:
 - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
 - [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
 
+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.
+
+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
 ----------------------
 

cmake/GtsamBuildTypes.cmake

@@ -3,56 +3,84 @@
 list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
 
 # Default to Release mode
-if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
+if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
-  set(CMAKE_BUILD_TYPE "Release" CACHE STRING
+    set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
-      "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo."
+      "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
-      FORCE)
+    set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
 endif()
 
 # Add option for using build type postfixes to allow installing multiple build modes
 option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build type to the name of compiled libraries" ON)
 
-# Add debugging flags but only on the first pass
+# Set custom compilation flags.
-if(NOT FIRST_PASS_DONE)
+# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
-  if(MSVC)
+# so that we don't "pollute" the global variable namespace in the cmake cache.
-    set(CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+  # Set all CMAKE_BUILD_TYPE flags:
-    set(CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+  # (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools)
-    set(CMAKE_C_FLAGS_RELWITHDEBINFO   "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
+if(MSVC)
-    set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS                  "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
-    set(CMAKE_C_FLAGS_RELEASE          "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS                "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
-    set(CMAKE_CXX_FLAGS_RELEASE        "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
-    set(CMAKE_C_FLAGS_TIMING           "${CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
-    set(CMAKE_CXX_FLAGS_TIMING         "${CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO   "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(CMAKE_EXE_LINKER_FLAGS_TIMING  "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(CMAKE_SHARED_LINKER_FLAGS_TIMING  "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_RELEASE          "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(CMAKE_MODULE_LINKER_FLAGS_TIMING  "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_RELEASE        "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING CMAKE_MODULE_LINKER_FLAGS_TIMING)
+    set(GTSAM_CMAKE_C_FLAGS_PROFILING        "${GTSAM_CMAKE_C_FLAGS_RELEASE}   /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(CMAKE_C_FLAGS_PROFILING        "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_PROFILING      "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(CMAKE_CXX_FLAGS_PROFILING      "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_TIMING           "${GTSAM_CMAKE_C_FLAGS_RELEASE}   /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-    set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_TIMING         "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-    set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
+else()
-    set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS                  "-std=c11   -Wall" CACHE STRING "Flags used by the compiler for all builds.")
-    mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
+    set(GTSAM_CMAKE_CXX_FLAGS                "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
-  else()
+    set(GTSAM_CMAKE_C_FLAGS_DEBUG            "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
-    set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} -std=c11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_DEBUG          "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
-    set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO   "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(CMAKE_C_FLAGS_RELWITHDEBINFO   "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_RELEASE          "   -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(CMAKE_C_FLAGS_RELEASE          "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_RELEASE        "   -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(CMAKE_CXX_FLAGS_RELEASE        "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_PROFILING        "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(CMAKE_C_FLAGS_TIMING           "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_PROFILING      "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(CMAKE_CXX_FLAGS_TIMING         "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
+    set(GTSAM_CMAKE_C_FLAGS_TIMING           "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-    set(CMAKE_EXE_LINKER_FLAGS_TIMING  "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
+    set(GTSAM_CMAKE_CXX_FLAGS_TIMING         "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-    set(CMAKE_SHARED_LINKER_FLAGS_TIMING  "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
-    mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING)
-    set(CMAKE_C_FLAGS_PROFILING        "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
-    set(CMAKE_CXX_FLAGS_PROFILING      "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
-    set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
-	set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
-    mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING)
-  endif()
 endif()
 
+set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING  "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
+set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING  "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
+set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING     "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
+
+set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
+set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
+set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING    "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
+
+mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING 
+                 GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING
+                 GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING 
+                 GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
+
+# 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_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS})
+set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG})
+set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG})
+set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO})
+set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO})
+set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE})
+set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE})
+set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING})
+set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING})
+set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING})
+set(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})
+set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING})
+
+set(CMAKE_SHARED_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING})
+set(CMAKE_MODULE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING})
+set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING})
+
 # Clang uses a template depth that is less than standard and is too small
 if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
     # Apple Clang before 5.0 does not support -ftemplate-depth.
@@ -63,10 +91,10 @@ endif()
 
 # Set up build type library postfixes
 if(GTSAM_BUILD_TYPE_POSTFIXES)
-	foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
+  foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
-		string(TOUPPER "${build_type}" build_type_toupper)
+    string(TOUPPER "${build_type}" build_type_toupper)
-		set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
+    set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
-	endforeach()
+  endforeach()
 endif()
 
 # Make common binary output directory when on Windows
@@ -78,17 +106,16 @@ endif()
 
 # Set up build type list for cmake-gui
 if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
-	if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
+  if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
-		set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
+    set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
-	endif()
+  endif()
 endif()
 
 # Set up build types for MSVC and XCode
-if(NOT FIRST_PASS_DONE)
+set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
-  set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
+      CACHE STRING "Build types available to MSVC and XCode")
-      CACHE STRING "Build types available to MSVC and XCode" FORCE)
+mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
-  mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES)
+set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
-endif()
 
 # Check build types
 string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
@@ -100,13 +127,9 @@ if(    NOT cmake_build_type_tolower STREQUAL ""
    AND NOT cmake_build_type_tolower STREQUAL "profiling"
    AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
    AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
-  message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
+  message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo, MinSizeRel (case-insensitive).")
 endif()
 
-# Mark that first pass is done
-set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times")
-mark_as_advanced(FIRST_PASS_DONE) 
-
 # Enable Visual Studio solution folders
 set_property(GLOBAL PROPERTY USE_FOLDERS On)
 

cmake/GtsamTesting.cmake

@@ -164,9 +164,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 				add_test(NAME ${script_name} COMMAND ${script_name})
 				add_dependencies(check.${groupName} ${script_name})
 				add_dependencies(check ${script_name})
-        add_dependencies(all.tests ${script_name})
+                add_dependencies(all.tests ${script_name})
 				if(NOT MSVC AND NOT XCODE_VERSION)
-				  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
+				  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
 				endif()
 			
 				# Add TOPSRCDIR
@@ -254,7 +254,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
 		# Add target dependencies
 		add_dependencies(${groupName} ${script_name})
 		if(NOT MSVC AND NOT XCODE_VERSION)
-		  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
+		  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
 		endif()
 
 		# Add TOPSRCDIR

doc/CMakeLists.txt

@@ -23,12 +23,17 @@ if (GTSAM_BUILD_DOCS)
     # GTSAM core subfolders
     set(gtsam_doc_subdirs 
         gtsam/base 
+        gtsam/discrete 
         gtsam/geometry 
         gtsam/inference 
-        gtsam/discrete 
         gtsam/linear 
+        gtsam/navigation 
         gtsam/nonlinear 
+        gtsam/sam 
+        gtsam/sfm 
         gtsam/slam 
+        gtsam/smart 
+        gtsam/symbolic 
         gtsam
     )
     

examples/RangeISAMExample_plaza2.cpp

@@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
 
   while (is) {
     double t, sender, range;
-	size_t receiver;
+    size_t receiver;
     is >> t >> sender >> receiver >> range;
     triples.push_back(RangeTriple(t, receiver, range));
   }

examples/SFMExample.cpp

@@ -99,7 +99,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
   initialEstimate.print("Initial Estimates:\n");
 
   /* Optimize the graph and print results */

examples/SFMExampleExpressions.cpp

@@ -88,7 +88,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initial.insert(Symbol('x', i), poses[i].compose(delta));
   for (size_t j = 0; j < points.size(); ++j)
-    initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
   cout << "initial error = " << graph.error(initial) << endl;
 
   /* Optimize the graph and print results */

examples/SelfCalibrationExample.cpp

@@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
 
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();

examples/VisualISAM2Example.cpp

@@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
       // Add initial guesses to all observed landmarks
       // Intentionally initialize the variables off from the ground truth
       for (size_t j = 0; j < points.size(); ++j)
-        initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+        initialEstimate.insert<Point3>(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15));
 
     } else {
       // Update iSAM with the new factors

examples/VisualISAMExample.cpp

@@ -120,7 +120,7 @@ int main(int argc, char* argv[]) {
       Point3 noise(-0.25, 0.20, 0.15);
       for (size_t j = 0; j < points.size(); ++j) {
         // Intentionally initialize the variables off from the ground truth
-        Point3 initial_lj = points[j].compose(noise);
+        Point3 initial_lj = points[j] + noise;
         initialEstimate.insert(Symbol('l', j), initial_lj);
       }
 

gtsam.h

@@ -127,13 +127,13 @@ namespace std {
         void pop_back();*/
     };
     //typedef std::vector
-  
+
     #include<list>
     template<T>
     class list
     {
-    
+
-    
+
     };
 
 }
@@ -360,17 +360,6 @@ class Point3 {
 
   // Group
   static gtsam::Point3 identity();
-  gtsam::Point3 inverse() const;
-  gtsam::Point3 compose(const gtsam::Point3& p2) const;
-  gtsam::Point3 between(const gtsam::Point3& p2) const;
-
-  // Manifold
-  gtsam::Point3 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Point3& p) const;
-
-  // Lie Group
-  static gtsam::Point3 Expmap(Vector v);
-  static Vector Logmap(const gtsam::Point3& p);
 
   // Standard Interface
   Vector vector() const;
@@ -1069,7 +1058,7 @@ class SymbolicBayesTree {
     void clear();
     void deleteCachedShortcuts();
     size_t numCachedSeparatorMarginals() const;
-  
+
   gtsam::SymbolicConditional* marginalFactor(size_t key) const;
   gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
   gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
@@ -1079,19 +1068,19 @@ class SymbolicBayesTree {
 //   BayesTreeClique();
 //   BayesTreeClique(CONDITIONAL* conditional);
 // //  BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
-// 
+//
 //   bool equals(const This& other, double tol) const;
 //   void print(string s) const;
 //   void printTree() const; // Default indent of ""
 //   void printTree(string indent) const;
 //   size_t numCachedSeparatorMarginals() const;
-// 
+//
 //   CONDITIONAL* conditional() const;
 //   bool isRoot() const;
 //   size_t treeSize() const;
 // //  const std::list<derived_ptr>& children() const { return children_; }
 // //  derived_ptr parent() const { return parent_.lock(); }
-// 
+//
 //   // FIXME: need wrapped versions graphs, BayesNet
 // //  BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
 // //  FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
@@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
 
   void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
   gtsam::JacobianFactor whiten() const;
-  
+
   pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
 
   void setModel(bool anyConstrained, const Vector& sigmas);
@@ -1422,7 +1411,7 @@ class GaussianFactorGraph {
   gtsam::GaussianFactorGraph clone() const;
   gtsam::GaussianFactorGraph negate() const;
 
-  // Optimizing and linear algebra  
+  // Optimizing and linear algebra
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
   gtsam::VectorValues optimizeGradientSearch() const;
@@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet {
     //Constructors
   GaussianBayesNet();
   GaussianBayesNet(const gtsam::GaussianConditional* conditional);
-  
+
   // Testable
   void print(string s) const;
   bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
@@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet {
   gtsam::GaussianConditional* back() const;
   void push_back(gtsam::GaussianConditional* conditional);
   void push_back(const gtsam::GaussianBayesNet& bayesNet);
-  
+
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
   gtsam::VectorValues optimizeGradientSearch() const;
@@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree {
   bool empty() const;
   size_t numCachedSeparatorMarginals() const;
   void saveGraph(string s) const;
-  
+
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimizeGradientSearch() const;
   gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
@@ -1753,7 +1742,8 @@ virtual class NonlinearFactor {
 
 virtual class NoiseModelFactor: gtsam::NonlinearFactor {
   void equals(const gtsam::NoiseModelFactor& other, double tol) const;
-  gtsam::noiseModel::Base* get_noiseModel() const;
+  gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
+  gtsam::noiseModel::Base* noiseModel() const;
   Vector unwhitenedError(const gtsam::Values& x) const;
   Vector whitenedError(const gtsam::Values& x) const;
 };
@@ -1995,7 +1985,7 @@ virtual class NonlinearOptimizerParams {
   void setVerbosity(string s);
 
   string getLinearSolverType() const;
-  
+
   void setLinearSolverType(string solver);
   void setOrdering(const gtsam::Ordering& ordering);
   void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
@@ -2162,6 +2152,8 @@ class ISAM2Result {
   size_t getCliques() const;
 };
 
+class FactorIndices {};
+
 class ISAM2 {
   ISAM2();
   ISAM2(const gtsam::ISAM2Params& params);
@@ -2174,8 +2166,8 @@ class ISAM2 {
 
   gtsam::ISAM2Result update();
   gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
+  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
   // TODO: wrap the full version of update
  //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
   //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
@@ -2488,10 +2480,30 @@ class NavState {
   gtsam::Pose3 pose() const;
 };
 
+#include <gtsam/navigation/PreintegratedRotation.h>
+virtual class PreintegratedRotationParams {
+  PreintegratedRotationParams();
+  void setGyroscopeCovariance(Matrix cov);
+  void setOmegaCoriolis(Vector omega);
+  void setBodyPSensor(const gtsam::Pose3& pose);
+
+  Matrix getGyroscopeCovariance() const;
+
+  // TODO(frank): allow optional
+  //  boost::optional<Vector3> getOmegaCoriolis() const;
+  //  boost::optional<Pose3>   getBodyPSensor()   const;
+};
+
 #include <gtsam/navigation/PreintegrationParams.h>
-class PreintegrationParams {
+virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
   PreintegrationParams(Vector n_gravity);
-  // TODO(frank): add setters/getters or make this MATLAB wrapper feature
+  void setAccelerometerCovariance(Matrix cov);
+  void setIntegrationCovariance(Matrix cov);
+  void setUse2ndOrderCoriolis(bool flag);
+
+  Matrix getAccelerometerCovariance() const;
+  Matrix getIntegrationCovariance()   const;
+  bool   getUse2ndOrderCoriolis()     const;
 };
 
 #include <gtsam/navigation/PreintegrationBase.h>

gtsam/3rdparty/CMakeLists.txt

@@ -43,7 +43,9 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 endif()
 
 option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
-add_subdirectory(metis)
+if(GTSAM_SUPPORT_NESTED_DISSECTION)
+	add_subdirectory(metis)
+endif()
 
 add_subdirectory(ceres)
 

gtsam/3rdparty/Eigen/.hg_archival.txt

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

gtsam/3rdparty/Eigen/.hgtags

@@ -30,3 +30,4 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
 10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4
 bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
 c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
+b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7

gtsam/3rdparty/Eigen/CMakeLists.txt

@@ -1,6 +1,5 @@
 project(Eigen)
-
+cmake_minimum_required(VERSION 2.8.5)
-cmake_minimum_required(VERSION 2.8.2)
 
 # guard against in-source builds
 
@@ -55,6 +54,7 @@ endif(EIGEN_HG_CHANGESET)
 
 
 include(CheckCXXCompilerFlag)
+include(GNUInstallDirs)
 
 set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
@@ -288,25 +288,26 @@ option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
 
-# the user modifiable install path for header files
+# Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR
-set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)")
+if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR)
-
+  set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR}
-# set the internal install path for header files which depends on wether the user modifiable
+      CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed")
-# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not.
-if(EIGEN_INCLUDE_INSTALL_DIR)
-  set(INCLUDE_INSTALL_DIR
-    ${EIGEN_INCLUDE_INSTALL_DIR}
-    CACHE INTERNAL
-    "The directory where we install the header files (internal)"
-  )
 else()
   set(INCLUDE_INSTALL_DIR
-    "include/eigen3"
+      "${CMAKE_INSTALL_INCLUDEDIR}/eigen3"
-    CACHE INTERNAL
+      CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed"
-    "The directory where we install the header files (internal)"
+      )
-  )
 endif()
 
+set(CMAKEPACKAGE_INSTALL_DIR
+    "${CMAKE_INSTALL_LIBDIR}/cmake/eigen3"
+    CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed"
+    )
+set(PKGCONFIG_INSTALL_DIR
+    "${CMAKE_INSTALL_DATADIR}/pkgconfig"
+    CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed"
+    )
+
 # similar to set_target_properties but append the property instead of overwriting it
 macro(ei_add_target_property target prop value)
 
@@ -324,21 +325,9 @@ install(FILES
   )
 
 if(EIGEN_BUILD_PKGCONFIG)
-    SET(path_separator ":")
+    configure_file(eigen3.pc.in eigen3.pc @ONLY)
-    STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}")
-    message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib")
-    FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search})
-    if(pkg_config_libdir)
-        SET(pkg_config_install_dir ${pkg_config_libdir})
-        message(STATUS "found ${pkg_config_libdir}/pkgconfig" )
-    else(pkg_config_libdir)
-        SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share)
-        message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" )
-    endif(pkg_config_libdir)
-
-    configure_file(eigen3.pc.in eigen3.pc)
     install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
-        DESTINATION ${pkg_config_install_dir}/pkgconfig
+        DESTINATION ${PKGCONFIG_INSTALL_DIR}
         )
 endif(EIGEN_BUILD_PKGCONFIG)
 
@@ -401,12 +390,15 @@ if(cmake_generator_tolower MATCHES "makefile")
   message(STATUS "--------------+--------------------------------------------------------------")
   message(STATUS "Command       |   Description")
   message(STATUS "--------------+--------------------------------------------------------------")
-  message(STATUS "make install  | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
+  message(STATUS "make install  | Install Eigen. Headers will be installed to:")
-  message(STATUS "              |     cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
+  message(STATUS "              |     <CMAKE_INSTALL_PREFIX>/<INCLUDE_INSTALL_DIR>")
-  message(STATUS "              |   Eigen headers will then be installed to:")
+  message(STATUS "              |   Using the following values:")
-  message(STATUS "              |     ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}")
+  message(STATUS "              |     CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
-  message(STATUS "              |   To install Eigen headers to a separate location, do:")
+  message(STATUS "              |     INCLUDE_INSTALL_DIR:  ${INCLUDE_INSTALL_DIR}")
-  message(STATUS "              |     cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
+  message(STATUS "              |   Change the install location of Eigen headers using:")
+  message(STATUS "              |     cmake . -DCMAKE_INSTALL_PREFIX=yourprefix")
+  message(STATUS "              |   Or:")
+  message(STATUS "              |     cmake . -DINCLUDE_INSTALL_DIR=yourdir")
   message(STATUS "make doc      | Generate the API documentation, requires Doxygen & LaTeX")
   message(STATUS "make check    | Build and run the unit-tests. Read this page:")
   message(STATUS "              |   http://eigen.tuxfamily.org/index.php?title=Tests")

gtsam/3rdparty/Eigen/Eigen/CholmodSupport

@@ -12,7 +12,7 @@ extern "C" {
 /** \ingroup Support_modules
   * \defgroup CholmodSupport_Module CholmodSupport module
   *
-  * This module provides an interface to the Cholmod library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
+  * This module provides an interface to the Cholmod library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
   * It provides the two following main factorization classes:
   * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
   * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).

gtsam/3rdparty/Eigen/Eigen/SPQRSupport

@@ -10,7 +10,7 @@
 /** \ingroup Support_modules
   * \defgroup SPQRSupport_Module SuiteSparseQR module
   * 
-  * This module provides an interface to the SPQR library, which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
+  * This module provides an interface to the SPQR library, which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
   *
   * \code
   * #include <Eigen/SPQRSupport>

gtsam/3rdparty/Eigen/Eigen/UmfPackSupport

@@ -12,7 +12,7 @@ extern "C" {
 /** \ingroup Support_modules
   * \defgroup UmfPackSupport_Module UmfPackSupport module
   *
-  * This module provides an interface to the UmfPack library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
+  * This module provides an interface to the UmfPack library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
   * It provides the following factorization class:
   * - class UmfPackLU: a multifrontal sequential LU factorization.
   *

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

@@ -38,7 +38,7 @@ struct traits<CwiseUnaryView<ViewOp, MatrixType> >
   typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
   enum {
     Flags = (traits<_MatrixTypeNested>::Flags & (HereditaryBits | LvalueBit | LinearAccessBit | DirectAccessBit)),
-    CoeffReadCost = traits<_MatrixTypeNested>::CoeffReadCost + functor_traits<ViewOp>::Cost,
+    CoeffReadCost = EIGEN_ADD_COST(traits<_MatrixTypeNested>::CoeffReadCost, functor_traits<ViewOp>::Cost),
     MatrixTypeInnerStride =  inner_stride_at_compile_time<MatrixType>::ret,
     // need to cast the sizeof's from size_t to int explicitly, otherwise:
     // "error: no integral type can represent all of the enumerator values

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

@@ -40,7 +40,7 @@ static inline void check_DenseIndex_is_signed() {
   */
 template<typename Derived> class DenseBase
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-  : public internal::special_scalar_op_base<Derived,typename internal::traits<Derived>::Scalar,
+  : public internal::special_scalar_op_base<Derived, typename internal::traits<Derived>::Scalar,
                                             typename NumTraits<typename internal::traits<Derived>::Scalar>::Real,
                                             DenseCoeffsBase<Derived> >
 #else

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

@@ -425,15 +425,18 @@ template<> struct gemv_selector<OnTheRight,ColMajor,true>
     ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs())
                                   * RhsBlasTraits::extractScalarFactor(prod.rhs());
 
+    // make sure Dest is a compile-time vector type (bug 1166)
+    typedef typename conditional<Dest::IsVectorAtCompileTime, Dest, typename Dest::ColXpr>::type ActualDest;
+
     enum {
       // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
       // on, the other hand it is good for the cache to pack the vector anyways...
-      EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1,
+      EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1),
       ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
-      MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal
+      MightCannotUseDest = (ActualDest::InnerStrideAtCompileTime!=1) || ComplexByReal
     };
 
-    gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
+    gemv_static_vector_if<ResScalar,ActualDest::SizeAtCompileTime,ActualDest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
 
     bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));
     bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible;
@@ -522,7 +525,7 @@ template<> struct gemv_selector<OnTheRight,RowMajor,true>
         actualLhs.rows(), actualLhs.cols(),
         actualLhs.data(), actualLhs.outerStride(),
         actualRhsPtr, 1,
-        dest.data(), dest.innerStride(),
+        dest.data(), dest.col(0).innerStride(), //NOTE  if dest is not a vector at compile-time, then dest.innerStride() might be wrong. (bug 1166)
         actualAlpha);
   }
 };

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

@@ -149,6 +149,10 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
       checkSanity();
     }
 
+    #ifdef EIGEN_MAPBASE_PLUGIN
+    #include EIGEN_MAPBASE_PLUGIN
+    #endif
+
   protected:
 
     void checkSanity() const

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

@@ -707,21 +707,21 @@ struct scalar_fuzzy_impl : scalar_fuzzy_default_impl<Scalar, NumTraits<Scalar>::
 
 template<typename Scalar, typename OtherScalar>
 inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y,
-                                   typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
+                              const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
 {
   return scalar_fuzzy_impl<Scalar>::template isMuchSmallerThan<OtherScalar>(x, y, precision);
 }
 
 template<typename Scalar>
 inline bool isApprox(const Scalar& x, const Scalar& y,
-                          typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
+                     const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
 {
   return scalar_fuzzy_impl<Scalar>::isApprox(x, y, precision);
 }
 
 template<typename Scalar>
 inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y,
-                                    typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
+                               const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
 {
   return scalar_fuzzy_impl<Scalar>::isApproxOrLessThan(x, y, precision);
 }

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

@@ -116,17 +116,17 @@ template<typename Lhs, typename Rhs, int Mode, int Index, int Size>
 struct triangular_solver_unroller<Lhs,Rhs,Mode,Index,Size,false> {
   enum {
     IsLower = ((Mode&Lower)==Lower),
-    I = IsLower ? Index : Size - Index - 1,
+    RowIndex = IsLower ? Index : Size - Index - 1,
-    S = IsLower ? 0     : I+1
+    S = IsLower ? 0     : RowIndex+1
   };
   static void run(const Lhs& lhs, Rhs& rhs)
   {
     if (Index>0)
-      rhs.coeffRef(I) -= lhs.row(I).template segment<Index>(S).transpose()
+      rhs.coeffRef(RowIndex) -= lhs.row(RowIndex).template segment<Index>(S).transpose()
                          .cwiseProduct(rhs.template segment<Index>(S)).sum();
 
     if(!(Mode & UnitDiag))
-      rhs.coeffRef(I) /= lhs.coeff(I,I);
+      rhs.coeffRef(RowIndex) /= lhs.coeff(RowIndex,RowIndex);
 
     triangular_solver_unroller<Lhs,Rhs,Mode,Index+1,Size>::run(lhs,rhs);
   }

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

@@ -76,14 +76,17 @@ template<typename Derived>
 template<typename Visitor>
 void DenseBase<Derived>::visit(Visitor& visitor) const
 {
+  typedef typename internal::remove_all<typename Derived::Nested>::type ThisNested;
+  typename Derived::Nested thisNested(derived());
+
   enum { unroll = SizeAtCompileTime != Dynamic
                    && CoeffReadCost != Dynamic
                    && (SizeAtCompileTime == 1 || internal::functor_traits<Visitor>::Cost != Dynamic)
                    && SizeAtCompileTime * CoeffReadCost + (SizeAtCompileTime-1) * internal::functor_traits<Visitor>::Cost
                       <= EIGEN_UNROLLING_LIMIT };
-  return internal::visitor_impl<Visitor, Derived,
+  return internal::visitor_impl<Visitor, ThisNested,
       unroll ? int(SizeAtCompileTime) : Dynamic
-    >::run(derived(), visitor);
+    >::run(thisNested, visitor);
 }
 
 namespace internal {

gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h

@@ -235,63 +235,27 @@ template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int*     from) { E
     return _mm_loadu_ps(from);
     #endif
   }
-  template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_pd(from); }
-  template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int*    from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from)); }
 #else
-// Fast unaligned loads. Note that here we cannot directly use intrinsics: this would
-// require pointer casting to incompatible pointer types and leads to invalid code
-// because of the strict aliasing rule. The "dummy" stuff are required to enforce
-// a correct instruction dependency.
-// TODO: do the same for MSVC (ICC is compatible)
 // NOTE: with the code below, MSVC's compiler crashes!
 
-#if defined(__GNUC__) && defined(__i386__)
-  // bug 195: gcc/i386 emits weird x87 fldl/fstpl instructions for _mm_load_sd
-  #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
-#elif defined(__clang__)
-  // bug 201: Segfaults in __mm_loadh_pd with clang 2.8
-  #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
-#else
-  #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 0
-#endif
-
 template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
 {
   EIGEN_DEBUG_UNALIGNED_LOAD
-#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
   return _mm_loadu_ps(from);
-#else
-  __m128d res;
-  res =  _mm_load_sd((const double*)(from)) ;
-  res =  _mm_loadh_pd(res, (const double*)(from+2)) ;
-  return _mm_castpd_ps(res);
-#endif
 }
+#endif
+
 template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
 {
   EIGEN_DEBUG_UNALIGNED_LOAD
-#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
   return _mm_loadu_pd(from);
-#else
-  __m128d res;
-  res = _mm_load_sd(from) ;
-  res = _mm_loadh_pd(res,from+1);
-  return res;
-#endif
 }
 template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from)
 {
   EIGEN_DEBUG_UNALIGNED_LOAD
-#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
+  return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
-  return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from));
-#else
-  __m128d res;
-  res =  _mm_load_sd((const double*)(from)) ;
-  res =  _mm_loadh_pd(res, (const double*)(from+2)) ;
-  return _mm_castpd_si128(res);
-#endif
 }
-#endif
+
 
 template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float*   from)
 {

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

@@ -140,8 +140,10 @@ static void run(Index rows, Index cols, Index depth,
       // Release all the sub blocks B'_j of B' for the current thread,
       // i.e., we simply decrement the number of users by 1
       for(Index j=0; j<threads; ++j)
+      {
         #pragma omp atomic
-        --(info[j].users);
+        info[j].users -= 1;
+      }
     }
   }
   else
@@ -390,13 +392,17 @@ class GeneralProduct<Lhs, Rhs, GemmProduct>
 
     GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs)
     {
+#if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG))
       typedef internal::scalar_product_op<LhsScalar,RhsScalar> BinOp;
       EIGEN_CHECK_BINARY_COMPATIBILIY(BinOp,LhsScalar,RhsScalar);
+#endif
     }
 
     template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const
     {
       eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols());
+      if(m_lhs.cols()==0 || m_lhs.rows()==0 || m_rhs.cols()==0)
+        return;
 
       typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(m_lhs);
       typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(m_rhs);

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

@@ -115,8 +115,9 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
           {
             // TODO write a small kernel handling this (can be shared with trsv)
             Index i  = IsLower ? k2+k1+k : k2-k1-k-1;
-            Index s  = IsLower ? k2+k1 : i+1;
             Index rs = actualPanelWidth - k - 1; // remaining size
+            Index s  = TriStorageOrder==RowMajor ? (IsLower ? k2+k1 : i+1)
+                                                 :  IsLower ? i+1 : i-rs;
 
             Scalar a = (Mode & UnitDiag) ? Scalar(1) : Scalar(1)/conj(tri(i,i));
             for (Index j=j2; j<j2+actual_cols; ++j)
@@ -133,7 +134,6 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
               }
               else
               {
-                Index s = IsLower ? i+1 : i-rs;
                 Scalar b = (other(i,j) *= a);
                 Scalar* r = &other(s,j);
                 const Scalar* l = &tri(s,i);

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

@@ -13,23 +13,292 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 2
-#define EIGEN_MINOR_VERSION 7
+#define EIGEN_MINOR_VERSION 8
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
                                                                  EIGEN_MINOR_VERSION>=z))))
+
+
+// Compiler identification, EIGEN_COMP_*
+
+/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
 #ifdef __GNUC__
+  #define EIGEN_COMP_GNUC 1
+#else
+  #define EIGEN_COMP_GNUC 0
+#endif
+
+/// \internal EIGEN_COMP_CLANG set to 1 if the compiler is clang (alias for __clang__)
+#if defined(__clang__)
+  #define EIGEN_COMP_CLANG 1
+#else
+  #define EIGEN_COMP_CLANG 0
+#endif
+
+
+/// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
+#if defined(__llvm__)
+  #define EIGEN_COMP_LLVM 1
+#else
+  #define EIGEN_COMP_LLVM 0
+#endif
+
+/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise
+#if defined(__INTEL_COMPILER)
+  #define EIGEN_COMP_ICC __INTEL_COMPILER
+#else
+  #define EIGEN_COMP_ICC 0
+#endif
+
+/// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw
+#if defined(__MINGW32__)
+  #define EIGEN_COMP_MINGW 1
+#else
+  #define EIGEN_COMP_MINGW 0
+#endif
+
+/// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio
+#if defined(__SUNPRO_CC)
+  #define EIGEN_COMP_SUNCC 1
+#else
+  #define EIGEN_COMP_SUNCC 0
+#endif
+
+/// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise.
+#if defined(_MSC_VER)
+  #define EIGEN_COMP_MSVC _MSC_VER
+#else
+  #define EIGEN_COMP_MSVC 0
+#endif
+
+/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC
+#if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC)
+  #define EIGEN_COMP_MSVC_STRICT _MSC_VER
+#else
+  #define EIGEN_COMP_MSVC_STRICT 0
+#endif
+
+/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++
+#if defined(__IBMCPP__) || defined(__xlc__)
+  #define EIGEN_COMP_IBM 1
+#else
+  #define EIGEN_COMP_IBM 0
+#endif
+
+/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler
+#if defined(__PGI)
+  #define EIGEN_COMP_PGI 1
+#else
+  #define EIGEN_COMP_PGI 0
+#endif
+
+/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
+#if defined(__CC_ARM) || defined(__ARMCC_VERSION)
+  #define EIGEN_COMP_ARM 1
+#else
+  #define EIGEN_COMP_ARM 0
+#endif
+
+
+/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.)
+#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM )
+  #define EIGEN_COMP_GNUC_STRICT 1
+#else
+  #define EIGEN_COMP_GNUC_STRICT 0
+#endif
+
+
+#if EIGEN_COMP_GNUC
   #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x)
+  #define EIGEN_GNUC_AT_MOST(x,y)  ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
+  #define EIGEN_GNUC_AT(x,y)       ( __GNUC__==x && __GNUC_MINOR__==y )
 #else
   #define EIGEN_GNUC_AT_LEAST(x,y) 0
+  #define EIGEN_GNUC_AT_MOST(x,y)  0
+  #define EIGEN_GNUC_AT(x,y)       0
 #endif
- 
+
-#ifdef __GNUC__
+// FIXME: could probably be removed as we do not support gcc 3.x anymore
-  #define EIGEN_GNUC_AT_MOST(x,y) ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
+#if EIGEN_COMP_GNUC && (__GNUC__ <= 3)
+#define EIGEN_GCC3_OR_OLDER 1
 #else
-  #define EIGEN_GNUC_AT_MOST(x,y) 0
+#define EIGEN_GCC3_OR_OLDER 0
 #endif
 
+
+// Architecture identification, EIGEN_ARCH_*
+
+#if defined(__x86_64__) || defined(_M_X64) || defined(__amd64)
+  #define EIGEN_ARCH_x86_64 1
+#else
+  #define EIGEN_ARCH_x86_64 0
+#endif
+
+#if defined(__i386__) || defined(_M_IX86) || defined(_X86_) || defined(__i386)
+  #define EIGEN_ARCH_i386 1
+#else
+  #define EIGEN_ARCH_i386 0
+#endif
+
+#if EIGEN_ARCH_x86_64 || EIGEN_ARCH_i386
+  #define EIGEN_ARCH_i386_OR_x86_64 1
+#else
+  #define EIGEN_ARCH_i386_OR_x86_64 0
+#endif
+
+/// \internal EIGEN_ARCH_ARM set to 1 if the architecture is ARM
+#if defined(__arm__)
+  #define EIGEN_ARCH_ARM 1
+#else
+  #define EIGEN_ARCH_ARM 0
+#endif
+
+/// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
+#if defined(__aarch64__)
+  #define EIGEN_ARCH_ARM64 1
+#else
+  #define EIGEN_ARCH_ARM64 0
+#endif
+
+#if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
+  #define EIGEN_ARCH_ARM_OR_ARM64 1
+#else
+  #define EIGEN_ARCH_ARM_OR_ARM64 0
+#endif
+
+/// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
+#if defined(__mips__) || defined(__mips)
+  #define EIGEN_ARCH_MIPS 1
+#else
+  #define EIGEN_ARCH_MIPS 0
+#endif
+
+/// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC
+#if defined(__sparc__) || defined(__sparc)
+  #define EIGEN_ARCH_SPARC 1
+#else
+  #define EIGEN_ARCH_SPARC 0
+#endif
+
+/// \internal EIGEN_ARCH_IA64 set to 1 if the architecture is Intel Itanium
+#if defined(__ia64__)
+  #define EIGEN_ARCH_IA64 1
+#else
+  #define EIGEN_ARCH_IA64 0
+#endif
+
+/// \internal EIGEN_ARCH_PPC set to 1 if the architecture is PowerPC
+#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC)
+  #define EIGEN_ARCH_PPC 1
+#else
+  #define EIGEN_ARCH_PPC 0
+#endif
+
+
+
+// Operating system identification, EIGEN_OS_*
+
+/// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
+#if defined(__unix__) || defined(__unix)
+  #define EIGEN_OS_UNIX 1
+#else
+  #define EIGEN_OS_UNIX 0
+#endif
+
+/// \internal EIGEN_OS_LINUX set to 1 if the OS is based on Linux kernel
+#if defined(__linux__)
+  #define EIGEN_OS_LINUX 1
+#else
+  #define EIGEN_OS_LINUX 0
+#endif
+
+/// \internal EIGEN_OS_ANDROID set to 1 if the OS is Android
+// note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain.
+#if defined(__ANDROID__) || defined(ANDROID)
+  #define EIGEN_OS_ANDROID 1
+#else
+  #define EIGEN_OS_ANDROID 0
+#endif
+
+/// \internal EIGEN_OS_GNULINUX set to 1 if the OS is GNU Linux and not Linux-based OS (e.g., not android)
+#if defined(__gnu_linux__) && !(EIGEN_OS_ANDROID)
+  #define EIGEN_OS_GNULINUX 1
+#else
+  #define EIGEN_OS_GNULINUX 0
+#endif
+
+/// \internal EIGEN_OS_BSD set to 1 if the OS is a BSD variant
+#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__)
+  #define EIGEN_OS_BSD 1
+#else
+  #define EIGEN_OS_BSD 0
+#endif
+
+/// \internal EIGEN_OS_MAC set to 1 if the OS is MacOS
+#if defined(__APPLE__)
+  #define EIGEN_OS_MAC 1
+#else
+  #define EIGEN_OS_MAC 0
+#endif
+
+/// \internal EIGEN_OS_QNX set to 1 if the OS is QNX
+#if defined(__QNX__)
+  #define EIGEN_OS_QNX 1
+#else
+  #define EIGEN_OS_QNX 0
+#endif
+
+/// \internal EIGEN_OS_WIN set to 1 if the OS is Windows based
+#if defined(_WIN32)
+  #define EIGEN_OS_WIN 1
+#else
+  #define EIGEN_OS_WIN 0
+#endif
+
+/// \internal EIGEN_OS_WIN64 set to 1 if the OS is Windows 64bits
+#if defined(_WIN64)
+  #define EIGEN_OS_WIN64 1
+#else
+  #define EIGEN_OS_WIN64 0
+#endif
+
+/// \internal EIGEN_OS_WINCE set to 1 if the OS is Windows CE
+#if defined(_WIN32_WCE)
+  #define EIGEN_OS_WINCE 1
+#else
+  #define EIGEN_OS_WINCE 0
+#endif
+
+/// \internal EIGEN_OS_CYGWIN set to 1 if the OS is Windows/Cygwin
+#if defined(__CYGWIN__)
+  #define EIGEN_OS_CYGWIN 1
+#else
+  #define EIGEN_OS_CYGWIN 0
+#endif
+
+/// \internal EIGEN_OS_WIN_STRICT set to 1 if the OS is really Windows and not some variants
+#if EIGEN_OS_WIN && !( EIGEN_OS_WINCE || EIGEN_OS_CYGWIN )
+  #define EIGEN_OS_WIN_STRICT 1
+#else
+  #define EIGEN_OS_WIN_STRICT 0
+#endif
+
+/// \internal EIGEN_OS_SUN set to 1 if the OS is SUN
+#if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
+  #define EIGEN_OS_SUN 1
+#else
+  #define EIGEN_OS_SUN 0
+#endif
+
+/// \internal EIGEN_OS_SOLARIS set to 1 if the OS is Solaris
+#if (defined(sun) || defined(__sun)) && (defined(__SVR4) || defined(__svr4__))
+  #define EIGEN_OS_SOLARIS 1
+#else
+  #define EIGEN_OS_SOLARIS 0
+#endif
+
+
 #if EIGEN_GNUC_AT_MOST(4,3) && !defined(__clang__)
   // see bug 89
   #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0
@@ -37,12 +306,6 @@
   #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
 #endif
 
-#if defined(__GNUC__) && (__GNUC__ <= 3)
-#define EIGEN_GCC3_OR_OLDER 1
-#else
-#define EIGEN_GCC3_OR_OLDER 0
-#endif
-
 // 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
 // 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
 // enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
@@ -104,7 +367,7 @@
 
 // Do we support r-value references?
 #if (__has_feature(cxx_rvalue_references) || \
-     defined(__GXX_EXPERIMENTAL_CXX0X__) || \
+     (defined(__cplusplus) && __cplusplus >= 201103L) || \
      (defined(_MSC_VER) && _MSC_VER >= 1600))
   #define EIGEN_HAVE_RVALUE_REFERENCES
 #endif

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

@@ -26,7 +26,7 @@
 
 #ifndef EIGEN_NO_STATIC_ASSERT
 
-  #if defined(__GXX_EXPERIMENTAL_CXX0X__) || (defined(_MSC_VER) && (_MSC_VER >= 1600))
+  #if __has_feature(cxx_static_assert) || (defined(__cplusplus) && __cplusplus >= 201103L) || (EIGEN_COMP_MSVC >= 1600)
 
     // if native static_assert is enabled, let's use it
     #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);

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

@@ -45,7 +45,6 @@ ComplexSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
 ComplexSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW>& matrix, bool computeU) \
 { \
   typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> MatrixType; \
-  typedef MatrixType::Scalar Scalar; \
   typedef MatrixType::RealScalar RealScalar; \
   typedef std::complex<RealScalar> ComplexScalar; \
 \

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

@@ -44,10 +44,6 @@ template<> inline \
 RealSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
 RealSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW>& matrix, bool computeU) \
 { \
-  typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> MatrixType; \
-  typedef MatrixType::Scalar Scalar; \
-  typedef MatrixType::RealScalar RealScalar; \
-\
   eigen_assert(matrix.cols() == matrix.rows()); \
 \
   lapack_int n = matrix.cols(), sdim, info; \

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

@@ -83,10 +83,17 @@ public:
   template<typename Derived>
   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
 
+  /** \returns the value of the rotation angle in radian */
   Scalar angle() const { return m_angle; }
+  /** \returns a read-write reference to the stored angle in radian */
   Scalar& angle() { return m_angle; }
 
+  /** \returns the rotation axis */
   const Vector3& axis() const { return m_axis; }
+  /** \returns a read-write reference to the stored rotation axis.
+    *
+    * \warning The rotation axis must remain a \b unit vector.
+    */
   Vector3& axis() { return m_axis; }
 
   /** Concatenates two rotations */

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

@@ -129,7 +129,7 @@ public:
     * determined by \a prec.
     *
     * \sa MatrixBase::isApprox() */
-  bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
+  bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
   { return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
 
 protected:

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

@@ -102,15 +102,15 @@ template<int Mode> struct transform_make_affine;
   *
   * However, unlike a plain matrix, the Transform class provides many features
   * simplifying both its assembly and usage. In particular, it can be composed
-  * with any other transformations (Transform,Translation,RotationBase,Matrix)
+  * with any other transformations (Transform,Translation,RotationBase,DiagonalMatrix)
   * and can be directly used to transform implicit homogeneous vectors. All these
   * operations are handled via the operator*. For the composition of transformations,
   * its principle consists to first convert the right/left hand sides of the product
   * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product.
   * Of course, internally, operator* tries to perform the minimal number of operations
   * according to the nature of each terms. Likewise, when applying the transform
-  * to non homogeneous vectors, the latters are automatically promoted to homogeneous
+  * to points, the latters are automatically promoted to homogeneous vectors
-  * one before doing the matrix product. The convertions to homogeneous representations
+  * before doing the matrix product. The conventions to homogeneous representations
   * are performed as follow:
   *
   * \b Translation t (Dim)x(1):
@@ -124,7 +124,7 @@ template<int Mode> struct transform_make_affine;
   * R & 0\\
   * 0\,...\,0 & 1
   * \end{array} \right) \f$
-  *
+  *<!--
   * \b Linear \b Matrix L (Dim)x(Dim):
   * \f$ \left( \begin{array}{cc}
   * L & 0\\
@@ -136,14 +136,20 @@ template<int Mode> struct transform_make_affine;
   * A\\
   * 0\,...\,0\,1
   * \end{array} \right) \f$
+  *-->
+  * \b Scaling \b DiagonalMatrix S (Dim)x(Dim):
+  * \f$ \left( \begin{array}{cc}
+  * S & 0\\
+  * 0\,...\,0 & 1
+  * \end{array} \right) \f$
   *
-  * \b Column \b vector v (Dim)x(1):
+  * \b Column \b point v (Dim)x(1):
   * \f$ \left( \begin{array}{c}
   * v\\
   * 1
   * \end{array} \right) \f$
   *
-  * \b Set \b of \b column \b vectors V1...Vn (Dim)x(n):
+  * \b Set \b of \b column \b points V1...Vn (Dim)x(n):
   * \f$ \left( \begin{array}{ccc}
   * v_1 & ... & v_n\\
   * 1 & ... & 1
@@ -384,26 +390,39 @@ public:
   /** \returns a writable expression of the translation vector of the transformation */
   inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); }
 
-  /** \returns an expression of the product between the transform \c *this and a matrix expression \a other
+  /** \returns an expression of the product between the transform \c *this and a matrix expression \a other.
     *
-    * The right hand side \a other might be either:
+    * The right-hand-side \a other can be either:
-    * \li a vector of size Dim,
     * \li an homogeneous vector of size Dim+1,
-    * \li a set of vectors of size Dim x Dynamic,
+    * \li a set of homogeneous vectors of size Dim+1 x N,
-    * \li a set of homogeneous vectors of size Dim+1 x Dynamic,
-    * \li a linear transformation matrix of size Dim x Dim,
-    * \li an affine transformation matrix of size Dim x Dim+1,
     * \li a transformation matrix of size Dim+1 x Dim+1.
+    *
+    * Moreover, if \c *this represents an affine transformation (i.e., Mode!=Projective), then \a other can also be:
+    * \li a point of size Dim (computes: \code this->linear() * other + this->translation()\endcode),
+    * \li a set of N points as a Dim x N matrix (computes: \code (this->linear() * other).colwise() + this->translation()\endcode),
+    *
+    * In all cases, the return type is a matrix or vector of same sizes as the right-hand-side \a other.
+    *
+    * If you want to interpret \a other as a linear or affine transformation, then first convert it to a Transform<> type,
+    * or do your own cooking.
+    *
+    * Finally, if you want to apply Affine transformations to vectors, then explicitly apply the linear part only:
+    * \code
+    * Affine3f A;
+    * Vector3f v1, v2;
+    * v2 = A.linear() * v1;
+    * \endcode
+    *
     */
   // note: this function is defined here because some compilers cannot find the respective declaration
   template<typename OtherDerived>
-  EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl<Transform, OtherDerived>::ResultType
+  EIGEN_STRONG_INLINE const typename OtherDerived::PlainObject
   operator * (const EigenBase<OtherDerived> &other) const
   { return internal::transform_right_product_impl<Transform, OtherDerived>::run(*this,other.derived()); }
 
   /** \returns the product expression of a transformation matrix \a a times a transform \a b
     *
-    * The left hand side \a other might be either:
+    * The left hand side \a other can be either:
     * \li a linear transformation matrix of size Dim x Dim,
     * \li an affine transformation matrix of size Dim x Dim+1,
     * \li a general transformation matrix of size Dim+1 x Dim+1.

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

@@ -162,7 +162,7 @@ public:
     * determined by \a prec.
     *
     * \sa MatrixBase::isApprox() */
-  bool isApprox(const Translation& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
+  bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
   { return m_coeffs.isApprox(other.m_coeffs, prec); }
 
 };

gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h

@@ -139,6 +139,8 @@ struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
   * By default the iterations start with x=0 as an initial guess of the solution.
   * One can control the start using the solveWithGuess() method.
   * 
+  * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
+  *
   * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
   */
 template< typename _MatrixType, int _UpLo, typename _Preconditioner>

gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h

@@ -688,7 +688,7 @@ struct solve_retval<FullPivLU<_MatrixType>, Rhs>
      */
 
     const Index rows = dec().rows(), cols = dec().cols(),
-              nonzero_pivots = dec().nonzeroPivots();
+              nonzero_pivots = dec().rank();
     eigen_assert(rhs().rows() == rows);
     const Index smalldim = (std::min)(rows, cols);
 

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h

@@ -8,7 +8,7 @@
 NOTE: this routine has been adapted from the CSparse library:
 
 Copyright (c) 2006, Timothy A. Davis.
-http://www.cise.ufl.edu/research/sparse/CSparse
+http://www.suitesparse.com
 
 CSparse is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public

gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h

@@ -41,12 +41,8 @@
 // 
 //   The colamd/symamd library is available at
 // 
-//       http://www.cise.ufl.edu/research/sparse/colamd/
+//       http://www.suitesparse.com
 
-//   This is the http://www.cise.ufl.edu/research/sparse/colamd/colamd.h
-//   file.  It is required by the colamd.c, colamdmex.c, and symamdmex.c
-//   files, and by any C code that calls the routines whose prototypes are
-//   listed below, or that uses the colamd/symamd definitions listed below.
   
 #ifndef EIGEN_COLAMD_H
 #define EIGEN_COLAMD_H
@@ -102,9 +98,6 @@ namespace internal {
 /* === Definitions ========================================================== */
 /* ========================================================================== */
 
-#define COLAMD_MAX(a,b) (((a) > (b)) ? (a) : (b))
-#define COLAMD_MIN(a,b) (((a) < (b)) ? (a) : (b))
-
 #define ONES_COMPLEMENT(r) (-(r)-1)
 
 /* -------------------------------------------------------------------------- */
@@ -516,7 +509,7 @@ static Index init_rows_cols  /* returns true if OK, or false otherwise */
     Col [col].start = p [col] ;
     Col [col].length = p [col+1] - p [col] ;
 
-    if (Col [col].length < 0)
+    if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200
     {
       /* column pointers must be non-decreasing */
       stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ;
@@ -739,8 +732,8 @@ static void init_scoring
 
   /* === Extract knobs ==================================================== */
 
-  dense_row_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_ROW] * n_col, n_col)) ;
+  dense_row_count = std::max<Index>(0, (std::min)(Index(knobs [COLAMD_DENSE_ROW] * n_col), n_col)) ;
-  dense_col_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_COL] * n_row, n_row)) ;
+  dense_col_count = std::max<Index>(0, (std::min)(Index(knobs [COLAMD_DENSE_COL] * n_row), n_row)) ;
   COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ;
   max_deg = 0 ;
   n_col2 = n_col ;
@@ -804,7 +797,7 @@ static void init_scoring
     else
     {
       /* keep track of max degree of remaining rows */
-      max_deg = COLAMD_MAX (max_deg, deg) ;
+      max_deg = (std::max)(max_deg, deg) ;
     }
   }
   COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ;
@@ -842,7 +835,7 @@ static void init_scoring
       /* add row's external degree */
       score += Row [row].shared1.degree - 1 ;
       /* guard against integer overflow */
-      score = COLAMD_MIN (score, n_col) ;
+      score = (std::min)(score, n_col) ;
     }
     /* determine pruned column length */
     col_length = (Index) (new_cp - &A [Col [c].start]) ;
@@ -914,7 +907,7 @@ static void init_scoring
       head [score] = c ;
 
       /* see if this score is less than current min */
-      min_score = COLAMD_MIN (min_score, score) ;
+      min_score = (std::min)(min_score, score) ;
 
 
     }
@@ -1040,7 +1033,7 @@ static Index find_ordering /* return the number of garbage collections */
 
     /* === Garbage_collection, if necessary ============================= */
 
-    needed_memory = COLAMD_MIN (pivot_col_score, n_col - k) ;
+    needed_memory = (std::min)(pivot_col_score, n_col - k) ;
     if (pfree + needed_memory >= Alen)
     {
       pfree = Eigen::internal::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ;
@@ -1099,7 +1092,7 @@ static Index find_ordering /* return the number of garbage collections */
 
     /* clear tag on pivot column */
     Col [pivot_col].shared1.thickness = pivot_col_thickness ;
-    max_deg = COLAMD_MAX (max_deg, pivot_row_degree) ;
+    max_deg = (std::max)(max_deg, pivot_row_degree) ;
 
 
     /* === Kill all rows used to construct pivot row ==================== */
@@ -1273,7 +1266,7 @@ static Index find_ordering /* return the number of garbage collections */
 	/* add set difference */
 	cur_score += row_mark - tag_mark ;
 	/* integer overflow... */
-	cur_score = COLAMD_MIN (cur_score, n_col) ;
+	cur_score = (std::min)(cur_score, n_col) ;
       }
 
       /* recompute the column's length */
@@ -1386,7 +1379,7 @@ static Index find_ordering /* return the number of garbage collections */
       cur_score -= Col [col].shared1.thickness ;
 
       /* make sure score is less or equal than the max score */
-      cur_score = COLAMD_MIN (cur_score, max_score) ;
+      cur_score = (std::min)(cur_score, max_score) ;
       COLAMD_ASSERT (cur_score >= 0) ;
 
       /* store updated score */
@@ -1409,7 +1402,7 @@ static Index find_ordering /* return the number of garbage collections */
       head [cur_score] = col ;
 
       /* see if this score is less than current min */
-      min_score = COLAMD_MIN (min_score, cur_score) ;
+      min_score = (std::min)(min_score, cur_score) ;
 
     }
 

gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h

@@ -49,7 +49,6 @@ ColPivHouseholderQR<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynami
 { \
   using std::abs; \
   typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic> MatrixType; \
-  typedef MatrixType::Scalar Scalar; \
   typedef MatrixType::RealScalar RealScalar; \
   Index rows = matrix.rows();\
   Index cols = matrix.cols();\

gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h

@@ -816,7 +816,7 @@ void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Index rows, Index cols, u
   
   if(m_cols>m_rows)   m_qr_precond_morecols.allocate(*this);
   if(m_rows>m_cols)   m_qr_precond_morerows.allocate(*this);
-  if(m_cols!=m_cols)  m_scaledMatrix.resize(rows,cols);
+  if(m_rows!=m_cols)  m_scaledMatrix.resize(rows,cols);
 }
 
 template<typename MatrixType, int QRPreconditioner>

gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h

@@ -45,8 +45,8 @@ JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPiv
 JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPivHouseholderQRPreconditioner>::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>& matrix, unsigned int computationOptions) \
 { \
   typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic> MatrixType; \
-  typedef MatrixType::Scalar Scalar; \
+  /*typedef MatrixType::Scalar Scalar;*/ \
-  typedef MatrixType::RealScalar RealScalar; \
+  /*typedef MatrixType::RealScalar RealScalar;*/ \
   allocate(matrix.rows(), matrix.cols(), computationOptions); \
 \
   /*const RealScalar precision = RealScalar(2) * NumTraits<Scalar>::epsilon();*/ \

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h

@@ -364,10 +364,11 @@ public:
 
   protected:
 
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
+
     typename SparseMatrixType::Nested m_matrix;
     Index m_outerStart;
     const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
-
 };
 
 //----------
@@ -528,7 +529,8 @@ public:
     const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
     const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
     const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
-
+  private:
+    Index nonZeros() const;
 };
 
 } // end namespace Eigen

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h

@@ -55,10 +55,9 @@ class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse>
     EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
     CwiseBinaryOpImpl()
     {
-      typedef typename internal::traits<Lhs>::StorageKind LhsStorageKind;
-      typedef typename internal::traits<Rhs>::StorageKind RhsStorageKind;
       EIGEN_STATIC_ASSERT((
-                (!internal::is_same<LhsStorageKind,RhsStorageKind>::value)
+                (!internal::is_same<typename internal::traits<Lhs>::StorageKind,
+                                    typename internal::traits<Rhs>::StorageKind>::value)
             ||  ((Lhs::Flags&RowMajorBit) == (Rhs::Flags&RowMajorBit))),
             THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH);
     }

gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h

@@ -35,9 +35,9 @@ class SparseView : public SparseMatrixBase<SparseView<MatrixType> >
 public:
   EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView)
 
-  SparseView(const MatrixType& mat, const Scalar& m_reference = Scalar(0),
+  explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0),
-             typename NumTraits<Scalar>::Real m_epsilon = NumTraits<Scalar>::dummy_precision()) : 
+                      const RealScalar &epsilon = NumTraits<Scalar>::dummy_precision())
-    m_matrix(mat), m_reference(m_reference), m_epsilon(m_epsilon) {}
+    : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
 
   class InnerIterator;
 

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h

@@ -13,32 +13,24 @@
 
 #include "details.h"
 
-// Define the explicit instantiation (e.g. necessary for the Intel compiler)
-#if defined(__INTEL_COMPILER) || defined(__GNUC__)
-  #define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...) template class std::deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >;
-#else
-  #define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...)
-#endif
-
 /**
  * This section contains a convenience MACRO which allows an easy specialization of
  * std::deque such that for data types with alignment issues the correct allocator
  * is used automatically.
  */
 #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \
-EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(__VA_ARGS__) \
 namespace std \
 { \
-  template<typename _Ay> \
+  template<> \
-  class deque<__VA_ARGS__, _Ay>  \
+  class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> >           \
     : public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
   { \
     typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \
   public: \
     typedef __VA_ARGS__ value_type; \
-    typedef typename deque_base::allocator_type allocator_type; \
+    typedef deque_base::allocator_type allocator_type; \
-    typedef typename deque_base::size_type size_type;  \
+    typedef deque_base::size_type size_type;  \
-    typedef typename deque_base::iterator iterator;  \
+    typedef deque_base::iterator iterator;  \
     explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {}  \
     template<typename InputIterator> \
     deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \

gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h

@@ -12,32 +12,24 @@
 
 #include "details.h"
 
-// Define the explicit instantiation (e.g. necessary for the Intel compiler)
-#if defined(__INTEL_COMPILER) || defined(__GNUC__)
-  #define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...) template class std::list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >;
-#else
-  #define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...)
-#endif
-
 /**
  * This section contains a convenience MACRO which allows an easy specialization of
  * std::list such that for data types with alignment issues the correct allocator
  * is used automatically.
  */
 #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \
-EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(__VA_ARGS__) \
 namespace std \
 { \
-  template<typename _Ay> \
+  template<> \
-  class list<__VA_ARGS__, _Ay>  \
+  class list<__VA_ARGS__, std::allocator<__VA_ARGS__> >           \
     : public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
   { \
     typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \
   public: \
     typedef __VA_ARGS__ value_type; \
-    typedef typename list_base::allocator_type allocator_type; \
+    typedef list_base::allocator_type allocator_type; \
-    typedef typename list_base::size_type size_type;  \
+    typedef list_base::size_type size_type;  \
-    typedef typename list_base::iterator iterator;  \
+    typedef list_base::iterator iterator;  \
     explicit list(const allocator_type& a = allocator_type()) : list_base(a) {}  \
     template<typename InputIterator> \
     list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \

gtsam/3rdparty/Eigen/doc/CMakeLists.txt

@@ -89,6 +89,7 @@ add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_
 add_custom_target(doc ALL
   COMMAND doxygen
   COMMAND doxygen Doxyfile-unsupported
+  COMMAND ${CMAKE_COMMAND} -E copy ${Eigen_BINARY_DIR}/doc/html/group__TopicUnalignedArrayAssert.html ${Eigen_BINARY_DIR}/doc/html/TopicUnalignedArrayAssert.html
   COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc
   COMMAND ${CMAKE_COMMAND} -E remove eigen-doc/eigen-doc.tgz
   COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc.tgz eigen-doc

gtsam/3rdparty/Eigen/doc/Manual.dox

@@ -121,6 +121,8 @@ namespace Eigen {
     \ingroup Sparse_chapter */
 /** \addtogroup TopicSparseSystems
     \ingroup Sparse_chapter */
+/** \addtogroup MatrixfreeSolverExample
+    \ingroup Sparse_chapter */
 
 /** \addtogroup Sparse_Reference
     \ingroup Sparse_chapter */

gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox

@@ -91,6 +91,7 @@ following macros are supported; none of them are defined by default.
  - \b EIGEN_MATRIX_PLUGIN - filename of plugin for extending the Matrix class.
  - \b EIGEN_MATRIXBASE_PLUGIN - filename of plugin for extending the MatrixBase class.
  - \b EIGEN_PLAINOBJECTBASE_PLUGIN - filename of plugin for extending the PlainObjectBase class.
+ - \b EIGEN_MAPBASE_PLUGIN - filename of plugin for extending the MapBase class.
  - \b EIGEN_QUATERNIONBASE_PLUGIN - filename of plugin for extending the QuaternionBase class.
  - \b EIGEN_SPARSEMATRIX_PLUGIN - filename of plugin for extending the SparseMatrix class.
  - \b EIGEN_SPARSEMATRIXBASE_PLUGIN - filename of plugin for extending the SparseMatrixBase class.

gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox

@@ -35,17 +35,17 @@ They are summarized in the following table:
     <td>Requires the <a href="http://pastix.gforge.inria.fr">PaStiX</a> package, \b CeCILL-C </td>
     <td>optimized for tough problems and symmetric patterns</td></tr>
 <tr><td>CholmodSupernodalLLT</td><td>\link CholmodSupport_Module CholmodSupport \endlink</td><td>Direct LLt factorization</td><td>SPD</td><td>Fill-in reducing, Leverage fast dense algebra</td>
-    <td>Requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td>
+    <td>Requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td>
     <td></td></tr>
 <tr><td>UmfPackLU</td><td>\link UmfPackSupport_Module UmfPackSupport \endlink</td><td>Direct LU factorization</td><td>Square</td><td>Fill-in reducing, Leverage fast dense algebra</td>
-    <td>Requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td>
+    <td>Requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td>
     <td></td></tr>
 <tr><td>SuperLU</td><td>\link SuperLUSupport_Module SuperLUSupport \endlink</td><td>Direct LU factorization</td><td>Square</td><td>Fill-in reducing, Leverage fast dense algebra</td>
     <td>Requires the <a href="http://crd-legacy.lbl.gov/~xiaoye/SuperLU/">SuperLU</a> library, (BSD-like)</td>
     <td></td></tr>
 <tr><td>SPQR</td><td>\link SPQRSupport_Module SPQRSupport \endlink  </td> <td> QR factorization </td> 
     <td> Any, rectangular</td><td>fill-in reducing, multithreaded, fast dense algebra</td>
-    <td> requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td><td>recommended for linear least-squares problems, has a rank-revealing feature</tr>
+    <td> requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td><td>recommended for linear least-squares problems, has a rank-revealing feature</tr>
 </table>
 
 Here \c SPD means symmetric positive definite.

gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox

@@ -21,7 +21,7 @@ i.e either row major or column major. The default is column major. Most arithmet
 <td> Resize/Reserve</td>
 <td> 
  \code
-    sm1.resize(m,n);      //Change sm1 to a m x n matrix. 
+    sm1.resize(m,n);      // Change sm1 to a m x n matrix.
     sm1.reserve(nnz);     // Allocate room for nnz nonzeros elements.   
   \endcode 
 </td>
@@ -151,10 +151,10 @@ It is easy to perform arithmetic operations on sparse matrices provided that the
 <td> Permutation </td>
 <td> 
 \code 
-perm.indices(); // Reference to the vector of indices
+perm.indices();      // Reference to the vector of indices
 sm1.twistedBy(perm); // Permute rows and columns
-sm2 = sm1 * perm; //Permute the columns
+sm2 = sm1 * perm;    // Permute the columns
-sm2 = perm * sm1; // Permute the columns
+sm2 = perm * sm1;    // Permute the columns
 \endcode 
 </td>
 <td> 
@@ -181,9 +181,9 @@ sm2 = perm * sm1; // Permute the columns
 
 \section sparseotherops Other supported operations
 <table class="manual">
-<tr><th>Operations</th> <th> Code </th> <th> Notes</th> </tr>
+<tr><th style="min-width:initial"> Code </th> <th> Notes</th> </tr>
+<tr><td colspan="2">Sub-matrices</td></tr>
 <tr>
-<td>Sub-matrices</td> 
 <td> 
 \code 
   sm1.block(startRow, startCol, rows, cols); 
@@ -193,25 +193,31 @@ sm2 = perm * sm1; // Permute the columns
   sm1.bottomLeftCorner( rows, cols);
   sm1.bottomRightCorner( rows, cols);
   \endcode
-</td> <td>  </td>
+</td><td>
+Contrary to dense matrices, here <strong>all these methods are read-only</strong>.\n
+See \ref TutorialSparse_SubMatrices and below for read-write sub-matrices.
+</td>
 </tr>
-<tr> 
+<tr class="alt"><td colspan="2"> Range </td></tr>
-<td> Range </td>
+<tr class="alt">
 <td> 
 \code 
-  sm1.innerVector(outer); 
+  sm1.innerVector(outer);           // RW
-  sm1.innerVectors(start, size);
+  sm1.innerVectors(start, size);    // RW
-  sm1.leftCols(size);
+  sm1.leftCols(size);               // RW
-  sm2.rightCols(size);
+  sm2.rightCols(size);              // RO because sm2 is row-major
-  sm1.middleRows(start, numRows);
+  sm1.middleRows(start, numRows);   // RO becasue sm1 is column-major
-  sm1.middleCols(start, numCols);
+  sm1.middleCols(start, numCols);   // RW
-  sm1.col(j);
+  sm1.col(j);                       // RW
 \endcode
 </td>
-<td>A inner vector is either a row (for row-major) or a column (for column-major). As stated earlier, the evaluation can be done in a matrix with different storage order </td>
+<td>
+A inner vector is either a row (for row-major) or a column (for column-major).\n
+As stated earlier, for a read-write sub-matrix (RW), the evaluation can be done in a matrix with different storage order.
+</td>
 </tr>
+<tr><td colspan="2"> Triangular and selfadjoint views</td></tr>
 <tr>
-<td> Triangular and selfadjoint views</td>
 <td> 
 \code
   sm2 = sm1.triangularview<Lower>();
@@ -222,26 +228,30 @@ sm2 = perm * sm1; // Permute the columns
 \code 
   \endcode </td>
 </tr>
-<tr> 
+<tr class="alt"><td colspan="2">Triangular solve </td></tr>
-<td>Triangular solve </td>
+<tr class="alt">
 <td> 
 \code 
  dv2 = sm1.triangularView<Upper>().solve(dv1);
- dv2 = sm1.topLeftCorner(size, size).triangularView<Lower>().solve(dv1);
+ dv2 = sm1.topLeftCorner(size, size)
+          .triangularView<Lower>().solve(dv1);
 \endcode 
 </td>
 <td> For general sparse solve, Use any suitable module described at \ref TopicSparseSystems </td>
 </tr>
+<tr><td colspan="2"> Low-level API</td></tr>
 <tr>
-<td> Low-level API</td>
 <td>
 \code
-sm1.valuePtr(); // Pointer to the values
+sm1.valuePtr();      // Pointer to the values
-sm1.innerIndextr(); // Pointer to the indices.
+sm1.innerIndextr();  // Pointer to the indices.
-sm1.outerIndexPtr(); //Pointer to the beginning of each inner vector
+sm1.outerIndexPtr(); // Pointer to the beginning of each inner vector
 \endcode
 </td>
-<td> If the matrix is not in compressed form, makeCompressed() should be called before. Note that these functions are mostly provided for interoperability purposes with external libraries. A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section</td>
+<td>
+If the matrix is not in compressed form, makeCompressed() should be called before.\n
+Note that these functions are mostly provided for interoperability purposes with external libraries.\n
+A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section</td>
 </tr>
 </table>
 */

gtsam/3rdparty/Eigen/doc/TutorialSparse.dox

@@ -241,11 +241,11 @@ In the following \em sm denotes a sparse matrix, \em sv a sparse vector, \em dm
 sm1.real()   sm1.imag()   -sm1                    0.5*sm1
 sm1+sm2      sm1-sm2      sm1.cwiseProduct(sm2)
 \endcode
-However, a strong restriction is that the storage orders must match. For instance, in the following example:
+However, <strong>a strong restriction is that the storage orders must match</strong>. For instance, in the following example:
 \code
 sm4 = sm1 + sm2 + sm3;
 \endcode
-sm1, sm2, and sm3 must all be row-major or all column major.
+sm1, sm2, and sm3 must all be row-major or all column-major.
 On the other hand, there is no restriction on the target matrix sm4.
 For instance, this means that for computing \f$ A^T + A \f$, the matrix \f$ A^T \f$ must be evaluated into a temporary matrix of compatible storage order:
 \code
@@ -307,6 +307,26 @@ sm2 = sm1.transpose() * P;
     \endcode
 
 
+\subsection TutorialSparse_SubMatrices Block operations
+
+Regarding read-access, sparse matrices expose the same API than for dense matrices to access to sub-matrices such as blocks, columns, and rows. See \ref TutorialBlockOperations for a detailed introduction.
+However, for performance reasons, writing to a sub-sparse-matrix is much more limited, and currently only contiguous sets of columns (resp. rows) of a column-major (resp. row-major) SparseMatrix are writable. Moreover, this information has to be known at compile-time, leaving out methods such as <tt>block(...)</tt> and <tt>corner*(...)</tt>. The available API for write-access to a SparseMatrix are summarized below:
+\code
+SparseMatrix<double,ColMajor> sm1;
+sm1.col(j) = ...;
+sm1.leftCols(ncols) = ...;
+sm1.middleCols(j,ncols) = ...;
+sm1.rightCols(ncols) = ...;
+
+SparseMatrix<double,RowMajor> sm2;
+sm2.row(i) = ...;
+sm2.topRows(nrows) = ...;
+sm2.middleRows(i,nrows) = ...;
+sm2.bottomRows(nrows) = ...;
+\endcode
+
+In addition, sparse matrices expose the SparseMatrixBase::innerVector() and SparseMatrixBase::innerVectors() methods, which are aliases to the col/middleCols methods for a column-major storage, and to the row/middleRows methods for a row-major storage.
+
 \subsection TutorialSparse_TriangularSelfadjoint Triangular and selfadjoint views
 
 Just as with dense matrices, the triangularView() function can be used to address a triangular part of the matrix, and perform triangular solves with a dense right hand side:

gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox

@@ -7,8 +7,8 @@ Hello! You are seeing this webpage because your program terminated on an asserti
 my_program: path/to/eigen/Eigen/src/Core/DenseStorage.h:44:
 Eigen::internal::matrix_array<T, Size, MatrixOptions, Align>::internal::matrix_array()
 [with T = double, int Size = 2, int MatrixOptions = 2, bool Align = true]:
-Assertion `(reinterpret_cast<size_t>(array) & 0xf) == 0 && "this assertion
+Assertion `(reinterpret_cast<size_t>(array) & (sizemask)) == 0 && "this assertion
-is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html
+is explained here: http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html
 **** READ THIS WEB PAGE !!! ****"' failed.
 </pre>
 
@@ -46,9 +46,9 @@ then you need to read this separate page: \ref TopicStructHavingEigenMembers "St
 
 Note that here, Eigen::Vector2d is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types".
 
-\section c2 Cause 2: STL Containers
+\section c2 Cause 2: STL Containers or manual memory allocation
 
-If you use STL Containers such as std::vector, std::map, ..., with Eigen objects, or with classes containing Eigen objects, like this,
+If you use STL Containers such as std::vector, std::map, ..., with %Eigen objects, or with classes containing %Eigen objects, like this,
 
 \code
 std::vector<Eigen::Matrix2f> my_vector;
@@ -60,6 +60,8 @@ then you need to read this separate page: \ref TopicStlContainers "Using STL Con
 
 Note that here, Eigen::Matrix2f is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types" and \ref TopicStructHavingEigenMembers "structures having such Eigen objects as member".
 
+The same issue will be exhibited by any classes/functions by-passing operator new to allocate memory, that is, by performing custom memory allocation followed by calls to the placement new operator. This is for instance typically the case of \c std::make_shared or \c std::allocate_shared for which is the solution is to use an \ref aligned_allocator "aligned allocator" as detailed in the \ref TopicStlContainers "solution for STL containers".
+
 \section c3 Cause 3: Passing Eigen objects by value
 
 If some function in your code is getting an Eigen object passed by value, like this,
@@ -107,7 +109,10 @@ Two possibilities:
       128-bit alignment code and thus preserves ABI compatibility, but completely disables vectorization.</li>
 </ul>
 
-For more information, see <a href="http://eigen.tuxfamily.org/index.php?title=FAQ#I_disabled_vectorization.2C_but_I.27m_still_getting_annoyed_about_alignment_issues.21">this FAQ</a>.
+If you want to know why defining EIGEN_DONT_VECTORIZE does not by itself disable 128-bit alignment and the assertion, here's the explanation:
+
+It doesn't disable the assertion, because otherwise code that runs fine without vectorization would suddenly crash when enabling vectorization.
+It doesn't disable 128bit alignment, because that would mean that vectorized and non-vectorized code are not mutually ABI-compatible. This ABI compatibility is very important, even for people who develop only an in-house application, as for instance one may want to have in the same application a vectorized path and a non-vectorized path.
 
 */
 

gtsam/3rdparty/Eigen/eigen3.pc.in

@@ -1,6 +1,9 @@
+prefix=@CMAKE_INSTALL_PREFIX@
+exec_prefix=${prefix}
+
 Name: Eigen3
 Description: A C++ template library for linear algebra: vectors, matrices, and related algorithms
 Requires:
-Version: ${EIGEN_VERSION_NUMBER}
+Version: @EIGEN_VERSION_NUMBER@
 Libs:
-Cflags: -I${INCLUDE_INSTALL_DIR}
+Cflags: -I${prefix}/@INCLUDE_INSTALL_DIR@

gtsam/3rdparty/Eigen/test/CMakeLists.txt

@@ -202,7 +202,9 @@ ei_add_test(geo_alignedbox)
 ei_add_test(stdvector)
 ei_add_test(stdvector_overload)
 ei_add_test(stdlist)
+ei_add_test(stdlist_overload)
 ei_add_test(stddeque)
+ei_add_test(stddeque_overload)
 ei_add_test(resize)
 ei_add_test(sparse_vector)
 ei_add_test(sparse_basic)

gtsam/3rdparty/Eigen/test/dynalloc.cpp

@@ -87,6 +87,32 @@ template<typename T> void check_dynaligned()
   delete obj;
 }
 
+template<typename T> void check_custom_new_delete()
+{
+  {
+    T* t = new T;
+    delete t;
+  }
+  
+  {
+    std::size_t N = internal::random<std::size_t>(1,10);
+    T* t = new T[N];
+    delete[] t;
+  }
+  
+#ifdef EIGEN_ALIGN
+  {
+    T* t = static_cast<T *>((T::operator new)(sizeof(T)));
+    (T::operator delete)(t, sizeof(T));
+  }
+  
+  {
+    T* t = static_cast<T *>((T::operator new)(sizeof(T)));
+    (T::operator delete)(t);
+  }
+#endif
+}
+
 void test_dynalloc()
 {
   // low level dynamic memory allocation
@@ -94,7 +120,9 @@ void test_dynalloc()
   CALL_SUBTEST(check_aligned_malloc());
   CALL_SUBTEST(check_aligned_new());
   CALL_SUBTEST(check_aligned_stack_alloc());
-
+  
+  // check static allocation, who knows ?
+  #if EIGEN_ALIGN_STATICALLY
   for (int i=0; i<g_repeat*100; ++i)
   {
     CALL_SUBTEST(check_dynaligned<Vector4f>() );
@@ -102,10 +130,13 @@ void test_dynalloc()
     CALL_SUBTEST(check_dynaligned<Matrix4f>() );
     CALL_SUBTEST(check_dynaligned<Vector4d>() );
     CALL_SUBTEST(check_dynaligned<Vector4i>() );
+    
+    CALL_SUBTEST( check_custom_new_delete<Vector4f>() );
+    CALL_SUBTEST( check_custom_new_delete<Vector2f>() );
+    CALL_SUBTEST( check_custom_new_delete<Matrix4f>() );
+    CALL_SUBTEST( check_custom_new_delete<MatrixXi>() );
   }
-  
+
-  // check static allocation, who knows ?
-  #if EIGEN_ALIGN_STATICALLY
   {
     MyStruct foo0;  VERIFY(size_t(foo0.avec.data())%ALIGNMENT==0);
     MyClassA fooA;  VERIFY(size_t(fooA.avec.data())%ALIGNMENT==0);

gtsam/3rdparty/Eigen/test/product.h

@@ -136,9 +136,27 @@ template<typename MatrixType> void product(const MatrixType& m)
   VERIFY_IS_APPROX(res.col(r).noalias() = square.adjoint() * square.col(r), (square.adjoint() * square.col(r)).eval());
   VERIFY_IS_APPROX(res.col(r).noalias() = square * square.col(r), (square * square.col(r)).eval());
 
+  // vector at runtime (see bug 1166)
+  {
+    RowSquareMatrixType ref(square);
+    ColSquareMatrixType ref2(square2);
+    ref = res = square;
+    VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square.transpose(),            (ref.row(0) = m1.col(0).transpose() * square.transpose()));
+    VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose()));
+    VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square,                        (ref.row(0) = m1.col(0).transpose() * square));
+    VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square,             (ref.row(0) = m1.col(0).transpose() * square));
+    ref2 = res2 = square2;
+    VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2.transpose(),                      (ref2.row(0) = m1.row(0) * square2.transpose()));
+    VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2.transpose(),           (ref2.row(0) = m1.row(0) * square2.transpose()));
+    VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2,                                  (ref2.row(0) = m1.row(0) * square2));
+    VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2,                       (ref2.row(0) = m1.row(0) * square2));
+  }
+
   // inner product
-  Scalar x = square2.row(c) * square2.col(c2);
+  {
-  VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
+    Scalar x = square2.row(c) * square2.col(c2);
+    VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
+  }
   
   // outer product
   VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
@@ -146,5 +164,18 @@ template<typename MatrixType> void product(const MatrixType& m)
   VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
   VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
   VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols));
-  VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));  
+  VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));
+
+  // Aliasing
+  {
+    ColVectorType x(cols); x.setRandom();
+    ColVectorType z(x);
+    ColVectorType y(cols); y.setZero();
+    ColSquareMatrixType A(cols,cols); A.setRandom();
+    // CwiseBinaryOp
+    VERIFY_IS_APPROX(x = y + A*x, A*z);
+    x = z;
+    // CwiseUnaryOp
+    VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z);
+  }
 }

gtsam/3rdparty/Eigen/test/product_large.cpp

@@ -9,6 +9,27 @@
 
 #include "product.h"
 
+template<typename T>
+void test_aliasing()
+{
+  int rows = internal::random<int>(1,12);
+  int cols = internal::random<int>(1,12);
+  typedef Matrix<T,Dynamic,Dynamic> MatrixType;
+  typedef Matrix<T,Dynamic,1> VectorType;
+  VectorType x(cols); x.setRandom();
+  VectorType z(x);
+  VectorType y(rows); y.setZero();
+  MatrixType A(rows,cols); A.setRandom();
+  // CwiseBinaryOp
+  VERIFY_IS_APPROX(x = y + A*x, A*z);
+  x = z;
+  // CwiseUnaryOp
+  VERIFY_IS_APPROX(x = T(1.)*(A*x), A*z);
+  x = z;
+  VERIFY_IS_APPROX(x = y+(-(A*x)), -A*z);
+  x = z;
+}
+
 void test_product_large()
 {
   for(int i = 0; i < g_repeat; i++) {
@@ -17,6 +38,8 @@ void test_product_large()
     CALL_SUBTEST_3( product(MatrixXi(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
     CALL_SUBTEST_4( product(MatrixXcf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2), internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2))) );
     CALL_SUBTEST_5( product(Matrix<float,Dynamic,Dynamic,RowMajor>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
+
+    CALL_SUBTEST_1( test_aliasing<float>() );
   }
 
 #if defined EIGEN_TEST_PART_6

gtsam/3rdparty/Eigen/test/product_notemporary.cpp

@@ -58,10 +58,19 @@ template<typename MatrixType> void product_notemporary(const MatrixType& m)
         r1 = internal::random<Index>(8,rows-r0);
 
   VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()), 1);
+  VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()).transpose(), 1);
   VERIFY_EVALUATION_COUNT( m3.noalias() = m1 * m2.adjoint(), 0);
 
+  VERIFY_EVALUATION_COUNT( m3 = s1 * (m1 * m2.transpose()), 1);
+  VERIFY_EVALUATION_COUNT( m3 = m3 + s1 * (m1 * m2.transpose()), 1);
   VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * (m1 * m2.transpose()), 0);
 
+  VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()), 1);
+  VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()).transpose(), 1);
+  VERIFY_EVALUATION_COUNT( m3.noalias() = m3 + m1 * m2.transpose(), 1);   // 0 in 3.3
+  VERIFY_EVALUATION_COUNT( m3.noalias() += m3 + m1 * m2.transpose(), 1);  // 0 in 3.3
+  VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 + m1 * m2.transpose(), 1);  // 0 in 3.3
+
   VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * m2.adjoint(), 0);
   VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * (m1*s3+m2*s2).adjoint(), 1);
   VERIFY_EVALUATION_COUNT( m3.noalias() = (s1 * m1).adjoint() * s2 * m2, 0);

gtsam/3rdparty/Eigen/test/ref.cpp

@@ -34,6 +34,18 @@ inline void on_temporary_creation(int) {
 
 // test Ref.h
 
+// Deal with i387 extended precision
+#if EIGEN_ARCH_i386 && !(EIGEN_ARCH_x86_64)
+
+#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(4,4)
+#pragma GCC optimize ("-ffloat-store")
+#else
+#undef VERIFY_IS_EQUAL
+#define VERIFY_IS_EQUAL(X,Y) VERIFY_IS_APPROX(X,Y)
+#endif
+
+#endif
+
 template<typename MatrixType> void ref_matrix(const MatrixType& m)
 {
   typedef typename MatrixType::Index Index;
@@ -71,7 +83,6 @@ template<typename MatrixType> void ref_matrix(const MatrixType& m)
   rm2 = m2.block(i,j,brows,bcols);
   VERIFY_IS_EQUAL(m1, m2);
   
-  
   ConstRefDynMat rm3 = m1.block(i,j,brows,bcols);
   m1.block(i,j,brows,bcols) *= 2;
   m2.block(i,j,brows,bcols) *= 2;

gtsam/3rdparty/Eigen/test/visitor.cpp

@@ -55,6 +55,11 @@ template<typename MatrixType> void matrixVisitor(const MatrixType& p)
   VERIFY_IS_APPROX(maxc, eigen_maxc);
   VERIFY_IS_APPROX(minc, m.minCoeff());
   VERIFY_IS_APPROX(maxc, m.maxCoeff());
+
+  eigen_maxc = (m.adjoint()*m).maxCoeff(&eigen_maxrow,&eigen_maxcol);
+  eigen_maxc = (m.adjoint()*m).eval().maxCoeff(&maxrow,&maxcol);
+  VERIFY(maxrow == eigen_maxrow);
+  VERIFY(maxcol == eigen_maxcol);
 }
 
 template<typename VectorType> void vectorVisitor(const VectorType& w)

gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3

@@ -177,7 +177,7 @@ template<typename _Scalar> class AlignedVector3
     }
 
     template<typename Derived>
-    inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=NumTraits<Scalar>::dummy_precision()) const
+    inline bool isApprox(const MatrixBase<Derived>& other, const RealScalar& eps=NumTraits<Scalar>::dummy_precision()) const
     {
       return m_coeffs.template head<3>().isApprox(other,eps);
     }

gtsam/3rdparty/ceres/fixed_array.h

@@ -33,7 +33,7 @@
 #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
 
 #include <cstddef>
-#include <Eigen/Core>
+#include <gtsam/3rdparty/ceres/eigen.h>
 #include <gtsam/3rdparty/ceres/macros.h>
 #include <gtsam/3rdparty/ceres/manual_constructor.h>
 

gtsam/3rdparty/ceres/jet.h

@@ -162,7 +162,7 @@
 #include <limits>
 #include <string>
 
-#include <Eigen/Core>
+#include <gtsam/3rdparty/ceres/eigen.h>
 #include <gtsam/3rdparty/ceres/fpclassify.h>
 
 namespace ceres {

gtsam/3rdparty/metis/CMakeLists.txt

@@ -20,7 +20,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
 endif()
 
 set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
-set(SHARED FALSE CACHE BOOL "build a shared library")
+set(METIS_SHARED TRUE CACHE BOOL "build a shared library")
 
 if(MSVC)
   set(METIS_INSTALL FALSE)
@@ -29,11 +29,11 @@ else()
 endif()
 
 # Configure libmetis library.
-if(SHARED)
+if(METIS_SHARED)
   set(METIS_LIBRARY_TYPE SHARED)
 else()
   set(METIS_LIBRARY_TYPE STATIC)
-endif(SHARED)
+endif(METIS_SHARED)
 
 include(${GKLIB_PATH}/GKlibSystem.cmake)
 # Add include directories.

gtsam/CMakeLists.txt

@@ -88,7 +88,9 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
 list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h")
 install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam)
 
-list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
+if(GTSAM_SUPPORT_NESTED_DISSECTION)
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
+endif()
 
 # Versions
 set(gtsam_version   ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})

gtsam/base/Lie.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -295,19 +295,18 @@ private:
 };
 
 /**
- *  Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
+ *  Three term approximation of the Baker-Campbell-Hausdorff formula
  *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
  *  it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
  *  formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
- *  http://en.wikipedia.org/wiki/Baker<EFBFBD>Campbell<EFBFBD>Hausdorff_formula
+ *  http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula
  */
 /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
 template<class T>
 T BCH(const T& X, const T& Y) {
   static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
   T X_Y = bracket(X, Y);
-  return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
+  return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
-      bracket(X, X_Y));
 }
 
 /**
@@ -328,6 +327,15 @@ T expm(const Vector& x, int K=7) {
   return T(expm(xhat,K));
 }
 
+/**
+ * Linear interpolation between X and Y by coefficient t in [0, 1].
+ */
+template <typename T>
+T interpolate(const T& X, const T& Y, double t) {
+  assert(t >= 0 && t <= 1);
+  return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
+}
+
 } // namespace gtsam
 
 /**

gtsam/base/Matrix.cpp

@@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
       0, // flags
       " ", // coeffSeparator
       ";\n", // rowSeparator
-	  " \t",  // rowPrefix
+      " \t",  // rowPrefix
       "", // rowSuffix
       "[\n", // matPrefix
       "\n  ]" // matSuffix
@@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
   list<boost::tuple<Vector, double, double> > results;
 
   Vector pseudo(m); // allocate storage for pseudo-inverse
-  Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once
+  Vector weights = sigmas.array().square().inverse(); // calculate weights once
 
   // We loop over all columns, because the columns that can be eliminated
   // are not necessarily contiguous. For each one, estimate the corresponding
@@ -704,11 +704,9 @@ void inplace_QR(Matrix& A){
   HCoeffsType hCoeffs(size);
   RowVectorType temp(cols);
 
-#ifdef GTSAM_USE_SYSTEM_EIGEN
+#if !EIGEN_VERSION_AT_LEAST(3,2,5)
-  // System-Eigen is used, and MKL is off
   Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>(A, hCoeffs, 48, temp.data());
 #else
-  // Patched Eigen is used, and MKL is either on or off
   Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
 #endif
 

gtsam/base/Matrix.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -46,29 +46,29 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> M
 
 // Create handy typedefs and constants for square-size matrices
 // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
-#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX)   \
+#define GTSAM_MAKE_MATRIX_DEFS(N)   \
-typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX;  \
+typedef Eigen::Matrix<double, N, N> Matrix##N;  \
-typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX;  \
+typedef Eigen::Matrix<double, 1, N> Matrix1##N;  \
-typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX;  \
+typedef Eigen::Matrix<double, 2, N> Matrix2##N;  \
-typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX;  \
+typedef Eigen::Matrix<double, 3, N> Matrix3##N;  \
-typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX;  \
+typedef Eigen::Matrix<double, 4, N> Matrix4##N;  \
-typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX;  \
+typedef Eigen::Matrix<double, 5, N> Matrix5##N;  \
-typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX;  \
+typedef Eigen::Matrix<double, 6, N> Matrix6##N;  \
-typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX;  \
+typedef Eigen::Matrix<double, 7, N> Matrix7##N;  \
-typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX;  \
+typedef Eigen::Matrix<double, 8, N> Matrix8##N;  \
-typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX;  \
+typedef Eigen::Matrix<double, 9, N> Matrix9##N;  \
-static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
+static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
-static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
+static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
 
-GTSAM_MAKE_TYPEDEFS(1,1);
+GTSAM_MAKE_MATRIX_DEFS(1);
-GTSAM_MAKE_TYPEDEFS(2,2);
+GTSAM_MAKE_MATRIX_DEFS(2);
-GTSAM_MAKE_TYPEDEFS(3,3);
+GTSAM_MAKE_MATRIX_DEFS(3);
-GTSAM_MAKE_TYPEDEFS(4,4);
+GTSAM_MAKE_MATRIX_DEFS(4);
-GTSAM_MAKE_TYPEDEFS(5,5);
+GTSAM_MAKE_MATRIX_DEFS(5);
-GTSAM_MAKE_TYPEDEFS(6,6);
+GTSAM_MAKE_MATRIX_DEFS(6);
-GTSAM_MAKE_TYPEDEFS(7,7);
+GTSAM_MAKE_MATRIX_DEFS(7);
-GTSAM_MAKE_TYPEDEFS(8,8);
+GTSAM_MAKE_MATRIX_DEFS(8);
-GTSAM_MAKE_TYPEDEFS(9,9);
+GTSAM_MAKE_MATRIX_DEFS(9);
 
 // Matrix expressions for accessing parts of matrices
 typedef Eigen::Block<Matrix> SubMatrix;
@@ -135,7 +135,7 @@ inline bool operator==(const Matrix& A, const Matrix& B) {
 }
 
 /**
- * inequality 
+ * inequality
  */
 inline bool operator!=(const Matrix& A, const Matrix& B) {
   return !(A==B);
@@ -371,7 +371,7 @@ GTSAM_EXPORT Matrix inverse(const Matrix& A);
  * m*n matrix -> m*m Q, m*n R
  * @param A a matrix
  * @return <Q,R> rotation matrix Q, upper triangular R
- */ 
+ */
 GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
 
 /**
@@ -434,7 +434,7 @@ GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool u
  * @param b an RHS vector
  * @param unit, set true if unit triangular
  * @return the solution x of L*x=b
- */ 
+ */
 GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false);
 
 /**

gtsam/base/Testable.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -13,20 +13,20 @@
  * @file    Testable.h
  * @brief   Concept check for values that can be used in unit tests
  * @author  Frank Dellaert
- * 
+ *
  * The necessary functions to implement for Testable are defined
  * below with additional details as to the interface.
  * The concept checking function will check whether or not
  * the function exists in derived class and throw compile-time errors.
- * 
+ *
  * print with optional string naming the object
  *     void print(const std::string& name) const = 0;
- * 
+ *
  * equality up to tolerance
  * tricky to implement, see NoiseModelFactor1 for an example
  * equals is not supposed to print out *anything*, just return true|false
  *     bool equals(const Derived& expected, double tol) const = 0;
- * 
+ *
  */
 
 // \callgraph
@@ -123,7 +123,7 @@ namespace gtsam {
     double tol_;
     equals_star(double tol = 1e-9) : tol_(tol) {}
     bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
-      if (!actual || !expected) return true;
+      if (!actual && !expected) return true;
       return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
     }
   };

gtsam/base/Vector.cpp

@@ -42,11 +42,6 @@ bool zero(const Vector& v) {
   return result;
 }
 
-/* ************************************************************************* */
-Vector repeat(size_t n, double value) {
-  return Vector::Constant(n, value);
-}
-
 /* ************************************************************************* */
 Vector delta(size_t n, size_t i, double value) {
   return Vector::Unit(n, i) * value;
@@ -176,28 +171,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
   return flag;
 }
 
-/* ************************************************************************* */
-ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
-  return v.segment(i1,i2-i1);
-}
-
-/* ************************************************************************* */
-void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
-  fullVector.segment(i, subVector.size()) = subVector;
-}
-
-/* ************************************************************************* */
-Vector emul(const Vector &a, const Vector &b) {
-  assert (b.size()==a.size());
-  return a.cwiseProduct(b);
-}
-
-/* ************************************************************************* */
-Vector ediv(const Vector &a, const Vector &b) {
-  assert (b.size()==a.size());
-  return a.cwiseQuotient(b);
-}
-
 /* ************************************************************************* */
 Vector ediv_(const Vector &a, const Vector &b) {
   size_t n = a.size();
@@ -210,36 +183,6 @@ Vector ediv_(const Vector &a, const Vector &b) {
   return c;
 }
 
-/* ************************************************************************* */
-double sum(const Vector &a) {
-  return a.sum();
-}
-
-/* ************************************************************************* */
-double norm_2(const Vector& v) {
-  return v.norm();
-}
-
-/* ************************************************************************* */
-Vector reciprocal(const Vector &a) {
-  return a.array().inverse();
-}
-
-/* ************************************************************************* */
-Vector esqrt(const Vector& v) {
-  return v.cwiseSqrt();
-}
-
-/* ************************************************************************* */
-Vector abs(const Vector& v) {
-  return v.cwiseAbs();
-}
-
-/* ************************************************************************* */
-double max(const Vector &a) {
-  return a.maxCoeff();
-}
-
 /* ************************************************************************* */
 // imperative version, pass in x
 double houseInPlace(Vector &v) {
@@ -292,7 +235,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
       // Basically, instead of doing a normal QR step with the weighted
       // pseudoinverse, we enforce the constraint by turning
       // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
-      pseudo = delta(m, i, 1 / a[i]);
+      pseudo = delta(m, i, 1.0 / a[i]);
       return inf;
     }
   }
@@ -363,6 +306,4 @@ Vector concatVectors(size_t nrVectors, ...)
   return concatVectors(vs);
 }
 
-/* ************************************************************************* */
-
 } // namespace gtsam

gtsam/base/Vector.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -20,7 +20,6 @@
 
 
 #pragma once
-
 #ifndef MKL_BLAS
 #define MKL_BLAS MKL_DOMAIN_BLAS
 #endif
@@ -41,24 +40,29 @@ typedef Eigen::VectorXd Vector;
 typedef Eigen::Matrix<double, 1, 1> Vector1;
 typedef Eigen::Vector2d Vector2;
 typedef Eigen::Vector3d Vector3;
-typedef Eigen::Matrix<double, 4, 1> Vector4;
+
-typedef Eigen::Matrix<double, 5, 1> Vector5;
+static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
-typedef Eigen::Matrix<double, 6, 1> Vector6;
+static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
-typedef Eigen::Matrix<double, 7, 1> Vector7;
+
-typedef Eigen::Matrix<double, 8, 1> Vector8;
+// Create handy typedefs and constants for vectors with N>3
-typedef Eigen::Matrix<double, 9, 1> Vector9;
+// VectorN and Z_Nx1, for N=1..9
-typedef Eigen::Matrix<double, 10, 1> Vector10;
+#define GTSAM_MAKE_VECTOR_DEFS(N)                \
+  typedef Eigen::Matrix<double, N, 1> Vector##N; \
+  static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
+
+GTSAM_MAKE_VECTOR_DEFS(4);
+GTSAM_MAKE_VECTOR_DEFS(5);
+GTSAM_MAKE_VECTOR_DEFS(6);
+GTSAM_MAKE_VECTOR_DEFS(7);
+GTSAM_MAKE_VECTOR_DEFS(8);
+GTSAM_MAKE_VECTOR_DEFS(9);
+GTSAM_MAKE_VECTOR_DEFS(10);
+GTSAM_MAKE_VECTOR_DEFS(11);
+GTSAM_MAKE_VECTOR_DEFS(12);
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
 
-/**
- * Create vector initialized to a constant value
- * @param n is the size of the vector
- * @param value is a constant value to insert into the vector
- */
-GTSAM_EXPORT Vector repeat(size_t n, double value);
-
 /**
  * Create basis vector of dimension n,
  * with a constant in spot i
@@ -89,7 +93,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);}
  * @param n size
  */
 inline Vector ones(size_t n) { return Vector::Ones(n); }
-  
+
 /**
  * check if all zero
  */
@@ -184,39 +188,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector&
  */
 GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
 
-/**
- * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
- * @param v Vector
- * @param i1 first row index
- * @param i2 last  row index + 1
- * @return subvector v(i1:i2)
- */
-GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
-
-/**
- * Inserts a subvector into a vector IN PLACE
- * @param fullVector is the vector to be changed
- * @param subVector is the vector to insert
- * @param i is the index where the subvector should be inserted
- */
-GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
-
-/**
- * elementwise multiplication
- * @param a first vector
- * @param b second vector
- * @return vector [a(i)*b(i)]
- */
-GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);
-
-/**
- * elementwise division
- * @param a first vector
- * @param b second vector
- * @return vector [a(i)/b(i)]
- */
-GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
-
 /**
  * elementwise division, but 0/0 = 0, not inf
  * @param a first vector
@@ -225,49 +196,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
  */
 GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
 
-/**
- * sum vector elements
- * @param a vector
- * @return sum_i a(i)
- */
-GTSAM_EXPORT double sum(const Vector &a);
-
-/**
- * Calculates L2 norm for a vector
- * modeled after boost.ublas for compatibility
- * @param v vector
- * @return the L2 norm
- */
-GTSAM_EXPORT double norm_2(const Vector& v);
-
-/**
- * Elementwise reciprocal of vector elements
- * @param a vector
- * @return [1/a(i)]
- */
-GTSAM_EXPORT Vector reciprocal(const Vector &a);
-
-/**
- * Elementwise sqrt of vector elements
- * @param v is a vector
- * @return [sqrt(a(i))]
- */
-GTSAM_EXPORT Vector esqrt(const Vector& v);
-
-/**
- * Absolute values of vector elements
- * @param v is a vector
- * @return [abs(a(i))]
- */
-GTSAM_EXPORT Vector abs(const Vector& v);
-
-/**
- * Return the max element of a vector
- * @param a is a vector
- * @return max(a)
- */
-GTSAM_EXPORT double max(const Vector &a);
-
 /**
  * Dot product
  */
@@ -344,6 +272,21 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
  */
 GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
 
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();}
+GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
+GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
+GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
+GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();}
+GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();}
+GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
+GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
+GTSAM_EXPORT inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
+GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
+GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();}
+#endif
+
 } // namespace gtsam
 
 #include <boost/serialization/nvp.hpp>

gtsam/base/VectorSpace.h

@@ -43,7 +43,7 @@ struct VectorSpaceImpl {
       ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
     if (H1) *H1 = Jacobian::Identity();
     if (H2) *H2 = Jacobian::Identity();
-    return origin + (Class)v;
+    return origin + v;
   }
 
   /// @}
@@ -117,7 +117,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
       ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
     if (H1) *H1 = Eye(origin);
     if (H2) *H2 = Eye(origin);
-    return origin + Class(v);
+    return origin + v;
   }
 
   /// @}
@@ -159,12 +159,34 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
   /// @}
 };
 
-/// A helper that implements the traits interface for GTSAM vector space types.
+/// Requirements on type to pass it to Manifold template below
-/// To use this for your gtsam type, define:
+template<class Class>
-/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
+struct HasVectorSpacePrereqs {
+
+  enum { dim = Class::dimension };
+
+  Class p, q;
+  Vector v;
+
+  BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
+    p = Class::identity();  // identity
+    q = p + p;              // addition
+    q = p - p;              // subtraction
+    v = p.vector();         // conversion to vector
+    q = p + v;              // addition of a vector on the right
+  }
+};
+
+/// A helper that implements the traits interface for *classes* that define vector spaces
+/// To use this for your class, define:
+/// template<> struct traits<Class> : public VectorSpaceTraits<Class> {};
+/// The class needs to support the requirements defined by HasVectorSpacePrereqs above
 template<class Class>
 struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
 
+  // Check that Class has the necessary machinery
+  BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
+
   typedef vector_space_tag structure_category;
 
   /// @name Group
@@ -180,12 +202,11 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
   /// @}
 };
 
-/// Both VectorSpaceTRaits and Testable
+/// VectorSpace provides both Testable and VectorSpaceTraits
 template<class Class>
 struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
 
-/// A helper that implements the traits interface for GTSAM lie groups.
+/// A helper that implements the traits interface for scalar vector spaces. Usage:
-/// To use this for your gtsam type, define:
 /// template<> struct traits<Type> : public ScalarTraits<Type> { };
 template<typename Scalar>
 struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {

gtsam/base/VerticalBlockMatrix.h

@@ -165,24 +165,24 @@ namespace gtsam {
       return variableColOffsets_[actualBlock];
     }
 
+    /** Get the apparent first row of the underlying matrix for all operations */
+    const DenseIndex& rowStart() const { return rowStart_; }
+
     /** Get or set the apparent first row of the underlying matrix for all operations */
     DenseIndex& rowStart() { return rowStart_; }
 
+    /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
+    const DenseIndex& rowEnd() const { return rowEnd_; }
+
     /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
     DenseIndex& rowEnd() { return rowEnd_; }
 
+    /** Get the apparent first block for all operations */
+    const DenseIndex& firstBlock() const { return blockStart_; }
+
     /** Get or set the apparent first block for all operations */
     DenseIndex& firstBlock() { return blockStart_; }
 
-    /** Get the apparent first row of the underlying matrix for all operations */
-    DenseIndex rowStart() const { return rowStart_; }
-
-    /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
-    DenseIndex rowEnd() const { return rowEnd_; }
-
-    /** Get the apparent first block for all operations */
-    DenseIndex firstBlock() const { return blockStart_; }
-
     /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
     const Matrix& matrix() const { return matrix_; }
 

gtsam/base/numericalDerivative.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -131,11 +131,11 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
   typedef typename TraitsX::TangentVector TangentX;
 
   // get value at x, and corresponding chart
-  Y hx = h(x);
+  const Y hx = h(x);
 
   // Bit of a hack for now to find number of rows
-  TangentY zeroY = TraitsY::Local(hx, hx);
+  const TangentY zeroY = TraitsY::Local(hx, hx);
-  size_t m = zeroY.size();
+  const size_t m = zeroY.size();
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
   TangentX dx;
@@ -143,12 +143,12 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
 
   // Fill in Jacobian H
   Matrix H = zeros(m, N);
-  double factor = 1.0 / (2.0 * delta);
+  const double factor = 1.0 / (2.0 * delta);
   for (int j = 0; j < N; j++) {
     dx(j) = delta;
-    TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
+    const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = -delta;
-    TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
+    const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = 0;
     H.col(j) << (dy1 - dy2) * factor;
   }

gtsam/base/serializationTestHelpers.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -11,7 +11,7 @@
 
 /**
  * @file serializationTestHelpers.h
- * @brief 
+ * @brief
  * @author Alex Cunningham
  * @author Richard Roberts
  * @date Feb 7, 2012
@@ -30,6 +30,11 @@ const bool verbose = false;
 namespace gtsam {
 namespace serializationTestHelpers {
 
+// templated default object creation so we only need to declare one friend (if applicable)
+template<class T>
+T create() {
+  return T();
+}
 
 // Templated round-trip serialization
 template<class T>
@@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) {
 // This version requires equality operator
 template<class T>
 bool equality(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtrip<T>(input,output);
   return input==output;
 }
@@ -52,7 +57,7 @@ bool equality(const T& input = T()) {
 // This version requires Testable
 template<class T>
 bool equalsObj(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtrip<T>(input,output);
   return assert_equal(input, output);
 }
@@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) {
 // De-referenced version for pointers, requires equals method
 template<class T>
 bool equalsDereferenced(const T& input) {
-  T output;
+  T output = create<T>();
   roundtrip<T>(input,output);
   return input->equals(*output);
 }
@@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) {
 // This version requires equality operator
 template<class T>
 bool equalityXML(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtripXML<T>(input,output);
   return input==output;
 }
@@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) {
 // This version requires Testable
 template<class T>
 bool equalsXML(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtripXML<T>(input,output);
   return assert_equal(input, output);
 }
@@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) {
 // This version is for pointers, requires equals method
 template<class T>
 bool equalsDereferencedXML(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtripXML<T>(input,output);
   return input->equals(*output);
 }
@@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) {
 // This version requires equality operator
 template<class T>
 bool equalityBinary(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtripBinary<T>(input,output);
   return input==output;
 }
@@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) {
 // This version requires Testable
 template<class T>
 bool equalsBinary(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtripBinary<T>(input,output);
   return assert_equal(input, output);
 }
@@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) {
 // This version is for pointers, requires equals method
 template<class T>
 bool equalsDereferencedBinary(const T& input = T()) {
-  T output;
+  T output = create<T>();
   roundtripBinary<T>(input,output);
   return input->equals(*output);
 }

gtsam/base/tests/testFastContainers.cpp

@@ -22,11 +22,11 @@ using namespace gtsam;
 /* ************************************************************************* */
 TEST( testFastContainers, KeySet ) {
 
-  FastVector<Key> init_vector;
+  KeyVector init_vector;
   init_vector += 2, 3, 4, 5;
 
-  FastSet<size_t> actSet(init_vector);
+  KeySet actSet(init_vector);
-  FastSet<size_t> expSet; expSet += 2, 3, 4, 5;
+  KeySet expSet; expSet += 2, 3, 4, 5;
   EXPECT(actSet == expSet);
 }
 

gtsam/base/tests/testVector.cpp

@@ -119,36 +119,6 @@ TEST(Vector, negate )
   EXPECT(assert_equal(b, -a));
 }
 
-/* ************************************************************************* */
-TEST(Vector, sub )
-{
-  Vector a(6);
-  a(0) = 10; a(1) = 20; a(2) = 3;
-  a(3) = 34; a(4) = 11; a(5) = 2;
-
-  Vector result(sub(a,2,5));
-
-  Vector b(3);
-  b(0) = 3; b(1) = 34; b(2) =11;
-
-  EXPECT(b==result);
-  EXPECT(assert_equal(b, result));
-}
-
-/* ************************************************************************* */
-TEST(Vector, subInsert )
-{
-  Vector big = zero(6),
-       small = ones(3);
-
-  size_t i = 2;
-  subInsert(big, small, i);
-
-  Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished();
-
-  EXPECT(assert_equal(expected, big));
-}
-
 /* ************************************************************************* */
 TEST(Vector, householder )
 {
@@ -198,7 +168,7 @@ TEST(Vector, weightedPseudoinverse )
   // create sigmas
   Vector sigmas(2);
   sigmas(0) = 0.1; sigmas(1) = 0.2;
-  Vector weights = reciprocal(emul(sigmas,sigmas));
+  Vector weights = sigmas.array().square().inverse();
 
   // perform solve
   Vector actual; double precision;
@@ -224,8 +194,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
   // create sigmas
   Vector sigmas(2);
   sigmas(0) = 0.0; sigmas(1) = 0.2;
-  Vector weights = reciprocal(emul(sigmas,sigmas));
+  Vector weights = sigmas.array().square().inverse();
-
   // perform solve
   Vector actual; double precision;
   boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
@@ -244,7 +213,7 @@ TEST(Vector, weightedPseudoinverse_nan )
 {
   Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
   Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
-  Vector weights = reciprocal(emul(sigmas,sigmas));
+  Vector weights = sigmas.array().square().inverse();
   Vector pseudo; double precision;
   boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
 
@@ -253,17 +222,6 @@ TEST(Vector, weightedPseudoinverse_nan )
   DOUBLES_EQUAL(100, precision, 1e-5);
 }
 
-/* ************************************************************************* */
-TEST(Vector, ediv )
-{
-  Vector a = Vector3(10., 20., 30.);
-  Vector b = Vector3(2.0, 5.0, 6.0);
-  Vector actual(ediv(a,b));
-
-  Vector c = Vector3(5.0, 4.0, 5.0);
-  EXPECT(assert_equal(c,actual));
-}
-
 /* ************************************************************************* */
 TEST(Vector, dot )
 {
@@ -303,13 +261,6 @@ TEST(Vector, greater_than )
   EXPECT(greaterThanOrEqual(v1, v2)); // test equals
 }
 
-/* ************************************************************************* */
-TEST(Vector, reciprocal )
-{
-  Vector v = Vector3(1.0, 2.0, 4.0);
-  EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
-}
-
 /* ************************************************************************* */
 TEST(Vector, linear_dependent )
 {

gtsam/base/types.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -25,6 +25,7 @@
 #include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #include <cstddef>
+#include <cstdint>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/task_scheduler_init.h>
@@ -53,7 +54,7 @@
 namespace gtsam {
 
   /// Integer nonlinear key type
-  typedef size_t Key;
+  typedef std::uint64_t Key;
 
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;

gtsam/config.h.in

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -32,11 +32,11 @@
 #cmakedefine GTSAM_USE_QUATERNIONS
 
 // Whether GTSAM is compiled to use Pose3::EXPMAP as the default coordinates mode for Pose3's retract and localCoordinates (otherwise, Pose3::FIRST_ORDER will be used)
-#cmakedefine GTSAM_POSE3_EXPMAP 
+#cmakedefine GTSAM_POSE3_EXPMAP
 
 // Whether GTSAM is compiled to use Rot3::EXPMAP as the default coordinates mode for Rot3's retract and localCoordinates (otherwise, Pose3::CAYLEY will be used)
 #ifndef GTSAM_USE_QUATERNIONS
-	#cmakedefine GTSAM_ROT3_EXPMAP 
+	#cmakedefine GTSAM_ROT3_EXPMAP
 #endif
 
 // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
@@ -60,4 +60,11 @@
 // Option for not throwing the CheiralityException for points that are behind a camera
 #cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION
 
+// Make sure dependent projects that want it can see deprecated functions
+#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
 
+// Publish flag about Eigen typedef
+#cmakedefine GTSAM_USE_VECTOR3_POINTS
+
+// Support Metis-based nested dissection
+#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -421,16 +421,16 @@ TEST(ADT, conversion)
   ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6");
   dot(fDiscreteKey, "conversion-f1");
 
-  std::map<size_t, size_t> ordering;
+  std::map<Key, Key> keyMap;
-  ordering[0] = 5;
+  keyMap[0] = 5;
-  ordering[1] = 2;
+  keyMap[1] = 2;
 
-  AlgebraicDecisionTree<size_t> fIndexKey(fDiscreteKey, ordering);
+  AlgebraicDecisionTree<Key> fIndexKey(fDiscreteKey, keyMap);
   //  f1.print("f1");
   //  f2.print("f2");
   dot(fIndexKey, "conversion-f2");
 
-  Assignment<size_t> x00, x01, x02, x10, x11, x12;
+  Assignment<Key> x00, x01, x02, x10, x11, x12;
   x00[5] = 0, x00[2] = 0;
   x01[5] = 0, x01[2] = 1;
   x10[5] = 1, x10[2] = 0;

gtsam/geometry/Cal3_S2.h

@@ -95,6 +95,11 @@ public:
     return fy_;
   }
 
+  /// aspect ratio
+  inline double aspectRatio() const {
+    return fx_/fy_;
+  }
+
   /// skew
   inline double skew() const {
     return s_;

gtsam/geometry/CameraSet.h

@@ -220,17 +220,15 @@ public:
   static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
       const Matrix& E, const Vector& b, const double lambda = 0.0,
       bool diagonalDamping = false) {
-    SymmetricBlockMatrix augmentedHessian;
     if (E.cols() == 2) {
       Matrix2 P;
       ComputePointCovariance(P, E, lambda, diagonalDamping);
-      augmentedHessian = SchurComplement(Fblocks, E, P, b);
+      return SchurComplement(Fblocks, E, P, b);
     } else {
       Matrix3 P;
       ComputePointCovariance(P, E, lambda, diagonalDamping);
-      augmentedHessian = SchurComplement(Fblocks, E, P, b);
+      return SchurComplement(Fblocks, E, P, b);
     }
-    return augmentedHessian;
   }
 
   /**

gtsam/geometry/EssentialMatrix.cpp

@@ -12,6 +12,17 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
+                                                          OptionalJacobian<5, 3> H1,
+                                                          OptionalJacobian<5, 2> H2) {
+  if (H1)
+    *H1 << I_3x3, Matrix23::Zero();
+  if (H2)
+    *H2 << Matrix32::Zero(), I_2x2;
+  return EssentialMatrix(aRb, aTb);
+}
+
 /* ************************************************************************* */
 EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
     OptionalJacobian<5, 6> H) {
@@ -107,7 +118,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) {
   Rot3 R = E.rotation();
   Unit3 d = E.direction();
   os.precision(10);
-  os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
+  os << R.xyz().transpose() << " " << d.point3().transpose() << " ";
   return os;
 }
 

gtsam/geometry/EssentialMatrix.h

@@ -52,6 +52,11 @@ public:
       Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
   }
 
+  /// Named constructor with derivatives
+  static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
+                                                  OptionalJacobian<5, 3> H1 = boost::none,
+                                                  OptionalJacobian<5, 2> H2 = boost::none);
+
   /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
   static EssentialMatrix FromPose3(const Pose3& _1P2_,
       OptionalJacobian<5, 6> H = boost::none);

gtsam/geometry/OrientedPlane3.cpp

@@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
   Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
 
   Vector3 unit_vec = n_rotated.unitVector();
-  double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
+  double pred_d = n_.unitVector().dot(xr.translation()) + d_;
 
   if (Hr) {
     *Hr = zeros(3, 6);
@@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
     Hr->block<1, 3>(2, 3) = unit_vec;
   }
   if (Hp) {
-    Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
+    Vector2 hpp = n_.basis().transpose() * xr.translation();
     *Hp = Z_3x3;
     Hp->block<2, 2>(0, 0) = D_rotated_pose;
     Hp->block<1, 2>(2, 0) = hpp;
@@ -57,13 +57,31 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
   return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
 }
 
-
 /* ************************************************************************* */
 Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
   Vector2 n_error = -n_.localCoordinates(plane.n_);
   return Vector3(n_error(0), n_error(1), d_ - plane.d_);
 }
 
+/* ************************************************************************* */
+Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
+                                    OptionalJacobian<3, 3> H2) const {
+  Matrix22 H_n_error_this, H_n_error_other;
+  Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0,
+                                   H2 ? &H_n_error_other : 0);
+
+  double d_error = d_ - other.d_;
+
+  if (H1) {
+    *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1;
+  }
+  if (H2) {
+    *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1;
+  }
+
+  return Vector3(n_error(0), n_error(1), d_error);
+}
+
 /* ************************************************************************* */
 OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
   return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));

gtsam/geometry/OrientedPlane3.h

@@ -114,6 +114,15 @@ public:
    */
   Vector3 error(const OrientedPlane3& plane) const;
 
+  /** Computes the error between the two planes, with derivatives.
+   *  This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
+   *  Unit3::localCoordinates. This one has correct derivatives.
+   *  NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
+   * @param the other plane
+   */
+  Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
+                      OptionalJacobian<3, 3> H2 = boost::none) const;
+
   /// Dimensionality of tangent space = 3 DOF
   inline static size_t Dim() {
     return 3;

gtsam/geometry/PinholePose.h

@@ -328,6 +328,11 @@ public:
   virtual ~PinholePose() {
   }
 
+  /// return shared pointer to calibration
+  const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
+    return K_;
+  }
+
   /// return calibration
   virtual const CALIBRATION& calibration() const {
     return *K_;

gtsam/geometry/Point2.cpp

@@ -140,4 +140,10 @@ ostream &operator<<(ostream &os, const Point2& p) {
   return os;
 }
 
+/* ************************************************************************* */
+ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
+  os << p.first << " <-> " << p.second;
+  return os;
+}
+
 } // namespace gtsam

gtsam/geometry/Point2.h

@@ -50,7 +50,7 @@ public:
   /// @{
 
   /// construct from 2D vector
-  Point2(const Vector2& v) {
+  explicit Point2(const Vector2& v) {
     x_ = v(0);
     y_ = v(1);
   }
@@ -112,6 +112,11 @@ public:
   /// inverse
   inline Point2 operator- () const {return Point2(-x_,-y_);}
 
+  /// add vector on right
+  inline Point2 operator +(const Vector2& v) const {
+    return Point2(x_ + v[0], y_ + v[1]);
+  }
+
   /// add
   inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
 
@@ -194,6 +199,10 @@ private:
   /// @}
 };
 
+// Convenience typedef
+typedef std::pair<Point2, Point2> Point2Pair;
+std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
+
 // For MATLAB wrapper
 typedef std::vector<Point2> Point2Vector;
 

gtsam/geometry/Point3.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -15,66 +15,54 @@
  */
 
 #include <gtsam/geometry/Point3.h>
+#include <cmath>
 
 using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
+#ifndef GTSAM_USE_VECTOR3_POINTS
-bool Point3::equals(const Point3 & q, double tol) const {
+bool Point3::equals(const Point3 &q, double tol) const {
-  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
+  return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
-      && fabs(z_ - q.z()) < tol);
+          fabs(z() - q.z()) < tol);
 }
 
-/* ************************************************************************* */
-
 void Point3::print(const string& s) const {
   cout << s << *this << endl;
 }
 
 /* ************************************************************************* */
-
+double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
-bool Point3::operator==(const Point3& q) const {
-  return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
-}
-
-/* ************************************************************************* */
-Point3 Point3::operator+(const Point3& q) const {
-  return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
-}
-
-/* ************************************************************************* */
-Point3 Point3::operator-(const Point3& q) const {
-  return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
-}
-
-/* ************************************************************************* */
-Point3 Point3::operator*(double s) const {
-  return Point3(x_ * s, y_ * s, z_ * s);
-}
-
-/* ************************************************************************* */
-Point3 Point3::operator/(double s) const {
-  return Point3(x_ / s, y_ / s, z_ / s);
-}
-
-/* ************************************************************************* */
-double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
                         OptionalJacobian<1, 3> H2) const {
-  double d = (p2 - *this).norm();
+  return gtsam::distance(*this,q,H1,H2);
-  if (H1) {
+}
-    *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
-    *H1 = *H1 *(1. / d);
-  }
 
-  if (H2) {
+double Point3::norm(OptionalJacobian<1,3> H) const {
-    *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
+  return gtsam::norm(*this, H);
-    *H2 = *H2 *(1. / d);
+}
-  }
+
-  return d;
+Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
+  return gtsam::normalize(*this, H);
+}
+
+Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
+                     OptionalJacobian<3, 3> H2) const {
+  return gtsam::cross(*this, q, H1, H2);
+}
+
+double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
+                   OptionalJacobian<1, 3> H2) const {
+  return gtsam::dot(*this, q, H1, H2);
 }
 
 /* ************************************************************************* */
+ostream &operator<<(ostream &os, const Point3& p) {
+  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
+  return os;
+}
+
+/* ************************************************************************* */
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
 Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   if (H1) *H1 = I_3x3;
@@ -82,68 +70,66 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
   return *this + q;
 }
 
-/* ************************************************************************* */
 Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   if (H1) *H1 = I_3x3;
-  if (H2) *H2 = I_3x3;
+  if (H2) *H2 = -I_3x3;
   return *this - q;
 }
+#endif
 
+#endif
 /* ************************************************************************* */
-Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
+double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
-  if (H_p) {
+                OptionalJacobian<1, 3> H2) {
-    *H_p << skewSymmetric(-q.vector());
+  double d = (q - p1).norm();
+  if (H1) {
+    *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
+    *H1 = *H1 *(1. / d);
   }
-  if (H_q) {
+  if (H2) {
-    *H_q << skewSymmetric(vector());
+    *H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z();
+    *H2 = *H2 *(1. / d);
   }
-
+  return d;
-  return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
-      x_ * q.y_ - y_ * q.x_);
 }
 
-/* ************************************************************************* */
+double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
-double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
+  double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
-  if (H_p) {
-    *H_p << q.vector().transpose();
-  }
-  if (H_q) {
-    *H_q << vector().transpose();
-  }
-
-  return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
-}
-
-/* ************************************************************************* */
-double Point3::norm(OptionalJacobian<1,3> H) const {
-  double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
   if (H) {
     if (fabs(r) > 1e-10)
-      *H << x_ / r, y_ / r, z_ / r;
+      *H << p.x() / r, p.y() / r, p.z() / r;
     else
-      *H << 1, 1, 1; // really infinity, why 1 ?
+      *H << 1, 1, 1;  // really infinity, why 1 ?
   }
   return r;
 }
 
-/* ************************************************************************* */
+Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
-Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
+  Point3 normalized = p / norm(p);
-  Point3 normalized = *this / norm();
   if (H) {
     // 3*3 Derivative
-    double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
+    double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();
-    double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
+    double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z();
-    *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
+    *H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2;
     *H /= pow(x2 + y2 + z2, 1.5);
   }
   return normalized;
 }
 
-/* ************************************************************************* */
+Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
-ostream &operator<<(ostream &os, const Point3& p) {
+             OptionalJacobian<3, 3> H2) {
-  os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
+  if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
-  return os;
+  if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
+  return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(),
+                p.x() * q.y() - p.y() * q.x());
+}
+
+double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
+           OptionalJacobian<1, 3> H2) {
+  if (H1) *H1 << q.x(), q.y(), q.z();
+  if (H2) *H2 << p.x(), p.y(), p.z();
+  return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point3.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -21,46 +21,42 @@
 
 #pragma once
 
+#include <gtsam/config.h>
 #include <gtsam/base/VectorSpace.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 #include <boost/serialization/nvp.hpp>
-#include <cmath>
 
 namespace gtsam {
 
-  /**
+#ifdef GTSAM_USE_VECTOR3_POINTS
-   * A 3D point
+
+  /// As of GTSAM 4, in order to make GTSAM more lean,
+  /// it is now possible to just typedef Point3 to Vector3
+  typedef Vector3 Point3;
+
+#else
+
+/**
+   * A 3D point is just a Vector3 with some additional methods
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Point3 {
+class GTSAM_EXPORT Point3 : public Vector3 {
-
-  private:
-
-    double x_, y_, z_;
 
   public:
+
     enum { dimension = 3 };
 
     /// @name Standard Constructors
     /// @{
 
-    /// Default constructor creates a zero-Point3
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
-    Point3(): x_(0), y_(0), z_(0) {}
+    // Deprecated default constructor initializes to zero, in contrast to new behavior below
+    Point3() { setZero(); }
+#endif
 
-    /// Construct from x, y, and z coordinates
+    using Vector3::Vector3;
-    Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
-
-    /// @}
-    /// @name Advanced Constructors
-    /// @{
-
-    /// Construct from 3-element vector
-    explicit Point3(const Vector3& v) {
-      x_ = v(0);
-      y_ = v(1);
-      z_ = v(2);
-    }
 
     /// @}
     /// @name Testable
@@ -77,46 +73,21 @@ namespace gtsam {
     /// @{
 
     /// identity for group operation
-    inline static Point3 identity() { return Point3();}
+    inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
-
-    /// inverse
-    Point3 operator - () const { return Point3(-x_,-y_,-z_);}
-
-    /// add vector on right
-    inline Point3 operator +(const Vector3& v) const {
-      return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
-    }
-
-    /// add
-    Point3 operator + (const Point3& q) const;
-
-    /// subtract
-    Point3 operator - (const Point3& q) const;
 
     /// @}
     /// @name Vector Space
     /// @{
 
-    ///multiply with a scalar
-    Point3 operator * (double s) const;
-
-    ///divide by a scalar
-    Point3 operator / (double s) const;
-
     /** distance between two points */
     double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) const;
 
-    /** @deprecated The following function has been deprecated, use distance above */
-    inline double dist(const Point3& p2) const {
-      return (p2 - *this).norm();
-    }
-
     /** Distance of the point from the origin, with Jacobian */
     double norm(OptionalJacobian<1,3> H = boost::none) const;
 
     /** normalize, with optional Jacobian */
-    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
+    Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
 
     /** cross product @return this x q */
     Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@@ -130,34 +101,24 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
-    /// equality
+    /// return as Vector3
-    bool operator ==(const Point3& q) const;
+    const Vector3& vector() const { return *this; }
-
-    /** return vectorized form (column-wise)*/
-    Vector3 vector() const { return Vector3(x_,y_,z_); }
 
     /// get x
-    inline double x() const {return x_;}
+    inline double x() const {return (*this)[0];}
 
     /// get y
-    inline double y() const {return y_;}
+    inline double y() const {return (*this)[1];}
 
     /// get z
-    inline double z() const {return z_;}
+    inline double z() const {return (*this)[2];}
-
-    /** add two points, add(this,q) is same as this + q */
-    Point3 add (const Point3 &q,
-          OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
-
-    /** subtract two points, sub(this,q) is same as this - q */
-    Point3 sub (const Point3 &q,
-          OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
 
     /// @}
 
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
     /// @name Deprecated
     /// @{
     Point3 inverse() const { return -(*this);}
@@ -167,39 +128,62 @@ namespace gtsam {
     Point3 retract(const Vector3& v) const { return compose(Point3(v));}
     static Vector3 Logmap(const Point3& p) { return p.vector();}
     static Point3 Expmap(const Vector3& v) { return Point3(v);}
-    /// @}
+    inline double dist(const Point3& q) const { return (q - *this).norm(); }
+    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
+    Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
+               OptionalJacobian<3, 3> H2 = boost::none) const;
+    Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
+               OptionalJacobian<3, 3> H2 = boost::none) const;
+  /// @}
+#endif
 
   private:
 
-    /// @name Advanced Interface
-    /// @{
-
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
-      void serialize(ARCHIVE & ar, const unsigned int /*version*/)
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    {
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
-      ar & BOOST_SERIALIZATION_NVP(x_);
-      ar & BOOST_SERIALIZATION_NVP(y_);
-      ar & BOOST_SERIALIZATION_NVP(z_);
     }
-
-    /// @}
   };
 
-// Convenience typedef
-typedef std::pair<Point3, Point3> Point3Pair;
-std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
-
-/// Syntactic sugar for multiplying coordinates by a scalar s*p
-inline Point3 operator*(double s, const Point3& p) { return p*s;}
-
 template<>
 struct traits<Point3> : public internal::VectorSpace<Point3> {};
 
 template<>
 struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 
+#endif
+
+// Convenience typedef
+typedef std::pair<Point3, Point3> Point3Pair;
+std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+
+/// distance between two points
+double distance(const Point3& p1, const Point3& q,
+                OptionalJacobian<1, 3> H1 = boost::none,
+                OptionalJacobian<1, 3> H2 = boost::none);
+
+/// Distance of the point from the origin, with Jacobian
+double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
+
+/// normalize, with optional Jacobian
+Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
+
+/// cross product @return this x q
+Point3 cross(const Point3& p, const Point3& q,
+             OptionalJacobian<3, 3> H_p = boost::none,
+             OptionalJacobian<3, 3> H_q = boost::none);
+
+/// dot product
+double dot(const Point3& p, const Point3& q,
+           OptionalJacobian<1, 3> H_p = boost::none,
+           OptionalJacobian<1, 3> H_q = boost::none);
+
+// Convenience typedef
+typedef std::pair<Point3, Point3> Point3Pair;
+std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
+
 template <typename A1, typename A2>
 struct Range;
 
@@ -209,7 +193,7 @@ struct Range<Point3, Point3> {
   double operator()(const Point3& p, const Point3& q,
                     OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) {
-    return p.distance(q, H1, H2);
+    return distance(p, q, H1, H2);
   }
 };
 

gtsam/geometry/Pose2.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -216,7 +216,7 @@ Point2 Pose2::transform_from(const Point2& point,
   OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
   OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
   const Point2 q = r_.rotate(point, Hrotation, Hpoint);
-  if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix();
+  if (Htranslation) *Htranslation = (Hpoint ? *Hpoint : r_.matrix());
   return q + t_;
 }
 

gtsam/geometry/Pose3.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -36,6 +36,14 @@ Pose3::Pose3(const Pose2& pose2) :
         Point3(pose2.x(), pose2.y(), 0)) {
 }
 
+/* ************************************************************************* */
+Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1,
+                    OptionalJacobian<6, 3> H2) {
+  if (H1) *H1 << I_3x3, Z_3x3;
+  if (H2) *H2 << Z_3x3, R.transpose();
+  return Pose3(R, t);
+}
+
 /* ************************************************************************* */
 Pose3 Pose3::inverse() const {
   Rot3 Rt = R_.inverse();
@@ -48,8 +56,7 @@ Pose3 Pose3::inverse() const {
 // Experimental - unit tests of derivatives based on it do not check out yet
 Matrix6 Pose3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
-  const Vector3 t = t_.vector();
+  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
-  Matrix3 A = skewSymmetric(t) * R;
   Matrix6 adj;
   adj << R, Z_3x3, A, R;
   return adj;
@@ -101,12 +108,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
 void Pose3::print(const string& s) const {
   cout << s;
   R_.print("R:\n");
-  t_.print("t: ");
+  cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';";
 }
 
 /* ************************************************************************* */
 bool Pose3::equals(const Pose3& pose, double tol) const {
-  return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
+  return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
 }
 
 /* ************************************************************************* */
@@ -115,14 +122,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
   if (H) *H = ExpmapDerivative(xi);
 
   // get angular velocity omega and translational velocity v from twist xi
-  Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
+  Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
 
-  Rot3 R = Rot3::Expmap(omega.vector());
+  Rot3 R = Rot3::Expmap(omega);
   double theta2 = omega.dot(omega);
   if (theta2 > std::numeric_limits<double>::epsilon()) {
-    Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
+    Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
-    Point3 omega_cross_v = omega.cross(v);    // points towards axis
+    Vector3 omega_cross_v = omega.cross(v);    // points towards axis
-    Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
+    Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
     return Pose3(R, t);
   } else {
     return Pose3(R, v);
@@ -132,19 +139,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
 /* ************************************************************************* */
 Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
   if (H) *H = LogmapDerivative(p);
-  Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
+  const Vector3 w = Rot3::Logmap(p.rotation());
-  double t = w.norm();
+  const Vector3 T = p.translation();
+  const double t = w.norm();
   if (t < 1e-10) {
     Vector6 log;
     log << w, T;
     return log;
   } else {
-    Matrix3 W = skewSymmetric(w / t);
+    const Matrix3 W = skewSymmetric(w / t);
     // Formula from Agrawal06iros, equation (14)
     // simplified with Mathematica, and multiplying in T to avoid matrix math
-    double Tan = tan(0.5 * t);
+    const double Tan = tan(0.5 * t);
-    Vector3 WT = W * T;
+    const Vector3 WT = W * T;
-    Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
+    const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
     Vector6 log;
     log << w, u;
     return log;
@@ -178,7 +186,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
     H->topLeftCorner<3,3>() = DR;
   }
   Vector6 xi;
-  xi << omega, T.translation().vector();
+  xi << omega, T.translation();
   return xi;
 #endif
 }
@@ -193,8 +201,8 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
  *  The closed-form formula is similar to formula 102 in Barfoot14tro)
  */
 static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
-  const Vector3 w = xi.head<3>();
+  const auto w = xi.head<3>();
-  const Vector3 v = xi.tail<3>();
+  const auto v = xi.tail<3>();
   const Matrix3 V = skewSymmetric(v);
   const Matrix3 W = skewSymmetric(w);
 
@@ -260,11 +268,20 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
   return t_;
 }
 
+/* ************************************************************************* */
+
+const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const {
+  if (H) {
+    *H << I_3x3, Z_3x3;
+  }
+  return R_;
+}
+
 /* ************************************************************************* */
 Matrix4 Pose3::matrix() const {
-  static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
+  static const auto A14 = Eigen::RowVector4d(0,0,0,1);
   Matrix4 mat;
-  mat << R_.matrix(), t_.vector(), A14;
+  mat << R_.matrix(), t_, A14;
   return mat;
 }
 
@@ -275,6 +292,14 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
   return Pose3(cRv, t);
 }
 
+/* ************************************************************************* */
+Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1,
+                                                  OptionalJacobian<6, 6> H2) const {
+  if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap();
+  if (H2) *H2 = I_6x6;
+  return inverse() * pose;
+}
+
 /* ************************************************************************* */
 Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
     OptionalJacobian<3,3> Dpoint) const {
@@ -288,7 +313,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
   if (Dpoint) {
     *Dpoint = R;
   }
-  return Point3(R * p.vector()) + t_;
+  return R_ * p + t_;
 }
 
 /* ************************************************************************* */
@@ -297,7 +322,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 Rt = R_.transpose();
-  const Point3 q(Rt*(p - t_).vector());
+  const Point3 q(Rt*(p - t_));
   if (Dpose) {
     const double wx = q.x(), wy = q.y(), wz = q.z();
     (*Dpose) <<
@@ -321,7 +346,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
     return local.norm();
   } else {
     Matrix13 D_r_local;
-    const double r = local.norm(D_r_local);
+    const double r = norm(local, D_r_local);
     if (H1) *H1 = D_r_local * D_local_pose;
     if (H2) *H2 = D_r_local * D_local_point;
     return r;
@@ -355,46 +380,61 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
 }
 
 /* ************************************************************************* */
-boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
+boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
-  const size_t n = pairs.size();
+  const size_t n = abPointPairs.size();
   if (n < 3)
-    return boost::none; // we need at least three pairs
+    return boost::none;  // we need at least three pairs
 
   // calculate centroids
-  Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
+  Point3 aCentroid(0,0,0), bCentroid(0,0,0);
-  BOOST_FOREACH(const Point3Pair& pair, pairs) {
+  for(const Point3Pair& abPair: abPointPairs) {
-    cp += pair.first.vector();
+    aCentroid += abPair.first;
-    cq += pair.second.vector();
+    bCentroid += abPair.second;
   }
   double f = 1.0 / n;
-  cp *= f;
+  aCentroid *= f;
-  cq *= f;
+  bCentroid *= f;
 
   // Add to form H matrix
   Matrix3 H = Z_3x3;
-  BOOST_FOREACH(const Point3Pair& pair, pairs) {
+  for(const Point3Pair& abPair: abPointPairs) {
-    Vector3 dp = pair.first.vector() - cp;
+    Point3 da = abPair.first - aCentroid;
-    Vector3 dq = pair.second.vector() - cq;
+    Point3 db = abPair.second - bCentroid;
-    H += dp * dq.transpose();
+    H += db * da.transpose();
-  }
+    }
 
-// Compute SVD
+  // Compute SVD
-  Matrix U, V;
+  Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  Vector S;
+  Matrix U = svd.matrixU();
-  svd(H, U, S, V);
+  Vector S = svd.singularValues();
+  Matrix V = svd.matrixV();
+
+  // Check rank
+  if (S[1] < 1e-10)
+    return boost::none;
 
   // Recover transform with correction from Eggert97machinevisionandapplications
   Matrix3 UVtranspose = U * V.transpose();
   Matrix3 detWeighting = I_3x3;
   detWeighting(2, 2) = UVtranspose.determinant();
-  Rot3 R(Matrix3(V * detWeighting * U.transpose()));
+  Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
-  Point3 t = Point3(cq) - R * Point3(cp);
+  Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
-  return Pose3(R, t);
+  return Pose3(aRb, aTb);
+}
+
+boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
+  vector<Point3Pair> abPointPairs;
+  BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) {
+    abPointPairs.push_back(make_pair(baPair.second, baPair.first));
+  }
+  return Pose3::Align(abPointPairs);
 }
 
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
-  os << pose.rotation() << "\n" << pose.translation() << endl;
+  os << pose.rotation() << "\n";
+  const Point3& t = pose.translation();
+  os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
   return os;
 }
 

gtsam/geometry/Pose3.h

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -52,8 +52,7 @@ public:
   /// @{
 
   /** Default constructor is origin */
-  Pose3() {
+ Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
-  }
 
   /** Copy constructor */
   Pose3(const Pose3& pose) :
@@ -65,11 +64,6 @@ public:
       R_(R), t_(t) {
   }
 
-  /** Construct from R,t, where t \in vector3 */
-  Pose3(const Rot3& R, const Vector3& t) :
-      R_(R), t_(t) {
-  }
-
   /** Construct from Pose2 */
   explicit Pose3(const Pose2& pose2);
 
@@ -79,6 +73,19 @@ public:
           T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
   }
 
+  /// Named constructor with derivatives
+  static Pose3 Create(const Rot3& R, const Point3& t,
+                      OptionalJacobian<6, 3> H1 = boost::none,
+                      OptionalJacobian<6, 3> H2 = boost::none);
+
+  /**
+   *  Create Pose3 by aligning two point pairs
+   *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
+   *  Meant to replace the deprecated function 'align', which orders the pairs the opposite way.
+   *  Note this allows for noise on the points but in that case the mapping will not be exact.
+   */
+  static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
+
   /// @}
   /// @name Testable
   /// @{
@@ -219,9 +226,7 @@ public:
   /// @{
 
   /// get rotation
-  const Rot3& rotation() const {
+  const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const;
-    return R_;
-  }
 
   /// get translation
   const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;
@@ -244,9 +249,15 @@ public:
   /** convert to 4*4 matrix */
   Matrix4 matrix() const;
 
-  /** receives a pose in world coordinates and transforms it to local coordinates */
+  /** receives a pose in local coordinates and transforms it to world coordinates
+  * @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use
+  * transform_pose_to instead. */
   Pose3 transform_to(const Pose3& pose) const;
 
+  /** receives a pose in world coordinates and transforms it to local coordinates */
+  Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none,
+                                             OptionalJacobian<6, 6> H2 = boost::none) const;
+
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
@@ -297,7 +308,7 @@ public:
   GTSAM_EXPORT
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
 
-private:
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -323,11 +334,11 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
 }
 
 /**
- * Calculate pose between a vector of 3D point correspondences (p,q)
+ * Calculate pose between a vector of 3D point correspondences (b_point, a_point)
- * where q = Pose3::transform_from(p) = t + R*p
+ * where a_point = Pose3::transform_from(b_point) = t + R*b_point
+ * @deprecated: use Pose3::Align with point pairs ordered the opposite way
  */
-typedef std::pair<Point3, Point3> Point3Pair;
+GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs);
-GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 
 // For MATLAB wrapper
 typedef std::vector<Pose3> Pose3Vector;

gtsam/geometry/Rot3.cpp

@@ -1,6 +1,6 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -42,6 +42,58 @@ Rot3 Rot3::Random(boost::mt19937& rng) {
   return AxisAngle(axis, angle);
 }
 
+
+
+/* ************************************************************************* */
+Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
+  // if a_p is already aligned with b_p, return the identity rotation
+  if (std::abs(a_p.dot(b_p)) > 0.999999999) {
+    return Rot3();
+  }
+
+  // Check axis was not degenerate cross product
+  const Vector3 z = axis.unitVector();
+  if (z.hasNaN())
+    throw std::runtime_error("AlignSinglePair: axis has Nans");
+
+  // Now, calculate rotation that takes b_p to a_p
+  const Matrix3 P = I_3x3 - z * z.transpose();  // orthogonal projector
+  const Vector3 a_po = P * a_p.unitVector();    // point in a orthogonal to axis
+  const Vector3 b_po = P * b_p.unitVector();    // point in b orthogonal to axis
+  const Vector3 x = a_po.normalized();          // x-axis in axis-orthogonal plane, along a_p vector
+  const Vector3 y = z.cross(x);                 // y-axis in axis-orthogonal plane
+  const double u = x.dot(b_po);                 // x-coordinate for b_po
+  const double v = y.dot(b_po);                 // y-coordinate for b_po
+  double angle = std::atan2(v, u);
+  return Rot3::AxisAngle(z, -angle);
+}
+
+/* ************************************************************************* */
+Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
+                         const Unit3& a_q, const Unit3& b_q) {
+  // there are three frames in play:
+  // a: the first frame in which p and q are measured
+  // b: the second frame in which p and q are measured
+  // i: intermediate, after aligning first pair
+
+  // First, find rotation around that aligns a_p and b_p
+  Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);
+
+  // Rotate points in frame b to the intermediate frame,
+  // in which we expect the point p to be aligned now
+  Unit3 i_q = i_R_b * b_q;
+  assert(assert_equal(a_p, i_R_b * b_p, 1e-6));
+
+  // Now align second pair: we need to align i_q to a_q
+  Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
+  assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
+  assert(assert_equal(a_q, a_R_i * i_q, 1e-6));
+
+  // The desired rotation is the product of both
+  Rot3 a_R_b = a_R_i * i_R_b;
+  return a_R_b;
+}
+
 /* ************************************************************************* */
 bool Rot3::equals(const Rot3 & R, double tol) const {
   return equal_with_abs_tol(matrix(), R.matrix(), tol);
@@ -82,7 +134,7 @@ Unit3 Rot3::operator*(const Unit3& p) const {
 Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
     OptionalJacobian<3,3> H2) const {
   const Matrix3& Rt = transpose();
-  Point3 q(Rt * p.vector()); // q = Rt*p
+  Point3 q(Rt * p); // q = Rt*p
   const double wx = q.x(), wy = q.y(), wz = q.z();
   if (H1)
     *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
@@ -172,10 +224,7 @@ ostream &operator<<(ostream &os, const Rot3& R) {
 
 /* ************************************************************************* */
 Rot3 Rot3::slerp(double t, const Rot3& other) const {
-  // amazingly simple in GTSAM :-)
+  return interpolate(*this, other, t);
-  assert(t>=0 && t<=1);
-  Vector3 omega = Logmap(between(other));
-  return compose(Expmap(t * omega));
 }
 
 /* ************************************************************************* */