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

Conflicts:
	gtsam_unstable/geometry/tests/testSimilarity3.cpp

.gitignore

@@ -6,3 +6,5 @@
 /examples/Data/pose3example-rewritten.txt
 *.txt.user
 *.txt.user.6d59f0c
+/python-build/
+*.pydevproject

CMakeLists.txt

@@ -64,6 +64,8 @@ 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_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
 
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
@@ -82,6 +84,10 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
 	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled.  The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of 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.")
+endif()
+
 # Flags for choosing default packaging tools
 set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
 set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
@@ -131,7 +137,7 @@ endif()
 
 if(NOT (${Boost_VERSION} LESS 105600))
 	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
-	add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
+	add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
 endif()
 
 ###############################################################################
@@ -158,6 +164,12 @@ else()
 	set(GTSAM_USE_TBB 0)  # This will go into config.h
 endif()
 
+###############################################################################
+# Prohibit Timing build mode in combination with TBB
+if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE  STREQUAL "Timing"))
+      message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
+endif()
+
 
 ###############################################################################
 # Find Google perftools
@@ -173,6 +185,11 @@ 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")  
+    endif()
 else()
     set(GTSAM_USE_EIGEN_MKL 0)
     set(EIGEN_USE_MKL_ALL 0)
@@ -266,7 +283,8 @@ endif()
 
 # Include boost - use 'BEFORE' so that a specific boost specified to CMake
 # takes precedence over a system-installed one.
-include_directories(BEFORE ${Boost_INCLUDE_DIR})
+# Use 'SYSTEM' to ignore compiler warnings in Boost headers
+include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
 
 # Add includes for source directories 'BEFORE' boost and any system include
 # paths so that the compiler uses GTSAM headers in our source directory instead
@@ -293,10 +311,21 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   endif()
 endif()
 
+# As of XCode 7, clang also complains about this
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
+    add_definitions(-Wno-unused-local-typedefs)
+  endif()
+endif()
+
 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
 
@@ -325,6 +354,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	add_subdirectory(matlab)
 endif()
 
+# Python wrap
+if (GTSAM_BUILD_PYTHON)
+	include(GtsamPythonWrap)
+
+	# 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 
+	#       comments on the next lines
+	
+	# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
+	add_subdirectory(python)
+
+endif()
+
 # Build gtsam_unstable
 if (GTSAM_BUILD_UNSTABLE)
     add_subdirectory(gtsam_unstable)
@@ -375,6 +418,8 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
 # Print configuration variables
 message(STATUS "===============================================================")
 message(STATUS "================  Configuration Options  ======================")
+message(STATUS "  CMAKE_CXX_COMPILER_ID type     : ${CMAKE_CXX_COMPILER_ID}")
+message(STATUS "  CMAKE_CXX_COMPILER_VERSION     : ${CMAKE_CXX_COMPILER_VERSION}")
 message(STATUS "Build flags                                               ")
 print_config_flag(${GTSAM_BUILD_TESTS}                 "Build Tests                    ")
 print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS}       "Build examples with 'make all' ")
@@ -435,10 +480,22 @@ print_config_flag(${GTSAM_USE_QUATERNIONS}             "Quaternions as default R
 print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency checking   ")
 print_config_flag(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap    ")
 print_config_flag(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap   ")
+print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4}   "Deprecated in GTSAM 4 allowed  ")
 
 message(STATUS "MATLAB toolbox flags                                      ")
 print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX}      "Install matlab toolbox         ")
 print_config_flag(${GTSAM_BUILD_WRAP}                  "Build Wrap                     ")
+
+message(STATUS "Python module flags                                       ")
+
+if(GTSAM_PYTHON_WARNINGS)
+	message(STATUS "  Build python module            : No - dependencies missing")
+else()
+	print_config_flag(${GTSAM_BUILD_PYTHON}                "Build python module            ")
+endif()
+if(GTSAM_BUILD_PYTHON)
+	message(STATUS "  Python version                 : ${GTSAM_PYTHON_VERSION}")
+endif()
 message(STATUS "===============================================================")
 
 # Print warnings at the end
@@ -451,6 +508,9 @@ endif()
 if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
 	message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP.  Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
 endif()
+if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
+	message(WARNING "${GTSAM_PYTHON_WARNINGS}")
+endif()
 
 # Include CPack *after* all flags
 include(CPack)

README.md

@@ -31,6 +31,7 @@ Prerequisites:
 
 - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
+- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 
 Optional prerequisites - used automatically if findable by CMake:
 
@@ -46,6 +47,6 @@ See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
 
 GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
 
-Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
-
+
 GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

THANKS

@@ -38,6 +38,10 @@ at Uni Zurich:
 
 * Christian Forster
 
+at LAAS-CNRS
+
+* Ellon Paiva
+
 Many thanks for your hard work!!!!
 
 Frank Dellaert

cmake/FindNumPy.cmake

@@ -0,0 +1,102 @@
+# - Find the NumPy libraries
+# This module finds if NumPy is installed, and sets the following variables
+# indicating where it is.
+#
+# TODO: Update to provide the libraries and paths for linking npymath lib.
+#
+#  NUMPY_FOUND               - was NumPy found
+#  NUMPY_VERSION             - the version of NumPy found as a string
+#  NUMPY_VERSION_MAJOR       - the major version number of NumPy
+#  NUMPY_VERSION_MINOR       - the minor version number of NumPy
+#  NUMPY_VERSION_PATCH       - the patch version number of NumPy
+#  NUMPY_VERSION_DECIMAL     - e.g. version 1.6.1 is 10601
+#  NUMPY_INCLUDE_DIRS        - path to the NumPy include files
+
+#============================================================================
+# Copyright 2012 Continuum Analytics, Inc.
+#
+# MIT License
+#
+# Permission is hereby granted, free of charge, to any person obtaining
+# a copy of this software and associated documentation files
+# (the "Software"), to deal in the Software without restriction, including
+# without limitation the rights to use, copy, modify, merge, publish,
+# distribute, sublicense, and/or sell copies of the Software, and to permit
+# persons to whom the Software is furnished to do so, subject to
+# the following conditions:
+#
+# The above copyright notice and this permission notice shall be included
+# in all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+#============================================================================
+
+# Finding NumPy involves calling the Python interpreter
+if(NumPy_FIND_REQUIRED)
+    find_package(PythonInterp REQUIRED)
+else()
+    find_package(PythonInterp)
+endif()
+
+if(NOT PYTHONINTERP_FOUND)
+    set(NUMPY_FOUND FALSE)
+    return()
+endif()
+
+execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
+    "import numpy as n; print(n.__version__); print(n.get_include());"
+    RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS
+    OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT
+    ERROR_VARIABLE _NUMPY_ERROR_VALUE
+    OUTPUT_STRIP_TRAILING_WHITESPACE)
+
+if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0)
+    if(NumPy_FIND_REQUIRED)
+        message(FATAL_ERROR
+            "NumPy import failure:\n${_NUMPY_ERROR_VALUE}")
+    endif()
+    set(NUMPY_FOUND FALSE)
+    return()
+endif()
+
+# Convert the process output into a list
+string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT})
+string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES})
+# Just in case there is unexpected output from the Python command.
+list(GET _NUMPY_VALUES -2 NUMPY_VERSION)
+list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS)
+
+string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}")
+if("${_VER_CHECK}" STREQUAL "")
+    # The output from Python was unexpected. Raise an error always
+    # here, because we found NumPy, but it appears to be corrupted somehow.
+    message(FATAL_ERROR
+        "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n")
+    return()
+endif()
+
+# Make sure all directory separators are '/'
+string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS})
+
+# Get the major and minor version numbers
+string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION})
+list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR)
+list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR)
+list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH)
+string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH})
+math(EXPR NUMPY_VERSION_DECIMAL
+    "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}")
+
+find_package_message(NUMPY
+    "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}"
+    "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}")
+
+set(NUMPY_FOUND TRUE)
+

cmake/GtsamBuildTypes.cmake

@@ -34,19 +34,19 @@ if(NOT FIRST_PASS_DONE)
     set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_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 CMAKE_MODULE_LINKER_FLAGS_PROFILING)
   else()
-    set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    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(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    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(CMAKE_C_FLAGS_RELWITHDEBINFO   "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
+    set(CMAKE_C_FLAGS_RELWITHDEBINFO   "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
-    set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
+    set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
-    set(CMAKE_C_FLAGS_RELEASE          "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
+    set(CMAKE_C_FLAGS_RELEASE          "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
-    set(CMAKE_CXX_FLAGS_RELEASE        "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
+    set(CMAKE_CXX_FLAGS_RELEASE        "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
     set(CMAKE_C_FLAGS_TIMING           "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
     set(CMAKE_CXX_FLAGS_TIMING         "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
     set(CMAKE_EXE_LINKER_FLAGS_TIMING  "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
     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        "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
+    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      "-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)

cmake/GtsamPythonWrap.cmake

@@ -1,5 +1,5 @@
 #Setup cache options
-option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF)
+set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version")
 set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation")
 set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python")
 if(NOT GTSAM_PYTHON_INSTALL_PATH)
@@ -8,13 +8,13 @@ endif()
 
 #Author: Paul Furgale Modified by Andrew Melim
 function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
-  # Boost
+  # # Boost
-  find_package(Boost COMPONENTS python filesystem system REQUIRED)
+  # find_package(Boost COMPONENTS python filesystem system REQUIRED)
-  include_directories(${Boost_INCLUDE_DIRS})
+  # include_directories(${Boost_INCLUDE_DIRS})
 
-  # Find Python
+  # # Find Python
-  FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
+  # FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
-  INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
+  # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
 
   IF(APPLE)
     # The apple framework headers don't include the numpy headers for some reason.
@@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
     ENDIF()
   ENDIF(APPLE)
 
+  if(MSVC)
+    add_library(${moduleName}_python MODULE ${ARGN})
+    set_target_properties(${moduleName}_python PROPERTIES
+        OUTPUT_NAME         ${moduleName}_python
+        CLEAN_DIRECT_OUTPUT 1
+        VERSION             1
+        SOVERSION           0
+        SUFFIX              ".pyd")
+      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
 
-  # Create a static library version
+    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
-  add_library(${TARGET_NAME} SHARED ${ARGN})
+    message(${PYLIB_OUTPUT_FILE})
+    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
+    set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
 
-  target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared)
+  ELSE()
-      set_target_properties(${TARGET_NAME} PROPERTIES 
+    # Create a shared library
-          OUTPUT_NAME         ${TARGET_NAME}
+    add_library(${moduleName}_python SHARED ${generated_cpp_file})
-          CLEAN_DIRECT_OUTPUT 1
-          VERSION             1
-          SOVERSION           0)
 
+    set_target_properties(${moduleName}_python PROPERTIES
+        OUTPUT_NAME         ${moduleName}_python
+        CLEAN_DIRECT_OUTPUT 1)
+      target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
+    # On OSX and Linux, the python library must end in the extension .so. Build this
+    # filename here.
+    get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
+    set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
+    message(${PYLIB_OUTPUT_FILE})
+    get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
+    set(PYLIB_SO_NAME lib${moduleName}_python.so)
+  ENDIF(MSVC)
 
-  # On OSX and Linux, the python library must end in the extension .so. Build this
+  # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package
-  # filename here.
+  set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam)
-  get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION)
+  # Cause the library to be output in the correct directory.
-  get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
+  add_custom_command(TARGET ${moduleName}_python
-  set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so)
+    POST_BUILD
+    COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+    COMMENT "Copying library files to python directory" )
 
   # Cause the library to be output in the correct directory.
   add_custom_command(TARGET ${TARGET_NAME}
@@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
   get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES)
   list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
   set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") 
-endfunction(wrap_python)
+endfunction(wrap_python)
+
+# Macro to get list of subdirectories
+macro(SUBDIRLIST result curdir)
+  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+  set(dirlist "")
+  foreach(child ${children})
+    if(IS_DIRECTORY ${curdir}/${child})
+        list(APPEND dirlist ${child})
+    endif()
+  endforeach()
+  set(${result} ${dirlist})
+endmacro()

cmake/GtsamTesting.cmake

@@ -195,7 +195,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			add_test(NAME ${target_name} COMMAND ${target_name})
 			add_dependencies(check.${groupName} ${target_name})
 			add_dependencies(check ${target_name})
-			add_dependencies(all.tests ${script_name})
+			if(NOT XCODE_VERSION)
+				add_dependencies(all.tests ${script_name})
+			endif()
 		
 			# Add TOPSRCDIR
 			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")

doc/.gitignore

@@ -1 +1,3 @@
 /html/
+*.lyx~
+*.bib~

doc/LieGroups.lyx

@@ -76,335 +76,10 @@ Frank Dellaert
 \end_layout
 
 \begin_layout Standard
-\begin_inset Note Comment
+\begin_inset CommandInset include
-status open
+LatexCommand include
+filename "macros.lyx"
 
-\begin_layout Plain Layout
-Derivatives
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
-{\frac{\partial#1}{\partial#2}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
-{#1\biggr\rvert_{#2}}
-\end_inset
-
- 
-\begin_inset FormulaMacro
-\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
-{\at{\deriv{#1}{#2}}{#3}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status open
-
-\begin_layout Plain Layout
-Lie Groups
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\xhat}{\hat{x}}
-{\hat{x}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\yhat}{\hat{y}}
-{\hat{y}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\Ad}[1]{Ad_{#1}}
-{Ad_{#1}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
-{\mathbf{\mathop{Ad}}{}_{#1}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\define}{\stackrel{\Delta}{=}}
-{\stackrel{\Delta}{=}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\gg}{\mathfrak{g}}
-{\mathfrak{g}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\Rn}{\mathbb{R}^{n}}
-{\mathbb{R}^{n}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status open
-
-\begin_layout Plain Layout
-SO(2), 1
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
-{\mathfrak{\mathbb{R}^{2}}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\SOtwo}{SO(2)}
-{SO(2)}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\sotwo}{\mathfrak{so(2)}}
-{\mathfrak{so(2)}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\that}{\hat{\theta}}
-{\hat{\theta}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\skew}[1]{[#1]_{+}}
-{[#1]_{+}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status open
-
-\begin_layout Plain Layout
-SE(2), 3
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\SEtwo}{SE(2)}
-{SE(2)}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\setwo}{\mathfrak{se(2)}}
-{\mathfrak{se(2)}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\Skew}[1]{[#1]_{\times}}
-{[#1]_{\times}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status open
-
-\begin_layout Plain Layout
-SO(3), 3
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}}
-{\mathfrak{\mathbb{R}^{3}}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\SOthree}{SO(3)}
-{SO(3)}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\sothree}{\mathfrak{so(3)}}
-{\mathfrak{so(3)}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\what}{\hat{\omega}}
-{\hat{\omega}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status open
-
-\begin_layout Plain Layout
-SE(3),6
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}}
-{\mathfrak{\mathbb{R}^{6}}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\SEthree}{SE(3)}
-{SE(3)}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\sethree}{\mathfrak{se(3)}}
-{\mathfrak{se(3)}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\xihat}{\hat{\xi}}
-{\hat{\xi}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status open
-
-\begin_layout Plain Layout
-Aff(2),6
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\Afftwo}{Aff(2)}
-{Aff(2)}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\afftwo}{\mathfrak{aff(2)}}
-{\mathfrak{aff(2)}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\aa}{a}
-{a}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\ahat}{\hat{a}}
-{\hat{a}}
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset Note Comment
-status collapsed
-
-\begin_layout Plain Layout
-SL(3),8
-\end_layout
-
-\end_inset
-
-
-\end_layout
-
-\begin_layout Standard
-\begin_inset FormulaMacro
-\newcommand{\SLthree}{SL(3)}
-{SL(3)}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\slthree}{\mathfrak{sl(3)}}
-{\mathfrak{sl(3)}}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\hh}{h}
-{h}
-\end_inset
-
-
-\begin_inset FormulaMacro
-\newcommand{\hhat}{\hat{h}}
-{\hat{h}}
 \end_inset
 
 

doc/macros.lyx

@@ -1,42 +1,60 @@
-#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
+#LyX 2.0 created this file. For more info see http://www.lyx.org/
-\lyxformat 345
+\lyxformat 413
 \begin_document
 \begin_header
 \textclass article
 \use_default_options true
+\maintain_unincluded_children false
 \language english
+\language_package default
 \inputencoding auto
+\fontencoding global
 \font_roman default
 \font_sans default
 \font_typewriter default
 \font_default_family default
+\use_non_tex_fonts false
 \font_sc false
 \font_osf false
 \font_sf_scale 100
 \font_tt_scale 100
 
 \graphics default
+\default_output_format default
+\output_sync 0
+\bibtex_command default
+\index_command default
 \paperfontsize default
 \use_hyperref false
 \papersize default
 \use_geometry false
 \use_amsmath 1
 \use_esint 1
+\use_mhchem 1
+\use_mathdots 0
 \cite_engine basic
 \use_bibtopic false
+\use_indices false
 \paperorientation portrait
+\suppress_date false
+\use_refstyle 0
+\index Index
+\shortcut idx
+\color #008000
+\end_index
 \secnumdepth 3
 \tocdepth 3
 \paragraph_separation indent
-\defskip medskip
+\paragraph_indentation default
 \quotes_language english
 \papercolumns 1
 \papersides 1
 \paperpagestyle default
 \tracking_changes false
 \output_changes false
-\author "" 
+\html_math_output 0
-\author "" 
+\html_css_as_file 0
+\html_be_strict false
 \end_header
 
 \begin_body
@@ -62,14 +80,14 @@ Derivatives
 
 
 \begin_inset FormulaMacro
-\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
+\newcommand{\at}[1]{#1\biggr\vert_{\#2}}
-{#1\biggr\rvert_{#2}}
+{#1\biggr\vert_{\#2}}
 \end_inset
 
  
 \begin_inset FormulaMacro
 \newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
-{\at{\deriv{#1}{#2}}{#3}}
+{\at{\deriv{#1}{#2}}#3}
 \end_inset
 
 
@@ -107,6 +125,15 @@ Lie Groups
 \end_inset
 
 
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
+{\mathbf{\mathop{Ad}}{}_{#1}}
+\end_inset
+
+
 \end_layout
 
 \begin_layout Standard
@@ -144,6 +171,12 @@ SO(2)
 \end_layout
 
 \begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Rone}{\mathfrak{\mathbb{R}}}
+{\mathfrak{\mathbb{R}}}
+\end_inset
+
+
 \begin_inset FormulaMacro
 \newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
 {\mathfrak{\mathbb{R}^{2}}}
@@ -202,6 +235,12 @@ SE(2)
 \end_inset
 
 
+\begin_inset FormulaMacro
+\newcommand{\Skew}[1]{[#1]_{\times}}
+{[#1]_{\times}}
+\end_inset
+
+
 \end_layout
 
 \begin_layout Standard
@@ -243,7 +282,7 @@ SO(3)
 
 
 \begin_inset FormulaMacro
-\newcommand{\Skew}[1]{[#1]_{\times}}
+\renewcommand{\Skew}[1]{[#1]_{\times}}
 {[#1]_{\times}}
 \end_inset
 
@@ -288,6 +327,86 @@ SE(3)
 \end_inset
 
 
+\end_layout
+
+\begin_layout Standard
+\begin_inset Note Comment
+status open
+
+\begin_layout Plain Layout
+Aff(2),6
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\Afftwo}{Aff(2)}
+{Aff(2)}
+\end_inset
+
+
+\begin_inset FormulaMacro
+\newcommand{\afftwo}{\mathfrak{aff(2)}}
+{\mathfrak{aff(2)}}
+\end_inset
+
+
+\begin_inset FormulaMacro
+\newcommand{\aa}{a}
+{a}
+\end_inset
+
+
+\begin_inset FormulaMacro
+\newcommand{\ahat}{\hat{a}}
+{\hat{a}}
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Note Comment
+status collapsed
+
+\begin_layout Plain Layout
+SL(3),8
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset FormulaMacro
+\newcommand{\SLthree}{SL(3)}
+{SL(3)}
+\end_inset
+
+
+\begin_inset FormulaMacro
+\newcommand{\slthree}{\mathfrak{sl(3)}}
+{\mathfrak{sl(3)}}
+\end_inset
+
+
+\begin_inset FormulaMacro
+\newcommand{\hh}{h}
+{h}
+\end_inset
+
+
+\begin_inset FormulaMacro
+\newcommand{\hhat}{\hat{h}}
+{\hat{h}}
+\end_inset
+
+
 \end_layout
 
 \end_body

doc/math.lyx

@@ -1237,21 +1237,28 @@ reference "eq:ApproximateObjective"
 \end_inset
 
 .
- In particular, the notion of an exponential map allows us to define an
+ In particular, the notion of an exponential map allows us to define a mapping
- incremental transformation as tracing out a geodesic curve on the group
+ from 
- manifold along a certain 
 \series bold
-tangent vector
+local coordinates
 \series default
  
 \begin_inset Formula $\xi$
+\end_inset
+
+ back to a neighborhood in 
+\begin_inset Formula $G$
+\end_inset
+
+ around 
+\begin_inset Formula $a$
 \end_inset
 
 , 
 \begin_inset Formula 
-\[
+\begin{equation}
-a\oplus\xi\define a\exp\left(\hat{\xi}\right)
+a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap}
-\]
+\end{equation}
 
 \end_inset
 
@@ -1263,11 +1270,12 @@ with
 \begin_inset Formula $n$
 \end_inset
 
--dimensional Lie group, 
+-dimensional Lie group.
+ Above, 
 \begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
 \end_inset
 
- the Lie algebra element corresponding to the vector 
+ is the Lie algebra element corresponding to the vector 
 \begin_inset Formula $\xi$
 \end_inset
 
@@ -1305,7 +1313,7 @@ For the Lie group
 \end_inset
 
  is denoted as 
-\begin_inset Formula $\omega$
+\begin_inset Formula $\omega t$
 \end_inset
 
  and represents an angular displacement.
@@ -1314,17 +1322,17 @@ For the Lie group
 \end_inset
 
  is a skew symmetric matrix denoted as 
-\begin_inset Formula $\Skew{\omega}\in\sothree$
+\begin_inset Formula $\Skew{\omega t}\in\sothree$
 \end_inset
 
 , and is given by
 \begin_inset Formula 
 \[
-\Skew{\omega}=\left[\begin{array}{ccc}
+\Skew{\omega t}=\left[\begin{array}{ccc}
 0 & -\omega_{z} & \omega_{y}\\
 \omega_{z} & 0 & -\omega_{x}\\
 -\omega_{y} & \omega_{x} & 0
-\end{array}\right]
+\end{array}\right]t
 \]
 
 \end_inset
@@ -1334,12 +1342,136 @@ Finally, the increment
 \end_inset
 
  corresponds to an incremental rotation 
-\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$
+\begin_inset Formula $R\oplus\omega t=Re^{\Skew{\omega t}}$
 \end_inset
 
 .
 \end_layout
 
+\begin_layout Subsection
+Local Coordinates vs.
+ Tangent Vectors
+\end_layout
+
+\begin_layout Standard
+In differential geometry, 
+\series bold
+tangent vectors
+\series default
+ 
+\begin_inset Formula $v\in T_{a}G$
+\end_inset
+
+ at 
+\begin_inset Formula $a$
+\end_inset
+
+ are elements of the Lie algebra 
+\begin_inset Formula $\mathfrak{g}$
+\end_inset
+
+, and are defined as
+\begin_inset Formula 
+\[
+v\define\Jac{\gamma(t)}t{t=0}
+\]
+
+\end_inset
+
+where 
+\begin_inset Formula $\gamma$
+\end_inset
+
+ is some curve that passes through 
+\begin_inset Formula $a$
+\end_inset
+
+ at 
+\begin_inset Formula $t=0$
+\end_inset
+
+, i.e.
+ 
+\begin_inset Formula $\gamma(0)=a$
+\end_inset
+
+.
+ In particular, for any fixed local coordinate 
+\begin_inset Formula $\xi$
+\end_inset
+
+ the map 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:expmap"
+
+\end_inset
+
+ can be used to define a 
+\series bold
+geodesic curve
+\series default
+ on the group manifold defined by 
+\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
+\end_inset
+
+, and the corresponding tangent vector is given by 
+\begin_inset Formula 
+\begin{equation}
+\Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector}
+\end{equation}
+
+\end_inset
+
+This defines the mapping between local coordinates 
+\begin_inset Formula $\xi\in\Rn$
+\end_inset
+
+ and actual tangent vectors 
+\begin_inset Formula $a\xihat\in g$
+\end_inset
+
+: the vector 
+\begin_inset Formula $\xi$
+\end_inset
+
+ defines a direction of travel on the manifold, but does so in the local
+ coordinate frame 
+\begin_inset Formula $a$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Example
+Assume a rigid body's attitude is described by 
+\begin_inset Formula $R_{b}^{n}(t)$
+\end_inset
+
+, where the indices denote the navigation frame 
+\begin_inset Formula $N$
+\end_inset
+
+ and body frame 
+\begin_inset Formula $B$
+\end_inset
+
+, respectively.
+ An extrinsically calibrated gyroscope measures the angular velocity 
+\begin_inset Formula $\omega^{b}$
+\end_inset
+
+, in the body frame, and the corresponding tangent vector is 
+\begin_inset Formula 
+\[
+\dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}}
+\]
+
+\end_inset
+
+ 
+\end_layout
+
 \begin_layout Subsection
 Derivatives
 \end_layout
@@ -1352,7 +1484,7 @@ reference "def:differentiable"
 
 \end_inset
 
- to map exponential coordinates 
+ to map local coordinates 
 \begin_inset Formula $\xi$
 \end_inset
 
@@ -1368,7 +1500,7 @@ reference "def:differentiable"
 \begin_inset Formula $Df_{a}$
 \end_inset
 
- locally approximates a function 
+ approximates the function 
 \begin_inset Formula $f$
 \end_inset
 
@@ -1378,6 +1510,10 @@ reference "def:differentiable"
 
  to 
 \begin_inset Formula $\Reals m$
+\end_inset
+
+ in a neighborhood around 
+\begin_inset Formula $a$
 \end_inset
 
 :
@@ -1455,41 +1591,6 @@ derivative
 .
 \end_layout
 
-\begin_layout Standard
-Note that the vectors 
-\begin_inset Formula $\xi$
-\end_inset
-
- can be viewed as lying in the tangent space to 
-\begin_inset Formula $G$
-\end_inset
-
- at 
-\begin_inset Formula $a$
-\end_inset
-
-, but defining this rigorously would take us on a longer tour of differential
- geometry.
- Informally, 
-\begin_inset Formula $\xi$
-\end_inset
-
- is simply the direction, in a local coordinate frame, that is locally tangent
- at 
-\begin_inset Formula $a$
-\end_inset
-
- to a geodesic curve 
-\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
-\end_inset
-
- traced out by the exponential map, with 
-\begin_inset Formula $\gamma(0)=a$
-\end_inset
-
-.
-\end_layout
-
 \begin_layout Subsection
 Derivative of an Action
 \begin_inset CommandInset label
@@ -3000,7 +3101,7 @@ f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g)
 \end_layout
 
 \begin_layout Subsection
-Derivative of the Exponential and Logarithm Map
+Derivative of the Exponential Map
 \end_layout
 
 \begin_layout Theorem
@@ -3064,17 +3165,196 @@ For
 \begin_inset Formula $\xi\neq0$
 \end_inset
 
-, things are not simple, see .
+, things are not simple.
+ As with pushforwards above, we will be looking for an 
+\begin_inset Formula $n\times n$
+\end_inset
+
  
-\begin_inset Flex URL
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\uuline off
+\uwave off
+\noun off
+\color none
+Jacobian 
+\begin_inset Formula $f'(\xi)$
+\end_inset
+
+ such that
+\family default
+\series default
+\shape default
+\size default
+\emph default
+\bar default
+\strikeout default
+\uuline default
+\uwave default
+\noun default
+\color inherit
+
+\begin_inset Formula 
+\begin{equation}
+f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp}
+\end{equation}
+
+\end_inset
+
+Differential geometry tells us that for any Lie algebra element 
+\begin_inset Formula $\xihat\in\mathfrak{g}$
+\end_inset
+
+ there exists a 
+\emph on
+linear
+\emph default
+ map 
+\begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$
+\end_inset
+
+, which is given by
+\begin_inset Foot
 status collapsed
 
 \begin_layout Plain Layout
+See 
+\begin_inset Flex URL
+status open
 
-http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti
+\begin_layout Plain Layout
-al-of-the-exponential/
+
+http://deltaepsilons.wordpress.com/2009/11/06/
 \end_layout
 
+\end_inset
+
+ or 
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map
+\end_layout
+
+\end_inset
+
+.
+\end_layout
+
+\end_inset
+
+
+\begin_inset Formula 
+\begin{equation}
+d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp}
+\end{equation}
+
+\end_inset
+
+with 
+\begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$
+\end_inset
+
+ and 
+\begin_inset Formula $ad_{\xihat}$
+\end_inset
+
+ itself a linear map taking 
+\begin_inset Formula $\hat{x}$
+\end_inset
+
+ to 
+\begin_inset Formula $[\xihat,\hat{x}]$
+\end_inset
+
+, the Lie bracket.
+ The actual formula above is not really as important as the fact that the
+ linear map exists, although it is expressed directly in terms of tangent
+ vectors to 
+\begin_inset Formula $\mathfrak{g}$
+\end_inset
+
+ and 
+\begin_inset Formula $G$
+\end_inset
+
+.
+ Equation 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:dexp"
+
+\end_inset
+
+ is a tangent vector, and comparing with 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:tangent-vector"
+
+\end_inset
+
+ we see that it maps to local coordinates 
+\begin_inset Formula $y$
+\end_inset
+
+ as follows:
+\begin_inset Formula 
+\[
+\yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}
+\]
+
+\end_inset
+
+which can be used to construct the Jacobian 
+\begin_inset Formula $f'(\xi)$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Example
+For 
+\begin_inset Formula $\SOthree$
+\end_inset
+
+, the operator 
+\begin_inset Formula $ad_{\xihat}$
+\end_inset
+
+ is simply a matrix multiplication when representing 
+\begin_inset Formula $\sothree$
+\end_inset
+
+ using 3-vectors, i.e., 
+\begin_inset Formula $ad_{\xihat}x=\xihat x$
+\end_inset
+
+, and the 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ Jacobian corresponding to 
+\begin_inset Formula $d\exp$
+\end_inset
+
+ is 
+\begin_inset Formula 
+\[
+f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k}
+\]
+
+\end_inset
+
+which, similar to the exponential map, has a simple closed form expression
+ for 
+\begin_inset Formula $\SOthree$
 \end_inset
 
 .
@@ -3097,7 +3377,7 @@ Retractions
 
 \begin_layout Standard
 \begin_inset FormulaMacro
-\newcommand{\retract}{\mathcal{R}}
+\renewcommand{\retract}{\mathcal{R}}
 {\mathcal{R}}
 \end_inset
 
@@ -6797,7 +7077,7 @@ Then
 \begin_layout Standard
 \begin_inset CommandInset bibtex
 LatexCommand bibtex
-bibfiles "/Users/dellaert/papers/refs"
+bibfiles "refs"
 options "plain"
 
 \end_inset

doc/refs.bib

@@ -0,0 +1,26 @@
+@article{Iserles00an,
+  title =	 {Lie-group methods},
+  author =	 {Iserles, Arieh and Munthe-Kaas, Hans Z and
+                  N{\o}rsett, Syvert P and Zanna, Antonella},
+  journal =	 {Acta Numerica 2000},
+  volume =	 {9},
+  pages =	 {215--365},
+  year =	 {2000},
+  publisher =	 {Cambridge Univ Press}
+}
+
+@book{Murray94book,
+  title =	 {A mathematical introduction to robotic manipulation},
+  author =	 {Murray, Richard M and Li, Zexiang and Sastry, S
+                  Shankar and Sastry, S Shankara},
+  year =	 {1994},
+  publisher =	 {CRC press}
+}
+
+@book{Spivak65book,
+  title =	 {Calculus on manifolds},
+  author =	 {Spivak, Michael},
+  volume =	 {1},
+  year =	 {1965},
+  publisher =	 {WA Benjamin New York}
+}

examples/CreateSFMExampleData.cpp

@@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
   SfM_data data;
 
   // Create two cameras
-  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 identity, aPb(aRb, aTb);
   data.cameras.push_back(SfM_Camera(pose1, K));
@@ -66,7 +66,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 void create5PointExample1() {
 
   // Create two cameras poses
-  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
@@ -85,7 +85,7 @@ void create5PointExample1() {
 void create5PointExample2() {
 
   // Create two cameras poses
-  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(10, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 

examples/Data/.gitignore

@@ -0,0 +1 @@
+*.txt

examples/PlanarSLAMExample.cpp

@@ -42,7 +42,7 @@
 // Also, we will initialize the robot at the origin using a Prior factor.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 
 // When the factors are created, we will add them to a Factor Graph. As the factors we are using
 // are nonlinear factors, we will need a Nonlinear Factor Graph.

examples/RangeISAMExample_plaza2.cpp

@@ -39,7 +39,7 @@
 // have been provided with the library for solving robotics SLAM problems.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/slam/dataset.h>
 
 // Standard headers, added last, so we know headers above work on their own

examples/SFMExample.cpp

@@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
   // Intentionally initialize the variables off from the ground truth
   Values initialEstimate;
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    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.print("Initial Estimates:\n");

examples/SFMExampleExpressions.cpp

@@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
 
   // Create perturbed initial
   Values initial;
-  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
     initial.insert(Symbol('x', i), poses[i].compose(delta));
   for (size_t j = 0; j < points.size(); ++j)

examples/SFMExample_SmartFactor.cpp

@@ -34,6 +34,9 @@ using namespace gtsam;
 // Make the typename short so it looks much cleaner
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
+// create a typedef to the camera type
+typedef PinholePose<Cal3_S2> Camera;
+
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
 
@@ -55,12 +58,12 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j) {
 
     // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
-    SmartFactor::shared_ptr smartfactor(new SmartFactor());
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
 
       // generate the 2D measurement
-      SimpleCamera camera(poses[i], *K);
+      Camera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
 
       // call add() function to add measurement into a single factor, here we need to add:
@@ -68,7 +71,7 @@ int main(int argc, char* argv[]) {
       //    2. the corresponding camera's key
       //    3. camera noise model
       //    4. camera calibration
-      smartfactor->add(measurement, i, measurementNoise, K);
+      smartfactor->add(measurement, i);
     }
 
     // insert the smart factor in the graph
@@ -77,24 +80,24 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is
   // still under-constrained. Here we add a prior on the second pose x1, so this will
   // fix the scale by indicating the distance between x0 and x1.
   // Because these two are fixed, the rest of the poses will be also be fixed.
-  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
+  graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
 
   graph.print("Factor Graph:\n");
 
   // Create the initial estimate to the solution
   // Intentionally initialize the variables off from the ground truth
   Values initialEstimate;
-  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(i, poses[i].compose(delta));
+    initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
   initialEstimate.print("Initial Estimates:\n");
 
   // Optimize the graph and print results

examples/SFMExample_SmartFactorPCG.cpp

@@ -30,6 +30,9 @@ using namespace gtsam;
 // Make the typename short so it looks much cleaner
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
+// create a typedef to the camera type
+typedef PinholePose<Cal3_S2> Camera;
+
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
 
@@ -51,16 +54,16 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j) {
 
     // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
-    SmartFactor::shared_ptr smartfactor(new SmartFactor());
+    SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
 
       // generate the 2D measurement
-      SimpleCamera camera(poses[i], *K);
+      Camera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
 
       // call add() function to add measurement into a single factor, here we need to add:
-      smartfactor->add(measurement, i, measurementNoise, K);
+      smartfactor->add(measurement, i);
     }
 
     // insert the smart factor in the graph
@@ -69,18 +72,18 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
-  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+  graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
 
   // Fix the scale ambiguity by adding a prior
-  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
+  graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
 
   // Create the initial estimate to the solution
   Values initialEstimate;
-  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(i, poses[i].compose(delta));
+    initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
 
   // We will use LM in the outer optimization loop, but by specifying "Iterative" below
   // We indicate that an iterative linear solver should be used.

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -0,0 +1,144 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SFMExample.cpp
+ * @brief   This file is to compare the ordering performance for COLAMD vs METIS.
+ * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
+ * @author  Frank Dellaert, Zhaoyang Lv
+ */
+
+// For an explanation of headers, see SFMExample.cpp
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+
+#include <gtsam/base/timing.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::C;
+using symbol_shorthand::P;
+
+// We will be using a projection factor that ties a SFM_Camera to a 3D point.
+// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// and has a total of 9 free parameters
+typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
+
+/* ************************************************************************* */
+int main (int argc, char* argv[]) {
+
+  // Find default file, but if an argument is given, try loading a file
+  string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  if (argc>1) filename = string(argv[1]);
+
+  // Load the SfM data from file
+  SfM_data mydata;
+  readBAL(filename, mydata);
+  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+
+  // Create a factor graph
+  NonlinearFactorGraph graph;
+
+  // We share *one* noiseModel between all projection factors
+  noiseModel::Isotropic::shared_ptr noise =
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+
+  // Add measurements to the factor graph
+  size_t j = 0;
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
+    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
+      size_t i = m.first;
+      Point2 uv = m.second;
+      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
+    }
+    j += 1;
+  }
+
+  // Add a prior on pose x1. This indirectly specifies where the origin is.
+  // and a prior on the position of the first landmark to fix the scale
+  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
+  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
+
+  // Create initial estimate
+  Values initial;
+  size_t i = 0; j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+
+  /** ---------------  COMPARISON  -----------------------**/
+  /** ----------------------------------------------------**/
+
+  LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
+  try {
+    params_using_METIS.setVerbosity("ERROR");
+    gttic_(METIS_ORDERING);
+    params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
+    gttoc_(METIS_ORDERING);
+
+    params_using_COLAMD.setVerbosity("ERROR");
+    gttic_(COLAMD_ORDERING);
+    params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
+    gttoc_(COLAMD_ORDERING);
+  } catch (exception& e) {
+    cout << e.what();
+  }
+
+  // expect they have different ordering results
+  if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
+    cout << "COLAMD and METIS produce the same ordering. "
+         << "Problem here!!!" << endl;
+  }
+
+  /* Optimize the graph with METIS and COLAMD and time the results */
+
+  Values result_METIS, result_COLAMD;
+  try {
+    gttic_(OPTIMIZE_WITH_METIS);
+    LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
+    result_METIS = lm_METIS.optimize();
+    gttoc_(OPTIMIZE_WITH_METIS);
+
+    gttic_(OPTIMIZE_WITH_COLAMD);
+    LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
+    result_COLAMD = lm_COLAMD.optimize();
+    gttoc_(OPTIMIZE_WITH_COLAMD);
+  } catch (exception& e) {
+    cout << e.what();
+  }
+
+
+  { // printing the result
+
+    cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
+    cout << "METIS final error: " << graph.error(result_METIS) << endl;
+
+    cout << endl << endl;
+
+    cout << "Time comparison by solving " << filename << " results:" << endl;
+    cout << boost::format("%1% point tracks and %2% cameras\n") \
+            % mydata.number_tracks() % mydata.number_cameras() \
+         << endl;
+
+    tictoc_print_();
+  }
+
+
+  return 0;
+}
+/* ************************************************************************* */
+

examples/SelfCalibrationExample.cpp

@@ -82,7 +82,7 @@ int main(int argc, char* argv[]) {
   Values initialEstimate;
   initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    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)));
 

examples/SolverComparer.cpp

@@ -32,7 +32,7 @@
 */
 
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose2.h>
@@ -44,6 +44,7 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/archive/binary_oarchive.hpp>
@@ -575,7 +576,7 @@ void runStats()
 {
   cout << "Gathering statistics..." << endl;
   GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
-  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
+  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
   treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
 
   ofstream file;

examples/TimeTBB.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/global_includes.h>
 #include <gtsam/base/Matrix.h>
+
 #include <boost/assign/list_of.hpp>
 #include <boost/foreach.hpp>
 #include <map>

examples/VisualISAM2Example.cpp

@@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
 
     // Add an initial guess for the current pose
     // Intentionally initialize the variables off from the ground truth
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
 
     // If this is the first iteration, add a prior on the first pose to set the coordinate frame
     // and a prior on the first landmark to set the scale

examples/VisualISAMExample.cpp

@@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
     }
 
     // Intentionally initialize the variables off from the ground truth
-    Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+    Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
     Pose3 initial_xi = poses[i].compose(noise);
 
     // Add an initial guess for the current pose

gtsam.h

@@ -1,4 +1,5 @@
 /**
+
  * GTSAM Wrap Module Definition
  *
  * These are the current classes available through the matlab toolbox interface,
@@ -156,7 +157,7 @@ virtual class Value {
   size_t dim() const;
 };
 
-#include <gtsam/base/LieScalar_Deprecated.h>
+#include <gtsam/base/deprecated/LieScalar.h>
 class LieScalar {
   // Standard constructors
   LieScalar();
@@ -185,7 +186,7 @@ class LieScalar {
   static Vector Logmap(const gtsam::LieScalar& p);
 };
 
-#include <gtsam/base/LieVector_Deprecated.h>
+#include <gtsam/base/deprecated/LieVector.h>
 class LieVector {
   // Standard constructors
   LieVector();
@@ -217,7 +218,7 @@ class LieVector {
   void serialize() const;
 };
 
-#include <gtsam/base/LieMatrix_Deprecated.h>
+#include <gtsam/base/deprecated/LieMatrix.h>
 class LieMatrix {
   // Standard constructors
   LieMatrix();
@@ -433,12 +434,12 @@ class Rot3 {
   static gtsam::Rot3 Rz(double t);
   static gtsam::Rot3 RzRyRx(double x, double y, double z);
   static gtsam::Rot3 RzRyRx(Vector xyz);
-  static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading)
+  static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading)
-  static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude)
+  static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude)
-  static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft)
+  static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
-  static gtsam::Rot3 ypr(double y, double p, double r);
+  static gtsam::Rot3 Ypr(double y, double p, double r);
-  static gtsam::Rot3 quaternion(double w, double x, double y, double z);
+  static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
-  static gtsam::Rot3 rodriguez(Vector v);
+  static gtsam::Rot3 Rodrigues(Vector v);
 
   // Testable
   void print(string s) const;
@@ -812,7 +813,7 @@ class CalibratedCamera {
 
   // Action on Point3
   gtsam::Point2 project(const gtsam::Point3& point) const;
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
 
   // Standard Interface
   gtsam::Pose3 pose() const;
@@ -848,7 +849,7 @@ class PinholeCamera {
   static size_t Dim();
 
   // Transformations and measurement functions
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@@ -884,7 +885,7 @@ virtual class SimpleCamera {
   static size_t Dim();
 
   // Transformations and measurement functions
-  static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
@@ -915,7 +916,7 @@ class StereoCamera {
   // Standard Interface
   gtsam::Pose3 pose() const;
   double baseline() const;
-  gtsam::Cal3_S2Stereo* calibration() const;
+  gtsam::Cal3_S2Stereo calibration() const;
 
   // Manifold
   gtsam::StereoCamera retract(const Vector& d) const;
@@ -1654,12 +1655,12 @@ char symbolChr(size_t key);
 size_t symbolIndex(size_t key);
 
 // Default keyformatter
-void printKeyList  (const gtsam::KeyList& keys);
+void PrintKeyList  (const gtsam::KeyList& keys);
-void printKeyList  (const gtsam::KeyList& keys, string s);
+void PrintKeyList  (const gtsam::KeyList& keys, string s);
-void printKeyVector(const gtsam::KeyVector& keys);
+void PrintKeyVector(const gtsam::KeyVector& keys);
-void printKeyVector(const gtsam::KeyVector& keys, string s);
+void PrintKeyVector(const gtsam::KeyVector& keys, string s);
-void printKeySet   (const gtsam::KeySet& keys);
+void PrintKeySet   (const gtsam::KeySet& keys);
-void printKeySet   (const gtsam::KeySet& keys, string s);
+void PrintKeySet   (const gtsam::KeySet& keys, string s);
 
 #include <gtsam/inference/LabeledSymbol.h>
 class LabeledSymbol {
@@ -1776,7 +1777,7 @@ class Values {
   void swap(gtsam::Values& values);
 
   bool exists(size_t j) const;
-  gtsam::KeyList keys() const;
+  gtsam::KeyVector keys() const;
 
   gtsam::VectorValues zeroVectors() const;
 
@@ -1893,8 +1894,6 @@ class KeySet {
 class KeyVector {
   KeyVector();
   KeyVector(const gtsam::KeyVector& other);
-  KeyVector(const gtsam::KeySet& other);
-  KeyVector(const gtsam::KeyList& other);
 
   // Note: no print function
 
@@ -2264,7 +2263,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
 };
 
 
-#include <gtsam/slam/RangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
@@ -2274,20 +2273,16 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
-
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-// Commented out by Frank Dec 2014: not poses!
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
-// If needed, we need a RangeFactor that takes a camera, extracts the pose
+typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-// Should be easy with Expressions
+typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
-//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
-//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
-//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
-//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
 
 
-#include <gtsam/slam/BearingFactor.h>
+#include <gtsam/sam/BearingFactor.h>
-template<POSE, POINT, ROTATION>
+template<POSE, POINT, BEARING>
 virtual class BearingFactor : gtsam::NoiseModelFactor {
-  BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
+  BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
 
   // enabling serialization functionality
   void serialize() const;
@@ -2295,19 +2290,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
 
-
+#include <gtsam/sam/BearingRangeFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
+template<POSE, POINT, BEARING, RANGE>
-template<POSE, POINT, ROTATION>
 virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
-  BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
+  BearingRangeFactor(size_t poseKey, size_t pointKey,
-
+      const BEARING& measuredBearing, const RANGE& measuredRange,
-  pair<ROTATION, double> measured() const;
+      const gtsam::noiseModel::Base* noiseModel);
 
   // enabling serialization functionality
   void serialize() const;
 };
 
-typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
 
 
 #include <gtsam/slam/ProjectionFactor.h>
@@ -2354,18 +2348,33 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/SmartProjectionFactor.h>
+class SmartProjectionParams {
+  SmartProjectionParams();
+  // TODO(frank): make these work:
+  //  void setLinearizationMode(LinearizationMode linMode);
+  //  void setDegeneracyMode(DegeneracyMode degMode);
+  void setRankTolerance(double rankTol);
+  void setEnableEPI(bool enableEPI);
+  void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
+  void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
+};
+
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
 template<CALIBRATION>
-virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
+virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
 
-  SmartProjectionPoseFactor(double rankTol, double linThreshold,
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
-      bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
+      const CALIBRATION* K);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
+      const gtsam::Pose3& body_P_sensor);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
+      const gtsam::Pose3& body_P_sensor,
+      const gtsam::SmartProjectionParams& params);
 
-  SmartProjectionPoseFactor(double rankTol);
+  void add(const gtsam::Point2& measured_i, size_t poseKey_i);
-  SmartProjectionPoseFactor();
-
-  void add(const gtsam::Point2& measured_i, size_t poseKey_i,
-      const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
 
   // enabling serialization functionality
   //void serialize() const;
@@ -2438,7 +2447,7 @@ namespace imuBias {
 #include <gtsam/navigation/ImuBias.h>
 
 class ConstantBias {
-  // Standard Constructor
+  // Constructors
   ConstantBias();
   ConstantBias(Vector biasAcc, Vector biasGyro);
 
@@ -2470,136 +2479,143 @@ class ConstantBias {
 
 }///\namespace imuBias
 
-#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/NavState.h>
-class PoseVelocityBias{
+class NavState {
-    PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
+  // Constructors
-  };
+  NavState();
-class ImuFactorPreintegratedMeasurements {
+  NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
-  // Standard Constructor
+  NavState(const gtsam::Pose3& pose, Vector v);
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
-  // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
+  bool equals(const gtsam::NavState& expected, double tol) const;
+
+  // Access
+  gtsam::Rot3 attitude() const;
+  gtsam::Point3 position() const;
+  Vector velocity() const;
+  gtsam::Pose3 pose() const;
+};
+
+#include <gtsam/navigation/PreintegrationParams.h>
+class PreintegrationParams {
+  PreintegrationParams(Vector n_gravity);
+  // TODO(frank): add setters/getters or make this MATLAB wrapper feature
+};
+
+#include <gtsam/navigation/PreintegrationBase.h>
+virtual class PreintegrationBase {
+  // Constructors
+  PreintegrationBase(const gtsam::PreintegrationParams* params);
+  PreintegrationBase(const gtsam::PreintegrationParams* params,
+      const gtsam::imuBias::ConstantBias& bias);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::PreintegrationBase& expected, double tol);
 
   double deltaTij() const;
-  Matrix deltaRij() const;
+  gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;
   Vector deltaVij() const;
   Vector biasHatVector() const;
-  Matrix delPdelBiasAcc() const;
-  Matrix delPdelBiasOmega() const;
-  Matrix delVdelBiasAcc() const;
-  Matrix delVdelBiasOmega() const;
-  Matrix delRdelBiasOmega() const;
-  Matrix preintMeasCov() const;
 
   // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
+  gtsam::NavState predict(const gtsam::NavState& state_i,
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+      const gtsam::imuBias::ConstantBias& bias) const;
-  gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
-      Vector gravity, Vector omegaCoriolis) const;
 };
 
-virtual class ImuFactor : gtsam::NonlinearFactor {
+#include <gtsam/navigation/ImuFactor.h>
-  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
+virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
+  // Constructors
-  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
+  PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
-      const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
+  PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
-      const gtsam::Pose3& body_P_sensor);
+      const gtsam::imuBias::ConstantBias& bias);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
+
   // Standard Interface
-  gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
+      double deltaT);
+  void resetIntegration();
+  Matrix preintMeasCov() const;
+};
+
+virtual class ImuFactor: gtsam::NonlinearFactor {
+  ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
+      size_t bias,
+      const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
+
+  // Standard Interface
+  gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
+  Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
+      const gtsam::Pose3& pose_j, Vector vel_j,
+      const gtsam::imuBias::ConstantBias& bias);
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
-class CombinedImuFactorPreintegratedMeasurements {
+virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
-  // Standard Constructor
-  CombinedImuFactorPreintegratedMeasurements(
-      const gtsam::imuBias::ConstantBias& bias,
-      Matrix measuredAccCovariance,
-      Matrix measuredOmegaCovariance,
-      Matrix integrationErrorCovariance,
-      Matrix biasAccCovariance,
-      Matrix biasOmegaCovariance,
-      Matrix biasAccOmegaInit);
-  CombinedImuFactorPreintegratedMeasurements(
-      const gtsam::imuBias::ConstantBias& bias,
-      Matrix measuredAccCovariance,
-      Matrix measuredOmegaCovariance,
-      Matrix integrationErrorCovariance,
-      Matrix biasAccCovariance,
-      Matrix biasOmegaCovariance,
-      Matrix biasAccOmegaInit,
-      bool use2ndOrderIntegration);
-  // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
-
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
+  bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
-
+      double tol);
-  double deltaTij() const;
-  Matrix deltaRij() const;
-  Vector deltaPij() const;
-  Vector deltaVij() const;
-  Vector biasHatVector() const;
-  Matrix delPdelBiasAcc() const;
-  Matrix delPdelBiasOmega() const;
-  Matrix delVdelBiasAcc() const;
-  Matrix delVdelBiasOmega() const;
-  Matrix delRdelBiasOmega() const;
-  Matrix preintMeasCov() const;
 
   // Standard Interface
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
+  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
-  void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+      double deltaT);
-  gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
+  void resetIntegration();
-      Vector gravity, Vector omegaCoriolis) const;
+  Matrix preintMeasCov() const;
 };
 
-virtual class CombinedImuFactor : gtsam::NonlinearFactor {
+virtual class CombinedImuFactor: gtsam::NonlinearFactor {
-  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
+  CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
+      size_t bias_i, size_t bias_j,
+      const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
+
   // Standard Interface
-  gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
+  Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
+      const gtsam::Pose3& pose_j, Vector vel_j,
+      const gtsam::imuBias::ConstantBias& bias_i,
+      const gtsam::imuBias::ConstantBias& bias_j);
 };
 
 #include <gtsam/navigation/AHRSFactor.h>
-class AHRSFactorPreintegratedMeasurements {
+class PreintegratedAhrsMeasurements {
   // Standard Constructor
-  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
-  AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
+  PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
-  AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
+  PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
+  bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
 
   // get Data
-  Matrix deltaRij() const;
+  gtsam::Rot3 deltaRij() const;
   double deltaTij() const;
   Vector biasHat() const;
 
   // Standard Interface
   void integrateMeasurement(Vector measuredOmega, double deltaT);
-  void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
   void resetIntegration() ;
 };
 
 virtual class AHRSFactor : gtsam::NonlinearFactor {
   AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
-      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
   AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
-      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
       const gtsam::Pose3& body_P_sensor);
 
   // Standard Interface
-  gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
+  gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
   Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
       Vector bias) const;
   gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
-      const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
+      const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
       Vector omegaCoriolis) const;
 };
 

gtsam/3rdparty/Eigen/.hg_archival.txt

@@ -1,4 +1,4 @@
 repo: 8a21fd850624c931e448cbcfb38168cb2717c790
-node: 10219c95fe653d4962aa9db4946f6fbea96dd740
+node: b30b87236a1b1552af32ac34075ee5696a9b5a33
 branch: 3.2
-tag: 3.2.4
+tag: 3.2.7

gtsam/3rdparty/Eigen/.hgtags

@@ -27,3 +27,6 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
 6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1
 1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2
 36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3
+10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4
+bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
+c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6

gtsam/3rdparty/Eigen/CMakeLists.txt

@@ -301,7 +301,7 @@ if(EIGEN_INCLUDE_INSTALL_DIR)
   )
 else()
   set(INCLUDE_INSTALL_DIR
-    "${CMAKE_INSTALL_PREFIX}/include/eigen3"
+    "include/eigen3"
     CACHE INTERNAL
     "The directory where we install the header files (internal)"
   )
@@ -404,7 +404,7 @@ if(cmake_generator_tolower MATCHES "makefile")
   message(STATUS "make install  | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
   message(STATUS "              |     cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
   message(STATUS "              |   Eigen headers will then be installed to:")
-  message(STATUS "              |     ${INCLUDE_INSTALL_DIR}")
+  message(STATUS "              |     ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}")
   message(STATUS "              |   To install Eigen headers to a separate location, do:")
   message(STATUS "              |     cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
   message(STATUS "make doc      | Generate the API documentation, requires Doxygen & LaTeX")

gtsam/3rdparty/Eigen/Eigen/Core

@@ -123,7 +123,7 @@
     #undef bool
     #undef vector
     #undef pixel
-  #elif defined  __ARM_NEON__
+  #elif defined  __ARM_NEON
     #define EIGEN_VECTORIZE
     #define EIGEN_VECTORIZE_NEON
     #include <arm_neon.h>

gtsam/3rdparty/Eigen/Eigen/SparseCore

@@ -14,7 +14,7 @@
 /** 
   * \defgroup SparseCore_Module SparseCore module
   *
-  * This module provides a sparse matrix representation, and basic associatd matrix manipulations
+  * This module provides a sparse matrix representation, and basic associated matrix manipulations
   * and operations.
   *
   * See the \ref TutorialSparse "Sparse tutorial"

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h

@@ -235,6 +235,11 @@ template<typename _MatrixType, int _UpLo> class LDLT
     }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
 
     /** \internal
       * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
@@ -434,6 +439,8 @@ template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
 template<typename MatrixType, int _UpLo>
 LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
 {
+  check_template_parameters();
+  
   eigen_assert(a.rows()==a.cols());
   const Index size = a.rows();
 
@@ -457,7 +464,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
   */
 template<typename MatrixType, int _UpLo>
 template<typename Derived>
-LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename NumTraits<typename MatrixType::Scalar>::Real& sigma)
+LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Derived>& w, const typename LDLT<MatrixType,_UpLo>::RealScalar& sigma)
 {
   const Index size = w.rows();
   if (m_isInitialized)

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h

@@ -174,6 +174,12 @@ template<typename _MatrixType, int _UpLo> class LLT
     LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     /** \internal
       * Used to compute and store L
       * The strict upper part is not used and even not initialized.
@@ -283,7 +289,7 @@ template<typename Scalar> struct llt_inplace<Scalar, Lower>
         return k;
       mat.coeffRef(k,k) = x = sqrt(x);
       if (k>0 && rs>0) A21.noalias() -= A20 * A10.adjoint();
-      if (rs>0) A21 *= RealScalar(1)/x;
+      if (rs>0) A21 /= x;
     }
     return -1;
   }
@@ -384,6 +390,8 @@ template<typename MatrixType> struct LLT_Traits<MatrixType,Upper>
 template<typename MatrixType, int _UpLo>
 LLT<MatrixType,_UpLo>& LLT<MatrixType,_UpLo>::compute(const MatrixType& a)
 {
+  check_template_parameters();
+  
   eigen_assert(a.rows()==a.cols());
   const Index size = a.rows();
   m_matrix.resize(size, size);

gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h

@@ -60,7 +60,7 @@ template<> struct mkl_llt<EIGTYPE> \
     lda = m.outerStride(); \
 \
     info = LAPACKE_##MKLPREFIX##potrf( matrix_order, uplo, size, (MKLTYPE*)a, lda ); \
-    info = (info==0) ? -1 : 1; \
+    info = (info==0) ? -1 : info>0 ? info-1 : size; \
     return info; \
   } \
 }; \

gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h

@@ -78,7 +78,7 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat)
   {
     res.itype = CHOLMOD_INT;
   }
-  else if (internal::is_same<_Index,UF_long>::value)
+  else if (internal::is_same<_Index,SuiteSparse_long>::value)
   {
     res.itype = CHOLMOD_LONG;
   }
@@ -395,7 +395,7 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl
     CholmodSimplicialLLT(const MatrixType& matrix) : Base()
     {
       init();
-      compute(matrix);
+      Base::compute(matrix);
     }
 
     ~CholmodSimplicialLLT() {}
@@ -442,7 +442,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp
     CholmodSimplicialLDLT(const MatrixType& matrix) : Base()
     {
       init();
-      compute(matrix);
+      Base::compute(matrix);
     }
 
     ~CholmodSimplicialLDLT() {}
@@ -487,7 +487,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper
     CholmodSupernodalLLT(const MatrixType& matrix) : Base()
     {
       init();
-      compute(matrix);
+      Base::compute(matrix);
     }
 
     ~CholmodSupernodalLLT() {}
@@ -534,7 +534,7 @@ class CholmodDecomposition : public CholmodBase<_MatrixType, _UpLo, CholmodDecom
     CholmodDecomposition(const MatrixType& matrix) : Base()
     {
       init();
-      compute(matrix);
+      Base::compute(matrix);
     }
 
     ~CholmodDecomposition() {}

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

@@ -124,6 +124,21 @@ class Array
     }
 #endif
 
+#ifdef EIGEN_HAVE_RVALUE_REFERENCES
+    Array(Array&& other)
+      : Base(std::move(other))
+    {
+      Base::_check_template_params();
+      if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
+        Base::_set_noalias(other);
+    }
+    Array& operator=(Array&& other)
+    {
+      other.swap(*this);
+      return *this;
+    }
+#endif
+
     /** Constructs a vector or row-vector with given dimension. \only_for_vectors
       *
       * Note that this is only useful for dynamic-size vectors. For fixed-size vectors,

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

@@ -46,9 +46,6 @@ template<typename Derived> class ArrayBase
 
     typedef ArrayBase Eigen_BaseClassForSpecializationOfGlobalMathFuncImpl;
 
-    using internal::special_scalar_op_base<Derived,typename internal::traits<Derived>::Scalar,
-                typename NumTraits<typename internal::traits<Derived>::Scalar>::Real>::operator*;
-
     typedef typename internal::traits<Derived>::StorageKind StorageKind;
     typedef typename internal::traits<Derived>::Index Index;
     typedef typename internal::traits<Derived>::Scalar Scalar;
@@ -56,6 +53,7 @@ template<typename Derived> class ArrayBase
     typedef typename NumTraits<Scalar>::Real RealScalar;
 
     typedef DenseBase<Derived> Base;
+    using Base::operator*;
     using Base::RowsAtCompileTime;
     using Base::ColsAtCompileTime;
     using Base::SizeAtCompileTime;

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

@@ -439,19 +439,26 @@ struct assign_impl<Derived1, Derived2, SliceVectorizedTraversal, NoUnrolling, Ve
   typedef typename Derived1::Index Index;
   static inline void run(Derived1 &dst, const Derived2 &src)
   {
-    typedef packet_traits<typename Derived1::Scalar> PacketTraits;
+    typedef typename Derived1::Scalar Scalar;
+    typedef packet_traits<Scalar> PacketTraits;
     enum {
       packetSize = PacketTraits::size,
       alignable = PacketTraits::AlignedOnScalar,
-      dstAlignment = alignable ? Aligned : int(assign_traits<Derived1,Derived2>::DstIsAligned) ,
+      dstIsAligned = assign_traits<Derived1,Derived2>::DstIsAligned,
+      dstAlignment = alignable ? Aligned : int(dstIsAligned),
       srcAlignment = assign_traits<Derived1,Derived2>::JointAlignment
     };
+    const Scalar *dst_ptr = &dst.coeffRef(0,0);
+    if((!bool(dstIsAligned)) && (size_t(dst_ptr) % sizeof(Scalar))>0)
+    {
+      // the pointer is not aligend-on scalar, so alignment is not possible
+      return assign_impl<Derived1,Derived2,DefaultTraversal,NoUnrolling>::run(dst, src);
+    }
     const Index packetAlignedMask = packetSize - 1;
     const Index innerSize = dst.innerSize();
     const Index outerSize = dst.outerSize();
     const Index alignedStep = alignable ? (packetSize - dst.outerStride() % packetSize) & packetAlignedMask : 0;
-    Index alignedStart = ((!alignable) || assign_traits<Derived1,Derived2>::DstIsAligned) ? 0
+    Index alignedStart = ((!alignable) || bool(dstIsAligned)) ? 0 : internal::first_aligned(dst_ptr, innerSize);
-                       : internal::first_aligned(&dst.coeffRef(0,0), innerSize);
 
     for(Index outer = 0; outer < outerSize; ++outer)
     {

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

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

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

@@ -1,154 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_COMMAINITIALIZER_H
-#define EIGEN_COMMAINITIALIZER_H
-
-namespace Eigen { 
-
-/** \class CommaInitializer
-  * \ingroup Core_Module
-  *
-  * \brief Helper class used by the comma initializer operator
-  *
-  * This class is internally used to implement the comma initializer feature. It is
-  * the return type of MatrixBase::operator<<, and most of the time this is the only
-  * way it is used.
-  *
-  * \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
-  */
-template<typename XprType>
-struct CommaInitializer
-{
-  typedef typename XprType::Scalar Scalar;
-  typedef typename XprType::Index Index;
-
-  inline CommaInitializer(XprType& xpr, const Scalar& s)
-    : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
-  {
-    m_xpr.coeffRef(0,0) = s;
-  }
-
-  template<typename OtherDerived>
-  inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
-    : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
-  {
-    m_xpr.block(0, 0, other.rows(), other.cols()) = other;
-  }
-
-  /* Copy/Move constructor which transfers ownership. This is crucial in 
-   * absence of return value optimization to avoid assertions during destruction. */
-  // FIXME in C++11 mode this could be replaced by a proper RValue constructor
-  inline CommaInitializer(const CommaInitializer& o)
-  : m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
-    // Mark original object as finished. In absence of R-value references we need to const_cast:
-    const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
-    const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
-    const_cast<CommaInitializer&>(o).m_currentBlockRows = 0;
-  }
-
-  /* inserts a scalar value in the target matrix */
-  CommaInitializer& operator,(const Scalar& s)
-  {
-    if (m_col==m_xpr.cols())
-    {
-      m_row+=m_currentBlockRows;
-      m_col = 0;
-      m_currentBlockRows = 1;
-      eigen_assert(m_row<m_xpr.rows()
-        && "Too many rows passed to comma initializer (operator<<)");
-    }
-    eigen_assert(m_col<m_xpr.cols()
-      && "Too many coefficients passed to comma initializer (operator<<)");
-    eigen_assert(m_currentBlockRows==1);
-    m_xpr.coeffRef(m_row, m_col++) = s;
-    return *this;
-  }
-
-  /* inserts a matrix expression in the target matrix */
-  template<typename OtherDerived>
-  CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
-  {
-    if(other.cols()==0 || other.rows()==0)
-      return *this;
-    if (m_col==m_xpr.cols())
-    {
-      m_row+=m_currentBlockRows;
-      m_col = 0;
-      m_currentBlockRows = other.rows();
-      eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
-        && "Too many rows passed to comma initializer (operator<<)");
-    }
-    eigen_assert(m_col<m_xpr.cols()
-      && "Too many coefficients passed to comma initializer (operator<<)");
-    eigen_assert(m_currentBlockRows==other.rows());
-    if (OtherDerived::SizeAtCompileTime != Dynamic)
-      m_xpr.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
-                              OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
-                    (m_row, m_col) = other;
-    else
-      m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other;
-    m_col += other.cols();
-    return *this;
-  }
-
-  inline ~CommaInitializer()
-  {
-    eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows()
-         && m_col == m_xpr.cols()
-         && "Too few coefficients passed to comma initializer (operator<<)");
-  }
-
-  /** \returns the built matrix once all its coefficients have been set.
-    * Calling finished is 100% optional. Its purpose is to write expressions
-    * like this:
-    * \code
-    * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
-    * \endcode
-    */
-  inline XprType& finished() { return m_xpr; }
-
-  XprType& m_xpr;   // target expression
-  Index m_row;              // current row id
-  Index m_col;              // current col id
-  Index m_currentBlockRows; // current block height
-};
-
-/** \anchor MatrixBaseCommaInitRef
-  * Convenient operator to set the coefficients of a matrix.
-  *
-  * The coefficients must be provided in a row major order and exactly match
-  * the size of the matrix. Otherwise an assertion is raised.
-  *
-  * Example: \include MatrixBase_set.cpp
-  * Output: \verbinclude MatrixBase_set.out
-  * 
-  * \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order.
-  *
-  * \sa CommaInitializer::finished(), class CommaInitializer
-  */
-template<typename Derived>
-inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
-{
-  return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
-}
-
-/** \sa operator<<(const Scalar&) */
-template<typename Derived>
-template<typename OtherDerived>
-inline CommaInitializer<Derived>
-DenseBase<Derived>::operator<<(const DenseBase<OtherDerived>& other)
-{
-  return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_COMMAINITIALIZER_H

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

@@ -81,7 +81,8 @@ struct traits<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
         )
      ),
     Flags = (Flags0 & ~RowMajorBit) | (LhsFlags & RowMajorBit),
-    CoeffReadCost = LhsCoeffReadCost + RhsCoeffReadCost + functor_traits<BinaryOp>::Cost
+    Cost0 = EIGEN_ADD_COST(LhsCoeffReadCost,RhsCoeffReadCost),
+    CoeffReadCost = EIGEN_ADD_COST(Cost0,functor_traits<BinaryOp>::Cost)
   };
 };
 } // end namespace internal

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

@@ -47,7 +47,7 @@ struct traits<CwiseUnaryOp<UnaryOp, XprType> >
     Flags = _XprTypeNested::Flags & (
       HereditaryBits | LinearAccessBit | AlignedBit
       | (functor_traits<UnaryOp>::PacketAccess ? PacketAccessBit : 0)),
-    CoeffReadCost = _XprTypeNested::CoeffReadCost + functor_traits<UnaryOp>::Cost
+    CoeffReadCost = EIGEN_ADD_COST(_XprTypeNested::CoeffReadCost, functor_traits<UnaryOp>::Cost)
   };
 };
 }

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

@@ -41,14 +41,13 @@ 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,
-                                     typename NumTraits<typename internal::traits<Derived>::Scalar>::Real>
+                                            typename NumTraits<typename internal::traits<Derived>::Scalar>::Real,
+                                            DenseCoeffsBase<Derived> >
 #else
   : public DenseCoeffsBase<Derived>
 #endif // not EIGEN_PARSED_BY_DOXYGEN
 {
   public:
-    using internal::special_scalar_op_base<Derived,typename internal::traits<Derived>::Scalar,
-                typename NumTraits<typename internal::traits<Derived>::Scalar>::Real>::operator*;
 
     class InnerIterator;
 
@@ -63,8 +62,9 @@ template<typename Derived> class DenseBase
     typedef typename internal::traits<Derived>::Scalar Scalar;
     typedef typename internal::packet_traits<Scalar>::type PacketScalar;
     typedef typename NumTraits<Scalar>::Real RealScalar;
+    typedef internal::special_scalar_op_base<Derived,Scalar,RealScalar, DenseCoeffsBase<Derived> > Base;
 
-    typedef DenseCoeffsBase<Derived> Base;
+    using Base::operator*;
     using Base::derived;
     using Base::const_cast_derived;
     using Base::rows;
@@ -183,10 +183,6 @@ template<typename Derived> class DenseBase
     /** \returns the number of nonzero coefficients which is in practice the number
       * of stored coefficients. */
     inline Index nonZeros() const { return size(); }
-    /** \returns true if either the number of rows or the number of columns is equal to 1.
-      * In other words, this function returns
-      * \code rows()==1 || cols()==1 \endcode
-      * \sa rows(), cols(), IsVectorAtCompileTime. */
 
     /** \returns the outer size.
       *
@@ -266,11 +262,13 @@ template<typename Derived> class DenseBase
     template<typename OtherDerived>
     Derived& operator=(const ReturnByValue<OtherDerived>& func);
 
-#ifndef EIGEN_PARSED_BY_DOXYGEN
+    /** \internal Copies \a other into *this without evaluating other. \returns a reference to *this. */
-    /** Copies \a other into *this without evaluating other. \returns a reference to *this. */
     template<typename OtherDerived>
     Derived& lazyAssign(const DenseBase<OtherDerived>& other);
-#endif // not EIGEN_PARSED_BY_DOXYGEN
+
+    /** \internal Evaluates \a other into *this. \returns a reference to *this. */
+    template<typename OtherDerived>
+    Derived& lazyAssign(const ReturnByValue<OtherDerived>& other);
 
     CommaInitializer<Derived> operator<< (const Scalar& s);
 

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

@@ -122,33 +122,41 @@ template<typename T, int Size, int _Rows, int _Cols, int _Options> class DenseSt
 {
     internal::plain_array<T,Size,_Options> m_data;
   public:
-    inline DenseStorage() {}
+    DenseStorage() {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert)
+    DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()) {}
-    inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {}
+    DenseStorage(const DenseStorage& other) : m_data(other.m_data) {}
-    inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); }
+    DenseStorage& operator=(const DenseStorage& other)
-    static inline DenseIndex rows(void) {return _Rows;}
+    {
-    static inline DenseIndex cols(void) {return _Cols;}
+      if (this != &other) m_data = other.m_data;
-    inline void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {}
+      return *this;
-    inline void resize(DenseIndex,DenseIndex,DenseIndex) {}
+    }
-    inline const T *data() const { return m_data.array; }
+    DenseStorage(DenseIndex,DenseIndex,DenseIndex) {}
-    inline T *data() { return m_data.array; }
+    void swap(DenseStorage& other) { std::swap(m_data,other.m_data); }
+    static DenseIndex rows(void) {return _Rows;}
+    static DenseIndex cols(void) {return _Cols;}
+    void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {}
+    void resize(DenseIndex,DenseIndex,DenseIndex) {}
+    const T *data() const { return m_data.array; }
+    T *data() { return m_data.array; }
 };
 
 // null matrix
 template<typename T, int _Rows, int _Cols, int _Options> class DenseStorage<T, 0, _Rows, _Cols, _Options>
 {
   public:
-    inline DenseStorage() {}
+    DenseStorage() {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert) {}
+    DenseStorage(internal::constructor_without_unaligned_array_assert) {}
-    inline DenseStorage(DenseIndex,DenseIndex,DenseIndex) {}
+    DenseStorage(const DenseStorage&) {}
-    inline void swap(DenseStorage& ) {}
+    DenseStorage& operator=(const DenseStorage&) { return *this; }
-    static inline DenseIndex rows(void) {return _Rows;}
+    DenseStorage(DenseIndex,DenseIndex,DenseIndex) {}
-    static inline DenseIndex cols(void) {return _Cols;}
+    void swap(DenseStorage& ) {}
-    inline void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {}
+    static DenseIndex rows(void) {return _Rows;}
-    inline void resize(DenseIndex,DenseIndex,DenseIndex) {}
+    static DenseIndex cols(void) {return _Cols;}
-    inline const T *data() const { return 0; }
+    void conservativeResize(DenseIndex,DenseIndex,DenseIndex) {}
-    inline T *data() { return 0; }
+    void resize(DenseIndex,DenseIndex,DenseIndex) {}
+    const T *data() const { return 0; }
+    T *data() { return 0; }
 };
 
 // more specializations for null matrices; these are necessary to resolve ambiguities
@@ -168,18 +176,29 @@ template<typename T, int Size, int _Options> class DenseStorage<T, Size, Dynamic
     DenseIndex m_rows;
     DenseIndex m_cols;
   public:
-    inline DenseStorage() : m_rows(0), m_cols(0) {}
+    DenseStorage() : m_rows(0), m_cols(0) {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert)
+    DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {}
-    inline DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) : m_rows(nbRows), m_cols(nbCols) {}
+    DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows), m_cols(other.m_cols) {}
-    inline void swap(DenseStorage& other)
+    DenseStorage& operator=(const DenseStorage& other)
+    {
+      if (this != &other)
+      {
+        m_data = other.m_data;
+        m_rows = other.m_rows;
+        m_cols = other.m_cols;
+      }
+      return *this;
+    }
+    DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) : m_rows(nbRows), m_cols(nbCols) {}
+    void swap(DenseStorage& other)
     { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
-    inline DenseIndex rows() const {return m_rows;}
+    DenseIndex rows() const {return m_rows;}
-    inline DenseIndex cols() const {return m_cols;}
+    DenseIndex cols() const {return m_cols;}
-    inline void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) { m_rows = nbRows; m_cols = nbCols; }
+    void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) { m_rows = nbRows; m_cols = nbCols; }
-    inline void resize(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) { m_rows = nbRows; m_cols = nbCols; }
+    void resize(DenseIndex, DenseIndex nbRows, DenseIndex nbCols) { m_rows = nbRows; m_cols = nbCols; }
-    inline const T *data() const { return m_data.array; }
+    const T *data() const { return m_data.array; }
-    inline T *data() { return m_data.array; }
+    T *data() { return m_data.array; }
 };
 
 // dynamic-size matrix with fixed-size storage and fixed width
@@ -188,17 +207,27 @@ template<typename T, int Size, int _Cols, int _Options> class DenseStorage<T, Si
     internal::plain_array<T,Size,_Options> m_data;
     DenseIndex m_rows;
   public:
-    inline DenseStorage() : m_rows(0) {}
+    DenseStorage() : m_rows(0) {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert)
+    DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {}
-    inline DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {}
+    DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {}
-    inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
+    DenseStorage& operator=(const DenseStorage& other)
-    inline DenseIndex rows(void) const {return m_rows;}
+    {
-    inline DenseIndex cols(void) const {return _Cols;}
+      if (this != &other)
-    inline void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; }
+      {
-    inline void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; }
+        m_data = other.m_data;
-    inline const T *data() const { return m_data.array; }
+        m_rows = other.m_rows;
-    inline T *data() { return m_data.array; }
+      }
+      return *this;
+    }
+    DenseStorage(DenseIndex, DenseIndex nbRows, DenseIndex) : m_rows(nbRows) {}
+    void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
+    DenseIndex rows(void) const {return m_rows;}
+    DenseIndex cols(void) const {return _Cols;}
+    void conservativeResize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; }
+    void resize(DenseIndex, DenseIndex nbRows, DenseIndex) { m_rows = nbRows; }
+    const T *data() const { return m_data.array; }
+    T *data() { return m_data.array; }
 };
 
 // dynamic-size matrix with fixed-size storage and fixed height
@@ -207,17 +236,27 @@ template<typename T, int Size, int _Rows, int _Options> class DenseStorage<T, Si
     internal::plain_array<T,Size,_Options> m_data;
     DenseIndex m_cols;
   public:
-    inline DenseStorage() : m_cols(0) {}
+    DenseStorage() : m_cols(0) {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert)
+    DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {}
-    inline DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {}
+    DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {}
-    inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
+    DenseStorage& operator=(const DenseStorage& other)
-    inline DenseIndex rows(void) const {return _Rows;}
+    {
-    inline DenseIndex cols(void) const {return m_cols;}
+      if (this != &other)
-    inline void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; }
+      {
-    inline void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; }
+        m_data = other.m_data;
-    inline const T *data() const { return m_data.array; }
+        m_cols = other.m_cols;
-    inline T *data() { return m_data.array; }
+      }
+      return *this;
+    }
+    DenseStorage(DenseIndex, DenseIndex, DenseIndex nbCols) : m_cols(nbCols) {}
+    void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
+    DenseIndex rows(void) const {return _Rows;}
+    DenseIndex cols(void) const {return m_cols;}
+    void conservativeResize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; }
+    void resize(DenseIndex, DenseIndex, DenseIndex nbCols) { m_cols = nbCols; }
+    const T *data() const { return m_data.array; }
+    T *data() { return m_data.array; }
 };
 
 // purely dynamic matrix.
@@ -227,18 +266,35 @@ template<typename T, int _Options> class DenseStorage<T, Dynamic, Dynamic, Dynam
     DenseIndex m_rows;
     DenseIndex m_cols;
   public:
-    inline DenseStorage() : m_data(0), m_rows(0), m_cols(0) {}
+    DenseStorage() : m_data(0), m_rows(0), m_cols(0) {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert)
+    DenseStorage(internal::constructor_without_unaligned_array_assert)
        : m_data(0), m_rows(0), m_cols(0) {}
-    inline DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols)
+    DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols)
       : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_rows(nbRows), m_cols(nbCols)
     { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN }
-    inline ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, m_rows*m_cols); }
+#ifdef EIGEN_HAVE_RVALUE_REFERENCES
-    inline void swap(DenseStorage& other)
+    DenseStorage(DenseStorage&& other)
+      : m_data(std::move(other.m_data))
+      , m_rows(std::move(other.m_rows))
+      , m_cols(std::move(other.m_cols))
+    {
+      other.m_data = nullptr;
+    }
+    DenseStorage& operator=(DenseStorage&& other)
+    {
+      using std::swap;
+      swap(m_data, other.m_data);
+      swap(m_rows, other.m_rows);
+      swap(m_cols, other.m_cols);
+      return *this;
+    }
+#endif
+    ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, m_rows*m_cols); }
+    void swap(DenseStorage& other)
     { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
-    inline DenseIndex rows(void) const {return m_rows;}
+    DenseIndex rows(void) const {return m_rows;}
-    inline DenseIndex cols(void) const {return m_cols;}
+    DenseIndex cols(void) const {return m_cols;}
-    inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols)
+    void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex nbCols)
     {
       m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, m_rows*m_cols);
       m_rows = nbRows;
@@ -258,8 +314,11 @@ template<typename T, int _Options> class DenseStorage<T, Dynamic, Dynamic, Dynam
       m_rows = nbRows;
       m_cols = nbCols;
     }
-    inline const T *data() const { return m_data; }
+    const T *data() const { return m_data; }
-    inline T *data() { return m_data; }
+    T *data() { return m_data; }
+  private:
+    DenseStorage(const DenseStorage&);
+    DenseStorage& operator=(const DenseStorage&);
 };
 
 // matrix with dynamic width and fixed height (so that matrix has dynamic size).
@@ -268,15 +327,30 @@ template<typename T, int _Rows, int _Options> class DenseStorage<T, Dynamic, _Ro
     T *m_data;
     DenseIndex m_cols;
   public:
-    inline DenseStorage() : m_data(0), m_cols(0) {}
+    DenseStorage() : m_data(0), m_cols(0) {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {}
+    DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {}
-    inline DenseStorage(DenseIndex size, DenseIndex, DenseIndex nbCols) : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_cols(nbCols)
+    DenseStorage(DenseIndex size, DenseIndex, DenseIndex nbCols) : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_cols(nbCols)
     { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN }
-    inline ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Rows*m_cols); }
+#ifdef EIGEN_HAVE_RVALUE_REFERENCES
-    inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
+    DenseStorage(DenseStorage&& other)
-    static inline DenseIndex rows(void) {return _Rows;}
+      : m_data(std::move(other.m_data))
-    inline DenseIndex cols(void) const {return m_cols;}
+      , m_cols(std::move(other.m_cols))
-    inline void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols)
+    {
+      other.m_data = nullptr;
+    }
+    DenseStorage& operator=(DenseStorage&& other)
+    {
+      using std::swap;
+      swap(m_data, other.m_data);
+      swap(m_cols, other.m_cols);
+      return *this;
+    }
+#endif
+    ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Rows*m_cols); }
+    void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
+    static DenseIndex rows(void) {return _Rows;}
+    DenseIndex cols(void) const {return m_cols;}
+    void conservativeResize(DenseIndex size, DenseIndex, DenseIndex nbCols)
     {
       m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, _Rows*m_cols);
       m_cols = nbCols;
@@ -294,8 +368,11 @@ template<typename T, int _Rows, int _Options> class DenseStorage<T, Dynamic, _Ro
       }
       m_cols = nbCols;
     }
-    inline const T *data() const { return m_data; }
+    const T *data() const { return m_data; }
-    inline T *data() { return m_data; }
+    T *data() { return m_data; }
+  private:
+    DenseStorage(const DenseStorage&);
+    DenseStorage& operator=(const DenseStorage&);
 };
 
 // matrix with dynamic height and fixed width (so that matrix has dynamic size).
@@ -304,15 +381,30 @@ template<typename T, int _Cols, int _Options> class DenseStorage<T, Dynamic, Dyn
     T *m_data;
     DenseIndex m_rows;
   public:
-    inline DenseStorage() : m_data(0), m_rows(0) {}
+    DenseStorage() : m_data(0), m_rows(0) {}
-    inline DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {}
+    DenseStorage(internal::constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {}
-    inline DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex) : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_rows(nbRows)
+    DenseStorage(DenseIndex size, DenseIndex nbRows, DenseIndex) : m_data(internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size)), m_rows(nbRows)
     { EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN }
-    inline ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Cols*m_rows); }
+#ifdef EIGEN_HAVE_RVALUE_REFERENCES
-    inline void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
+    DenseStorage(DenseStorage&& other)
-    inline DenseIndex rows(void) const {return m_rows;}
+      : m_data(std::move(other.m_data))
-    static inline DenseIndex cols(void) {return _Cols;}
+      , m_rows(std::move(other.m_rows))
-    inline void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex)
+    {
+      other.m_data = nullptr;
+    }
+    DenseStorage& operator=(DenseStorage&& other)
+    {
+      using std::swap;
+      swap(m_data, other.m_data);
+      swap(m_rows, other.m_rows);
+      return *this;
+    }
+#endif
+    ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Cols*m_rows); }
+    void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
+    DenseIndex rows(void) const {return m_rows;}
+    static DenseIndex cols(void) {return _Cols;}
+    void conservativeResize(DenseIndex size, DenseIndex nbRows, DenseIndex)
     {
       m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, m_rows*_Cols);
       m_rows = nbRows;
@@ -330,8 +422,11 @@ template<typename T, int _Cols, int _Options> class DenseStorage<T, Dynamic, Dyn
       }
       m_rows = nbRows;
     }
-    inline const T *data() const { return m_data; }
+    const T *data() const { return m_data; }
-    inline T *data() { return m_data; }
+    T *data() { return m_data; }
+  private:
+    DenseStorage(const DenseStorage&);
+    DenseStorage& operator=(const DenseStorage&);
 };
 
 } // end namespace Eigen

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

@@ -34,8 +34,9 @@ struct traits<DiagonalProduct<MatrixType, DiagonalType, ProductOrder> >
     _Vectorizable = bool(int(MatrixType::Flags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagonalType::DiagonalVectorType::Flags)&PacketAccessBit))),
     _LinearAccessMask = (RowsAtCompileTime==1 || ColsAtCompileTime==1) ? LinearAccessBit : 0,
 
-    Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0) | AlignedBit,//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit),
+    Flags = ((HereditaryBits|_LinearAccessMask|AlignedBit) & (unsigned int)(MatrixType::Flags)) | (_Vectorizable ? PacketAccessBit : 0),//(int(MatrixType::Flags)&int(DiagonalType::DiagonalVectorType::Flags)&AlignedBit),
-    CoeffReadCost = NumTraits<Scalar>::MulCost + MatrixType::CoeffReadCost + DiagonalType::DiagonalVectorType::CoeffReadCost
+    Cost0 = EIGEN_ADD_COST(NumTraits<Scalar>::MulCost, MatrixType::CoeffReadCost),
+    CoeffReadCost = EIGEN_ADD_COST(Cost0,DiagonalType::DiagonalVectorType::CoeffReadCost)
   };
 };
 }

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

@@ -259,6 +259,47 @@ template<> struct functor_traits<scalar_boolean_or_op> {
   };
 };
 
+/** \internal
+  * \brief Template functors for comparison of two scalars
+  * \todo Implement packet-comparisons
+  */
+template<typename Scalar, ComparisonName cmp> struct scalar_cmp_op;
+
+template<typename Scalar, ComparisonName cmp>
+struct functor_traits<scalar_cmp_op<Scalar, cmp> > {
+  enum {
+    Cost = NumTraits<Scalar>::AddCost,
+    PacketAccess = false
+  };
+};
+
+template<ComparisonName Cmp, typename Scalar>
+struct result_of<scalar_cmp_op<Scalar, Cmp>(Scalar,Scalar)> {
+  typedef bool type;
+};
+
+
+template<typename Scalar> struct scalar_cmp_op<Scalar, cmp_EQ> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
+  EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a==b;}
+};
+template<typename Scalar> struct scalar_cmp_op<Scalar, cmp_LT> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
+  EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a<b;}
+};
+template<typename Scalar> struct scalar_cmp_op<Scalar, cmp_LE> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
+  EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a<=b;}
+};
+template<typename Scalar> struct scalar_cmp_op<Scalar, cmp_UNORD> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
+  EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return !(a<=b || b<=a);}
+};
+template<typename Scalar> struct scalar_cmp_op<Scalar, cmp_NEQ> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cmp_op)
+  EIGEN_STRONG_INLINE bool operator()(const Scalar& a, const Scalar& b) const {return a!=b;}
+};
+
 // unary functors:
 
 /** \internal

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

@@ -257,7 +257,7 @@ template<typename Lhs, typename Rhs>
 class GeneralProduct<Lhs, Rhs, OuterProduct>
   : public ProductBase<GeneralProduct<Lhs,Rhs,OuterProduct>, Lhs, Rhs>
 {
-    template<typename T> struct IsRowMajor : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {};
+    template<typename T> struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {};
     
   public:
     EIGEN_PRODUCT_PUBLIC_INTERFACE(GeneralProduct)
@@ -281,22 +281,22 @@ class GeneralProduct<Lhs, Rhs, OuterProduct>
     
     template<typename Dest>
     inline void evalTo(Dest& dest) const {
-      internal::outer_product_selector_run(*this, dest, set(), IsRowMajor<Dest>());
+      internal::outer_product_selector_run(*this, dest, set(), is_row_major<Dest>());
     }
     
     template<typename Dest>
     inline void addTo(Dest& dest) const {
-      internal::outer_product_selector_run(*this, dest, add(), IsRowMajor<Dest>());
+      internal::outer_product_selector_run(*this, dest, add(), is_row_major<Dest>());
     }
 
     template<typename Dest>
     inline void subTo(Dest& dest) const {
-      internal::outer_product_selector_run(*this, dest, sub(), IsRowMajor<Dest>());
+      internal::outer_product_selector_run(*this, dest, sub(), is_row_major<Dest>());
     }
 
     template<typename Dest> void scaleAndAddTo(Dest& dest, const Scalar& alpha) const
     {
-      internal::outer_product_selector_run(*this, dest, adds(alpha), IsRowMajor<Dest>());
+      internal::outer_product_selector_run(*this, dest, adds(alpha), is_row_major<Dest>());
     }
 };
 

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

@@ -123,7 +123,7 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
       return internal::ploadt<PacketScalar, LoadMode>(m_data + index * innerStride());
     }
 
-    inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
+    explicit inline MapBase(PointerType dataPtr) : m_data(dataPtr), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
     {
       EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
       checkSanity();
@@ -157,7 +157,7 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
                                         internal::inner_stride_at_compile_time<Derived>::ret==1),
                           PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1);
       eigen_assert(EIGEN_IMPLIES(internal::traits<Derived>::Flags&AlignedBit, (size_t(m_data) % 16) == 0)
-                   && "data is not aligned");
+                   && "input pointer is not aligned on a 16 byte boundary");
     }
 
     PointerType m_data;

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

@@ -294,7 +294,7 @@ struct hypot_impl
     RealScalar _x = abs(x);
     RealScalar _y = abs(y);
     RealScalar p = (max)(_x, _y);
-    if(p==RealScalar(0)) return 0;
+    if(p==RealScalar(0)) return RealScalar(0);
     RealScalar q = (min)(_x, _y);
     RealScalar qp = q/p;
     return p * sqrt(RealScalar(1) + qp*qp);

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

@@ -211,6 +211,21 @@ class Matrix
       : Base(internal::constructor_without_unaligned_array_assert())
     { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED }
 
+#ifdef EIGEN_HAVE_RVALUE_REFERENCES
+    Matrix(Matrix&& other)
+      : Base(std::move(other))
+    {
+      Base::_check_template_params();
+      if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
+        Base::_set_noalias(other);
+    }
+    Matrix& operator=(Matrix&& other)
+    {
+      other.swap(*this);
+      return *this;
+    }
+#endif
+
     /** \brief Constructs a vector or row-vector with given dimension. \only_for_vectors
       *
       * Note that this is only useful for dynamic-size vectors. For fixed-size vectors,

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

@@ -159,13 +159,11 @@ template<typename Derived> class MatrixBase
     template<typename OtherDerived>
     Derived& operator=(const ReturnByValue<OtherDerived>& other);
 
-#ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename ProductDerived, typename Lhs, typename Rhs>
     Derived& lazyAssign(const ProductBase<ProductDerived, Lhs,Rhs>& other);
 
     template<typename MatrixPower, typename Lhs, typename Rhs>
     Derived& lazyAssign(const MatrixPowerProduct<MatrixPower, Lhs,Rhs>& other);
-#endif // not EIGEN_PARSED_BY_DOXYGEN
 
     template<typename OtherDerived>
     Derived& operator+=(const MatrixBase<OtherDerived>& other);
@@ -442,6 +440,15 @@ template<typename Derived> class MatrixBase
     template<typename OtherScalar>
     void applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j);
 
+///////// SparseCore module /////////
+
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE const typename SparseMatrixBase<OtherDerived>::template CwiseProductDenseReturnType<Derived>::Type
+    cwiseProduct(const SparseMatrixBase<OtherDerived> &other) const
+    {
+      return other.cwiseProduct(derived());
+    }
+
 ///////// MatrixFunctions module /////////
 
     typedef typename internal::stem_function<Scalar>::type StemFunction;

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

@@ -250,6 +250,35 @@ class PermutationBase : public EigenBase<Derived>
     template<typename Other> friend
     inline PlainPermutationType operator*(const Transpose<PermutationBase<Other> >& other, const PermutationBase& perm)
     { return PlainPermutationType(internal::PermPermProduct, other.eval(), perm); }
+    
+    /** \returns the determinant of the permutation matrix, which is either 1 or -1 depending on the parity of the permutation.
+      *
+      * This function is O(\c n) procedure allocating a buffer of \c n booleans.
+      */
+    Index determinant() const
+    {
+      Index res = 1;
+      Index n = size();
+      Matrix<bool,RowsAtCompileTime,1,0,MaxRowsAtCompileTime> mask(n);
+      mask.fill(false);
+      Index r = 0;
+      while(r < n)
+      {
+        // search for the next seed
+        while(r<n && mask[r]) r++;
+        if(r>=n)
+          break;
+        // we got one, let's follow it until we are back to the seed
+        Index k0 = r++;
+        mask.coeffRef(k0) = true;
+        for(Index k=indices().coeff(k0); k!=k0; k=indices().coeff(k))
+        {
+          mask.coeffRef(k) = true;
+          res = -res;
+        }
+      }
+      return res;
+    }
 
   protected:
 

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

@@ -437,6 +437,36 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
     }
 #endif
 
+#ifdef EIGEN_HAVE_RVALUE_REFERENCES
+    PlainObjectBase(PlainObjectBase&& other)
+      : m_storage( std::move(other.m_storage) )
+    {
+    }
+
+    PlainObjectBase& operator=(PlainObjectBase&& other)
+    {
+      using std::swap;
+      swap(m_storage, other.m_storage);
+      return *this;
+    }
+#endif
+
+    /** Copy constructor */
+    EIGEN_STRONG_INLINE PlainObjectBase(const PlainObjectBase& other)
+      : m_storage()
+    {
+      _check_template_params();
+      lazyAssign(other);
+    }
+
+    template<typename OtherDerived>
+    EIGEN_STRONG_INLINE PlainObjectBase(const DenseBase<OtherDerived> &other)
+      : m_storage()
+    {
+      _check_template_params();
+      lazyAssign(other);
+    }
+
     EIGEN_STRONG_INLINE PlainObjectBase(Index a_size, Index nbRows, Index nbCols)
       : m_storage(a_size, nbRows, nbCols)
     {
@@ -573,6 +603,8 @@ class PlainObjectBase : public internal::dense_xpr_base<Derived>::type
                  : (rows() == other.rows() && cols() == other.cols())))
         && "Size mismatch. Automatic resizing is disabled because EIGEN_NO_AUTOMATIC_RESIZING is defined");
       EIGEN_ONLY_USED_FOR_DEBUG(other);
+      if(this->size()==0)
+        resizeLike(other);
       #else
       resizeLike(other);
       #endif

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

@@ -247,8 +247,9 @@ struct redux_impl<Func, Derived, LinearVectorizedTraversal, NoUnrolling>
   }
 };
 
-template<typename Func, typename Derived>
+// NOTE: for SliceVectorizedTraversal we simply bypass unrolling
-struct redux_impl<Func, Derived, SliceVectorizedTraversal, NoUnrolling>
+template<typename Func, typename Derived, int Unrolling>
+struct redux_impl<Func, Derived, SliceVectorizedTraversal, Unrolling>
 {
   typedef typename Derived::Scalar Scalar;
   typedef typename packet_traits<Scalar>::type PacketScalar;

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

@@ -108,7 +108,8 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
       OuterStrideMatch = Derived::IsVectorAtCompileTime
                       || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime),
       AlignmentMatch = (_Options!=Aligned) || ((PlainObjectType::Flags&AlignedBit)==0) || ((traits<Derived>::Flags&AlignedBit)==AlignedBit),
-      MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch
+      ScalarTypeMatch = internal::is_same<typename PlainObjectType::Scalar, typename Derived::Scalar>::value,
+      MatchAtCompileTime = HasDirectAccess && StorageOrderMatch && InnerStrideMatch && OuterStrideMatch && AlignmentMatch && ScalarTypeMatch
     };
     typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
   };
@@ -187,9 +188,11 @@ protected:
 template<typename PlainObjectType, int Options, typename StrideType> class Ref
   : public RefBase<Ref<PlainObjectType, Options, StrideType> >
 {
+  private:
     typedef internal::traits<Ref> Traits;
     template<typename Derived>
-    inline Ref(const PlainObjectBase<Derived>& expr);
+    inline Ref(const PlainObjectBase<Derived>& expr,
+               typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0);
   public:
 
     typedef RefBase<Ref> Base;
@@ -198,13 +201,15 @@ template<typename PlainObjectType, int Options, typename StrideType> class Ref
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename Derived>
-    inline Ref(PlainObjectBase<Derived>& expr)
+    inline Ref(PlainObjectBase<Derived>& expr,
+               typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
     {
       EIGEN_STATIC_ASSERT(static_cast<bool>(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
       Base::construct(expr.derived());
     }
     template<typename Derived>
-    inline Ref(const DenseBase<Derived>& expr)
+    inline Ref(const DenseBase<Derived>& expr,
+               typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
     #else
     template<typename Derived>
     inline Ref(DenseBase<Derived>& expr)
@@ -231,13 +236,23 @@ template<typename TPlainObjectType, int Options, typename StrideType> class Ref<
     EIGEN_DENSE_PUBLIC_INTERFACE(Ref)
 
     template<typename Derived>
-    inline Ref(const DenseBase<Derived>& expr)
+    inline Ref(const DenseBase<Derived>& expr,
+               typename internal::enable_if<bool(Traits::template match<Derived>::ScalarTypeMatch),Derived>::type* = 0)
     {
 //      std::cout << match_helper<Derived>::HasDirectAccess << "," << match_helper<Derived>::OuterStrideMatch << "," << match_helper<Derived>::InnerStrideMatch << "\n";
 //      std::cout << int(StrideType::OuterStrideAtCompileTime) << " - " << int(Derived::OuterStrideAtCompileTime) << "\n";
 //      std::cout << int(StrideType::InnerStrideAtCompileTime) << " - " << int(Derived::InnerStrideAtCompileTime) << "\n";
       construct(expr.derived(), typename Traits::template match<Derived>::type());
     }
+    
+    inline Ref(const Ref& other) : Base(other) {
+      // copy constructor shall not copy the m_object, to avoid unnecessary malloc and copy
+    }
+
+    template<typename OtherRef>
+    inline Ref(const RefBase<OtherRef>& other) {
+      construct(other.derived(), typename Traits::template match<OtherRef>::type());
+    }
 
   protected:
 

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

@@ -72,6 +72,8 @@ template<typename Derived> class ReturnByValue
     const Unusable& coeff(Index,Index) const { return *reinterpret_cast<const Unusable*>(this); }
     Unusable& coeffRef(Index) { return *reinterpret_cast<Unusable*>(this); }
     Unusable& coeffRef(Index,Index) { return *reinterpret_cast<Unusable*>(this); }
+    template<int LoadMode>  Unusable& packet(Index) const;
+    template<int LoadMode>  Unusable& packet(Index, Index) const;
 #endif
 };
 
@@ -83,6 +85,15 @@ Derived& DenseBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
   return derived();
 }
 
+template<typename Derived>
+template<typename OtherDerived>
+Derived& DenseBase<Derived>::lazyAssign(const ReturnByValue<OtherDerived>& other)
+{
+  other.evalTo(derived());
+  return derived();
+}
+
+
 } // end namespace Eigen
 
 #endif // EIGEN_RETURNBYVALUE_H

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

@@ -180,15 +180,9 @@ inline Derived& DenseBase<Derived>::operator*=(const Scalar& other)
 template<typename Derived>
 inline Derived& DenseBase<Derived>::operator/=(const Scalar& other)
 {
-  typedef typename internal::conditional<NumTraits<Scalar>::IsInteger,
-                                        internal::scalar_quotient_op<Scalar>,
-                                        internal::scalar_product_op<Scalar> >::type BinOp;
   typedef typename Derived::PlainObject PlainObject;
-  SelfCwiseBinaryOp<BinOp, Derived, typename PlainObject::ConstantReturnType> tmp(derived());
+  SelfCwiseBinaryOp<internal::scalar_quotient_op<Scalar>, Derived, typename PlainObject::ConstantReturnType> tmp(derived());
-  Scalar actual_other;
+  tmp = PlainObject::Constant(rows(),cols(), other);
-  if(NumTraits<Scalar>::IsInteger)  actual_other = other;
-  else                              actual_other = Scalar(1)/other;
-  tmp = PlainObject::Constant(rows(),cols(), actual_other);
   return derived();
 }
 

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

@@ -384,6 +384,7 @@ template<> EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a)
   a_lo = vget_low_s32(a);
   a_hi = vget_high_s32(a);
   max = vpmax_s32(a_lo, a_hi);
+  max = vpmax_s32(max, max);
 
   return vget_lane_s32(max, 0);
 }

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

@@ -126,7 +126,7 @@ Packet4f pexp<Packet4f>(const Packet4f& _x)
   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f);
   _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f);
 
-  Packet4f tmp = _mm_setzero_ps(), fx;
+  Packet4f tmp, fx;
   Packet4i emm0;
 
   // clamp x
@@ -195,7 +195,7 @@ Packet2d pexp<Packet2d>(const Packet2d& _x)
   _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_C2, 1.42860682030941723212e-6);
   static const __m128i p4i_1023_0 = _mm_setr_epi32(1023, 1023, 0, 0);
 
-  Packet2d tmp = _mm_setzero_pd(), fx;
+  Packet2d tmp, fx;
   Packet4i emm0;
 
   // clamp x
@@ -279,7 +279,7 @@ Packet4f psin<Packet4f>(const Packet4f& _x)
   _EIGEN_DECLARE_CONST_Packet4f(coscof_p2,  4.166664568298827E-002f);
   _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
 
-  Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, sign_bit, y;
+  Packet4f xmm1, xmm2, xmm3, sign_bit, y;
 
   Packet4i emm0, emm2;
   sign_bit = x;
@@ -378,7 +378,7 @@ Packet4f pcos<Packet4f>(const Packet4f& _x)
   _EIGEN_DECLARE_CONST_Packet4f(coscof_p2,  4.166664568298827E-002f);
   _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
 
-  Packet4f xmm1, xmm2 = _mm_setzero_ps(), xmm3, y;
+  Packet4f xmm1, xmm2, xmm3, y;
   Packet4i emm0, emm2;
 
   x = pabs(x);

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

@@ -134,7 +134,7 @@ class CoeffBasedProduct
     };
 
     typedef internal::product_coeff_impl<CanVectorizeInner ? InnerVectorizedTraversal : DefaultTraversal,
-                                   Unroll ? (InnerSize==0 ? 0 : InnerSize-1) : Dynamic,
+                                   Unroll ? InnerSize : Dynamic,
                                    _LhsNested, _RhsNested, Scalar> ScalarCoeffImpl;
 
     typedef CoeffBasedProduct<LhsNested,RhsNested,NestByRefBit> LazyCoeffBasedProductType;
@@ -185,7 +185,7 @@ class CoeffBasedProduct
     {
       PacketScalar res;
       internal::product_packet_impl<Flags&RowMajorBit ? RowMajor : ColMajor,
-                              Unroll ? (InnerSize==0 ? 0 : InnerSize-1) : Dynamic,
+                              Unroll ? InnerSize : Dynamic,
                               _LhsNested, _RhsNested, PacketScalar, LoadMode>
         ::run(row, col, m_lhs, m_rhs, res);
       return res;
@@ -243,7 +243,17 @@ struct product_coeff_impl<DefaultTraversal, UnrollingIndex, Lhs, Rhs, RetScalar>
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
   {
     product_coeff_impl<DefaultTraversal, UnrollingIndex-1, Lhs, Rhs, RetScalar>::run(row, col, lhs, rhs, res);
-    res += lhs.coeff(row, UnrollingIndex) * rhs.coeff(UnrollingIndex, col);
+    res += lhs.coeff(row, UnrollingIndex-1) * rhs.coeff(UnrollingIndex-1, col);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename RetScalar>
+struct product_coeff_impl<DefaultTraversal, 1, Lhs, Rhs, RetScalar>
+{
+  typedef typename Lhs::Index Index;
+  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
+  {
+    res = lhs.coeff(row, 0) * rhs.coeff(0, col);
   }
 };
 
@@ -251,9 +261,9 @@ template<typename Lhs, typename Rhs, typename RetScalar>
 struct product_coeff_impl<DefaultTraversal, 0, Lhs, Rhs, RetScalar>
 {
   typedef typename Lhs::Index Index;
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
+  static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, RetScalar &res)
   {
-    res = lhs.coeff(row, 0) * rhs.coeff(0, col);
+    res = RetScalar(0);
   }
 };
 
@@ -293,6 +303,16 @@ struct product_coeff_vectorized_unroller<0, Lhs, Rhs, Packet>
   }
 };
 
+template<typename Lhs, typename Rhs, typename RetScalar>
+struct product_coeff_impl<InnerVectorizedTraversal, 0, Lhs, Rhs, RetScalar>
+{
+  typedef typename Lhs::Index Index;
+  static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, RetScalar &res)
+  {
+    res = 0;
+  }
+};
+
 template<int UnrollingIndex, typename Lhs, typename Rhs, typename RetScalar>
 struct product_coeff_impl<InnerVectorizedTraversal, UnrollingIndex, Lhs, Rhs, RetScalar>
 {
@@ -302,8 +322,7 @@ struct product_coeff_impl<InnerVectorizedTraversal, UnrollingIndex, Lhs, Rhs, Re
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
   {
     Packet pres;
-    product_coeff_vectorized_unroller<UnrollingIndex+1-PacketSize, Lhs, Rhs, Packet>::run(row, col, lhs, rhs, pres);
+    product_coeff_vectorized_unroller<UnrollingIndex-PacketSize, Lhs, Rhs, Packet>::run(row, col, lhs, rhs, pres);
-    product_coeff_impl<DefaultTraversal,UnrollingIndex,Lhs,Rhs,RetScalar>::run(row, col, lhs, rhs, res);
     res = predux(pres);
   }
 };
@@ -371,7 +390,7 @@ struct product_packet_impl<RowMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode>
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res)
   {
     product_packet_impl<RowMajor, UnrollingIndex-1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs, res);
-    res =  pmadd(pset1<Packet>(lhs.coeff(row, UnrollingIndex)), rhs.template packet<LoadMode>(UnrollingIndex, col), res);
+    res =  pmadd(pset1<Packet>(lhs.coeff(row, UnrollingIndex-1)), rhs.template packet<LoadMode>(UnrollingIndex-1, col), res);
   }
 };
 
@@ -382,12 +401,12 @@ struct product_packet_impl<ColMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode>
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res)
   {
     product_packet_impl<ColMajor, UnrollingIndex-1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs, res);
-    res =  pmadd(lhs.template packet<LoadMode>(row, UnrollingIndex), pset1<Packet>(rhs.coeff(UnrollingIndex, col)), res);
+    res =  pmadd(lhs.template packet<LoadMode>(row, UnrollingIndex-1), pset1<Packet>(rhs.coeff(UnrollingIndex-1, col)), res);
   }
 };
 
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct product_packet_impl<RowMajor, 0, Lhs, Rhs, Packet, LoadMode>
+struct product_packet_impl<RowMajor, 1, Lhs, Rhs, Packet, LoadMode>
 {
   typedef typename Lhs::Index Index;
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res)
@@ -397,7 +416,7 @@ struct product_packet_impl<RowMajor, 0, Lhs, Rhs, Packet, LoadMode>
 };
 
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
-struct product_packet_impl<ColMajor, 0, Lhs, Rhs, Packet, LoadMode>
+struct product_packet_impl<ColMajor, 1, Lhs, Rhs, Packet, LoadMode>
 {
   typedef typename Lhs::Index Index;
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet &res)
@@ -406,16 +425,35 @@ struct product_packet_impl<ColMajor, 0, Lhs, Rhs, Packet, LoadMode>
   }
 };
 
+template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct product_packet_impl<RowMajor, 0, Lhs, Rhs, Packet, LoadMode>
+{
+  typedef typename Lhs::Index Index;
+  static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Packet &res)
+  {
+    res = pset1<Packet>(0);
+  }
+};
+
+template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
+struct product_packet_impl<ColMajor, 0, Lhs, Rhs, Packet, LoadMode>
+{
+  typedef typename Lhs::Index Index;
+  static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Packet &res)
+  {
+    res = pset1<Packet>(0);
+  }
+};
+
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct product_packet_impl<RowMajor, Dynamic, Lhs, Rhs, Packet, LoadMode>
 {
   typedef typename Lhs::Index Index;
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet& res)
   {
-    eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix");
+    res = pset1<Packet>(0);
-    res = pmul(pset1<Packet>(lhs.coeff(row, 0)),rhs.template packet<LoadMode>(0, col));
+    for(Index i = 0; i < lhs.cols(); ++i)
-      for(Index i = 1; i < lhs.cols(); ++i)
+      res =  pmadd(pset1<Packet>(lhs.coeff(row, i)), rhs.template packet<LoadMode>(i, col), res);
-        res =  pmadd(pset1<Packet>(lhs.coeff(row, i)), rhs.template packet<LoadMode>(i, col), res);
   }
 };
 
@@ -425,10 +463,9 @@ struct product_packet_impl<ColMajor, Dynamic, Lhs, Rhs, Packet, LoadMode>
   typedef typename Lhs::Index Index;
   static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Packet& res)
   {
-    eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix");
+    res = pset1<Packet>(0);
-    res = pmul(lhs.template packet<LoadMode>(row, 0), pset1<Packet>(rhs.coeff(0, col)));
+    for(Index i = 0; i < lhs.cols(); ++i)
-      for(Index i = 1; i < lhs.cols(); ++i)
+      res =  pmadd(lhs.template packet<LoadMode>(row, i), pset1<Packet>(rhs.coeff(i, col)), res);
-        res =  pmadd(lhs.template packet<LoadMode>(row, i), pset1<Packet>(rhs.coeff(i, col)), res);
   }
 };
 

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

@@ -125,19 +125,22 @@ void parallelize_gemm(const Functor& func, Index rows, Index cols, bool transpos
   if(transpose)
     std::swap(rows,cols);
 
-  Index blockCols = (cols / threads) & ~Index(0x3);
-  Index blockRows = (rows / threads) & ~Index(0x7);
-  
   GemmParallelInfo<Index>* info = new GemmParallelInfo<Index>[threads];
 
-  #pragma omp parallel for schedule(static,1) num_threads(threads)
+  #pragma omp parallel num_threads(threads)
-  for(Index i=0; i<threads; ++i)
   {
+    Index i = omp_get_thread_num();
+    // Note that the actual number of threads might be lower than the number of request ones.
+    Index actual_threads = omp_get_num_threads();
+    
+    Index blockCols = (cols / actual_threads) & ~Index(0x3);
+    Index blockRows = (rows / actual_threads) & ~Index(0x7);
+    
     Index r0 = i*blockRows;
-    Index actualBlockRows = (i+1==threads) ? rows-r0 : blockRows;
+    Index actualBlockRows = (i+1==actual_threads) ? rows-r0 : blockRows;
 
     Index c0 = i*blockCols;
-    Index actualBlockCols = (i+1==threads) ? cols-c0 : blockCols;
+    Index actualBlockCols = (i+1==actual_threads) ? cols-c0 : blockCols;
 
     info[i].rhs_start = c0;
     info[i].rhs_length = actualBlockCols;

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

@@ -109,7 +109,7 @@ struct product_triangular_matrix_matrix_trmm<EIGTYPE,Index,Mode,true, \
 /* Non-square case - doesn't fit to MKL ?TRMM. Fall to default triangular product or call MKL ?GEMM*/ \
    if (rows != depth) { \
 \
-     int nthr = mkl_domain_get_max_threads(MKL_BLAS); \
+     int nthr = mkl_domain_get_max_threads(EIGEN_MKL_DOMAIN_BLAS); \
 \
      if (((nthr==1) && (((std::max)(rows,depth)-diagSize)/(double)diagSize < 0.5))) { \
      /* Most likely no benefit to call TRMM or GEMM from MKL*/ \
@@ -223,7 +223,7 @@ struct product_triangular_matrix_matrix_trmm<EIGTYPE,Index,Mode,false, \
 /* Non-square case - doesn't fit to MKL ?TRMM. Fall to default triangular product or call MKL ?GEMM*/ \
    if (cols != depth) { \
 \
-     int nthr = mkl_domain_get_max_threads(MKL_BLAS); \
+     int nthr = mkl_domain_get_max_threads(EIGEN_MKL_DOMAIN_BLAS); \
 \
      if ((nthr==1) && (((std::max)(cols,depth)-diagSize)/(double)diagSize < 0.5)) { \
      /* Most likely no benefit to call TRMM or GEMM from MKL*/ \

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

@@ -302,9 +302,12 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conj
                 for (Index i=0; i<actual_mc; ++i)
                   r[i] -= a[i] * b;
               }
-              Scalar b = (Mode & UnitDiag) ? Scalar(1) : Scalar(1)/conj(rhs(j,j));
+              if((Mode & UnitDiag)==0)
-              for (Index i=0; i<actual_mc; ++i)
+              {
-                r[i] *= b;
+                Scalar b = conj(rhs(j,j));
+                for (Index i=0; i<actual_mc; ++i)
+                  r[i] /= b;
+              }
             }
 
             // pack the just computed part of lhs to A

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

@@ -433,6 +433,19 @@ struct MatrixXpr {};
 /** The type used to identify an array expression */
 struct ArrayXpr {};
 
+namespace internal {
+  /** \internal
+  * Constants for comparison functors
+  */
+  enum ComparisonName {
+    cmp_EQ = 0,
+    cmp_LT = 1,
+    cmp_LE = 2,
+    cmp_UNORD = 3,
+    cmp_NEQ = 4
+  };
+}
+
 } // end namespace Eigen
 
 #endif // EIGEN_CONSTANTS_H

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

@@ -235,6 +235,9 @@ template<typename Scalar> class Rotation2D;
 template<typename Scalar> class AngleAxis;
 template<typename Scalar,int Dim> class Translation;
 
+// Sparse module:
+template<typename Derived> class SparseMatrixBase;
+
 #ifdef EIGEN2_SUPPORT
 template<typename Derived, int _Dim> class eigen2_RotationBase;
 template<typename Lhs, typename Rhs> class eigen2_Cross;

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

@@ -76,6 +76,38 @@
 #include <mkl_lapacke.h>
 #define EIGEN_MKL_VML_THRESHOLD 128
 
+/* MKL_DOMAIN_BLAS, etc are defined only in 10.3 update 7 */
+/* MKL_BLAS, etc are not defined in 11.2 */
+#ifdef MKL_DOMAIN_ALL
+#define EIGEN_MKL_DOMAIN_ALL MKL_DOMAIN_ALL
+#else
+#define EIGEN_MKL_DOMAIN_ALL MKL_ALL
+#endif
+
+#ifdef MKL_DOMAIN_BLAS
+#define EIGEN_MKL_DOMAIN_BLAS MKL_DOMAIN_BLAS
+#else
+#define EIGEN_MKL_DOMAIN_BLAS MKL_BLAS
+#endif
+
+#ifdef MKL_DOMAIN_FFT
+#define EIGEN_MKL_DOMAIN_FFT MKL_DOMAIN_FFT
+#else
+#define EIGEN_MKL_DOMAIN_FFT MKL_FFT
+#endif
+
+#ifdef MKL_DOMAIN_VML
+#define EIGEN_MKL_DOMAIN_VML MKL_DOMAIN_VML
+#else
+#define EIGEN_MKL_DOMAIN_VML MKL_VML
+#endif
+
+#ifdef MKL_DOMAIN_PARDISO
+#define EIGEN_MKL_DOMAIN_PARDISO MKL_DOMAIN_PARDISO
+#else
+#define EIGEN_MKL_DOMAIN_PARDISO MKL_PARDISO
+#endif
+
 namespace Eigen {
 
 typedef std::complex<double> dcomplex;

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

@@ -13,7 +13,7 @@
 
 #define EIGEN_WORLD_VERSION 3
 #define EIGEN_MAJOR_VERSION 2
-#define EIGEN_MINOR_VERSION 4
+#define EIGEN_MINOR_VERSION 7
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@@ -96,6 +96,20 @@
 #define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
 #endif
 
+// A Clang feature extension to determine compiler features.
+// We use it to determine 'cxx_rvalue_references'
+#ifndef __has_feature
+# define __has_feature(x) 0
+#endif
+
+// Do we support r-value references?
+#if (__has_feature(cxx_rvalue_references) || \
+     defined(__GXX_EXPERIMENTAL_CXX0X__) || \
+     (defined(_MSC_VER) && _MSC_VER >= 1600))
+  #define EIGEN_HAVE_RVALUE_REFERENCES
+#endif
+
+
 // Cross compiler wrapper around LLVM's __has_builtin
 #ifdef __has_builtin
 #  define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
@@ -278,6 +292,7 @@ namespace Eigen {
   #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler
 #endif
 
+#define EIGEN_ALIGN8  EIGEN_ALIGN_TO_BOUNDARY(8)
 #define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16)
 
 #if EIGEN_ALIGN_STATICALLY
@@ -332,8 +347,11 @@ namespace Eigen {
   }
 #endif
 
-#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
+/** \internal
-  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
+ * \brief Macro to manually inherit assignment operators.
+ * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined.
+ */
+#define EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
 
 /**
 * Just a side note. Commenting within defines works only by documenting
@@ -405,6 +423,8 @@ namespace Eigen {
 #define EIGEN_SIZE_MAX(a,b) (((int)a == Dynamic || (int)b == Dynamic) ? Dynamic \
                            : ((int)a >= (int)b) ? (int)a : (int)b)
 
+#define EIGEN_ADD_COST(a,b) int(a)==Dynamic || int(b)==Dynamic ? Dynamic : int(a)+int(b)
+
 #define EIGEN_LOGICAL_XOR(a,b) (((a) || (b)) && !((a) && (b)))
 
 #define EIGEN_IMPLIES(a,b) (!(a) || (b))

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

@@ -523,7 +523,7 @@ template<typename T> struct smart_copy_helper<T,false> {
 // you can overwrite Eigen's default behavior regarding alloca by defining EIGEN_ALLOCA
 // to the appropriate stack allocation function
 #ifndef EIGEN_ALLOCA
-  #if (defined __linux__)
+  #if (defined __linux__) || (defined __APPLE__) || (defined alloca)
     #define EIGEN_ALLOCA alloca
   #elif defined(_MSC_VER)
     #define EIGEN_ALLOCA _alloca

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

@@ -366,17 +366,17 @@ struct dense_xpr_base<Derived, ArrayXpr>
 
 /** \internal Helper base class to add a scalar multiple operator
   * overloads for complex types */
-template<typename Derived,typename Scalar,typename OtherScalar,
+template<typename Derived, typename Scalar, typename OtherScalar, typename BaseType,
          bool EnableIt = !is_same<Scalar,OtherScalar>::value >
-struct special_scalar_op_base : public DenseCoeffsBase<Derived>
+struct special_scalar_op_base : public BaseType
 {
   // dummy operator* so that the
   // "using special_scalar_op_base::operator*" compiles
   void operator*() const;
 };
 
-template<typename Derived,typename Scalar,typename OtherScalar>
+template<typename Derived,typename Scalar,typename OtherScalar, typename BaseType>
-struct special_scalar_op_base<Derived,Scalar,OtherScalar,true>  : public DenseCoeffsBase<Derived>
+struct special_scalar_op_base<Derived,Scalar,OtherScalar,BaseType,true>  : public BaseType
 {
   const CwiseUnaryOp<scalar_multiple2_op<Scalar,OtherScalar>, Derived>
   operator*(const OtherScalar& scalar) const

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

@@ -234,6 +234,12 @@ template<typename _MatrixType> class ComplexEigenSolver
     }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     EigenvectorType m_eivec;
     EigenvalueType m_eivalues;
     ComplexSchur<MatrixType> m_schur;
@@ -251,6 +257,8 @@ template<typename MatrixType>
 ComplexEigenSolver<MatrixType>& 
 ComplexEigenSolver<MatrixType>::compute(const MatrixType& matrix, bool computeEigenvectors)
 {
+  check_template_parameters();
+  
   // this code is inspired from Jampack
   eigen_assert(matrix.cols() == matrix.rows());
 

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

@@ -298,6 +298,13 @@ template<typename _MatrixType> class EigenSolver
     void doComputeEigenvectors();
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+      EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL);
+    }
+    
     MatrixType m_eivec;
     EigenvalueType m_eivalues;
     bool m_isInitialized;
@@ -364,6 +371,8 @@ template<typename MatrixType>
 EigenSolver<MatrixType>& 
 EigenSolver<MatrixType>::compute(const MatrixType& matrix, bool computeEigenvectors)
 {
+  check_template_parameters();
+  
   using std::sqrt;
   using std::abs;
   eigen_assert(matrix.cols() == matrix.rows());

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

@@ -263,6 +263,13 @@ template<typename _MatrixType> class GeneralizedEigenSolver
     }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+      EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL);
+    }
+    
     MatrixType m_eivec;
     ComplexVectorType m_alphas;
     VectorType m_betas;
@@ -290,6 +297,8 @@ template<typename MatrixType>
 GeneralizedEigenSolver<MatrixType>&
 GeneralizedEigenSolver<MatrixType>::compute(const MatrixType& A, const MatrixType& B, bool computeEigenvectors)
 {
+  check_template_parameters();
+  
   using std::sqrt;
   using std::abs;
   eigen_assert(A.cols() == A.rows() && B.cols() == A.rows() && B.cols() == B.rows());

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

@@ -240,10 +240,10 @@ namespace Eigen {
             m_S.coeffRef(i,j) = Scalar(0.0);
             m_S.rightCols(dim-j-1).applyOnTheLeft(i-1,i,G.adjoint());
             m_T.rightCols(dim-i+1).applyOnTheLeft(i-1,i,G.adjoint());
+            // update Q
+            if (m_computeQZ)
+              m_Q.applyOnTheRight(i-1,i,G);
           }
-          // update Q
-          if (m_computeQZ)
-            m_Q.applyOnTheRight(i-1,i,G);
           // kill T(i,i-1)
           if(m_T.coeff(i,i-1)!=Scalar(0))
           {
@@ -251,10 +251,10 @@ namespace Eigen {
             m_T.coeffRef(i,i-1) = Scalar(0.0);
             m_S.applyOnTheRight(i,i-1,G);
             m_T.topRows(i).applyOnTheRight(i,i-1,G);
+            // update Z
+            if (m_computeQZ)
+              m_Z.applyOnTheLeft(i,i-1,G.adjoint());
           }
-          // update Z
-          if (m_computeQZ)
-            m_Z.applyOnTheLeft(i,i-1,G.adjoint());
         }
       }
     }

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

@@ -234,7 +234,7 @@ template<typename _MatrixType> class RealSchur
     typedef Matrix<Scalar,3,1> Vector3s;
 
     Scalar computeNormOfT();
-    Index findSmallSubdiagEntry(Index iu, const Scalar& norm);
+    Index findSmallSubdiagEntry(Index iu);
     void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift);
     void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo);
     void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector);
@@ -286,7 +286,7 @@ RealSchur<MatrixType>& RealSchur<MatrixType>::computeFromHessenberg(const HessMa
   {
     while (iu >= 0)
     {
-      Index il = findSmallSubdiagEntry(iu, norm);
+      Index il = findSmallSubdiagEntry(iu);
 
       // Check for convergence
       if (il == iu) // One root found
@@ -343,16 +343,14 @@ inline typename MatrixType::Scalar RealSchur<MatrixType>::computeNormOfT()
 
 /** \internal Look for single small sub-diagonal element and returns its index */
 template<typename MatrixType>
-inline typename MatrixType::Index RealSchur<MatrixType>::findSmallSubdiagEntry(Index iu, const Scalar& norm)
+inline typename MatrixType::Index RealSchur<MatrixType>::findSmallSubdiagEntry(Index iu)
 {
   using std::abs;
   Index res = iu;
   while (res > 0)
   {
     Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res));
-    if (s == 0.0)
+    if (abs(m_matT.coeff(res,res-1)) <= NumTraits<Scalar>::epsilon() * s)
-      s = norm;
-    if (abs(m_matT.coeff(res,res-1)) < NumTraits<Scalar>::epsilon() * s)
       break;
     res--;
   }
@@ -457,9 +455,7 @@ inline void RealSchur<MatrixType>::initFrancisQRStep(Index il, Index iu, const V
     const Scalar lhs = m_matT.coeff(im,im-1) * (abs(v.coeff(1)) + abs(v.coeff(2)));
     const Scalar rhs = v.coeff(0) * (abs(m_matT.coeff(im-1,im-1)) + abs(Tmm) + abs(m_matT.coeff(im+1,im+1)));
     if (abs(lhs) < NumTraits<Scalar>::epsilon() * rhs)
-    {
       break;
-    }
   }
 }
 

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

@@ -80,6 +80,8 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
     /** \brief Scalar type for matrices of type \p _MatrixType. */
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::Index Index;
+    
+    typedef Matrix<Scalar,Size,Size,ColMajor,MaxColsAtCompileTime,MaxColsAtCompileTime> EigenvectorsType;
 
     /** \brief Real scalar type for \p _MatrixType.
       *
@@ -225,7 +227,7 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
       *
       * \sa eigenvalues()
       */
-    const MatrixType& eigenvectors() const
+    const EigenvectorsType& eigenvectors() const
     {
       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
@@ -351,7 +353,12 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
     #endif // EIGEN2_SUPPORT
 
   protected:
-    MatrixType m_eivec;
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
+    EigenvectorsType m_eivec;
     RealVectorType m_eivalues;
     typename TridiagonalizationType::SubDiagonalType m_subdiag;
     ComputationInfo m_info;
@@ -376,7 +383,7 @@ template<typename _MatrixType> class SelfAdjointEigenSolver
   * "implicit symmetric QR step with Wilkinson shift"
   */
 namespace internal {
-template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
+template<typename RealScalar, typename Scalar, typename Index>
 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n);
 }
 
@@ -384,6 +391,8 @@ template<typename MatrixType>
 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
 ::compute(const MatrixType& matrix, int options)
 {
+  check_template_parameters();
+  
   using std::abs;
   eigen_assert(matrix.cols() == matrix.rows());
   eigen_assert((options&~(EigVecMask|GenEigMask))==0
@@ -406,7 +415,7 @@ SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
 
   // declare some aliases
   RealVectorType& diag = m_eivalues;
-  MatrixType& mat = m_eivec;
+  EigenvectorsType& mat = m_eivec;
 
   // map the matrix coefficients to [-1:1] to avoid over- and underflow.
   mat = matrix.template triangularView<Lower>();
@@ -442,7 +451,7 @@ SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
     while (start>0 && m_subdiag[start-1]!=0)
       start--;
 
-    internal::tridiagonal_qr_step<MatrixType::Flags&RowMajorBit ? RowMajor : ColMajor>(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
+    internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
   }
 
   if (iter <= m_maxIterations * n)
@@ -490,7 +499,13 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
   typedef typename SolverType::MatrixType MatrixType;
   typedef typename SolverType::RealVectorType VectorType;
   typedef typename SolverType::Scalar Scalar;
+  typedef typename MatrixType::Index Index;
+  typedef typename SolverType::EigenvectorsType EigenvectorsType;
   
+  /** \internal
+   * Computes the roots of the characteristic polynomial of \a m.
+   * For numerical stability m.trace() should be near zero and to avoid over- or underflow m should be normalized.
+   */
   static inline void computeRoots(const MatrixType& m, VectorType& roots)
   {
     using std::sqrt;
@@ -510,158 +525,123 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
     // Construct the parameters used in classifying the roots of the equation
     // and in solving the equation for the roots in closed form.
     Scalar c2_over_3 = c2*s_inv3;
-    Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
+    Scalar a_over_3 = (c2*c2_over_3 - c1)*s_inv3;
-    if (a_over_3 > Scalar(0))
+    if(a_over_3<Scalar(0))
       a_over_3 = Scalar(0);
 
     Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
 
-    Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
+    Scalar q = a_over_3*a_over_3*a_over_3 - half_b*half_b;
-    if (q > Scalar(0))
+    if(q<Scalar(0))
       q = Scalar(0);
 
     // Compute the eigenvalues by solving for the roots of the polynomial.
-    Scalar rho = sqrt(-a_over_3);
+    Scalar rho = sqrt(a_over_3);
-    Scalar theta = atan2(sqrt(-q),half_b)*s_inv3;
+    Scalar theta = atan2(sqrt(q),half_b)*s_inv3;  // since sqrt(q) > 0, atan2 is in [0, pi] and theta is in [0, pi/3]
     Scalar cos_theta = cos(theta);
     Scalar sin_theta = sin(theta);
-    roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
+    // roots are already sorted, since cos is monotonically decreasing on [0, pi]
-    roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
+    roots(0) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta); // == 2*rho*cos(theta+2pi/3)
-    roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
+    roots(1) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta); // == 2*rho*cos(theta+ pi/3)
-
+    roots(2) = c2_over_3 + Scalar(2)*rho*cos_theta;
-    // Sort in increasing order.
-    if (roots(0) >= roots(1))
-      std::swap(roots(0),roots(1));
-    if (roots(1) >= roots(2))
-    {
-      std::swap(roots(1),roots(2));
-      if (roots(0) >= roots(1))
-        std::swap(roots(0),roots(1));
-    }
   }
-  
+
+  static inline bool extract_kernel(MatrixType& mat, Ref<VectorType> res, Ref<VectorType> representative)
+  {
+    using std::abs;
+    Index i0;
+    // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal):
+    mat.diagonal().cwiseAbs().maxCoeff(&i0);
+    // mat.col(i0) is a good candidate for an orthogonal vector to the current eigenvector,
+    // so let's save it:
+    representative = mat.col(i0);
+    Scalar n0, n1;
+    VectorType c0, c1;
+    n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm();
+    n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm();
+    if(n0>n1) res = c0/std::sqrt(n0);
+    else      res = c1/std::sqrt(n1);
+
+    return true;
+  }
+
   static inline void run(SolverType& solver, const MatrixType& mat, int options)
   {
-    using std::sqrt;
     eigen_assert(mat.cols() == 3 && mat.cols() == mat.rows());
     eigen_assert((options&~(EigVecMask|GenEigMask))==0
             && (options&EigVecMask)!=EigVecMask
             && "invalid option parameter");
     bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
     
-    MatrixType& eivecs = solver.m_eivec;
+    EigenvectorsType& eivecs = solver.m_eivec;
     VectorType& eivals = solver.m_eivalues;
   
-    // map the matrix coefficients to [-1:1] to avoid over- and underflow.
+    // Shift the matrix to the mean eigenvalue and map the matrix coefficients to [-1:1] to avoid over- and underflow.
-    Scalar scale = mat.cwiseAbs().maxCoeff();
+    Scalar shift = mat.trace() / Scalar(3);
-    MatrixType scaledMat = mat / scale;
+    // TODO Avoid this copy. Currently it is necessary to suppress bogus values when determining maxCoeff and for computing the eigenvectors later
+    MatrixType scaledMat = mat.template selfadjointView<Lower>();
+    scaledMat.diagonal().array() -= shift;
+    Scalar scale = scaledMat.cwiseAbs().maxCoeff();
+    if(scale > 0) scaledMat /= scale;   // TODO for scale==0 we could save the remaining operations
 
     // compute the eigenvalues
     computeRoots(scaledMat,eivals);
 
-    // compute the eigen vectors
+    // compute the eigenvectors
     if(computeEigenvectors)
     {
-      Scalar safeNorm2 = Eigen::NumTraits<Scalar>::epsilon();
       if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
       {
+        // All three eigenvalues are numerically the same
         eivecs.setIdentity();
       }
       else
       {
-        scaledMat = scaledMat.template selfadjointView<Lower>();
         MatrixType tmp;
         tmp = scaledMat;
 
+        // Compute the eigenvector of the most distinct eigenvalue
         Scalar d0 = eivals(2) - eivals(1);
         Scalar d1 = eivals(1) - eivals(0);
-        int k =  d0 > d1 ? 2 : 0;
+        Index k(0), l(2);
-        d0 = d0 > d1 ? d0 : d1;
+        if(d0 > d1)
-
-        tmp.diagonal().array () -= eivals(k);
-        VectorType cross;
-        Scalar n;
-        n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();
-
-        if(n>safeNorm2)
         {
-          eivecs.col(k) = cross / sqrt(n);
+          std::swap(k,l);
+          d0 = d1;
+        }
+
+        // Compute the eigenvector of index k
+        {
+          tmp.diagonal().array () -= eivals(k);
+          // By construction, 'tmp' is of rank 2, and its kernel corresponds to the respective eigenvector.
+          extract_kernel(tmp, eivecs.col(k), eivecs.col(l));
+        }
+
+        // Compute eigenvector of index l
+        if(d0<=2*Eigen::NumTraits<Scalar>::epsilon()*d1)
+        {
+          // If d0 is too small, then the two other eigenvalues are numerically the same,
+          // and thus we only have to ortho-normalize the near orthogonal vector we saved above.
+          eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l))*eivecs.col(l);
+          eivecs.col(l).normalize();
         }
         else
         {
-          n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm();
+          tmp = scaledMat;
+          tmp.diagonal().array () -= eivals(l);
 
-          if(n>safeNorm2)
+          VectorType dummy;
-          {
+          extract_kernel(tmp, eivecs.col(l), dummy);
-            eivecs.col(k) = cross / sqrt(n);
-          }
-          else
-          {
-            n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm();
-
-            if(n>safeNorm2)
-            {
-              eivecs.col(k) = cross / sqrt(n);
-            }
-            else
-            {
-              // the input matrix and/or the eigenvaues probably contains some inf/NaN,
-              // => exit
-              // scale back to the original size.
-              eivals *= scale;
-
-              solver.m_info = NumericalIssue;
-              solver.m_isInitialized = true;
-              solver.m_eigenvectorsOk = computeEigenvectors;
-              return;
-            }
-          }
         }
 
-        tmp = scaledMat;
+        // Compute last eigenvector from the other two
-        tmp.diagonal().array() -= eivals(1);
+        eivecs.col(1) = eivecs.col(2).cross(eivecs.col(0)).normalized();
-
-        if(d0<=Eigen::NumTraits<Scalar>::epsilon())
-        {
-          eivecs.col(1) = eivecs.col(k).unitOrthogonal();
-        }
-        else
-        {
-          n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm();
-          if(n>safeNorm2)
-          {
-            eivecs.col(1) = cross / sqrt(n);
-          }
-          else
-          {
-            n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm();
-            if(n>safeNorm2)
-              eivecs.col(1) = cross / sqrt(n);
-            else
-            {
-              n = (cross = eivecs.col(k).cross(tmp.row(2))).squaredNorm();
-              if(n>safeNorm2)
-                eivecs.col(1) = cross / sqrt(n);
-              else
-              {
-                // we should never reach this point,
-                // if so the last two eigenvalues are likely to be very close to each other
-                eivecs.col(1) = eivecs.col(k).unitOrthogonal();
-              }
-            }
-          }
-
-          // make sure that eivecs[1] is orthogonal to eivecs[2]
-          // FIXME: this step should not be needed
-          Scalar d = eivecs.col(1).dot(eivecs.col(k));
-          eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized();
-        }
-
-        eivecs.col(k==2 ? 0 : 2) = eivecs.col(k).cross(eivecs.col(1)).normalized();
       }
     }
+
     // Rescale back to the original size.
     eivals *= scale;
+    eivals.array() += shift;
     
     solver.m_info = Success;
     solver.m_isInitialized = true;
@@ -675,11 +655,12 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,2
   typedef typename SolverType::MatrixType MatrixType;
   typedef typename SolverType::RealVectorType VectorType;
   typedef typename SolverType::Scalar Scalar;
+  typedef typename SolverType::EigenvectorsType EigenvectorsType;
   
   static inline void computeRoots(const MatrixType& m, VectorType& roots)
   {
     using std::sqrt;
-    const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*m(1,0)*m(1,0));
+    const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*numext::abs2(m(1,0)));
     const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1));
     roots(0) = t1 - t0;
     roots(1) = t1 + t0;
@@ -688,13 +669,15 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,2
   static inline void run(SolverType& solver, const MatrixType& mat, int options)
   {
     using std::sqrt;
+    using std::abs;
+
     eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows());
     eigen_assert((options&~(EigVecMask|GenEigMask))==0
             && (options&EigVecMask)!=EigVecMask
             && "invalid option parameter");
     bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
     
-    MatrixType& eivecs = solver.m_eivec;
+    EigenvectorsType& eivecs = solver.m_eivec;
     VectorType& eivals = solver.m_eivalues;
   
     // map the matrix coefficients to [-1:1] to avoid over- and underflow.
@@ -708,22 +691,29 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,2
     // compute the eigen vectors
     if(computeEigenvectors)
     {
-      scaledMat.diagonal().array () -= eivals(1);
+      if((eivals(1)-eivals(0))<=abs(eivals(1))*Eigen::NumTraits<Scalar>::epsilon())
-      Scalar a2 = numext::abs2(scaledMat(0,0));
-      Scalar c2 = numext::abs2(scaledMat(1,1));
-      Scalar b2 = numext::abs2(scaledMat(1,0));
-      if(a2>c2)
       {
-        eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0);
+        eivecs.setIdentity();
-        eivecs.col(1) /= sqrt(a2+b2);
       }
       else
       {
-        eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0);
+        scaledMat.diagonal().array () -= eivals(1);
-        eivecs.col(1) /= sqrt(c2+b2);
+        Scalar a2 = numext::abs2(scaledMat(0,0));
-      }
+        Scalar c2 = numext::abs2(scaledMat(1,1));
+        Scalar b2 = numext::abs2(scaledMat(1,0));
+        if(a2>c2)
+        {
+          eivecs.col(1) << -scaledMat(1,0), scaledMat(0,0);
+          eivecs.col(1) /= sqrt(a2+b2);
+        }
+        else
+        {
+          eivecs.col(1) << -scaledMat(1,1), scaledMat(1,0);
+          eivecs.col(1) /= sqrt(c2+b2);
+        }
 
-      eivecs.col(0) << eivecs.col(1).unitOrthogonal();
+        eivecs.col(0) << eivecs.col(1).unitOrthogonal();
+      }
     }
     
     // Rescale back to the original size.
@@ -746,7 +736,7 @@ SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
 }
 
 namespace internal {
-template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
+template<typename RealScalar, typename Scalar, typename Index>
 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n)
 {
   using std::abs;
@@ -798,8 +788,7 @@ static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index sta
     // apply the givens rotation to the unit matrix Q = Q * G
     if (matrixQ)
     {
-      // FIXME if StorageOrder == RowMajor this operation is not very efficient
+      Map<Matrix<Scalar,Dynamic,Dynamic,ColMajor> > q(matrixQ,n,n);
-      Map<Matrix<Scalar,Dynamic,Dynamic,StorageOrder> > q(matrixQ,n,n);
       q.applyOnTheRight(k,k+1,rot);
     }
   }

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

@@ -19,10 +19,12 @@ namespace Eigen {
   *
   * \brief An axis aligned box
   *
-  * \param _Scalar the type of the scalar coefficients
+  * \tparam _Scalar the type of the scalar coefficients
-  * \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
+  * \tparam _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
   *
   * This class represents an axis aligned box as a pair of the minimal and maximal corners.
+  * \warning The result of most methods is undefined when applied to an empty box. You can check for empty boxes using isEmpty().
+  * \sa alignedboxtypedefs
   */
 template <typename _Scalar, int _AmbientDim>
 class AlignedBox
@@ -40,18 +42,21 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
   /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */
   enum CornerType
   {
-    /** 1D names */
+    /** 1D names @{ */
     Min=0, Max=1,
+    /** @} */
 
-    /** Added names for 2D */
+    /** Identifier for 2D corner @{ */
     BottomLeft=0, BottomRight=1,
     TopLeft=2, TopRight=3,
+    /** @} */
 
-    /** Added names for 3D */
+    /** Identifier for 3D corner  @{ */
     BottomLeftFloor=0, BottomRightFloor=1,
     TopLeftFloor=2, TopRightFloor=3,
     BottomLeftCeil=4, BottomRightCeil=5,
     TopLeftCeil=6, TopRightCeil=7
+    /** @} */
   };
 
 
@@ -63,34 +68,33 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
   inline explicit AlignedBox(Index _dim) : m_min(_dim), m_max(_dim)
   { setEmpty(); }
 
-  /** Constructs a box with extremities \a _min and \a _max. */
+  /** Constructs a box with extremities \a _min and \a _max.
+   * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty. */
   template<typename OtherVectorType1, typename OtherVectorType2>
   inline AlignedBox(const OtherVectorType1& _min, const OtherVectorType2& _max) : m_min(_min), m_max(_max) {}
 
   /** Constructs a box containing a single point \a p. */
   template<typename Derived>
-  inline explicit AlignedBox(const MatrixBase<Derived>& a_p)
+  inline explicit AlignedBox(const MatrixBase<Derived>& p) : m_min(p), m_max(m_min)
-  {
+  { }
-    typename internal::nested<Derived,2>::type p(a_p.derived());
-    m_min = p;
-    m_max = p;
-  }
 
   ~AlignedBox() {}
 
   /** \returns the dimension in which the box holds */
   inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); }
 
-  /** \deprecated use isEmpty */
+  /** \deprecated use isEmpty() */
   inline bool isNull() const { return isEmpty(); }
 
-  /** \deprecated use setEmpty */
+  /** \deprecated use setEmpty() */
   inline void setNull() { setEmpty(); }
 
-  /** \returns true if the box is empty. */
+  /** \returns true if the box is empty.
+   * \sa setEmpty */
   inline bool isEmpty() const { return (m_min.array() > m_max.array()).any(); }
 
-  /** Makes \c *this an empty box. */
+  /** Makes \c *this an empty box.
+   * \sa isEmpty */
   inline void setEmpty()
   {
     m_min.setConstant( ScalarTraits::highest() );
@@ -159,7 +163,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
    * a uniform distribution */
   inline VectorType sample() const
   {
-    VectorType r;
+    VectorType r(dim());
     for(Index d=0; d<dim(); ++d)
     {
       if(!ScalarTraits::IsInteger)
@@ -175,27 +179,34 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
 
   /** \returns true if the point \a p is inside the box \c *this. */
   template<typename Derived>
-  inline bool contains(const MatrixBase<Derived>& a_p) const
+  inline bool contains(const MatrixBase<Derived>& p) const
   {
-    typename internal::nested<Derived,2>::type p(a_p.derived());
+    typename internal::nested<Derived,2>::type p_n(p.derived());
-    return (m_min.array()<=p.array()).all() && (p.array()<=m_max.array()).all();
+    return (m_min.array()<=p_n.array()).all() && (p_n.array()<=m_max.array()).all();
   }
 
   /** \returns true if the box \a b is entirely inside the box \c *this. */
   inline bool contains(const AlignedBox& b) const
   { return (m_min.array()<=(b.min)().array()).all() && ((b.max)().array()<=m_max.array()).all(); }
 
-  /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. */
+  /** \returns true if the box \a b is intersecting the box \c *this.
+   * \sa intersection, clamp */
+  inline bool intersects(const AlignedBox& b) const
+  { return (m_min.array()<=(b.max)().array()).all() && ((b.min)().array()<=m_max.array()).all(); }
+
+  /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this.
+   * \sa extend(const AlignedBox&) */
   template<typename Derived>
-  inline AlignedBox& extend(const MatrixBase<Derived>& a_p)
+  inline AlignedBox& extend(const MatrixBase<Derived>& p)
   {
-    typename internal::nested<Derived,2>::type p(a_p.derived());
+    typename internal::nested<Derived,2>::type p_n(p.derived());
-    m_min = m_min.cwiseMin(p);
+    m_min = m_min.cwiseMin(p_n);
-    m_max = m_max.cwiseMax(p);
+    m_max = m_max.cwiseMax(p_n);
     return *this;
   }
 
-  /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. */
+  /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this.
+   * \sa merged, extend(const MatrixBase&) */
   inline AlignedBox& extend(const AlignedBox& b)
   {
     m_min = m_min.cwiseMin(b.m_min);
@@ -203,7 +214,9 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
     return *this;
   }
 
-  /** Clamps \c *this by the box \a b and returns a reference to \c *this. */
+  /** Clamps \c *this by the box \a b and returns a reference to \c *this.
+   * \note If the boxes don't intersect, the resulting box is empty.
+   * \sa intersection(), intersects() */
   inline AlignedBox& clamp(const AlignedBox& b)
   {
     m_min = m_min.cwiseMax(b.m_min);
@@ -211,11 +224,15 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
     return *this;
   }
 
-  /** Returns an AlignedBox that is the intersection of \a b and \c *this */
+  /** Returns an AlignedBox that is the intersection of \a b and \c *this
+   * \note If the boxes don't intersect, the resulting box is empty.
+   * \sa intersects(), clamp, contains()  */
   inline AlignedBox intersection(const AlignedBox& b) const
   {return AlignedBox(m_min.cwiseMax(b.m_min), m_max.cwiseMin(b.m_max)); }
 
-  /** Returns an AlignedBox that is the union of \a b and \c *this */
+  /** Returns an AlignedBox that is the union of \a b and \c *this.
+   * \note Merging with an empty box may result in a box bigger than \c *this. 
+   * \sa extend(const AlignedBox&) */
   inline AlignedBox merged(const AlignedBox& b) const
   { return AlignedBox(m_min.cwiseMin(b.m_min), m_max.cwiseMax(b.m_max)); }
 
@@ -231,20 +248,20 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
 
   /** \returns the squared distance between the point \a p and the box \c *this,
     * and zero if \a p is inside the box.
-    * \sa exteriorDistance()
+    * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const AlignedBox&)
     */
   template<typename Derived>
-  inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& a_p) const;
+  inline Scalar squaredExteriorDistance(const MatrixBase<Derived>& p) const;
 
   /** \returns the squared distance between the boxes \a b and \c *this,
     * and zero if the boxes intersect.
-    * \sa exteriorDistance()
+    * \sa exteriorDistance(const AlignedBox&), squaredExteriorDistance(const MatrixBase&)
     */
   inline Scalar squaredExteriorDistance(const AlignedBox& b) const;
 
   /** \returns the distance between the point \a p and the box \c *this,
     * and zero if \a p is inside the box.
-    * \sa squaredExteriorDistance()
+    * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const AlignedBox&)
     */
   template<typename Derived>
   inline NonInteger exteriorDistance(const MatrixBase<Derived>& p) const
@@ -252,7 +269,7 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
 
   /** \returns the distance between the boxes \a b and \c *this,
     * and zero if the boxes intersect.
-    * \sa squaredExteriorDistance()
+    * \sa squaredExteriorDistance(const AlignedBox&), exteriorDistance(const MatrixBase&)
     */
   inline NonInteger exteriorDistance(const AlignedBox& b) const
   { using std::sqrt; return sqrt(NonInteger(squaredExteriorDistance(b))); }

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

@@ -131,7 +131,7 @@ public:
     m_angle = Scalar(other.angle());
   }
 
-  static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
+  static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
 
   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
     * determined by \a prec.
@@ -165,8 +165,8 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived
   Scalar n2 = q.vec().squaredNorm();
   if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
   {
-    m_angle = 0;
+    m_angle = Scalar(0);
-    m_axis << 1, 0, 0;
+    m_axis << Scalar(1), Scalar(0), Scalar(0);
   }
   else
   {

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

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

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

@@ -102,11 +102,11 @@ public:
   /** \returns a quaternion representing an identity rotation
     * \sa MatrixBase::Identity()
     */
-  static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
+  static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); }
 
   /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity()
     */
-  inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
+  inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; }
 
   /** \returns the squared norm of the quaternion's coefficients
     * \sa QuaternionBase::norm(), MatrixBase::squaredNorm()
@@ -161,7 +161,7 @@ public:
   { return coeffs().isApprox(other.coeffs(), prec); }
 
 	/** return the result vector of \a v through the rotation*/
-  EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
+  EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
 
   /** \returns \c *this with scalar type casted to \a NewScalarType
     *
@@ -231,7 +231,7 @@ class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
 public:
   typedef _Scalar Scalar;
 
-  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion)
   using Base::operator*=;
 
   typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
@@ -341,7 +341,7 @@ class Map<const Quaternion<_Scalar>, _Options >
   public:
     typedef _Scalar Scalar;
     typedef typename internal::traits<Map>::Coefficients Coefficients;
-    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
     using Base::operator*=;
 
     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
@@ -378,7 +378,7 @@ class Map<Quaternion<_Scalar>, _Options >
   public:
     typedef _Scalar Scalar;
     typedef typename internal::traits<Map>::Coefficients Coefficients;
-    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
     using Base::operator*=;
 
     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
@@ -461,7 +461,7 @@ EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const Quaterni
   */
 template <class Derived>
 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
-QuaternionBase<Derived>::_transformVector(Vector3 v) const
+QuaternionBase<Derived>::_transformVector(const Vector3& v) const
 {
     // Note that this algorithm comes from the optimization by hand
     // of the conversion to a Matrix followed by a Matrix/Vector product.
@@ -637,7 +637,7 @@ inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Der
 {
   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
   Scalar n2 = this->squaredNorm();
-  if (n2 > 0)
+  if (n2 > Scalar(0))
     return Quaternion<Scalar>(conjugate().coeffs() / n2);
   else
   {
@@ -667,12 +667,10 @@ template <class OtherDerived>
 inline typename internal::traits<Derived>::Scalar
 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
 {
-  using std::acos;
+  using std::atan2;
   using std::abs;
-  Scalar d = abs(this->dot(other));
+  Quaternion<Scalar> d = (*this) * other.conjugate();
-  if (d>=Scalar(1))
+  return Scalar(2) * atan2( d.vec().norm(), abs(d.w()) );
-    return Scalar(0);
-  return Scalar(2) * acos(d);
 }
 
  
@@ -712,7 +710,7 @@ QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerive
     scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
     scale1 = sin( ( t * theta) ) / sinTheta;
   }
-  if(d<0) scale1 = -scale1;
+  if(d<Scalar(0)) scale1 = -scale1;
 
   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
 }

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

@@ -65,10 +65,10 @@ class DiagonalPreconditioner
       {
         typename MatType::InnerIterator it(mat,j);
         while(it && it.index()!=j) ++it;
-        if(it && it.index()==j)
+        if(it && it.index()==j && it.value()!=Scalar(0))
           m_invdiag(j) = Scalar(1)/it.value();
         else
-          m_invdiag(j) = 0;
+          m_invdiag(j) = Scalar(1);
       }
       m_isInitialized = true;
       return *this;

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

@@ -151,20 +151,7 @@ struct traits<BiCGSTAB<_MatrixType,_Preconditioner> >
   * \endcode
   * 
   * By default the iterations start with x=0 as an initial guess of the solution.
-  * One can control the start using the solveWithGuess() method. Here is a step by
+  * One can control the start using the solveWithGuess() method.
-  * step execution example starting with a random guess and printing the evolution
-  * of the estimated error:
-  * * \code
-  * x = VectorXd::Random(n);
-  * solver.setMaxIterations(1);
-  * int i = 0;
-  * do {
-  *   x = solver.solveWithGuess(b,x);
-  *   std::cout << i << " : " << solver.error() << std::endl;
-  *   ++i;
-  * } while (solver.info()!=Success && i<100);
-  * \endcode
-  * Note that such a step by step excution is slightly slower.
   * 
   * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
   */
@@ -199,7 +186,8 @@ public:
     * this class becomes invalid. Call compute() to update it with the new
     * matrix A, or modify a copy of A.
     */
-  BiCGSTAB(const MatrixType& A) : Base(A) {}
+  template<typename MatrixDerived>
+  explicit BiCGSTAB(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
 
   ~BiCGSTAB() {}
   

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

@@ -112,9 +112,9 @@ struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
   * This class allows to solve for A.x = b sparse linear problems using a conjugate gradient algorithm.
   * The sparse matrix A must be selfadjoint. The vectors x and b can be either dense or sparse.
   *
-  * \tparam _MatrixType the type of the sparse matrix A, can be a dense or a sparse matrix.
+  * \tparam _MatrixType the type of the matrix A, can be a dense or a sparse matrix.
-  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
+  * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower,
-  *               or Upper. Default is Lower.
+  *               Upper, or Lower|Upper in which the full matrix entries will be considered. Default is Lower.
   * \tparam _Preconditioner the type of the preconditioner. Default is DiagonalPreconditioner
   *
   * The maximal number of iterations and tolerance value can be controlled via the setMaxIterations()
@@ -137,20 +137,7 @@ struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
   * \endcode
   * 
   * By default the iterations start with x=0 as an initial guess of the solution.
-  * One can control the start using the solveWithGuess() method. Here is a step by
+  * One can control the start using the solveWithGuess() method.
-  * step execution example starting with a random guess and printing the evolution
-  * of the estimated error:
-  * * \code
-  * x = VectorXd::Random(n);
-  * cg.setMaxIterations(1);
-  * int i = 0;
-  * do {
-  *   x = cg.solveWithGuess(b,x);
-  *   std::cout << i << " : " << cg.error() << std::endl;
-  *   ++i;
-  * } while (cg.info()!=Success && i<100);
-  * \endcode
-  * Note that such a step by step excution is slightly slower.
   * 
   * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
   */
@@ -189,7 +176,8 @@ public:
     * this class becomes invalid. Call compute() to update it with the new
     * matrix A, or modify a copy of A.
     */
-  ConjugateGradient(const MatrixType& A) : Base(A) {}
+  template<typename MatrixDerived>
+  explicit ConjugateGradient(const EigenBase<MatrixDerived>& A) : Base(A.derived()) {}
 
   ~ConjugateGradient() {}
   
@@ -213,6 +201,10 @@ public:
   template<typename Rhs,typename Dest>
   void _solveWithGuess(const Rhs& b, Dest& x) const
   {
+    typedef typename internal::conditional<UpLo==(Lower|Upper),
+                                           const MatrixType&,
+                                           SparseSelfAdjointView<const MatrixType, UpLo>
+                                          >::type MatrixWrapperType;
     m_iterations = Base::maxIterations();
     m_error = Base::m_tolerance;
 
@@ -222,8 +214,7 @@ public:
       m_error = Base::m_tolerance;
 
       typename Dest::ColXpr xj(x,j);
-      internal::conjugate_gradient(mp_matrix->template selfadjointView<UpLo>(), b.col(j), xj,
+      internal::conjugate_gradient(MatrixWrapperType(*mp_matrix), b.col(j), xj, Base::m_preconditioner, m_iterations, m_error);
-                                   Base::m_preconditioner, m_iterations, m_error);
     }
 
     m_isInitialized = true;
@@ -234,7 +225,7 @@ public:
   template<typename Rhs,typename Dest>
   void _solve(const Rhs& b, Dest& x) const
   {
-    x.setOnes();
+    x.setZero();
     _solveWithGuess(b,x);
   }
 

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

@@ -150,7 +150,6 @@ class IncompleteLUT : internal::noncopyable
     {
       analyzePattern(amat); 
       factorize(amat);
-      m_isInitialized = m_factorizationIsOk;
       return *this;
     }
 
@@ -160,7 +159,7 @@ class IncompleteLUT : internal::noncopyable
     template<typename Rhs, typename Dest>
     void _solve(const Rhs& b, Dest& x) const
     {
-      x = m_Pinv * b;  
+      x = m_Pinv * b;
       x = m_lu.template triangularView<UnitLower>().solve(x);
       x = m_lu.template triangularView<Upper>().solve(x);
       x = m_P * x; 
@@ -223,18 +222,29 @@ template<typename _MatrixType>
 void IncompleteLUT<Scalar>::analyzePattern(const _MatrixType& amat)
 {
   // Compute the Fill-reducing permutation
+  // Since ILUT does not perform any numerical pivoting,
+  // it is highly preferable to keep the diagonal through symmetric permutations.
+#ifndef EIGEN_MPL2_ONLY
+  // To this end, let's symmetrize the pattern and perform AMD on it.
   SparseMatrix<Scalar,ColMajor, Index> mat1 = amat;
   SparseMatrix<Scalar,ColMajor, Index> mat2 = amat.transpose();
-  // Symmetrize the pattern
   // FIXME for a matrix with nearly symmetric pattern, mat2+mat1 is the appropriate choice.
   //       on the other hand for a really non-symmetric pattern, mat2*mat1 should be prefered...
   SparseMatrix<Scalar,ColMajor, Index> AtA = mat2 + mat1;
-  AtA.prune(keep_diag());
+  AMDOrdering<Index> ordering;
-  internal::minimum_degree_ordering<Scalar, Index>(AtA, m_P);  // Then compute the AMD ordering...
+  ordering(AtA,m_P);
-
+  m_Pinv  = m_P.inverse(); // cache the inverse permutation
-  m_Pinv  = m_P.inverse(); // ... and the inverse permutation
+#else
+  // If AMD is not available, (MPL2-only), then let's use the slower COLAMD routine.
+  SparseMatrix<Scalar,ColMajor, Index> mat1 = amat;
+  COLAMDOrdering<Index> ordering;
+  ordering(mat1,m_Pinv);
+  m_P = m_Pinv.inverse();
+#endif
 
   m_analysisIsOk = true;
+  m_factorizationIsOk = false;
+  m_isInitialized = false;
 }
 
 template <typename Scalar>
@@ -442,6 +452,7 @@ void IncompleteLUT<Scalar>::factorize(const _MatrixType& amat)
   m_lu.makeCompressed();
 
   m_factorizationIsOk = true;
+  m_isInitialized = m_factorizationIsOk;
   m_info = Success;
 }
 

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

@@ -49,10 +49,11 @@ public:
     * this class becomes invalid. Call compute() to update it with the new
     * matrix A, or modify a copy of A.
     */
-  IterativeSolverBase(const MatrixType& A)
+  template<typename InputDerived>
+  IterativeSolverBase(const EigenBase<InputDerived>& A)
   {
     init();
-    compute(A);
+    compute(A.derived());
   }
 
   ~IterativeSolverBase() {}
@@ -62,9 +63,11 @@ public:
     * Currently, this function mostly call analyzePattern on the preconditioner. In the future
     * we might, for instance, implement column reodering for faster matrix vector products.
     */
-  Derived& analyzePattern(const MatrixType& A)
+  template<typename InputDerived>
+  Derived& analyzePattern(const EigenBase<InputDerived>& A)
   {
-    m_preconditioner.analyzePattern(A);
+    grabInput(A.derived());
+    m_preconditioner.analyzePattern(*mp_matrix);
     m_isInitialized = true;
     m_analysisIsOk = true;
     m_info = Success;
@@ -80,11 +83,12 @@ public:
     * this class becomes invalid. Call compute() to update it with the new
     * matrix A, or modify a copy of A.
     */
-  Derived& factorize(const MatrixType& A)
+  template<typename InputDerived>
+  Derived& factorize(const EigenBase<InputDerived>& A)
   {
+    grabInput(A.derived());
     eigen_assert(m_analysisIsOk && "You must first call analyzePattern()"); 
-    mp_matrix = &A;
+    m_preconditioner.factorize(*mp_matrix);
-    m_preconditioner.factorize(A);
     m_factorizationIsOk = true;
     m_info = Success;
     return derived();
@@ -100,10 +104,11 @@ public:
     * this class becomes invalid. Call compute() to update it with the new
     * matrix A, or modify a copy of A.
     */
-  Derived& compute(const MatrixType& A)
+  template<typename InputDerived>
+  Derived& compute(const EigenBase<InputDerived>& A)
   {
-    mp_matrix = &A;
+    grabInput(A.derived());
-    m_preconditioner.compute(A);
+    m_preconditioner.compute(*mp_matrix);
     m_isInitialized = true;
     m_analysisIsOk = true;
     m_factorizationIsOk = true;
@@ -212,6 +217,28 @@ public:
   }
 
 protected:
+
+  template<typename InputDerived>
+  void grabInput(const EigenBase<InputDerived>& A)
+  {
+    // we const cast to prevent the creation of a MatrixType temporary by the compiler.
+    grabInput_impl(A.const_cast_derived());
+  }
+
+  template<typename InputDerived>
+  void grabInput_impl(const EigenBase<InputDerived>& A)
+  {
+    m_copyMatrix = A;
+    mp_matrix = &m_copyMatrix;
+  }
+
+  void grabInput_impl(MatrixType& A)
+  {
+    if(MatrixType::RowsAtCompileTime==Dynamic && MatrixType::ColsAtCompileTime==Dynamic)
+      m_copyMatrix.resize(0,0);
+    mp_matrix = &A;
+  }
+
   void init()
   {
     m_isInitialized = false;
@@ -220,6 +247,7 @@ protected:
     m_maxIterations = -1;
     m_tolerance = NumTraits<Scalar>::epsilon();
   }
+  MatrixType m_copyMatrix;
   const MatrixType* mp_matrix;
   Preconditioner m_preconditioner;
 

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

@@ -374,6 +374,12 @@ template<typename _MatrixType> class FullPivLU
     inline Index cols() const { return m_lu.cols(); }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     MatrixType m_lu;
     PermutationPType m_p;
     PermutationQType m_q;
@@ -418,6 +424,8 @@ FullPivLU<MatrixType>::FullPivLU(const MatrixType& matrix)
 template<typename MatrixType>
 FullPivLU<MatrixType>& FullPivLU<MatrixType>::compute(const MatrixType& matrix)
 {
+  check_template_parameters();
+  
   // the permutations are stored as int indices, so just to be sure:
   eigen_assert(matrix.rows()<=NumTraits<int>::highest() && matrix.cols()<=NumTraits<int>::highest());
   

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

@@ -171,6 +171,12 @@ template<typename _MatrixType> class PartialPivLU
     inline Index cols() const { return m_lu.cols(); }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     MatrixType m_lu;
     PermutationType m_p;
     TranspositionType m_rowsTranspositions;
@@ -386,6 +392,8 @@ void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions, t
 template<typename MatrixType>
 PartialPivLU<MatrixType>& PartialPivLU<MatrixType>::compute(const MatrixType& matrix)
 {
+  check_template_parameters();
+  
   // the row permutation is stored as int indices, so just to be sure:
   eigen_assert(matrix.rows()<NumTraits<int>::highest());
   

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

@@ -137,22 +137,27 @@ void minimum_degree_ordering(SparseMatrix<Scalar,ColMajor,Index>& C, Permutation
     degree[i] = len[i];                 // degree of node i
   }
   mark = internal::cs_wclear<Index>(0, 0, w, n);         /* clear w */
-  elen[n] = -2;                         /* n is a dead element */
-  Cp[n] = -1;                           /* n is a root of assembly tree */
-  w[n] = 0;                             /* n is a dead element */
   
   /* --- Initialize degree lists ------------------------------------------ */
   for(i = 0; i < n; i++)
   {
+    bool has_diag = false;
+    for(p = Cp[i]; p<Cp[i+1]; ++p)
+      if(Ci[p]==i)
+      {
+        has_diag = true;
+        break;
+      }
+   
     d = degree[i];
-    if(d == 0)                         /* node i is empty */
+    if(d == 1 && has_diag)           /* node i is empty */
     {
       elen[i] = -2;                 /* element i is dead */
       nel++;
       Cp[i] = -1;                   /* i is a root of assembly tree */
       w[i] = 0;
     }
-    else if(d > dense)                 /* node i is dense */
+    else if(d > dense || !has_diag)  /* node i is dense or has no structural diagonal element */
     {
       nv[i] = 0;                    /* absorb i into element n */
       elen[i] = -1;                 /* node i is dead */
@@ -168,6 +173,10 @@ void minimum_degree_ordering(SparseMatrix<Scalar,ColMajor,Index>& C, Permutation
     }
   }
   
+  elen[n] = -2;                         /* n is a dead element */
+  Cp[n] = -1;                           /* n is a root of assembly tree */
+  w[n] = 0;                             /* n is a dead element */
+  
   while (nel < n)                         /* while (selecting pivots) do */
   {
     /* --- Select node of minimum approximate degree -------------------- */

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

@@ -384,6 +384,12 @@ template<typename _MatrixType> class ColPivHouseholderQR
     }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     MatrixType m_qr;
     HCoeffsType m_hCoeffs;
     PermutationType m_colsPermutation;
@@ -422,6 +428,8 @@ typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::logAbsDetermina
 template<typename MatrixType>
 ColPivHouseholderQR<MatrixType>& ColPivHouseholderQR<MatrixType>::compute(const MatrixType& matrix)
 {
+  check_template_parameters();
+  
   using std::abs;
   Index rows = matrix.rows();
   Index cols = matrix.cols();
@@ -463,20 +471,10 @@ ColPivHouseholderQR<MatrixType>& ColPivHouseholderQR<MatrixType>::compute(const
     // we store that back into our table: it can't hurt to correct our table.
     m_colSqNorms.coeffRef(biggest_col_index) = biggest_col_sq_norm;
 
-    // if the current biggest column is smaller than epsilon times the initial biggest column,
+    // Track the number of meaningful pivots but do not stop the decomposition to make
-    // terminate to avoid generating nan/inf values.
+    // sure that the initial matrix is properly reproduced. See bug 941.
-    // Note that here, if we test instead for "biggest == 0", we get a failure every 1000 (or so)
+    if(m_nonzero_pivots==size && biggest_col_sq_norm < threshold_helper * RealScalar(rows-k))
-    // repetitions of the unit test, with the result of solve() filled with large values of the order
-    // of 1/(size*epsilon).
-    if(biggest_col_sq_norm < threshold_helper * RealScalar(rows-k))
-    {
       m_nonzero_pivots = k;
-      m_hCoeffs.tail(size-k).setZero();
-      m_qr.bottomRightCorner(rows-k,cols-k)
-          .template triangularView<StrictlyLower>()
-          .setZero();
-      break;
-    }
 
     // apply the transposition to the columns
     m_colsTranspositions.coeffRef(k) = biggest_col_index;
@@ -505,7 +503,7 @@ ColPivHouseholderQR<MatrixType>& ColPivHouseholderQR<MatrixType>::compute(const
   }
 
   m_colsPermutation.setIdentity(PermIndexType(cols));
-  for(PermIndexType k = 0; k < m_nonzero_pivots; ++k)
+  for(PermIndexType k = 0; k < size/*m_nonzero_pivots*/; ++k)
     m_colsPermutation.applyTranspositionOnTheRight(k, PermIndexType(m_colsTranspositions.coeff(k)));
 
   m_det_pq = (number_of_transpositions%2) ? -1 : 1;
@@ -555,13 +553,15 @@ struct solve_retval<ColPivHouseholderQR<_MatrixType>, Rhs>
 
 } // end namespace internal
 
-/** \returns the matrix Q as a sequence of householder transformations */
+/** \returns the matrix Q as a sequence of householder transformations.
+  * You can extract the meaningful part only by using:
+  * \code qr.householderQ().setLength(qr.nonzeroPivots()) \endcode*/
 template<typename MatrixType>
 typename ColPivHouseholderQR<MatrixType>::HouseholderSequenceType ColPivHouseholderQR<MatrixType>
   ::householderQ() const
 {
   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-  return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate()).setLength(m_nonzero_pivots);
+  return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate());
 }
 
 /** \return the column-pivoting Householder QR decomposition of \c *this.

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

@@ -63,12 +63,12 @@ ColPivHouseholderQR<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynami
 \
   m_nonzero_pivots = 0; \
   m_maxpivot = RealScalar(0);\
-  m_colsPermutation.resize((int)cols); \
+  m_colsPermutation.resize(cols); \
   m_colsPermutation.indices().setZero(); \
 \
-  lapack_int lda = (lapack_int) m_qr.outerStride(), i; \
+  lapack_int lda = m_qr.outerStride(), i; \
   lapack_int matrix_order = MKLCOLROW; \
-  LAPACKE_##MKLPREFIX##geqp3( matrix_order, (lapack_int)rows, (lapack_int)cols, (MKLTYPE*)m_qr.data(), lda, (lapack_int*)m_colsPermutation.indices().data(), (MKLTYPE*)m_hCoeffs.data()); \
+  LAPACKE_##MKLPREFIX##geqp3( matrix_order, rows, cols, (MKLTYPE*)m_qr.data(), lda, (lapack_int*)m_colsPermutation.indices().data(), (MKLTYPE*)m_hCoeffs.data()); \
   m_isInitialized = true; \
   m_maxpivot=m_qr.diagonal().cwiseAbs().maxCoeff(); \
   m_hCoeffs.adjointInPlace(); \

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

@@ -368,6 +368,12 @@ template<typename _MatrixType> class FullPivHouseholderQR
     RealScalar maxPivot() const { return m_maxpivot; }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     MatrixType m_qr;
     HCoeffsType m_hCoeffs;
     IntDiagSizeVectorType m_rows_transpositions;
@@ -407,6 +413,8 @@ typename MatrixType::RealScalar FullPivHouseholderQR<MatrixType>::logAbsDetermin
 template<typename MatrixType>
 FullPivHouseholderQR<MatrixType>& FullPivHouseholderQR<MatrixType>::compute(const MatrixType& matrix)
 {
+  check_template_parameters();
+  
   using std::abs;
   Index rows = matrix.rows();
   Index cols = matrix.cols();

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

@@ -189,6 +189,12 @@ template<typename _MatrixType> class HouseholderQR
     const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
 
   protected:
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
+    
     MatrixType m_qr;
     HCoeffsType m_hCoeffs;
     RowVectorType m_temp;
@@ -349,6 +355,8 @@ struct solve_retval<HouseholderQR<_MatrixType>, Rhs>
 template<typename MatrixType>
 HouseholderQR<MatrixType>& HouseholderQR<MatrixType>::compute(const MatrixType& matrix)
 {
+  check_template_parameters();
+  
   Index rows = matrix.rows();
   Index cols = matrix.cols();
   Index size = (std::min)(rows,cols);

gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h

@@ -47,7 +47,7 @@ namespace Eigen {
  * You can then apply it to a vector.
  * 
  * R is the sparse triangular factor. Use matrixQR() to get it as SparseMatrix.
- * NOTE : The Index type of R is always UF_long. You can get it with SPQR::Index
+ * NOTE : The Index type of R is always SuiteSparse_long. You can get it with SPQR::Index
  * 
  * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<>
  * NOTE 
@@ -59,24 +59,18 @@ class SPQR
   public:
     typedef typename _MatrixType::Scalar Scalar;
     typedef typename _MatrixType::RealScalar RealScalar;
-    typedef UF_long Index ; 
+    typedef SuiteSparse_long Index ;
     typedef SparseMatrix<Scalar, ColMajor, Index> MatrixType;
     typedef PermutationMatrix<Dynamic, Dynamic> PermutationType;
   public:
     SPQR() 
-      : m_isInitialized(false),
+      : m_isInitialized(false), m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits<Scalar>::epsilon()), m_useDefaultThreshold(true)
-      m_ordering(SPQR_ORDERING_DEFAULT),
-      m_allow_tol(SPQR_DEFAULT_TOL),
-      m_tolerance (NumTraits<Scalar>::epsilon())
     { 
       cholmod_l_start(&m_cc);
     }
     
-    SPQR(const _MatrixType& matrix) 
+    SPQR(const _MatrixType& matrix)
-    : m_isInitialized(false),
+    : m_isInitialized(false), m_ordering(SPQR_ORDERING_DEFAULT), m_allow_tol(SPQR_DEFAULT_TOL), m_tolerance (NumTraits<Scalar>::epsilon()), m_useDefaultThreshold(true)
-      m_ordering(SPQR_ORDERING_DEFAULT),
-      m_allow_tol(SPQR_DEFAULT_TOL),
-      m_tolerance (NumTraits<Scalar>::epsilon())
     {
       cholmod_l_start(&m_cc);
       compute(matrix);
@@ -101,10 +95,26 @@ class SPQR
       if(m_isInitialized) SPQR_free();
 
       MatrixType mat(matrix);
+      
+      /* Compute the default threshold as in MatLab, see:
+       * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
+       * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3 
+       */
+      RealScalar pivotThreshold = m_tolerance;
+      if(m_useDefaultThreshold) 
+      {
+        using std::max;
+        RealScalar max2Norm = 0.0;
+        for (int j = 0; j < mat.cols(); j++) max2Norm = (max)(max2Norm, mat.col(j).norm());
+        if(max2Norm==RealScalar(0))
+          max2Norm = RealScalar(1);
+        pivotThreshold = 20 * (mat.rows() + mat.cols()) * max2Norm * NumTraits<RealScalar>::epsilon();
+      }
+      
       cholmod_sparse A; 
       A = viewAsCholmod(mat);
       Index col = matrix.cols();
-      m_rank = SuiteSparseQR<Scalar>(m_ordering, m_tolerance, col, &A, 
+      m_rank = SuiteSparseQR<Scalar>(m_ordering, pivotThreshold, col, &A, 
                              &m_cR, &m_E, &m_H, &m_HPinv, &m_HTau, &m_cc);
 
       if (!m_cR)
@@ -120,7 +130,7 @@ class SPQR
     /** 
      * Get the number of rows of the input matrix and the Q matrix
      */
-    inline Index rows() const {return m_H->nrow; }
+    inline Index rows() const {return m_cR->nrow; }
     
     /** 
      * Get the number of columns of the input matrix. 
@@ -145,16 +155,25 @@ class SPQR
     {
       eigen_assert(m_isInitialized && " The QR factorization should be computed first, call compute()");
       eigen_assert(b.cols()==1 && "This method is for vectors only");
-      
+
       //Compute Q^T * b
-      typename Dest::PlainObject y;
+      typename Dest::PlainObject y, y2;
       y = matrixQ().transpose() * b;
-        // Solves with the triangular matrix R
+      
+      // Solves with the triangular matrix R
       Index rk = this->rank();
-      y.topRows(rk) = this->matrixR().topLeftCorner(rk, rk).template triangularView<Upper>().solve(y.topRows(rk));
+      y2 = y;
-      y.bottomRows(cols()-rk).setZero();
+      y.resize((std::max)(cols(),Index(y.rows())),y.cols());
+      y.topRows(rk) = this->matrixR().topLeftCorner(rk, rk).template triangularView<Upper>().solve(y2.topRows(rk));
+
       // Apply the column permutation 
-      dest.topRows(cols()) = colsPermutation() * y.topRows(cols());
+      // colsPermutation() performs a copy of the permutation,
+      // so let's apply it manually:
+      for(Index i = 0; i < rk; ++i) dest.row(m_E[i]) = y.row(i);
+      for(Index i = rk; i < cols(); ++i) dest.row(m_E[i]).setZero();
+      
+//       y.bottomRows(y.rows()-rk).setZero();
+//       dest = colsPermutation() * y.topRows(cols());
       
       m_info = Success;
     }
@@ -197,7 +216,11 @@ class SPQR
     /// Set the fill-reducing ordering method to be used
     void setSPQROrdering(int ord) { m_ordering = ord;}
     /// Set the tolerance tol to treat columns with 2-norm < =tol as zero
-    void setPivotThreshold(const RealScalar& tol) { m_tolerance = tol; }
+    void setPivotThreshold(const RealScalar& tol)
+    {
+      m_useDefaultThreshold = false;
+      m_tolerance = tol;
+    }
     
     /** \returns a pointer to the SPQR workspace */
     cholmod_common *cholmodCommon() const { return &m_cc; }
@@ -230,6 +253,7 @@ class SPQR
     mutable cholmod_dense *m_HTau; // The Householder coefficients
     mutable Index m_rank; // The rank of the matrix
     mutable cholmod_common m_cc; // Workspace and parameters
+    bool m_useDefaultThreshold;     // Use default threshold
     template<typename ,typename > friend struct SPQR_QProduct;
 };
 

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

@@ -742,6 +742,11 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
 
   private:
     void allocate(Index rows, Index cols, unsigned int computationOptions);
+    
+    static void check_template_parameters()
+    {
+      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
+    }
 
   protected:
     MatrixUType m_matrixU;
@@ -818,6 +823,8 @@ template<typename MatrixType, int QRPreconditioner>
 JacobiSVD<MatrixType, QRPreconditioner>&
 JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsigned int computationOptions)
 {
+  check_template_parameters();
+  
   using std::abs;
   allocate(matrix.rows(), matrix.cols(), computationOptions);
 

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

@@ -57,6 +57,16 @@ public:
     inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols)
       : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols)
     {}
+    
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 :  m_outerStart);
+    }
+    
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
+    }
 
     EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
     EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
@@ -226,6 +236,21 @@ public:
       else
         return Map<const Matrix<Index,OuterSize,1> >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum();
     }
+    
+    inline Scalar& coeffRef(int row, int col)
+    {
+      return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
+    }
+    
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
+    }
+    
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
+    }
 
     const Scalar& lastCoeff() const
     {
@@ -313,6 +338,16 @@ public:
       else
         return Map<const Matrix<Index,OuterSize,1> >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum();
     }
+    
+    inline const Scalar coeff(int row, int col) const
+    {
+      return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 :  m_outerStart));
+    }
+    
+    inline const Scalar coeff(int index) const
+    {
+      return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index :  m_outerStart);
+    }
 
     const Scalar& lastCoeff() const
     {
@@ -355,7 +390,8 @@ const typename SparseMatrixBase<Derived>::ConstInnerVectorReturnType SparseMatri
   * is col-major (resp. row-major).
   */
 template<typename Derived>
-Block<Derived,Dynamic,Dynamic,true> SparseMatrixBase<Derived>::innerVectors(Index outerStart, Index outerSize)
+typename SparseMatrixBase<Derived>::InnerVectorsReturnType
+SparseMatrixBase<Derived>::innerVectors(Index outerStart, Index outerSize)
 {
   return Block<Derived,Dynamic,Dynamic,true>(derived(),
                                              IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
@@ -367,7 +403,8 @@ Block<Derived,Dynamic,Dynamic,true> SparseMatrixBase<Derived>::innerVectors(Inde
   * is col-major (resp. row-major). Read-only.
   */
 template<typename Derived>
-const Block<const Derived,Dynamic,Dynamic,true> SparseMatrixBase<Derived>::innerVectors(Index outerStart, Index outerSize) const
+const typename SparseMatrixBase<Derived>::ConstInnerVectorsReturnType
+SparseMatrixBase<Derived>::innerVectors(Index outerStart, Index outerSize) const
 {
   return Block<const Derived,Dynamic,Dynamic,true>(derived(),
                                                   IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
@@ -394,8 +431,8 @@ public:
       : m_matrix(xpr),
         m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0),
         m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
-        m_blockRows(xpr.rows()),
+        m_blockRows(BlockRows==1 ? 1 : xpr.rows()),
-        m_blockCols(xpr.cols())
+        m_blockCols(BlockCols==1 ? 1 : xpr.cols())
     {}
 
     /** Dynamic-size constructor
@@ -497,3 +534,4 @@ public:
 } // end namespace Eigen
 
 #endif // EIGEN_SPARSE_BLOCK_H
+

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

@@ -314,10 +314,10 @@ SparseMatrixBase<Derived>::operator+=(const SparseMatrixBase<OtherDerived>& othe
 
 template<typename Derived>
 template<typename OtherDerived>
-EIGEN_STRONG_INLINE const EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE
+EIGEN_STRONG_INLINE const typename SparseMatrixBase<Derived>::template CwiseProductDenseReturnType<OtherDerived>::Type
 SparseMatrixBase<Derived>::cwiseProduct(const MatrixBase<OtherDerived> &other) const
 {
-  return EIGEN_SPARSE_CWISE_PRODUCT_RETURN_TYPE(derived(), other.derived());
+  return typename CwiseProductDenseReturnType<OtherDerived>::Type(derived(), other.derived());
 }
 
 } // end namespace Eigen