Merge branch 'develop'

.gitignore

@@ -1,3 +1,6 @@
 /build*
+/doc*
 *.pyc
-*.DS_Store
+*.DS_Store
+/examples/Data/dubrovnik-3-7-pre-rewritten.txt
+/examples/Data/pose2example-rewritten.txt

CMakeLists.txt

@@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.6)
 
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 3)
-set (GTSAM_VERSION_MINOR 0)
+set (GTSAM_VERSION_MINOR 1)
 set (GTSAM_VERSION_PATCH 0)
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@@ -34,7 +34,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
   message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
 endif()
 
-# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout)
+# See whether gtsam_unstable is available (it will be present only if we're using a git checkout)
 if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable")
     set(GTSAM_UNSTABLE_AVAILABLE 1)
 else()
@@ -46,7 +46,6 @@ endif()
 # Set up options
 
 # Configurable Options
-option(GTSAM_BUILD_TIMING                "Enable/Disable building of timing scripts" OFF) # These do not currently work
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
 endif()
@@ -91,10 +90,10 @@ set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator")
 # If using Boost shared libs, disable auto linking
 if(MSVC)
 	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
-	add_definitions(-DBOOST_ALL_DYN_LINK)
 	# Disable autolinking
 	if(NOT Boost_USE_STATIC_LIBS)
 		add_definitions(-DBOOST_ALL_NO_LIB)
+	  add_definitions(-DBOOST_ALL_DYN_LINK)
 	endif()
 endif()
 
@@ -124,6 +123,11 @@ else()
 endif()
 
 
+if(${Boost_VERSION} EQUAL 105600)
+	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
+	add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
+endif()
+
 ###############################################################################
 # Find TBB
 find_package(TBB)
@@ -156,23 +160,23 @@ find_package(GooglePerfTools)
 
 ###############################################################################
 # Find MKL
-if(GTSAM_USE_EIGEN_MKL)
-    find_package(MKL)
+find_package(MKL)
 
-    if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
-        set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h
-        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})
-    endif()
+if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
+    set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h
+    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})
+else()
+    set(GTSAM_USE_EIGEN_MKL 0)
+    set(EIGEN_USE_MKL_ALL 0)
 endif()
 
-
 ###############################################################################
 # Find OpenMP (if we're also using MKL)
-if(GTSAM_USE_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
-    find_package(OpenMP)
+find_package(OpenMP)  # do this here to generate correct message if disabled
 
+if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
     if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
         set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
         set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
@@ -273,6 +277,13 @@ if(MSVC)
 	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
 endif()
 
+# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
+    add_definitions(-Wno-unused-local-typedefs)
+  endif()
+endif()
+
 if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
   add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()
@@ -297,6 +308,9 @@ add_subdirectory(tests)
 # Build examples
 add_subdirectory(examples)
 
+# Build timing
+add_subdirectory(timing)
+
 # Matlab toolbox
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 	add_subdirectory(matlab)
@@ -354,7 +368,7 @@ message(STATUS "================  Configuration Options  ======================"
 message(STATUS "Build flags                                               ")
 print_config_flag(${GTSAM_BUILD_TESTS}                 "Build Tests                    ")
 print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS}       "Build examples with 'make all' ")
-print_config_flag(${GTSAM_BUILD_TIMING}                "Build Timing scripts           ")
+print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS}         "Build timing scripts with 'make all'")
 if (DOXYGEN_FOUND)
     print_config_flag(${GTSAM_BUILD_DOCS}              "Build Docs                     ")
 endif()

DEVELOP

@@ -0,0 +1,19 @@
+Information for developers
+
+Coding Conventions:
+
+* Classes are Uppercase, methods and functions lowerMixedCase
+* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs
+* Use meaningful variable names, e.g., measurement not msm
+
+
+Windows:
+
+On Windows it is necessary to explicitly export all functions from the library
+which should be externally accessible. To do this, include the macro 
+GTSAM_EXPORT in your class or function definition.
+
+For example:
+class GTSAM_EXPORT MyClass { ... };
+
+GTSAM_EXPORT myFunction();

INSTALL

@@ -24,7 +24,7 @@ Optional dependent libraries:
    may be installed from the Ubuntu repositories, and for other platforms it
    may be downloaded from https://www.threadingbuildingblocks.org/
 
-Tested compilers
+Tested compilers:
 
 - GCC 4.2-4.7
 - OSX Clang 2.9-5.0
@@ -35,7 +35,12 @@ Tested systems:
 
 - Ubuntu 11.04 - 13.10
 - MacOS 10.6 - 10.9
-- Windows 7, 8
+- Windows 7, 8, 8.1
+
+Known issues:
+
+- MSVC 2013 is not yet supported because it cannot build the serialization module
+  of Boost 1.55 (or earlier). 
 
 2)
 GTSAM makes extensive use of debug assertions, and we highly recommend you work

cmake/FindMKL.cmake

@@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR
     /opt/intel/mkl/*/
     /opt/intel/cmkl/
     /opt/intel/cmkl/*/
+    /opt/intel/*/mkl/
     /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
         "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl"
         "C:/Program Files (x86)/Intel/Composer XE 2013/mkl"

cmake/GTSAMCMakeToolsConfig.cmake

@@ -1,3 +1,4 @@
 # This config file modifies CMAKE_MODULE_PATH so that the GTSAM-CMakeTools files may be included
- 
+
+set(GTSAM_CMAKE_TOOLS_DIR "${CMAKE_CURRENT_LIST_DIR}")
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}")

cmake/GtsamBuildTypes.cmake

@@ -56,7 +56,7 @@ endif()
 # Clang on Mac uses a template depth that is less than standard and is too small
 if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
 	if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
-		add_definitions(-ftemplate-depth=1024)
+		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
 	endif()
 endif()
 

cmake/GtsamMatlabWrap.cmake

@@ -28,13 +28,13 @@ endif()
 # finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
 # on the system path.
 list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
-find_program(mex_command ${mex_program_name}
+find_program(MEX_COMMAND ${mex_program_name}
 	PATHS ${matlab_bin_directories} ENV PATH
 	NO_DEFAULT_PATH)
-mark_as_advanced(FORCE mex_command)
+mark_as_advanced(FORCE MEX_COMMAND)
 # Now that we have mex, trace back to find the Matlab installation root
-get_filename_component(mex_command "${mex_command}" REALPATH)
-get_filename_component(mex_path "${mex_command}" PATH)
+get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
+get_filename_component(mex_path "${MEX_COMMAND}" PATH)
 get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
 set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
 
@@ -93,7 +93,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 	
 	# Paths for generated files
 	set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
-	set(generated_cpp_file "${PROJECT_BINARY_DIR}/wrap/${moduleName}/${moduleName}_wrapper.cpp")
+	set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
 	set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
 	
 	message(STATUS "Building wrap module ${moduleName}")
@@ -108,24 +108,87 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 		list(GET GTSAM_INCLUDE_DIR 0 installed_includes_path)
 		set(matlab_h_path "${installed_includes_path}/wrap")
 	endif()
-	
-	# Add -shared or -static suffix to targets
+
+	# If building a static mex module, add all cmake-linked libraries to the
+	# explicit link libraries list so that the next block of code can unpack
+	# any static libraries
+	set(automaticDependencies "")
+	foreach(lib ${moduleName} ${linkLibraries})
+	  #message("MODULE NAME: ${moduleName}")
+		if(TARGET "${lib}")
+            get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
+           # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
+            if(dependentLibraries)
+               list(APPEND automaticDependencies ${dependentLibraries})
+            endif()
+        endif()
+    endforeach()
+    
+    ## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module
+    ## This needs to be fixed!!
+    if(UNIX AND NOT APPLE)
+      list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
+        ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}
+        ${Boost_REGEX_LIBRARY_RELEASE})
+      if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
+        list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
+        if(GTSAM_MEX_BUILD_STATIC_MODULE)
+          #list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
+        endif()
+      endif()
+    endif()
+    
+    #message("AUTOMATIC DEPENDENCIES:  ${automaticDependencies}")
+    ## CHRIS: End temporary fix
+
+	# Separate dependencies
 	set(correctedOtherLibraries "")
 	set(otherLibraryTargets "")
     set(otherLibraryNontargets "")
-	foreach(lib ${moduleName} ${linkLibraries})
-		if(TARGET ${lib})
-			list(APPEND correctedOtherLibraries ${lib})
-			list(APPEND otherLibraryTargets ${lib})
-		elseif(TARGET ${lib}-shared) # Prefer the shared library if we have both shared and static)
-			list(APPEND correctedOtherLibraries ${lib}-shared)
-			list(APPEND otherLibraryTargets ${lib}-shared)
-		elseif(TARGET ${lib}-static)
-			list(APPEND correctedOtherLibraries ${lib}-static)
-			list(APPEND otherLibraryTargets ${lib}-static)
+    set(otherSourcesAndObjects "")
+	foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
+		if(TARGET "${lib}")
+		    if(GTSAM_MEX_BUILD_STATIC_MODULE)
+		    	get_target_property(target_sources ${lib} SOURCES)
+		    	list(APPEND otherSourcesAndObjects ${target_sources})
+		    else()
+				list(APPEND correctedOtherLibraries ${lib})
+				list(APPEND otherLibraryTargets ${lib})
+		    endif()
 		else()
-			list(APPEND correctedOtherLibraries ${lib})
-            list(APPEND otherLibraryNontargets ${lib})
+			get_filename_component(file_extension "${lib}" EXT)
+			get_filename_component(lib_name "${lib}" NAME_WE)
+			if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE)
+				# For building a static MEX module, unpack the static library
+				# and compile its object files into our module
+				file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
+				execute_process(COMMAND ar -x "${lib}"
+				                WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
+				                RESULT_VARIABLE ar_result)
+				if(NOT ar_result EQUAL 0)
+					message(FATAL_ERROR "Failed extracting ${lib}")
+				endif()
+
+				# Get list of object files
+				execute_process(COMMAND ar -t "${lib}"
+								OUTPUT_VARIABLE object_files
+				                RESULT_VARIABLE ar_result)
+				if(NOT ar_result EQUAL 0)
+					message(FATAL_ERROR "Failed listing ${lib}")
+				endif()
+
+				# Add directory to object files
+				string(REPLACE "\n" ";" object_files_list "${object_files}")
+				foreach(object_file ${object_files_list})
+					get_filename_component(file_extension "${object_file}" EXT)
+					if(file_extension STREQUAL ".o")
+						list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}")
+					endif()
+				endforeach()
+			else()
+				list(APPEND correctedOtherLibraries ${lib})
+	            list(APPEND otherLibraryNontargets ${lib})
+            endif()
 		endif()
 	endforeach()
     
@@ -144,7 +207,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 	file(MAKE_DIRECTORY "${generated_files_path}")
 	add_custom_command(
 		OUTPUT ${generated_cpp_file}
-		DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets}
+		DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
         COMMAND 
             wrap
             ${modulePath}
@@ -157,7 +220,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 	# Set up building of mex module
 	string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
 	string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader})
+	add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
 	target_link_libraries(${moduleName}_wrapper ${correctedOtherLibraries})
 	set_target_properties(${moduleName}_wrapper PROPERTIES
 		OUTPUT_NAME              "${moduleName}_wrapper"

cmake/GtsamTesting.cmake

@@ -38,21 +38,46 @@ endmacro()
 # 
 # Add scripts that will serve as examples of how to use the library.  A list of files or
 # glob patterns is specified, and one executable will be created for each matching .cpp
-# file.  These executables will not be installed.  They are build with 'make all' if
+# file.  These executables will not be installed.  They are built with 'make all' if
 # GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.
 #
 # Usage example:
 #   gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
 #
 # Arguments:
-#   globPatterns:  The list of files or glob patterns from which to create unit tests, with
-#                  one test created for each cpp file.  e.g. "*.cpp", or
+#   globPatterns:  The list of files or glob patterns from which to create examples, with
+#                  one program created for each cpp file.  e.g. "*.cpp", or
 #                  "A*.cpp;B*.cpp;MyExample.cpp".
 #   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
 #                  an empty string "" if nothing needs to be excluded.
 #   linkLibraries: The list of libraries to link to.
 macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
-	gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}")
+	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
+endmacro()
+
+
+###############################################################################
+# Macro:
+#
+# gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries)
+# 
+# Add scripts that time aspects of the library.  A list of files or
+# glob patterns is specified, and one executable will be created for each matching .cpp
+# file.  These executables will not be installed.  They are not built with 'make all',
+# but they may be built with 'make timing'.
+#
+# Usage example:
+#   gtsamAddTimingGlob("*.cpp" "DisabledTimingScript.cpp" "gtsam;GeographicLib")
+#
+# Arguments:
+#   globPatterns:  The list of files or glob patterns from which to create programs, with
+#                  one program created for each cpp file.  e.g. "*.cpp", or
+#                  "A*.cpp;B*.cpp;MyExample.cpp".
+#   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
+#                  an empty string "" if nothing needs to be excluded.
+#   linkLibraries: The list of libraries to link to.
+macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries)
+	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS})
 endmacro()
 
 
@@ -63,6 +88,7 @@ enable_testing()
 
 option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
+option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
 
 # Add option for combining unit tests
 if(MSVC OR XCODE_VERSION)
@@ -80,6 +106,9 @@ endif()
 # Add examples target
 add_custom_target(examples)
 
+# Add timing target
+add_custom_target(timing)
+
 # Include obsolete macros - will be removed in the near future
 include(GtsamTestingObsolete)
 
@@ -180,7 +209,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 endmacro()
 
 
-macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
+macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll)
     # Get all script files
     file(GLOB script_files ${globPatterns})
 
@@ -220,20 +249,22 @@ macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
 		target_link_libraries(${script_name} ${linkLibraries})
 	
 		# Add target dependencies
-		add_dependencies(examples ${script_name})
+		add_dependencies(${groupName} ${script_name})
 		if(NOT MSVC AND NOT XCODE_VERSION)
 		  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
 		endif()
 
 		# Add TOPSRCDIR
 		set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
-	
-		if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS)
+
+        # Exclude from all or not - note weird variable assignment because we're in a macro	
+	    set(buildWithAll_on ${buildWithAll})
+		if(NOT buildWithAll_on)
 			# Exclude from 'make all' and 'make install'
-			set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
+			set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON)
 		endif()
 
 		# Configure target folder (for MSVC and Xcode)
-		set_property(TARGET ${script_name} PROPERTY FOLDER "Examples")
+		set_property(TARGET ${script_name} PROPERTY FOLDER "${groupName}")
 	endforeach()
 endmacro()

cmake/example_project/CMakeLists.txt

@@ -1,173 +1,43 @@
 # This file should be used as a template for creating new projects using the CMake tools
 # This project has the following features
 #    - GTSAM linking
-#    - Boost linking
 #    - Unit tests via CppUnitLite
-#    - Automatic detection of sources and headers in subfolders
-#    - Installation of library and headers
-#    - Matlab wrap interface with within-project building
-#    - Use of GTSAM cmake macros
+#    - Scripts
+#    - Automatic MATLAB wrapper generation
 
 ###################################################################################
-# To create your own project, replace "myproject" with the actual name of your project
+# To create your own project, replace "example" with the actual name of your project
 cmake_minimum_required(VERSION 2.6)
-enable_testing()
-project(myproject CXX C)               
+project(example CXX C)
 
-# Add the cmake subfolder to the cmake module path - necessary to use macros
-set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake")
+# Include GTSAM CMake tools
+find_package(GTSAMCMakeTools)
+include(GtsamBuildTypes) # Load build type flags and default to Debug mode
+include(GtsamTesting)    # Easy functions for creating unit tests and scripts
+include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation
 
 # Ensure that local folder is searched before library folders
 include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
 
-# Load build type flags and default to Debug mode
-include(GtsamBuildTypes)
-
 ###################################################################################
-# Create a list of library dependencies 
-# These will be linked with executables
-set(library_deps "")
-set(linking_mode "static")
-
 # Find GTSAM components
 find_package(GTSAM REQUIRED) # Uses installed package
-list(APPEND library_deps gtsam-${linking_mode} gtsam_unstable-${linking_mode})
-
-# Include ransac
-find_package(ransac REQUIRED) # Uses installed package
-list(APPEND library_deps ransac-${linking_mode})
-
-# Boost - same requirement as gtsam
-find_package(Boost 1.43 COMPONENTS 
-    serialization 
-    system 
-    filesystem 
-    thread 
-    date_time 
-    REQUIRED)
-list(APPEND library_deps 
-    ${Boost_SERIALIZATION_LIBRARY} 
-    ${Boost_SYSTEM_LIBRARY} 
-    ${Boost_FILESYSTEM_LIBRARY}
-    ${Boost_THREAD_LIBRARY} 
-    ${Boost_DATE_TIME_LIBRARY})
-
-include_directories(${Boost_INCLUDE_DIR} ${GTSAM_INCLUDE_DIR} ${ransac_INCLUDE_DIR})
+include_directories(${GTSAM_INCLUDE_DIR})
 
 ###################################################################################
-# List subdirs to process tests/sources
-# Each of these will be scanned for new files
-set (myproject_subdirs 
-    "."   # ensure root folder gets included
-    stuff 
-    things
-    )
-
-# loop through subdirs to install and build up source lists
-set(myproject_lib_source "")
-set(myproject_tests_source "")
-set(myproject_scripts_source "")
-foreach(subdir ${myproject_subdirs})
-    # Installing headers
-    message(STATUS "Installing ${subdir}")
-    file(GLOB sub_myproject_headers "myproject/${subdir}/*.h")
-    install(FILES ${sub_myproject_headers} DESTINATION include/myproject/${subdir})
-    
-    # add sources to main sources list
-    file(GLOB subdir_srcs "myproject/${subdir}/*.cpp")
-    list(APPEND myproject_lib_source ${subdir_srcs})
-    
-    # add tests to main tests list
-    file(GLOB subdir_test_srcs "myproject/${subdir}/tests/*.cpp")
-    list(APPEND myproject_tests_source ${subdir_test_srcs})
-    
-    # add scripts to main tests list
-    file(GLOB subdir_scripts_srcs "myproject/${subdir}/scripts/*.cpp")
-    list(APPEND myproject_scripts_source ${subdir_scripts_srcs})
-endforeach(subdir)
-
-set(myproject_version   ${myproject_VERSION_MAJOR}.${myproject_VERSION_MINOR}.${myproject_VERSION_PATCH})
-set(myproject_soversion ${myproject_VERSION_MAJOR})
-message(STATUS "GTSAM Version: ${gtsam_version}")
-message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
-
-# Build library (static and shared versions)
-# Include installed versions
-SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-add_library(${PROJECT_NAME}-shared SHARED ${myproject_lib_source})
-set_target_properties(${PROJECT_NAME}-shared PROPERTIES 
-    OUTPUT_NAME         ${PROJECT_NAME}
-    CLEAN_DIRECT_OUTPUT 1)
-install(TARGETS myproject-shared EXPORT myproject-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-list(APPEND myproject_EXPORTED_TARGETS myproject-shared)
-
-add_library(${PROJECT_NAME}-static STATIC ${myproject_lib_source})
-set_target_properties(${PROJECT_NAME}-static PROPERTIES 
-    OUTPUT_NAME         ${PROJECT_NAME}
-    CLEAN_DIRECT_OUTPUT 1)
-install(TARGETS myproject-static EXPORT myproject-exports ARCHIVE DESTINATION lib)
-list(APPEND myproject_EXPORTED_TARGETS myproject-static)
-
-install(TARGETS ${PROJECT_NAME}-shared LIBRARY DESTINATION lib )
-install(TARGETS ${PROJECT_NAME}-static ARCHIVE DESTINATION lib )
-
-# Disabled tests - subtract these from the test files
-# Note the need for a full path
-set(disabled_tests 
-    "dummy"
-    #"${PROJECT_SOURCE_DIR}/myproject/geometry/tests/testCovarianceEllipse.cpp"
-)
-list(REMOVE_ITEM myproject_tests_source ${disabled_tests})
+# Build static library from common sources
+set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
+add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp)
+target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
 
 ###################################################################################
-# Build tests
-add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND})
-foreach(test_src_file ${myproject_tests_source})
-    get_filename_component(test_base ${test_src_file} NAME_WE)
-    message(STATUS "Adding test ${test_src_file} with base name ${test_base}" )
-    add_executable(${test_base} ${test_src_file})
-    target_link_libraries(${test_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite)
-    add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}/${test_base})
-    add_custom_target(${test_base}.run ${test_base} ${ARGN})
-    add_dependencies(check ${test_base})
-endforeach(test_src_file)
-
-# Build scripts
-foreach(script_src_file ${myproject_scripts_source})
-    get_filename_component(script_base ${script_src_file} NAME_WE)
-    message(STATUS "Adding script ${script_src_file} with base name ${script_base}" )
-    add_executable(${script_base} ${script_src_file})
-    target_link_libraries(${script_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite)
-    add_custom_target(${script_base}.run ${script_base} ${ARGN})
-endforeach(script_src_file)
+# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
+gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
 
 ###################################################################################
-# Matlab wrapping
-include(GtsamMatlabWrap)
-set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)")
-set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
-set(MYPROJECT_TOOLBOX_DIR "../matlab/myproject" CACHE PATH "Install folder for matlab toolbox - defaults to inside project")
-set(WRAP_HEADER_PATH "${GTSAM_DIR}/../../../include")
-set(MYPROJECT_TOOLBOX_FLAGS 
-    ${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${PROJECT_SOURCE_DIR} -I${PROJECT_SOURCE_DIR}/myproject -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${GTSAM_INCLUDE_DIR} -I${WRAP_HEADER_PATH} -Wl,-rpath,${CMAKE_BINARY_DIR}:${CMAKE_INSTALL_PREFIX}/lib)
-set(MYPROJECT_LIBRARY_DEPS gtsam gtsam_unstable ransac myproject)
-set(GTSAM_BUILD_MEX_BIN ON)
- 
-# Function to setup codegen, building and installation of the wrap toolbox
-# This wrap setup function assumes that the toolbox will be installed directly, 
-# with predictable matlab.h sourcing
-# params:
-#  moduleName       : the name of the module, interface file must be called moduleName.h
-#  mexFlags         : Compilation flags to be passed to the mex compiler 
-#  modulePath       : relative path to module markup header file (called moduleName.h)
-#  otherLibraries   : list of library targets this should depend on
-#  toolboxPath      : the directory in which to generate/build wrappers
-#  wrap_header_path : path to the installed wrap header  
-wrap_library_generic(myproject "${MYPROJECT_TOOLBOX_FLAGS}" "" "${MYPROJECT_LIBRARY_DEPS}" "${MYPROJECT_TOOLBOX_DIR}" "${WRAP_HEADER_PATH}")
+# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
+gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}")
 
 ###################################################################################
-# Create Install config and export files
-# This config file takes the place of FindXXX.cmake scripts
-include(GtsamMakeConfigFile)
-GtsamMakeConfigFile(myproject)
-export(TARGETS ${myproject_EXPORTED_TARGETS} FILE myproject-exports.cmake)
+# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library)
+wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "")

cmake/example_project/SayGoodbye.cpp

@@ -0,0 +1,23 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     SayGoodbye.cpp
+ * @brief    Example script for example project
+ * @author   Richard Roberts
+ */
+
+#include <example/PrintExamples.h>
+
+int main(int argc, char *argv[]) {
+  example::PrintExamples().sayGoodbye();
+  return 0;
+}

cmake/example_project/SayHello.cpp

@@ -0,0 +1,23 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     SayHello.cpp
+ * @brief    Example script for example project
+ * @author   Richard Roberts
+ */
+
+#include <example/PrintExamples.h>
+
+int main(int argc, char *argv[]) {
+  example::PrintExamples().sayHello();
+  return 0;
+}

cmake/example_project/example.h

@@ -0,0 +1,28 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     example.h
+ * @brief    Example wrapper interface file
+ * @author   Richard Roberts
+ */
+
+// This is an interface file for automatic MATLAB wrapper generation.  See
+// gtsam.h for full documentation and more examples.
+
+namespace example {
+
+class PrintExamples {
+  void sayHello() const;
+  void sayGoodbye() const;
+};
+
+}

cmake/example_project/example/PrintExamples.cpp

@@ -0,0 +1,44 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     print_examples.cpp
+ * @brief    Example library file
+ * @author   Richard Roberts
+ */
+
+#include <iostream>
+
+#include <example/PrintExamples.h>
+
+namespace example {
+
+void PrintExamples::sayHello() const {
+  std::cout << internal::getHelloString() << std::endl;
+}
+
+void PrintExamples::sayGoodbye() const {
+  std::cout << internal::getGoodbyeString() << std::endl;
+}
+
+namespace internal {
+
+std::string getHelloString() {
+  return "Hello!";
+}
+
+std::string getGoodbyeString() {
+  return "See you soon!";
+}
+
+}  // namespace internal
+
+}  // namespace example

cmake/example_project/example/PrintExamples.h

@@ -0,0 +1,41 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     print_examples.h
+ * @brief    Example library file
+ * @author   Richard Roberts
+ */
+
+#pragma once
+
+#include <string>
+
+namespace example {
+
+class PrintExamples {
+public:
+  /// Print a greeting
+  void sayHello() const;
+
+  /// Print a farewell
+  void sayGoodbye() const;
+};
+
+namespace internal {
+
+std::string getHelloString();
+
+std::string getGoodbyeString();
+
+}  // namespace internal
+
+}  // namespace example

cmake/example_project/tests/testExample.cpp

@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+
+ * 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     testExample.cpp
+ * @brief    Unit tests for example
+ * @author   Richard Roberts
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/TestableAssertions.h>
+
+#include <example/PrintExamples.h>
+
+using namespace gtsam;
+
+TEST(Example, HelloString) {
+  const std::string expectedString = "Hello!";
+  EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
+}
+
+TEST(Example, GoodbyeString) {
+  const std::string expectedString = "See you soon!";
+  EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+
+

cmake/example_project_simple/CMakeLists.txt

@@ -1,38 +0,0 @@
-# This file should be used as a template for creating new projects using the CMake tools
-# This project has the following features
-#    - GTSAM linking
-#    - Unit tests via CppUnitLite
-#    - Scripts
-
-###################################################################################
-# To create your own project, replace "myproject" with the actual name of your project
-cmake_minimum_required(VERSION 2.6)
-enable_testing()
-project(myproject CXX C)               
-
-# Add the cmake subfolder to the cmake module path - necessary to use macros
-list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
-
-# Ensure that local folder is searched before library folders
-include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
-
-# Load build type flags and default to Debug mode
-include(GtsamBuildTypes)
-
-###################################################################################
-# Find GTSAM components
-find_package(GTSAM REQUIRED) # Uses installed package
-include_directories(${GTSAM_INCLUDE_DIR})
-
-###################################################################################
-# Build static library from common sources
-add_library(${PROJECT_NAME} STATIC ${PROJECT_NAME}/MySourceFiles.cpp)
-target_link_libraries(${PROJECT_NAME} gtsam-shared)
-
-###################################################################################
-# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
-gtsam_add_subdir_tests(${PROJECT_NAME} "${PROJECT_NAME}" "${PROJECT_NAME}" "")
-
-###################################################################################
-# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
-gtsam_add_executables("${PROJECT_NAME}/myScripts.cpp" "${PROJECT_NAME}" "${PROJECT_NAME}" "")

examples/Data/VO_calibration00.txt

@@ -0,0 +1 @@
+718.856 718.856 0.0 607.1928 185.2157 0.5371657189

examples/Data/VO_calibration00s.txt

@@ -0,0 +1 @@
+718.856 718.856 0.0 607.1928 185.2157 0.5371657189

examples/Data/VO_camera_poses00.txt

@@ -0,0 +1,135 @@
+0  1  0  0  0  0  1  0  0 -0  0  1  0  0  0  0  1
+1     0.99999 -0.00268679 -0.00354618 6.43221e-05  0.00267957    0.999994 -0.00204036  -0.0073023  0.00355164  0.00203084    0.999992    0.676456           0           0           0           1
+2    0.999969 -0.00120771 -0.00772489  -0.0100328  0.00117985    0.999993   -0.003611  -0.0111185  0.00772919  0.00360178    0.999964     1.37125           0           0           0           1
+3    0.999931 -0.00128098  -0.0117006  -0.0237327  0.00122052    0.999986 -0.00517227  -0.0136538   0.0117071  0.00515763    0.999918     2.08563           0           0           0           1
+4      0.99986  5.79321e-05   -0.0167106   -0.0402272 -0.000155312     0.999983  -0.00582618   -0.0194327      0.01671   0.00582796     0.999843      2.81528            0            0            0            1
+5    0.999772 -0.00118366  -0.0213077  -0.0572378   0.0010545    0.999981 -0.00607208  -0.0278191   0.0213145  0.00604822    0.999755     3.56204           0           0           0           1
+6     0.999662  0.000544425   -0.0259946    -0.081545 -0.000735472     0.999973  -0.00734051   -0.0358844    0.0259899   0.00735714     0.999635      4.32265            0            0            0            1
+7    0.999513   0.0032602  -0.0310324   -0.112137  -0.0035101    0.999962 -0.00800188  -0.0447209   0.0310051  0.00810691    0.999486     5.09668           0           0           0           1
+8    0.999361  0.00349173  -0.0355658   -0.143594 -0.00372162    0.999973 -0.00639979  -0.0532611   0.0355425  0.00652807    0.999347     5.88701           0           0           0           1
+9    0.999185  0.00268131   -0.040271   -0.176401  -0.0028332    0.999989 -0.00371493  -0.0632884   0.0402606    0.003826    0.999182      6.6897           0           0           0           1
+10     0.99903  0.00226305  -0.0439747   -0.211687 -0.00231163    0.999997 -0.00105382   -0.072362   0.0439722  0.00115445    0.999032     7.50361           0           0           0           1
+11    0.998896  0.00366482  -0.0468376   -0.254125 -0.00374515    0.999992 -0.00162734  -0.0820263   0.0468312  0.00180096    0.998901     8.32333           0           0           0           1
+12    0.998775  0.00304285  -0.0493866   -0.295424 -0.00313866    0.999993 -0.00186268  -0.0885739   0.0493806  0.00201541    0.998778     9.15211           0           0           0           1
+13     0.998682  7.09894e-05   -0.0513155    -0.334647 -0.000203775     0.999997  -0.00258241   -0.0938889    0.0513152   0.00258946     0.998679      9.98839            0            0            0            1
+14     0.998565 -8.82523e-05   -0.0535542    -0.380835 -9.36659e-06     0.999998  -0.00182255     -0.10173    0.0535542   0.00182044     0.998563       10.832            0            0            0            1
+15    0.998481 -0.00146793  -0.0550718   -0.429135   0.0013525    0.999997 -0.00213307   -0.111427   0.0550748  0.00205535     0.99848      11.687           0           0           0           1
+16     0.998373  0.000738731   -0.0570218    -0.483426 -0.000993083      0.99999  -0.00443241    -0.122139    0.0570179   0.00448183     0.998363      12.5483            0            0            0            1
+17    0.998285  0.00120595  -0.0585258   -0.540056 -0.00162301    0.999974 -0.00707907   -0.132598   0.0585158  0.00716191    0.998261     13.4179           0           0           0           1
+18    0.998165  0.00516151   -0.060337     -0.6023 -0.00570195    0.999945 -0.00878826   -0.143753   0.0602883  0.00911617    0.998139     14.2952           0           0           0           1
+19    0.998101  0.00610094  -0.0612993    -0.66308 -0.00663017    0.999942 -0.00843386   -0.157854   0.0612443  0.00882427    0.998084     15.1802           0           0           0           1
+20    0.998014   0.0052997  -0.0627662   -0.722045 -0.00574767    0.999959  -0.0069587   -0.172847   0.0627268  0.00730564    0.998004      16.074           0           0           0           1
+21     0.99792  0.00591748  -0.0641975    -0.78346 -0.00627924    0.999966 -0.00543487   -0.186221   0.0641631  0.00582667    0.997922     16.9738           0           0           0           1
+22    0.997857  0.00547694  -0.0651993   -0.845347 -0.00584101    0.999968 -0.00539455   -0.199741   0.0651677  0.00576382    0.997858     17.8786           0           0           0           1
+23    0.997737  0.00536917  -0.0670282   -0.908218 -0.00579979    0.999964  -0.0062316   -0.212775   0.0669924  0.00660624    0.997732     18.7877           0           0           0           1
+24    0.997663  0.00386695  -0.0682185   -0.971291 -0.00435203    0.999966 -0.00696344   -0.226442   0.0681893  0.00724406    0.997646     19.7046           0           0           0           1
+25    0.997629  0.00410637  -0.0687004    -1.03663 -0.00448288    0.999976 -0.00532714   -0.239555   0.0686769  0.00562249    0.997623     20.6257           0           0           0           1
+26    0.997617  0.00588773  -0.0687501    -1.10557  -0.0060349     0.99998 -0.00193325   -0.254273   0.0687373  0.00234355    0.997632       21.55           0           0           0           1
+27    0.997662  0.00693766  -0.0679906    -1.17297 -0.00682806    0.999975   0.0018442    -0.26563   0.0680017 -0.00137565    0.997684     22.4875           0           0           0           1
+28    0.997774  0.00579785  -0.0664343    -1.23728 -0.00550265    0.999974  0.00462554   -0.271962   0.0664594 -0.00424968     0.99778     23.4285           0           0           0           1
+29    0.997872  0.00589563  -0.0649408    -1.30214 -0.00556012     0.99997  0.00534586   -0.277922   0.0649704  -0.0049734    0.997875     24.3732           0           0           0           1
+30    0.997958  0.00627024  -0.0635595    -1.36462 -0.00612984    0.999978  0.00240374   -0.285335   0.0635732 -0.00200922    0.997975      25.314           0           0           0           1
+31    0.998004  0.00714074  -0.0627411    -1.42783 -0.00731158     0.99997 -0.00249375   -0.293171   0.0627215  0.00294751    0.998027     26.2605           0           0           0           1
+32     0.99808   0.0063692  -0.0616159    -1.48954 -0.00671918    0.999962 -0.00547459   -0.302321   0.0615787  0.00587809    0.998085     27.2168           0           0           0           1
+33     0.99813  0.00376787  -0.0610159    -1.54654 -0.00404632    0.999982  -0.0044408   -0.313516   0.0609981  0.00467938    0.998127     28.1829           0           0           0           1
+34     0.998113   0.00193972   -0.0613743     -1.60668  -0.00191171     0.999998  0.000515183    -0.324411    0.0613752 -0.000396881     0.998115      29.1626            0            0            0            1
+35     0.99806  -0.0017885   -0.062228    -1.66532  0.00203402     0.99999  0.00388232   -0.335656   0.0622204 -0.00400136    0.998054     30.1428           0           0           0           1
+36    0.997945 -0.00917543  -0.0634115    -1.72059  0.00939451    0.999951  0.00315749   -0.343316   0.0633794 -0.00374672    0.997982     31.1244           0           0           0           1
+37     0.997825   -0.0112684   -0.0649459     -1.78049     0.011242     0.999937 -0.000771312    -0.350864    0.0649504  3.95099e-05     0.997888      32.1064            0            0            0            1
+38    0.997739  -0.0110126  -0.0662983    -1.85007   0.0107254    0.999932 -0.00468596   -0.361068   0.0663454  0.00396429    0.997789     33.0886           0           0           0           1
+39    0.997597 -0.00959503  -0.0686163    -1.92119  0.00924037    0.999942 -0.00548426   -0.373466   0.0686649  0.00483704    0.997628     34.0774           0           0           0           1
+40     0.99755  -0.0095802  -0.0693031    -1.99331  0.00931271    0.999948 -0.00418184   -0.387047   0.0693396  0.00352619    0.997587     35.0736           0           0           0           1
+41    0.997473 -0.00634387  -0.0707596     -2.0707  0.00626661     0.99998  -0.0013139   -0.403858   0.0707665  0.00086716    0.997493     36.0721           0           0           0           1
+42      0.99739  -0.00624366   -0.0719343     -2.14553   0.00625582      0.99998 -5.62375e-05    -0.416888    0.0719332 -0.000393917     0.997409      37.0728            0            0            0            1
+43    0.997312 -0.00473093  -0.0731254    -2.21909  0.00492848    0.999985  0.00252135   -0.428625   0.0731123 -0.00287497     0.99732     38.0643           0           0           0           1
+44    0.997318 -0.00467696  -0.0730348    -2.29215  0.00509473    0.999972  0.00553481   -0.440023   0.0730068 -0.00589206    0.997314     39.0618           0           0           0           1
+45     0.997274   0.00138304   -0.0737801     -2.37574 -0.000811217     0.999969   0.00777971    -0.447869    0.0737886  -0.00769865     0.997244      40.0548            0            0            0            1
+46     0.997262   0.00149131   -0.0739326     -2.45529 -0.000969511     0.999974   0.00709318    -0.454763    0.0739413  -0.00700208     0.997238      41.0557            0            0            0            1
+47    0.997266  0.00175929  -0.0738699    -2.53081 -0.00136899    0.999985  0.00533379   -0.460519   0.0738782 -0.00521809    0.997254     42.0518           0           0           0           1
+48    0.997253  0.00408494  -0.0739555    -2.61212 -0.00386552    0.999988  0.00310988   -0.469863   0.0739673 -0.00281546    0.997257     43.0493           0           0           0           1
+49    0.997185  0.00365371  -0.0748884    -2.68799 -0.00342799    0.999989  0.00314243    -0.47951   0.0748991 -0.00287687    0.997187     44.0473           0           0           0           1
+50    0.997077  0.00181435  -0.0763845    -2.76071 -0.00149292     0.99999  0.00426495   -0.487845   0.0763915 -0.00413845    0.997069     45.0403           0           0           0           1
+51    0.997018  0.00246727  -0.0771352    -2.84117 -0.00206285    0.999984  0.00532227   -0.499132   0.0771471 -0.00514727    0.997006     46.0244           0           0           0           1
+52    0.996991  0.00504805  -0.0773507    -2.92304 -0.00493379    0.999986  0.00166824   -0.510863   0.0773581 -0.00128158    0.997003      46.994           0           0           0           1
+53    0.996911  0.00581773  -0.0783264    -3.00373 -0.00604061    0.999978 -0.00260888   -0.521193   0.0783095  0.00307396    0.996924     47.9551           0           0           0           1
+54    0.996846  0.00678413  -0.0790757    -3.08343 -0.00711636    0.999967 -0.00392044   -0.534186   0.0790465  0.00447081    0.996861     48.9236           0           0           0           1
+55     0.996843   0.00557268   -0.0792034     -3.16262  -0.00562268     0.999984 -0.000408328     -0.54901    0.0791999  0.000852374     0.996858      49.9005            0            0            0            1
+56    0.996831  0.00375007  -0.0794568    -3.23868 -0.00354655     0.99999  0.00270227   -0.563036   0.0794661  -0.0024119    0.996835     50.8752           0           0           0           1
+57    0.996805  0.00190455  -0.0798474    -3.31582 -0.00164885    0.999993  0.00326822   -0.574113   0.0798531 -0.00312612    0.996802     51.8394           0           0           0           1
+58    0.996782 -0.00124932  -0.0801505    -3.39153  0.00141878    0.999997   0.0020573   -0.586659   0.0801477 -0.00216439    0.996781     52.8005           0           0           0           1
+59    0.996745  -0.0038025  -0.0805262     -3.4676   0.0038689    0.999992 0.000668539    -0.59892    0.080523 -0.00097791    0.996752     53.7575           0           0           0           1
+60    0.996643 -0.00519016  -0.0817059    -3.54489  0.00535256    0.999984  0.00176869    -0.60864   0.0816955 -0.00220009    0.996655      54.708           0           0           0           1
+61    0.996534  -0.0079249  -0.0828082    -3.62139  0.00842977    0.999948  0.00574894   -0.618858   0.0827583 -0.00642707    0.996549     55.6588           0           0           0           1
+62    0.996473 -0.00854289  -0.0834829    -3.69959  0.00945654      0.9999   0.0105549   -0.624401   0.0833844  -0.0113071    0.996453     56.6119           0           0           0           1
+63    0.996447 -0.00664747   -0.083957    -3.78502  0.00773966     0.99989   0.0126902   -0.629769   0.0838633  -0.0132949    0.996389     57.5607           0           0           0           1
+64    0.996335 -0.00522633  -0.0853755     -3.8689  0.00597793    0.999946  0.00855017   -0.636709   0.0853262  -0.0090292    0.996312     58.4941           0           0           0           1
+65     0.996221  -0.00343661   -0.0867892     -3.95276   0.00350579     0.999994  0.000644619    -0.644008    0.0867865 -0.000946448     0.996226      59.4131            0            0            0            1
+66    0.996144 -0.00149623  -0.0877201    -4.03806  0.00112725     0.99999 -0.00425562   -0.655271   0.0877256  0.00414033    0.996136     60.3236           0           0           0           1
+67    0.996055  0.00375138  -0.0886573    -4.12895 -0.00406723    0.999986 -0.00338223   -0.671324   0.0886434  0.00372948    0.996056     61.2274           0           0           0           1
+68    0.995922  0.00719305  -0.0899263    -4.21985  -0.0073202    0.999973 -0.00108421   -0.691307    0.089916  0.00173807    0.995948      62.125           0           0           0           1
+69     0.99582  0.00967277  -0.0908194    -4.30702 -0.00966905    0.999953 0.000481019   -0.708494   0.0908198 0.000399128    0.995867     63.0134           0           0           0           1
+70    0.995713   0.0102896  -0.0919182    -4.39131  -0.0103098    0.999947 0.000255248   -0.721276    0.091916 0.000693502    0.995767     63.8776           0           0           0           1
+71     0.99554   0.0119225  -0.0935844      -4.477  -0.0118725    0.999929  0.00109156   -0.734766   0.0935908 2.43836e-05    0.995611     64.7307           0           0           0           1
+72     0.995397    0.0126524   -0.0950024     -4.56121   -0.0125521      0.99992   0.00165348    -0.749039    0.0950157 -0.000453392     0.995476      65.5703            0            0            0            1
+73    0.995256   0.0126635  -0.0964665    -4.64297  -0.0125254    0.999919  0.00203772   -0.761909   0.0964846 -0.00081977    0.995334     66.3938           0           0           0           1
+74    0.995133   0.0127023  -0.0977168    -4.72623  -0.0124698    0.999918  0.00298947     -0.7711   0.0977468 -0.00175641     0.99521     67.2017           0           0           0           1
+75   0.994948   0.015548 -0.0991814   -4.81287 -0.0150604   0.999871 0.00566291  -0.780301  0.0992566 -0.0041406   0.995053    67.9995          0          0          0          1
+76    0.994794   0.0171065   -0.100462    -4.90076  -0.0162095    0.999821    0.009738   -0.788037    0.100611 -0.00805885    0.994893     68.7922           0           0           0           1
+77    0.994568   0.0201446   -0.102122    -4.98771  -0.0190681    0.999752   0.0115061   -0.794955    0.102328 -0.00949629    0.994705     69.5653           0           0           0           1
+78     0.99437   0.0229894    -0.10344    -5.07707  -0.0223435    0.999723  0.00739861   -0.804841    0.103581 -0.00504575    0.994608     70.3178           0           0           0           1
+79     0.99423   0.0228747     -0.1048    -5.16223  -0.0229899    0.999736 0.000108822    -0.81531    0.104774  0.00230114    0.994493     71.0475           0           0           0           1
+80    0.994077   0.0219938   -0.106431    -5.24094  -0.0227304    0.999725 -0.00571252   -0.825441    0.106276  0.00809789    0.994304     71.7584           0           0           0           1
+81    0.994023   0.0228054   -0.106762    -5.32437  -0.0236929    0.999694 -0.00705161   -0.831667    0.106569  0.00953897     0.99426     72.4548           0           0           0           1
+82     0.99386   0.0255543   -0.107653    -5.40648  -0.0260808    0.999654 -0.00348505   -0.846106    0.107527  0.00627133    0.994182     73.1337           0           0           0           1
+83    0.993702   0.0257681   -0.109048    -5.48436    -0.02605     0.99966 -0.00116096   -0.865059    0.108981  0.00399435    0.994036     73.7942           0           0           0           1
+84     0.99367   0.0225468   -0.110051     -5.5561  -0.0231761    0.999722 -0.00444219   -0.879535     0.10992  0.00696462    0.993916     74.4324           0           0           0           1
+85    0.993802   0.0143509   -0.110234    -5.61528  -0.0155198    0.999832 -0.00975281    -0.89282    0.110075   0.0114032    0.993858     75.0504           0           0           0           1
+86   0.993949     0.0102   -0.10937   -5.67796 -0.0118817   0.999821 -0.0147354  -0.904058     0.1092  0.0159457   0.993892    75.6535          0          0          0          1
+87   0.994244  0.0126451  -0.106395   -5.74524  -0.014328   0.999784 -0.0150673  -0.916949   0.106181   0.016505    0.99421    76.2455          0          0          0          1
+88   0.994592  0.0175824  -0.102356   -5.81375 -0.0189231   0.999747 -0.0121417  -0.930972   0.102117  0.0140129   0.994674    76.8222          0          0          0          1
+89    0.995077   0.0159295  -0.0978149    -5.86699  -0.0169123    0.999814 -0.00922648   -0.942909   0.0976498   0.0108353    0.995162     77.3862           0           0           0           1
+90    0.995665   0.0122046  -0.0922106    -5.90659  -0.0127358    0.999906 -0.00517412   -0.954487   0.0921387  0.00632606    0.995726     77.9355           0           0           0           1
+91    0.996426  0.00781104   -0.084105    -5.94257 -0.00798227    0.999967  -0.0016998   -0.970792   0.0840889  0.00236507    0.996455     78.4692           0           0           0           1
+92    0.997233   0.0114593  -0.0734453    -5.98049  -0.0116369     0.99993 -0.00198965   -0.981019   0.0734174  0.00283882    0.997297     78.9883           0           0           0           1
+93   0.998165  0.0165636 -0.0582361    -6.0154 -0.0167456   0.999856 -0.0026387   -0.99003   0.058184 0.00360906   0.998299    79.4952          0          0          0          1
+95    0.999635   0.0200255  -0.0181151    -6.02981  -0.0200623    0.999797 -0.00185529    -1.00502   0.0180742  0.00221804    0.999834     80.4727           0           0           0           1
+97     0.999162     0.015548     0.037857     -5.96801   -0.0155684     0.999879  0.000243918     -1.02389   -0.0378486 -0.000833085     0.999283      81.4025            0            0            0            1
+99    0.993959   0.0151454    0.108698    -5.84553  -0.0154328    0.999879   0.0018028    -1.04109   -0.108657 -0.00346942    0.994073     82.2952           0           0           0           1
+101    0.980499   0.0151504    0.195937    -5.64466  -0.0157106    0.999876  0.00130478    -1.05761   -0.195893 -0.00435763    0.980616     83.1489           0           0           0           1
+103    0.954186   0.0182833    0.298656    -5.36588  -0.0179595    0.999831 -0.00382887    -1.08348   -0.298675 -0.00171027    0.954353     83.9397           0           0           0           1
+105    0.910736   0.0194893    0.412529    -4.99648  -0.0175815     0.99981 -0.00842014    -1.10057   -0.412615 0.000415655    0.910905     84.6633           0           0           0           1
+107   0.848724  0.0183908   0.528517   -4.54701 -0.0135972   0.999824 -0.0129557   -1.12003  -0.528662 0.00380946   0.848824    85.2983          0          0          0          1
+109   0.772259  0.0170098    0.63508    -4.0183 -0.0106749   0.999848 -0.0137989   -1.14244  -0.635218 0.00387688   0.772323    85.8601          0          0          0          1
+111    0.684256   0.0156411    0.729074    -3.42903  -0.0102231    0.999877  -0.0118561    -1.16079    -0.72917 0.000659179    0.684332     86.3474           0           0           0           1
+113     0.590745     0.011826     0.806772     -2.77931 -0.000173089     0.999894   -0.0145301     -1.17886    -0.806858   0.00844396     0.590684      86.7443            0            0            0            1
+115    0.496173   0.0169181    0.868059    -2.10955 -0.00504039    0.999849  -0.0166057    -1.20339   -0.868209  0.00386392    0.496183     87.0847           0           0           0           1
+117   0.408192  0.0165355   0.912746   -1.40862 0.00231553   0.999814 -0.0191484    -1.2249  -0.912893 0.00992974   0.408078    87.3396          0          0          0          1
+119   0.333443 0.00543386   0.942754  -0.671521  0.0223493   0.999657 -0.0136666   -1.23662  -0.942505   0.025627   0.333208     87.522          0          0          0          1
+121   0.269054  0.0173163   0.962969  0.0638526 0.00961829    0.99974 -0.0206648   -1.26307  -0.963077  0.0148221   0.268818    87.7199          0          0          0          1
+123   0.214897  0.0233915   0.976357   0.843046 0.00677025   0.999653 -0.0254398   -1.30009  -0.976613  0.0120771   0.214664    87.8763          0          0          0          1
+125   0.171479   0.031054   0.984698    1.66216  0.0212619   0.999154 -0.0352125    -1.3179  -0.984958  0.0269747   0.170674    87.9743          0          0          0          1
+127  0.134011 0.0386308  0.990227   2.52547 0.0207141  0.998912 -0.041773  -1.34147 -0.990763 0.0261097  0.133065   88.0809         0         0         0         1
+129    0.10418  0.0310179   0.994075    3.44652  0.0195614   0.999256 -0.0332297   -1.39013  -0.994366  0.0229074   0.103496    88.1692          0          0          0          1
+131  0.0794366   0.027788   0.996453    4.42556  0.0261822   0.999208 -0.0299521   -1.42776  -0.996496  0.0284686  0.0786462    88.2295          0          0          0          1
+132  0.0693462   0.028443   0.997187    4.93885  0.0294969   0.999098 -0.0305488   -1.44582  -0.997156  0.0315324  0.0684447    88.2553          0          0          0          1
+133  0.0615414  0.0290168   0.997683    5.46907  0.0316982   0.999016 -0.0310108   -1.46406  -0.997601  0.0335332  0.0605611    88.2814          0          0          0          1
+134  0.0559347   0.029371   0.998002     6.0151  0.0334765    0.99895 -0.0312751   -1.48373  -0.997873   0.035159  0.0548927     88.307          0          0          0          1
+135  0.0504312  0.0304374   0.998264    6.58025  0.0349281    0.99887 -0.0322204   -1.50267  -0.998117  0.0364923  0.0493112    88.3306          0          0          0          1
+136  0.0445067  0.0311103   0.998525    7.16082  0.0355578   0.998832 -0.0327048   -1.52353  -0.998376  0.0369609  0.0433485    88.3531          0          0          0          1
+137   0.040243  0.0311989   0.998703    7.76375  0.0381603   0.998735 -0.0327376   -1.54487  -0.998461  0.0394283  0.0390016    88.3716          0          0          0          1
+138  0.0373982  0.0312027   0.998813    8.38568  0.0397152   0.998676 -0.0326855   -1.56772  -0.998511  0.0408905  0.0361095    88.3901          0          0          0          1
+139  0.0343726  0.0307634   0.998936    9.02449  0.0406913   0.998654 -0.0321549   -1.59059   -0.99858  0.0417533  0.0330745    88.4092          0          0          0          1
+140 0.0320861 0.0302694  0.999027   9.68038 0.0427798  0.998584  -0.03163  -1.61442 -0.998569  0.043753 0.0307457   88.4263         0         0         0         1
+141  0.0316452  0.0299561    0.99905    10.3542  0.0473602   0.998383 -0.0314363   -1.63856  -0.998376    0.04831  0.0301753    88.4381          0          0          0          1
+142  0.0327723   0.029714   0.999021    11.0457  0.0507142   0.998221 -0.0313539   -1.66282  -0.998175  0.0516921   0.031207    88.4556          0          0          0          1
+143  0.0353027  0.0297602   0.998933    11.7546  0.0522842   0.998133 -0.0315841   -1.68678  -0.998008  0.0533435  0.0336808    88.4781          0          0          0          1
+144  0.0392372  0.0297502   0.998787    12.4771  0.0547241   0.997993 -0.0318763   -1.71289   -0.99773  0.0559084  0.0375304    88.5062          0          0          0          1
+145  0.0437096  0.0293188   0.998614     13.219  0.0550685   0.997979 -0.0317105   -1.73922  -0.997525  0.0563782  0.0420067    88.5387          0          0          0          1
+146  0.0477725  0.0278103   0.998471    13.9764  0.0564652   0.997939 -0.0304971   -1.76499  -0.997261  0.0578358  0.0461037    88.5751          0          0          0          1
+147  0.0518486  0.0263145   0.998308    14.7472  0.0562418   0.997989 -0.0292271   -1.79222   -0.99707   0.057662  0.0502644    88.6178          0          0          0          1
+148  0.0560658  0.0242863   0.998132    15.5313  0.0531494   0.998214 -0.0272738   -1.82056  -0.997011  0.0545792  0.0546748    88.6693          0          0          0          1
+149  0.0600218  0.0233355   0.997924    16.3271  0.0522059   0.998285 -0.0264839   -1.84733  -0.996831  0.0536871  0.0587006    88.7243          0          0          0          1
+150  0.0641513  0.0243795   0.997642    17.1258  0.0492204   0.998408 -0.0275632   -1.87761  -0.996726  0.0508726  0.0628492    88.7821          0          0          0          1
+151  0.0672583   0.028483   0.997329     17.929  0.0470717   0.998389 -0.0316877   -1.91204  -0.996625  0.0490772  0.0658092     88.842          0          0          0          1
+152  0.0688453  0.0337446   0.997056    18.7357  0.0413971    0.99847 -0.0366509    -1.9468  -0.996768  0.0437985   0.067343    88.9041          0          0          0          1
+153  0.0686545  0.0370247   0.996953    19.5482  0.0387033    0.99846 -0.0397459   -1.98038  -0.996889  0.0413142  0.0671158    88.9665          0          0          0          1

examples/Data/VO_camera_poses00s.txt

@@ -0,0 +1,77 @@
+0  1  0  0  0  0  1  0  0 -0  0  1  0  0  0  0  1
+1     0.99999 -0.00268679 -0.00354618 6.43221e-05  0.00267957    0.999994 -0.00204036  -0.0073023  0.00355164  0.00203084    0.999992    0.676456           0           0           0           1
+2    0.999969 -0.00120771 -0.00772489  -0.0100328  0.00117985    0.999993   -0.003611  -0.0111185  0.00772919  0.00360178    0.999964     1.37125           0           0           0           1
+3    0.999931 -0.00128098  -0.0117006  -0.0237327  0.00122052    0.999986 -0.00517227  -0.0136538   0.0117071  0.00515763    0.999918     2.08563           0           0           0           1
+4      0.99986  5.79321e-05   -0.0167106   -0.0402272 -0.000155312     0.999983  -0.00582618   -0.0194327      0.01671   0.00582796     0.999843      2.81528            0            0            0            1
+5    0.999772 -0.00118366  -0.0213077  -0.0572378   0.0010545    0.999981 -0.00607208  -0.0278191   0.0213145  0.00604822    0.999755     3.56204           0           0           0           1
+6     0.999662  0.000544425   -0.0259946    -0.081545 -0.000735472     0.999973  -0.00734051   -0.0358844    0.0259899   0.00735714     0.999635      4.32265            0            0            0            1
+7    0.999513   0.0032602  -0.0310324   -0.112137  -0.0035101    0.999962 -0.00800188  -0.0447209   0.0310051  0.00810691    0.999486     5.09668           0           0           0           1
+8    0.999361  0.00349173  -0.0355658   -0.143594 -0.00372162    0.999973 -0.00639979  -0.0532611   0.0355425  0.00652807    0.999347     5.88701           0           0           0           1
+9    0.999185  0.00268131   -0.040271   -0.176401  -0.0028332    0.999989 -0.00371493  -0.0632884   0.0402606    0.003826    0.999182      6.6897           0           0           0           1
+10     0.99903  0.00226305  -0.0439747   -0.211687 -0.00231163    0.999997 -0.00105382   -0.072362   0.0439722  0.00115445    0.999032     7.50361           0           0           0           1
+11    0.998896  0.00366482  -0.0468376   -0.254125 -0.00374515    0.999992 -0.00162734  -0.0820263   0.0468312  0.00180096    0.998901     8.32333           0           0           0           1
+12    0.998775  0.00304285  -0.0493866   -0.295424 -0.00313866    0.999993 -0.00186268  -0.0885739   0.0493806  0.00201541    0.998778     9.15211           0           0           0           1
+13     0.998682  7.09894e-05   -0.0513155    -0.334647 -0.000203775     0.999997  -0.00258241   -0.0938889    0.0513152   0.00258946     0.998679      9.98839            0            0            0            1
+14     0.998565 -8.82523e-05   -0.0535542    -0.380835 -9.36659e-06     0.999998  -0.00182255     -0.10173    0.0535542   0.00182044     0.998563       10.832            0            0            0            1
+15    0.998481 -0.00146793  -0.0550718   -0.429135   0.0013525    0.999997 -0.00213307   -0.111427   0.0550748  0.00205535     0.99848      11.687           0           0           0           1
+16     0.998373  0.000738731   -0.0570218    -0.483426 -0.000993083      0.99999  -0.00443241    -0.122139    0.0570179   0.00448183     0.998363      12.5483            0            0            0            1
+17    0.998285  0.00120595  -0.0585258   -0.540056 -0.00162301    0.999974 -0.00707907   -0.132598   0.0585158  0.00716191    0.998261     13.4179           0           0           0           1
+18    0.998165  0.00516151   -0.060337     -0.6023 -0.00570195    0.999945 -0.00878826   -0.143753   0.0602883  0.00911617    0.998139     14.2952           0           0           0           1
+19    0.998101  0.00610094  -0.0612993    -0.66308 -0.00663017    0.999942 -0.00843386   -0.157854   0.0612443  0.00882427    0.998084     15.1802           0           0           0           1
+20    0.998014   0.0052997  -0.0627662   -0.722045 -0.00574767    0.999959  -0.0069587   -0.172847   0.0627268  0.00730564    0.998004      16.074           0           0           0           1
+21     0.99792  0.00591748  -0.0641975    -0.78346 -0.00627924    0.999966 -0.00543487   -0.186221   0.0641631  0.00582667    0.997922     16.9738           0           0           0           1
+22    0.997857  0.00547694  -0.0651993   -0.845347 -0.00584101    0.999968 -0.00539455   -0.199741   0.0651677  0.00576382    0.997858     17.8786           0           0           0           1
+23    0.997737  0.00536917  -0.0670282   -0.908218 -0.00579979    0.999964  -0.0062316   -0.212775   0.0669924  0.00660624    0.997732     18.7877           0           0           0           1
+24    0.997663  0.00386695  -0.0682185   -0.971291 -0.00435203    0.999966 -0.00696344   -0.226442   0.0681893  0.00724406    0.997646     19.7046           0           0           0           1
+25    0.997629  0.00410637  -0.0687004    -1.03663 -0.00448288    0.999976 -0.00532714   -0.239555   0.0686769  0.00562249    0.997623     20.6257           0           0           0           1
+26    0.997617  0.00588773  -0.0687501    -1.10557  -0.0060349     0.99998 -0.00193325   -0.254273   0.0687373  0.00234355    0.997632       21.55           0           0           0           1
+27    0.997662  0.00693766  -0.0679906    -1.17297 -0.00682806    0.999975   0.0018442    -0.26563   0.0680017 -0.00137565    0.997684     22.4875           0           0           0           1
+28    0.997774  0.00579785  -0.0664343    -1.23728 -0.00550265    0.999974  0.00462554   -0.271962   0.0664594 -0.00424968     0.99778     23.4285           0           0           0           1
+29    0.997872  0.00589563  -0.0649408    -1.30214 -0.00556012     0.99997  0.00534586   -0.277922   0.0649704  -0.0049734    0.997875     24.3732           0           0           0           1
+30    0.997958  0.00627024  -0.0635595    -1.36462 -0.00612984    0.999978  0.00240374   -0.285335   0.0635732 -0.00200922    0.997975      25.314           0           0           0           1
+31    0.998004  0.00714074  -0.0627411    -1.42783 -0.00731158     0.99997 -0.00249375   -0.293171   0.0627215  0.00294751    0.998027     26.2605           0           0           0           1
+32     0.99808   0.0063692  -0.0616159    -1.48954 -0.00671918    0.999962 -0.00547459   -0.302321   0.0615787  0.00587809    0.998085     27.2168           0           0           0           1
+33     0.99813  0.00376787  -0.0610159    -1.54654 -0.00404632    0.999982  -0.0044408   -0.313516   0.0609981  0.00467938    0.998127     28.1829           0           0           0           1
+34     0.998113   0.00193972   -0.0613743     -1.60668  -0.00191171     0.999998  0.000515183    -0.324411    0.0613752 -0.000396881     0.998115      29.1626            0            0            0            1
+35     0.99806  -0.0017885   -0.062228    -1.66532  0.00203402     0.99999  0.00388232   -0.335656   0.0622204 -0.00400136    0.998054     30.1428           0           0           0           1
+36    0.997945 -0.00917543  -0.0634115    -1.72059  0.00939451    0.999951  0.00315749   -0.343316   0.0633794 -0.00374672    0.997982     31.1244           0           0           0           1
+37     0.997825   -0.0112684   -0.0649459     -1.78049     0.011242     0.999937 -0.000771312    -0.350864    0.0649504  3.95099e-05     0.997888      32.1064            0            0            0            1
+38    0.997739  -0.0110126  -0.0662983    -1.85007   0.0107254    0.999932 -0.00468596   -0.361068   0.0663454  0.00396429    0.997789     33.0886           0           0           0           1
+39    0.997597 -0.00959503  -0.0686163    -1.92119  0.00924037    0.999942 -0.00548426   -0.373466   0.0686649  0.00483704    0.997628     34.0774           0           0           0           1
+40     0.99755  -0.0095802  -0.0693031    -1.99331  0.00931271    0.999948 -0.00418184   -0.387047   0.0693396  0.00352619    0.997587     35.0736           0           0           0           1
+41    0.997473 -0.00634387  -0.0707596     -2.0707  0.00626661     0.99998  -0.0013139   -0.403858   0.0707665  0.00086716    0.997493     36.0721           0           0           0           1
+42      0.99739  -0.00624366   -0.0719343     -2.14553   0.00625582      0.99998 -5.62375e-05    -0.416888    0.0719332 -0.000393917     0.997409      37.0728            0            0            0            1
+43    0.997312 -0.00473093  -0.0731254    -2.21909  0.00492848    0.999985  0.00252135   -0.428625   0.0731123 -0.00287497     0.99732     38.0643           0           0           0           1
+44    0.997318 -0.00467696  -0.0730348    -2.29215  0.00509473    0.999972  0.00553481   -0.440023   0.0730068 -0.00589206    0.997314     39.0618           0           0           0           1
+45     0.997274   0.00138304   -0.0737801     -2.37574 -0.000811217     0.999969   0.00777971    -0.447869    0.0737886  -0.00769865     0.997244      40.0548            0            0            0            1
+46     0.997262   0.00149131   -0.0739326     -2.45529 -0.000969511     0.999974   0.00709318    -0.454763    0.0739413  -0.00700208     0.997238      41.0557            0            0            0            1
+47    0.997266  0.00175929  -0.0738699    -2.53081 -0.00136899    0.999985  0.00533379   -0.460519   0.0738782 -0.00521809    0.997254     42.0518           0           0           0           1
+48    0.997253  0.00408494  -0.0739555    -2.61212 -0.00386552    0.999988  0.00310988   -0.469863   0.0739673 -0.00281546    0.997257     43.0493           0           0           0           1
+49    0.997185  0.00365371  -0.0748884    -2.68799 -0.00342799    0.999989  0.00314243    -0.47951   0.0748991 -0.00287687    0.997187     44.0473           0           0           0           1
+50    0.997077  0.00181435  -0.0763845    -2.76071 -0.00149292     0.99999  0.00426495   -0.487845   0.0763915 -0.00413845    0.997069     45.0403           0           0           0           1
+51    0.997018  0.00246727  -0.0771352    -2.84117 -0.00206285    0.999984  0.00532227   -0.499132   0.0771471 -0.00514727    0.997006     46.0244           0           0           0           1
+52    0.996991  0.00504805  -0.0773507    -2.92304 -0.00493379    0.999986  0.00166824   -0.510863   0.0773581 -0.00128158    0.997003      46.994           0           0           0           1
+53    0.996911  0.00581773  -0.0783264    -3.00373 -0.00604061    0.999978 -0.00260888   -0.521193   0.0783095  0.00307396    0.996924     47.9551           0           0           0           1
+54    0.996846  0.00678413  -0.0790757    -3.08343 -0.00711636    0.999967 -0.00392044   -0.534186   0.0790465  0.00447081    0.996861     48.9236           0           0           0           1
+55     0.996843   0.00557268   -0.0792034     -3.16262  -0.00562268     0.999984 -0.000408328     -0.54901    0.0791999  0.000852374     0.996858      49.9005            0            0            0            1
+56    0.996831  0.00375007  -0.0794568    -3.23868 -0.00354655     0.99999  0.00270227   -0.563036   0.0794661  -0.0024119    0.996835     50.8752           0           0           0           1
+57    0.996805  0.00190455  -0.0798474    -3.31582 -0.00164885    0.999993  0.00326822   -0.574113   0.0798531 -0.00312612    0.996802     51.8394           0           0           0           1
+58    0.996782 -0.00124932  -0.0801505    -3.39153  0.00141878    0.999997   0.0020573   -0.586659   0.0801477 -0.00216439    0.996781     52.8005           0           0           0           1
+59    0.996745  -0.0038025  -0.0805262     -3.4676   0.0038689    0.999992 0.000668539    -0.59892    0.080523 -0.00097791    0.996752     53.7575           0           0           0           1
+60    0.996643 -0.00519016  -0.0817059    -3.54489  0.00535256    0.999984  0.00176869    -0.60864   0.0816955 -0.00220009    0.996655      54.708           0           0           0           1
+61    0.996534  -0.0079249  -0.0828082    -3.62139  0.00842977    0.999948  0.00574894   -0.618858   0.0827583 -0.00642707    0.996549     55.6588           0           0           0           1
+62    0.996473 -0.00854289  -0.0834829    -3.69959  0.00945654      0.9999   0.0105549   -0.624401   0.0833844  -0.0113071    0.996453     56.6119           0           0           0           1
+63    0.996447 -0.00664747   -0.083957    -3.78502  0.00773966     0.99989   0.0126902   -0.629769   0.0838633  -0.0132949    0.996389     57.5607           0           0           0           1
+64    0.996335 -0.00522633  -0.0853755     -3.8689  0.00597793    0.999946  0.00855017   -0.636709   0.0853262  -0.0090292    0.996312     58.4941           0           0           0           1
+65     0.996221  -0.00343661   -0.0867892     -3.95276   0.00350579     0.999994  0.000644619    -0.644008    0.0867865 -0.000946448     0.996226      59.4131            0            0            0            1
+66    0.996144 -0.00149623  -0.0877201    -4.03806  0.00112725     0.99999 -0.00425562   -0.655271   0.0877256  0.00414033    0.996136     60.3236           0           0           0           1
+67    0.996055  0.00375138  -0.0886573    -4.12895 -0.00406723    0.999986 -0.00338223   -0.671324   0.0886434  0.00372948    0.996056     61.2274           0           0           0           1
+68    0.995922  0.00719305  -0.0899263    -4.21985  -0.0073202    0.999973 -0.00108421   -0.691307    0.089916  0.00173807    0.995948      62.125           0           0           0           1
+69     0.99582  0.00967277  -0.0908194    -4.30702 -0.00966905    0.999953 0.000481019   -0.708494   0.0908198 0.000399128    0.995867     63.0134           0           0           0           1
+70    0.995713   0.0102896  -0.0919182    -4.39131  -0.0103098    0.999947 0.000255248   -0.721276    0.091916 0.000693502    0.995767     63.8776           0           0           0           1
+71     0.99554   0.0119225  -0.0935844      -4.477  -0.0118725    0.999929  0.00109156   -0.734766   0.0935908 2.43836e-05    0.995611     64.7307           0           0           0           1
+72     0.995397    0.0126524   -0.0950024     -4.56121   -0.0125521      0.99992   0.00165348    -0.749039    0.0950157 -0.000453392     0.995476      65.5703            0            0            0            1
+73    0.995256   0.0126635  -0.0964665    -4.64297  -0.0125254    0.999919  0.00203772   -0.761909   0.0964846 -0.00081977    0.995334     66.3938           0           0           0           1
+74    0.995133   0.0127023  -0.0977168    -4.72623  -0.0124698    0.999918  0.00298947     -0.7711   0.0977468 -0.00175641     0.99521     67.2017           0           0           0           1
+75   0.994948   0.015548 -0.0991814   -4.81287 -0.0150604   0.999871 0.00566291  -0.780301  0.0992566 -0.0041406   0.995053    67.9995          0          0          0          1
+76    0.994794   0.0171065   -0.100462    -4.90076  -0.0162095    0.999821    0.009738   -0.788037    0.100611 -0.00805885    0.994893     68.7922           0           0           0           1

examples/Data/dubrovnik-3-7-pre-rewritten.txt

@@ -1,80 +0,0 @@
-3 7 19
-
-0 0 -385.989990234375 387.1199951171875
-1 0 -38.439998626708984375 492.1199951171875
-2 0 -667.91998291015625 123.1100006103515625
-0 1 383.8800048828125 -15.299989700317382812
-1 1 559.75 -106.15000152587890625
-0 2 591.54998779296875 136.44000244140625
-1 2 863.8599853515625 -23.469970703125
-2 2 494.720001220703125 112.51999664306640625
-0 3 592.5 125.75
-1 3 861.08001708984375 -35.219970703125
-2 3 498.540008544921875 101.55999755859375
-0 4 348.720001220703125 558.3800048828125
-1 4 776.030029296875 483.529998779296875
-2 4 7.7800288200378417969 326.350006103515625
-0 5 14.010009765625 96.420013427734375
-1 5 207.1300048828125 118.3600006103515625
-0 6 202.7599945068359375 340.989990234375
-1 6 543.18011474609375 294.80999755859375
-2 6 -58.419979095458984375 110.8300018310546875
-
-0.29656188120312942935
--0.035318354384285870207
-0.31252101755032046793
-0.47230274932665988752
--0.3572340863744113415
--2.0517704282499575896
-1430.031982421875
--7.5572756941255647689e-08
-3.2377570134516087119e-14
-
-0.28532097381985194184
--0.27699838370789808817
-0.048601169984112867206
--1.2598695987143850861
--0.049063798188844320869
--1.9586867140445654023
-1432.137451171875
--7.3171918302250560373e-08
-3.1759419042137054801e-14
-
-0.057491325683772541433
-0.34853090049579965592
-0.47985129303736057116
-8.1963904289063389541
-6.5146840788718787252
--3.8392804395897406344
-1572.047119140625
--1.5962623223231275915e-08
--1.6507904730136101212e-14
-
--11.317351620610928364
-3.3594874875767186673
--42.755222607849105998
-
-4.2648515634753199066
--8.4629358700849355301
--22.252086323427270997
-
-10.996977688149536689
--9.2123370180278048025
--29.206739014051372294
-
-10.935342607054865383
--9.4338917557810741954
--29.112263909175499776
-
-15.714024935401759819
-1.3745079651566265433
--59.286834979937104606
-
--1.3624227800805182031
--4.1979357415396094666
--21.034430148188398846
-
-6.7690173115899296974
--4.7352452433700786827
--53.605307875695892506
-

examples/Data/noisyToyGraph.txt

@@ -0,0 +1,9 @@
+VERTEX_SE2 0 0.000000 0.000000 0.000000
+VERTEX_SE2 1 0.774115 1.183389 1.576173
+VERTEX_SE2 2 -0.262420 2.047059 -3.127594
+VERTEX_SE2 3 -1.605649 0.993891 -1.561134
+EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

examples/Data/optimizedNoisyToyGraph.txt

@@ -0,0 +1,9 @@
+VERTEX_SE2 0 0.000000 -0.000000 0.000000
+VERTEX_SE2 1 0.955797 1.137643 1.543041
+VERTEX_SE2 2 0.129867 1.989651 3.201259
+VERTEX_SE2 3 -1.047715 0.933789 4.743682
+EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

examples/Data/orientationsNoisyToyGraph.txt

@@ -0,0 +1,9 @@
+VERTEX_SE2 0 0.000000 0.000000 0.000000
+VERTEX_SE2 1 0.000000 0.000000 1.565449
+VERTEX_SE2 2 0.000000 0.000000 3.134143
+VERTEX_SE2 3 0.000000 0.000000 4.710123
+EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
+EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

examples/Data/pose2example.txt

@@ -0,0 +1,23 @@
+VERTEX_SE2 0 0.000000 0.000000 0.000000
+VERTEX_SE2 1 1.030390 0.011350 -0.081596
+VERTEX_SE2 2 2.036137 -0.129733 -0.301887
+VERTEX_SE2 3 3.015097 -0.442395 -0.345514
+VERTEX_SE2 4 3.343949 0.506678 1.214715
+VERTEX_SE2 5 3.684491 1.464049 1.183785
+VERTEX_SE2 6 4.064626 2.414783 1.176333
+VERTEX_SE2 7 4.429778 3.300180 1.259169
+VERTEX_SE2 8 4.128877 2.321481 -1.825391
+VERTEX_SE2 9 3.884653 1.327509 -1.953016
+VERTEX_SE2 10 3.531067 0.388263 -2.148934
+EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699

examples/Data/pose3example-grid.txt

@@ -0,0 +1,71 @@
+VERTEX_SE3:QUAT 0 1.63791e-12 7.56548e-14 -3.02811e-12 5.35657e-13 2.43616e-13 9.71152e-14 1
+VERTEX_SE3:QUAT 1 1.01609 0.00274307 -0.0351514 -0.499545 0.247735 0.723569 -0.406854
+VERTEX_SE3:QUAT 2 1.99996 0.0304956 -0.040662 0.403501 -0.294714 -0.4254 0.754563
+VERTEX_SE3:QUAT 3 1.94371 1.06535 0.0118614 -0.0471731 -0.541615 0.820893 0.17482
+VERTEX_SE3:QUAT 4 0.962753 0.999477 0.0211017 -0.19663 -0.66009 0.470743 0.551379
+VERTEX_SE3:QUAT 5 -0.00956768 0.965396 -0.021854 -0.320221 -0.518368 0.47521 0.634766
+VERTEX_SE3:QUAT 6 -0.0863793 1.97682 0.000531117 -0.0173439 -0.573793 -0.450627 0.683663
+VERTEX_SE3:QUAT 7 0.918905 2.01556 -0.0139773 0.56169 -0.440513 0.199057 0.671438
+VERTEX_SE3:QUAT 8 1.92094 2.05524 0.0469884 0.0073084 -0.372357 -0.467582 0.801663
+VERTEX_SE3:QUAT 9 1.86182 2.05449 1.09237 0.0131731 -0.05784 0.0335652 0.997674
+VERTEX_SE3:QUAT 10 0.880176 2.02406 1.00997 -0.39342 -0.287909 0.757918 0.433462
+VERTEX_SE3:QUAT 11 -0.0960463 1.98653 0.995791 0.434103 -0.199044 0.585176 0.655367
+VERTEX_SE3:QUAT 12 -0.0911401 0.997117 0.988217 -0.0925477 0.572872 0.537294 0.612019
+VERTEX_SE3:QUAT 13 0.948316 1.02239 0.991745 0.142484 0.560062 0.750078 0.321578
+VERTEX_SE3:QUAT 14 1.92631 1.08945 1.06749 0.23878 0.380837 0.796564 -0.404269
+VERTEX_SE3:QUAT 15 1.95398 0.0777667 0.982353 -0.384392 0.58733 0.685207 -0.194366
+VERTEX_SE3:QUAT 16 0.946032 0.0482667 0.952308 -0.218979 0.186315 -0.494185 0.820437
+VERTEX_SE3:QUAT 17 -0.0625076 -0.034424 0.942171 0.514725 -0.185043 -0.44771 0.707371
+VERTEX_SE3:QUAT 18 -0.083807 -0.0106666 1.9853 0.00792651 1.98919e-05 -0.00128106 0.999968
+VERTEX_SE3:QUAT 19 0.918067 -0.000897795 1.92157 -0.342141 0.241241 -0.726975 0.544288
+VERTEX_SE3:QUAT 20 1.90041 0.0323631 2.00636 0.412572 -0.0930131 -0.133075 0.896339
+VERTEX_SE3:QUAT 21 1.84895 1.05013 2.0738 -0.580757 0.35427 0.729393 -0.0721062
+VERTEX_SE3:QUAT 22 0.880221 1.00671 1.99021 0.147752 0.355662 0.917953 0.095058
+VERTEX_SE3:QUAT 23 -0.0950872 1.00374 1.95013 -0.29909 -0.0578461 0.857019 0.415594
+VERTEX_SE3:QUAT 24 -0.111581 1.97979 1.98762 0.565153 0.214463 -0.523058 0.600848
+VERTEX_SE3:QUAT 25 0.837568 2.01589 2.03075 -0.284756 0.369992 0.875484 -0.124692
+VERTEX_SE3:QUAT 26 1.82708 2.05081 2.07052 0.254696 0.250865 0.653216 0.667462
+EDGE_SE3:QUAT 0 1 1.00497 0.002077 -0.015539 -0.508004 0.250433 0.711222 -0.416386 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 1 2 -0.200593 0.339956 -0.908079 -0.093598 0.151993 0.42829 0.885836 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 2 3 -0.922791 0.330629 -0.292682 0.365657 -0.051986 0.924849 -0.090813 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 3 4 0.893075 0.246476 0.331154 -0.285927 0.341221 -0.267609 0.854517 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 4 5 0.280674 0.244242 0.923726 0.035064 0.21101 0.083834 0.973251 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 5 6 0.955621 0.355669 -0.025152 -0.306713 0.131221 -0.781587 0.527096 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 6 7 -0.076631 0.636081 -0.771439 0.702021 0.326514 0.122181 0.620988 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 7 8 0.582761 -0.721177 -0.376875 -0.733841 -0.170725 -0.256653 0.605359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 8 9 0.600312 0.298765 0.767014 0.057612 0.332574 0.486324 0.805956 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 9 10 -0.986649 0.03008 -0.008766 -0.362177 -0.253215 0.763748 0.470531 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 10 11 0.275109 0.534769 0.823463 0.450708 -0.472399 -0.432689 0.621677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 11 12 -0.61882 0.024878 0.773748 0.0927029 0.786162 -0.21122 0.573359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 12 13 -0.175537 -0.730832 0.634529 -0.018628 0.006375 0.428306 0.903419 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 13 14 -0.700208 -0.245198 0.637353 -0.035865 0.273394 0.645363 0.712374 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 14 15 0.373495 0.373768 -0.846199 0.400323 0.310362 -0.422222 0.751762 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 15 16 0.648588 0.157829 0.72252 0.781502 -0.210141 -0.501005 -0.30674 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 16 17 -0.390339 -0.702656 -0.572321 0.765815 0.055816 0.032478 0.63981 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 17 18 -0.261114 0.908685 0.421318 -0.501833 0.166567 0.448468 0.720622 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 18 19 1.00815 0.012634 -0.029822 -0.347007 0.205082 -0.740641 0.537569 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 19 20 -0.162376 0.581623 0.810804 0.628338 0.075411 0.650639 0.41973 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 20 21 -0.358942 0.627689 -0.704045 -0.469133 0.542456 0.530583 -0.451816 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 21 22 0.362417 0.298352 0.854822 0.004058 -0.696926 0.140345 0.703265 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 22 23 0.934942 0.020321 -0.358044 -0.445461 0.260916 -0.379862 0.767589 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 23 24 0.741887 -0.657659 0.215293 -0.584859 0.196138 0.688031 0.38221 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 24 25 0.300145 0.82011 -0.39974 0.46538 -0.593595 -0.202131 0.624668 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 25 26 -0.85591 0.022701 -0.510794 0.12929 -0.685192 -0.503707 0.509978 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 0 5 0.026721 0.990497 -0.007651 -0.317476 -0.510239 0.467341 0.648427 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 3 8 0.390516 -0.401461 -0.830724 0.503106 -0.367814 0.780584 0.047806 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 4 1 -0.813838 -0.446181 0.319175 0.224903 -0.031827 0.97265 0.048561 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 4 13 0.571273 -0.805401 0.077339 0.892031 0.329761 0.275468 0.140201 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 5 12 0.389794 -0.882655 0.268063 0.712423 0.550662 0.275339 0.33677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 6 11 0.800298 0.505022 0.361738 0.739335 0.419366 0.443817 0.283801 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 10 13 -0.912531 0.430955 -0.018942 0.830493 -0.093519 0.272041 0.477001 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 12 23 -0.797606 0.437737 0.311476 -0.657137 -0.196625 0.136652 0.714728 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 13 22 -0.116836 0.952032 0.269398 -0.216437 0.086571 0.260965 0.936781 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 14 21 0.749295 0.373389 0.581641 0.253048 0.511007 -0.537262 0.621439 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 16 1 0.160985 0.555966 -0.811911 0.748057 0.122381 -0.369631 0.537407 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 18 23 0.028909 1.02689 -0.00265 -0.294167 -0.071607 0.850901 0.429308 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 19 16 -0.230711 0.750637 -0.607511 0.14647 -0.102538 0.297899 0.937704 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 20 15 -0.031986 -0.741129 -0.728721 -0.278926 0.731172 0.404675 -0.473103 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 22 19 -0.332601 0.704401 -0.687251 -0.372165 -0.054346 0.713024 0.591725 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 22 25 0.347067 -0.634646 0.657147 0.018567 0.476762 0.040939 0.877882 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 25 10 0.388971 -0.723981 -0.559653 -0.373459 -0.014654 -0.696123 0.612965 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
+EDGE_SE3:QUAT 26 21 -0.979482 -0.024822 0.043763 -0.326753 0.819942 0.292615 0.367837 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400

examples/Data/pose3example-offdiagonal-rewritten.txt

@@ -0,0 +1,3 @@
+VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
+VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
+EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000

examples/Data/pose3example-offdiagonal.txt

@@ -0,0 +1,3 @@
+VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
+EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000   10000.000000 2.000000 2.000000 2.000000 2.000000   10000.000000 3.000000 3.000000 3.000000   10000.000000 4.000000 4.000000   10000.000000 5.000000   10000.0000

examples/Data/pose3example-rewritten.txt

@@ -0,0 +1,11 @@
+VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
+VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
+VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446
+VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024
+VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488
+EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592076 0.30338 -0.513225 0.542222 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
+EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000

examples/Data/pose3example.txt

@@ -0,0 +1,11 @@
+VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
+VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
+VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
+VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
+EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 1 2   0.523923 0.776654 0.326659   0.311512 0.656877 -0.678505 0.105373   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 2 3   0.910927 0.055169 -0.411761   0.595795 -0.561677 0.079353 0.568551   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 3 4   0.775288 0.228798 -0.596923   -0.592077 0.303380 -0.513226 0.542221   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 1 4   -0.577841 0.628016 -0.543592   -0.125250 -0.534379 0.769122 0.327419   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 3 0   -0.623267 0.086928 0.773222   0.104639 0.627755 0.766795 0.083672   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000

examples/LocalizationExample.cpp

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

examples/OdometryExample.cpp

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

examples/PlanarSLAMExample.cpp

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

examples/Pose2SLAMExample.cpp

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

examples/Pose2SLAMExample_g2o.cpp

@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Pose2SLAMExample_g2o.cpp
+ * @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the
+ * optimization. Output is written on a file, in g2o format
+ * Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o
+ * @date May 15, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char *argv[]) {
+
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("noisyToyGraph.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  boost::tie(graph, initial) = readG2o(g2oFile);
+
+  // Add prior on the pose having index (key) = 0
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
+  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  if (argc < 3) {
+    result.print("result");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    writeG2o(*graph, result, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+  return 0;
+}

examples/Pose2SLAMExample_graph.cpp

@@ -32,7 +32,8 @@ int main (int argc, char** argv) {
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
-  boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
+  string graph_file = findExampleDataFile("w100.graph");
+  boost::tie(graph, initial) = load2D(graph_file, model);
   initial->print("Initial estimate:\n");
 
   // Add a Gaussian prior on first poses

examples/Pose2SLAMExample_lago.cpp

@@ -0,0 +1,64 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Pose2SLAMExample_lago.cpp
+ * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
+ * using LAGO (Linear Approximation for Graph Optimization). See class lago.h
+ * Output is written on a file, in g2o format
+ * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
+ * @date May 15, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/lago.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char *argv[]) {
+
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("noisyToyGraph.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  boost::tie(graph, initial) = readG2o(g2oFile);
+
+  // Add prior on the pose having index (key) = 0
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
+  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graphWithPrior.print();
+
+  std::cout << "Computing LAGO estimate" << std::endl;
+  Values estimateLago = lago::initialize(graphWithPrior);
+  std::cout << "done!" << std::endl;
+
+  if (argc < 3) {
+    estimateLago.print("estimateLago");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    writeG2o(*graph, estimateLago, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+
+  return 0;
+}

examples/Pose2SLAMwSPCG.cpp

@@ -104,7 +104,7 @@ int main(int argc, char** argv) {
   LevenbergMarquardtParams parameters;
   parameters.verbosity = NonlinearOptimizerParams::ERROR;
   parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
-  parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT;
+  parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
 
   {
     parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

examples/RangeISAMExample_plaza2.cpp

@@ -40,6 +40,7 @@
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/RangeFactor.h>
+#include <gtsam/slam/dataset.h>
 
 // Standard headers, added last, so we know headers above work on their own
 #include <boost/foreach.hpp>
@@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel;
 typedef pair<double, Pose2> TimedOdometry;
 list<TimedOdometry> readOdometry() {
   list<TimedOdometry> odometryList;
-  ifstream is("../../examples/Data/Plaza2_DR.txt");
-  if (!is)
-    throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
+  string data_file = findExampleDataFile("Plaza2_DR.txt");
+  ifstream is(data_file.c_str());
 
   while (is) {
     double t, distance_traveled, delta_heading;
@@ -78,9 +78,8 @@ list<TimedOdometry> readOdometry() {
 typedef boost::tuple<double, size_t, double> RangeTriple;
 vector<RangeTriple> readTriples() {
   vector<RangeTriple> triples;
-  ifstream is("../../examples/Data/Plaza2_TD.txt");
-  if (!is)
-    throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
+  string data_file = findExampleDataFile("Plaza2_TD.txt");
+  ifstream is(data_file.c_str());
 
   while (is) {
     double t, sender, receiver, range;

examples/SFMExample_SmartFactor.cpp

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------------
+
+ * 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_SmartFactor.cpp
+ * @brief   A structure-from-motion problem on a simulated dataset, using smart projection factor
+ * @author  Duy-Nguyen Ta
+ * @author  Jing Dong
+ * @author  Frank Dellaert
+ */
+
+/**
+ * A structure-from-motion example with landmarks
+ *  - The landmarks form a 10 meter cube
+ *  - The robot rotates around the landmarks, always facing towards the cube
+ */
+
+// For loading the data
+#include "SFMdata.h"
+
+// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
+#include <gtsam/geometry/Point2.h>
+
+// In GTSAM, measurement functions are represented as 'factors'.
+// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark,
+// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration,
+// The calibration should be known.
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+
+// Also, we will initialize the robot at some location using a Prior factor.
+#include <gtsam/slam/PriorFactor.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.
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+// Finally, once all of the factors have been added to our factor graph, we will want to
+// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
+// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
+// trust-region method known as Powell's Degleg
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+
+// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
+// nonlinear functions around an initial linearization point, then solve the linear system
+// to update the linearization point. This happens repeatedly until the solver converges
+// to a consistent set of variable values. This requires us to specify an initial guess
+// for each variable, held in a Values container.
+#include <gtsam/nonlinear/Values.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+// Make the typename short so it looks much cleaner
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
+    gtsam::Cal3_S2> SmartFactor;
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+
+  // Define the camera calibration parameters
+  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+  // Define the camera observation noise model
+  noiseModel::Isotropic::shared_ptr measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+
+  // Create the set of ground-truth landmarks and poses
+  vector<Point3> points = createPoints();
+  vector<Pose3> poses = createPoses();
+
+  // Create a factor graph
+  NonlinearFactorGraph graph;
+
+  // Simulated measurements from each camera pose, adding them to the factor graph
+  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());
+
+    for (size_t i = 0; i < poses.size(); ++i) {
+
+      // generate the 2D measurement
+      SimpleCamera 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:
+      //    1. the 2D measurement
+      //    2. the corresponding camera's key
+      //    3. camera noise model
+      //    4. camera calibration
+      smartfactor->add(measurement, i, measurementNoise, K);
+    }
+
+    // insert the smart factor in the graph
+    graph.push_back(smartfactor);
+  }
+
+  // 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(
+      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
+  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+
+  // 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.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));
+  for (size_t i = 0; i < poses.size(); ++i)
+    initialEstimate.insert(i, poses[i].compose(delta));
+  initialEstimate.print("Initial Estimates:\n");
+
+  // Optimize the graph and print results
+  Values result = DoglegOptimizer(graph, initialEstimate).optimize();
+  result.print("Final results:\n");
+
+  // A smart factor represent the 3D point inside the factor, not as a variable.
+  // The 3D position of the landmark is not explicitly calculated by the optimizer.
+  // To obtain the landmark's 3D position, we use the "point" method of the smart factor.
+  Values landmark_result;
+  for (size_t j = 0; j < points.size(); ++j) {
+
+    // The output of point() is in boost::optional<gtsam::Point3>, as sometimes
+    // the triangulation operation inside smart factor will encounter degeneracy.
+    boost::optional<Point3> point;
+
+    // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
+    SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
+    if (smart) {
+      point = smart->point(result);
+      if (point) // ignore if boost::optional return NULL
+        landmark_result.insert(j, *point);
+    }
+  }
+
+  landmark_result.print("Landmark results:\n");
+
+  return 0;
+}
+/* ************************************************************************* */
+

examples/SFMExample_bal.cpp

@@ -43,8 +43,7 @@ int main (int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfM_data mydata;
-  const bool success = readBAL(filename, mydata);
-  assert(success);
+  assert(readBAL(filename, mydata));
   cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph

examples/SolverComparer.cpp

@@ -13,6 +13,22 @@
 * @brief   Incremental and batch solving, timing, and accuracy comparisons
 * @author  Richard Roberts
 * @date    August, 2013
+*
+* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
+*
+* Solve in incremental and write to file w_inc:
+* ./SolverComparer --incremental -d w10000 -o w_inc
+*
+* You can then perturb that initialization to get batch something to optimize.
+* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
+* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
+*
+* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
+* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
+*
+* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
+* ./SolverComparer --compare w_inc w_batch
+*
 */
 
 #include <gtsam/base/timing.h>

examples/StereoVOExample.cpp

@@ -0,0 +1,76 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 SteroVOExample.cpp
+ * @brief A stereo visual odometry example
+ * @date May 25, 2014
+ * @author Stephen Camp
+ */
+
+/**
+ * A 3D stereo visual odometry example
+ *  - robot starts at origin
+ *  -moves forward 1 meter
+ *  -takes stereo readings on three landmarks
+ */
+ 
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/StereoFactor.h>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv){
+
+  //create graph object, add first pose at origin with key '1'
+  NonlinearFactorGraph graph;
+  Pose3 first_pose;
+  graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
+  
+  //create factor noise model with 3 sigmas of value 1
+  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  //create stereo camera calibration object with .2m between cameras
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
+
+  //create and add stereo factors between first pose (key value 1) and the three landmarks
+  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
+  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
+  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
+
+  //create and add stereo factors between second pose and the three landmarks
+  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
+  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
+  graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
+
+  //create Values object to contain initial estimates of camera poses and landmark locations
+  Values initial_estimate;
+
+  //create and add iniital estimates
+  initial_estimate.insert(1, first_pose);
+  initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
+  initial_estimate.insert(3, Point3(1, 1, 5));
+  initial_estimate.insert(4, Point3(-1, 1, 5));
+  initial_estimate.insert(5, Point3(0, -0.5, 5));
+
+  //create Levenberg-Marquardt optimizer for resulting factor graph, optimize
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  Values result = optimizer.optimize();
+
+  result.print("Final result:\n");
+
+  return 0;
+}

examples/StereoVOExample_large.cpp

@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------------
+
+* 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 SteroVOExample.cpp
+* @brief A stereo visual odometry example
+* @date May 25, 2014
+* @author Stephen Camp
+*/
+
+
+/**
+ * A 3D stereo visual odometry example
+ *  - robot starts at origin
+ *  -moves forward, taking periodic stereo measurements
+ *  -takes stereo readings of many landmarks
+ */
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/StereoFactor.h>
+#include <gtsam/slam/dataset.h>
+
+#include <string>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv){
+
+  Values initial_estimate;
+  NonlinearFactorGraph graph;
+  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+
+  string calibration_loc = findExampleDataFile("VO_calibration.txt");
+  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
+  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
+  
+  //read camera calibration info from file
+  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
+  double fx, fy, s, u0, v0, b;
+  ifstream calibration_file(calibration_loc.c_str());
+  cout << "Reading calibration info" << endl;
+  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
+
+  //create stereo camera calibration object
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
+  
+  ifstream pose_file(pose_loc.c_str());
+  cout << "Reading camera poses" << endl;
+  int pose_id;
+  MatrixRowMajor m(4,4);
+  //read camera pose parameters and use to make initial estimates of camera poses
+  while (pose_file >> pose_id) {
+    for (int i = 0; i < 16; i++) {
+      pose_file >> m.data()[i];
+    }
+    initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
+  }
+  
+  // camera and landmark keys
+  size_t x, l;
+
+  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
+  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  double uL, uR, v, X, Y, Z;
+  ifstream factor_file(factor_loc.c_str());
+  cout << "Reading stereo factors" << endl;
+  //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
+  while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
+    graph.push_back(
+        GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
+            Symbol('x', x), Symbol('l', l), K));
+    //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
+    if (!initial_estimate.exists(Symbol('l', l))) {
+      Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
+      //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
+      Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
+      initial_estimate.insert(Symbol('l', l), worldPoint);
+    }
+  }
+
+  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
+  //constrain the first pose such that it cannot change from its original value during optimization
+  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
+  // QR is much slower than Cholesky, but numerically more stable
+  graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
+
+  cout << "Optimizing" << endl;
+  //create Levenberg-Marquardt optimizer to optimize the factor graph
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  Values result = optimizer.optimize();
+
+  cout << "Final result sample:" << endl;
+  Values pose_values = result.filter<Pose3>();
+  pose_values.print("Final camera poses:\n");
+
+  return 0;
+}

examples/VisualISAMExample.cpp

@@ -14,6 +14,7 @@
  * @brief   A visualSLAM example for the structure-from-motion problem on a simulated dataset
  * This version uses iSAM to solve the problem incrementally
  * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
  */
 
 /**
@@ -61,7 +62,8 @@ int main(int argc, char* argv[]) {
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  noiseModel::Isotropic::shared_ptr noise = //
+      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -69,7 +71,8 @@ int main(int argc, char* argv[]) {
   // Create the set of ground-truth poses
   vector<Pose3> poses = createPoses();
 
-  // Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates
+  // Create a NonlinearISAM object which will relinearize and reorder the variables
+  // every "relinearizeInterval" updates
   int relinearizeInterval = 3;
   NonlinearISAM isam(relinearizeInterval);
 
@@ -82,32 +85,44 @@ int main(int argc, char* argv[]) {
 
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
+      // Create ground truth measurement
       SimpleCamera camera(poses[i], *K);
       Point2 measurement = camera.project(points[j]);
-      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
+      // Add measurement
+      graph.add(
+          GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
+              Symbol('x', i), Symbol('l', j), K));
     }
 
-    // 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))));
+    Pose3 noise(Rot3::rodriguez(-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
+    initialEstimate.insert(Symbol('x', i), initial_xi);
 
     // 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
     // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
     // adding it to iSAM.
-    if( i == 0) {
-      // Add a prior on pose x0
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
+    if (i == 0) {
+      // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
+      graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
       // Add a prior on landmark l0
-      noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
-      graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
+      noiseModel::Isotropic::shared_ptr pointNoise =
+          noiseModel::Isotropic::Sigma(3, 0.1);
+      graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
 
       // Add initial guesses to all observed landmarks
-      // Intentionally initialize the variables off from the ground truth
-      for (size_t j = 0; j < points.size(); ++j)
-        initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+      Point3 noise(-0.25, 0.20, 0.15);
+      for (size_t j = 0; j < points.size(); ++j) {
+        // Intentionally initialize the variables off from the ground truth
+        Point3 initial_lj = points[j].compose(noise);
+        initialEstimate.insert(Symbol('l', j), initial_lj);
+      }
 
     } else {
       // Update iSAM with the new factors

gtsam.h

@@ -1423,6 +1423,7 @@ virtual class GaussianBayesNet {
   void push_back(const gtsam::GaussianBayesNet& bayesNet);
   
   gtsam::VectorValues optimize() const;
+  gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
   gtsam::VectorValues optimizeGradientSearch() const;
   gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
   gtsam::VectorValues gradientAtZero() const;
@@ -1480,9 +1481,7 @@ class GaussianISAM {
 
 #include <gtsam/linear/IterativeSolver.h>
 virtual class IterativeOptimizationParameters {
-  string getKernel() const ;
   string getVerbosity() const;
-  void setKernel(string s) ;
   void setVerbosity(string s) ;
   void print() const;
 };
@@ -1515,7 +1514,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
   void print() const;
 };
 
-class SubgraphSolver  {
+virtual class SubgraphSolver  {
   SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   gtsam::VectorValues optimize() const;
@@ -1550,8 +1549,12 @@ char symbolChr(size_t key);
 size_t symbolIndex(size_t key);
 
 // Default keyformatter
-void printKeySet(const gtsam::KeySet& keys);
-void printKeySet(const gtsam::KeySet& keys, string s);
+void printKeyList  (const gtsam::KeyList& keys);
+void printKeyList  (const gtsam::KeyList& keys, string s);
+void printKeyVector(const gtsam::KeyVector& keys);
+void printKeyVector(const gtsam::KeyVector& keys, string s);
+void printKeySet   (const gtsam::KeySet& keys);
+void printKeySet   (const gtsam::KeySet& keys, string s);
 
 #include <gtsam/inference/LabeledSymbol.h>
 class LabeledSymbol {
@@ -1725,6 +1728,7 @@ class KeySet {
 
   // structure specific methods
   void insert(size_t key);
+  void merge(gtsam::KeySet& other);
   bool erase(size_t key); // returns true if value was removed
   bool count(size_t key) const; // returns true if value exists
 
@@ -1849,12 +1853,12 @@ virtual class NonlinearOptimizerParams {
   
   void setLinearSolverType(string solver);
   void setOrdering(const gtsam::Ordering& ordering);
-  void setIterativeParams(const gtsam::SubgraphSolverParameters &params);
+  void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
 
   bool isMultifrontal() const;
   bool isSequential() const;
   bool isCholmod() const;
-  bool isCG() const;
+  bool isIterative() const;
 };
 
 bool checkConvergence(double relativeErrorTreshold,
@@ -2140,6 +2144,8 @@ 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);
 
+  pair<ROTATION, double> measured() const;
+
   // enabling serialization functionality
   void serialize() const;
 };
@@ -2191,6 +2197,25 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+template<POSE, LANDMARK, CALIBRATION>
+virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
+
+  SmartProjectionPoseFactor(double rankTol, double linThreshold,
+      bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
+
+  SmartProjectionPoseFactor(double rankTol);
+  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;
+};
+
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
+
 
 #include <gtsam/slam/StereoFactor.h>
 template<POSE, LANDMARK>
@@ -2241,6 +2266,13 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
     gtsam::noiseModel::Base* model);
+void save2D(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
+    string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& estimate, string filename);
 
 //*************************************************************************
 // Navigation
@@ -2286,7 +2318,8 @@ virtual class ConstantBias : gtsam::Value {
 #include <gtsam/navigation/ImuFactor.h>
 class ImuFactorPreintegratedMeasurements {
   // Standard Constructor
-  ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
+  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
@@ -2324,6 +2357,15 @@ class CombinedImuFactorPreintegratedMeasurements {
       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
@@ -2337,8 +2379,7 @@ class CombinedImuFactorPreintegratedMeasurements {
 
 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,
-      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
-      const gtsam::noiseModel::Base* model);
+      const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
 
   // Standard Interface
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
@@ -2355,17 +2396,26 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
 namespace utilities {
 
   #include <matlab.h>
+  gtsam::KeyList createKeyList(Vector I);
+  gtsam::KeyList createKeyList(string s, Vector I);
+  gtsam::KeyVector createKeyVector(Vector I);
+  gtsam::KeyVector createKeyVector(string s, Vector I);
+  gtsam::KeySet createKeySet(Vector I);
+  gtsam::KeySet createKeySet(string s, Vector I);
   Matrix extractPoint2(const gtsam::Values& values);
   Matrix extractPoint3(const gtsam::Values& values);
   Matrix extractPose2(const gtsam::Values& values);
   gtsam::Values allPose3s(gtsam::Values& values);
   Matrix extractPose3(const gtsam::Values& values);
   void perturbPoint2(gtsam::Values& values, double sigma, int seed);
+  void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
   void perturbPoint3(gtsam::Values& values, double sigma, int seed);
   void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
+  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
+  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
 
 } //\namespace utilities
 

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

@@ -617,11 +617,12 @@
 
 #include "ccolamd.h"
 
-#include <stdlib.h>
 #include <math.h>
 #include <limits.h>
 
 #ifdef MATLAB_MEX_FILE
+#include <stdint.h>
+typedef uint16_t char16_t;
 #include "mex.h"
 #include "matrix.h"
 #endif

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

@@ -13,6 +13,9 @@
 
 #ifndef NPRINT
 #ifdef MATLAB_MEX_FILE
+#include <stdlib.h>
+#include <stdint.h>
+typedef uint16_t char16_t;
 #include "mex.h"
 int (*ccolamd_printf) (const char *, ...) = mexPrintf ;
 #else

gtsam/3rdparty/CMakeLists.txt

@@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 		endif()
 	endforeach(eigen_dir)
 
+	# do the same for the unsupported eigen folder
+	file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
+	
+	file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
+	foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
+		get_filename_component(filename ${unsupported_eigen_dir} NAME)
+		if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
+			list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
+			install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
+		endif()
+	endforeach(unsupported_eigen_dir)
+
 	# Add to project source
 	set(eigen_headers ${eigen_headers} PARENT_SCOPE)
+	# set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE)
 
 	# install Eigen - only the headers in our 3rdparty directory
 	install(DIRECTORY Eigen/Eigen
 		DESTINATION include/gtsam/3rdparty/Eigen
 		FILES_MATCHING PATTERN "*.h")
+	
+	install(DIRECTORY Eigen/unsupported/Eigen
+		DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
+		FILES_MATCHING PATTERN "*.h")
 endif()
 
 option(GTSAM_BUILD_METIS "Build metis library" ON)

gtsam/3rdparty/Eigen/CMakeLists.txt

@@ -204,7 +204,7 @@ if(NOT MSVC)
 
   option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
   if(EIGEN_TEST_NEON)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8")
     message(STATUS "Enabling NEON in tests/examples")
   endif()
 

gtsam/3rdparty/Eigen/CTestConfig.cmake

@@ -4,14 +4,10 @@
 ## # The following are required to uses Dart and the Cdash dashboard
 ##   ENABLE_TESTING()
 ##   INCLUDE(CTest)
-set(CTEST_PROJECT_NAME "Eigen")
+set(CTEST_PROJECT_NAME "Eigen3.2")
 set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
 
 set(CTEST_DROP_METHOD "http")
 set(CTEST_DROP_SITE "manao.inria.fr")
-set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen")
+set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2")
 set(CTEST_DROP_SITE_CDASH TRUE)
-set(CTEST_PROJECT_SUBPROJECTS
-Official
-Unsupported
-)

gtsam/3rdparty/Eigen/Eigen/Core

@@ -95,7 +95,7 @@
     extern "C" {
       // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
       // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
-      #ifdef __INTEL_COMPILER
+      #if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110
         #include <immintrin.h>
       #else
         #include <emmintrin.h>
@@ -165,7 +165,7 @@
 #endif
 
 // required for __cpuid, needs to be included after cmath
-#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64))
+#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE))
   #include <intrin.h>
 #endif
 

gtsam/3rdparty/Eigen/Eigen/Eigen2Support

@@ -14,12 +14,25 @@
 #error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header
 #endif
 
+#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING
+
+#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)
+#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)"
+#else
+#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)")
+#endif
+
+#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING
+
 #include "src/Core/util/DisableStupidWarnings.h"
 
 /** \ingroup Support_modules
   * \defgroup Eigen2Support_Module Eigen2 support module
-  * This module provides a couple of deprecated functions improving the compatibility with Eigen2.
   *
+  * \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
+  *
+  * This module provides a couple of deprecated functions improving the compatibility with Eigen2.
+  * 
   * To use it, define EIGEN2_SUPPORT before including any Eigen header
   * \code
   * #define EIGEN2_SUPPORT

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

@@ -16,7 +16,10 @@
 namespace Eigen { 
 
 namespace internal {
-template<typename MatrixType, int UpLo> struct LDLT_Traits;
+  template<typename MatrixType, int UpLo> struct LDLT_Traits;
+
+  // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
+  enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
 }
 
 /** \ingroup Cholesky_Module
@@ -69,7 +72,12 @@ template<typename _MatrixType, int _UpLo> class LDLT
       * The default constructor is useful in cases in which the user intends to
       * perform decompositions via LDLT::compute(const MatrixType&).
       */
-    LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {}
+    LDLT() 
+      : m_matrix(), 
+        m_transpositions(), 
+        m_sign(internal::ZeroSign),
+        m_isInitialized(false) 
+    {}
 
     /** \brief Default Constructor with memory preallocation
       *
@@ -81,6 +89,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
       : m_matrix(size, size),
         m_transpositions(size),
         m_temporary(size),
+        m_sign(internal::ZeroSign),
         m_isInitialized(false)
     {}
 
@@ -93,6 +102,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
       : m_matrix(matrix.rows(), matrix.cols()),
         m_transpositions(matrix.rows()),
         m_temporary(matrix.rows()),
+        m_sign(internal::ZeroSign),
         m_isInitialized(false)
     {
       compute(matrix);
@@ -139,7 +149,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
     inline bool isPositive() const
     {
       eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_sign == 1;
+      return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
     }
     
     #ifdef EIGEN2_SUPPORT
@@ -153,7 +163,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
     inline bool isNegative(void) const
     {
       eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      return m_sign == -1;
+      return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
     }
 
     /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
@@ -235,7 +245,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
     MatrixType m_matrix;
     TranspositionType m_transpositions;
     TmpMatrixType m_temporary;
-    int m_sign;
+    internal::SignMatrix m_sign;
     bool m_isInitialized;
 };
 
@@ -246,7 +256,7 @@ template<int UpLo> struct ldlt_inplace;
 template<> struct ldlt_inplace<Lower>
 {
   template<typename MatrixType, typename TranspositionType, typename Workspace>
-  static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
+  static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
   {
     using std::abs;
     typedef typename MatrixType::Scalar Scalar;
@@ -258,36 +268,19 @@ template<> struct ldlt_inplace<Lower>
     if (size <= 1)
     {
       transpositions.setIdentity();
-      if(sign)
-        *sign = numext::real(mat.coeff(0,0))>0 ? 1:-1;
+      if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef;
+      else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef;
+      else sign = ZeroSign;
       return true;
     }
 
-    RealScalar cutoff(0), biggest_in_corner;
-
     for (Index k = 0; k < size; ++k)
     {
       // Find largest diagonal element
       Index index_of_biggest_in_corner;
-      biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
+      mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
       index_of_biggest_in_corner += k;
 
-      if(k == 0)
-      {
-        // The biggest overall is the point of reference to which further diagonals
-        // are compared; if any diagonal is negligible compared
-        // to the largest overall, the algorithm bails.
-        cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
-      }
-
-      // Finish early if the matrix is not full rank.
-      if(biggest_in_corner < cutoff)
-      {
-        for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
-        if(sign) *sign = 0;
-        break;
-      }
-
       transpositions.coeffRef(k) = index_of_biggest_in_corner;
       if(k != index_of_biggest_in_corner)
       {
@@ -318,22 +311,27 @@ template<> struct ldlt_inplace<Lower>
 
       if(k>0)
       {
-        temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
+        temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
         mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
         if(rs>0)
           A21.noalias() -= A20 * temp.head(k);
       }
-      if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
-        A21 /= mat.coeffRef(k,k);
       
-      if(sign)
-      {
-        // LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right
-        int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0;
-        if(k == 0)
-          *sign = newSign;
-        else if(*sign != newSign)
-          *sign = 0;
+      // In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
+      // was smaller than the cutoff value. However, soince LDLT is not rank-revealing
+      // we should only make sure we do not introduce INF or NaN values.
+      // LAPACK also uses 0 as the cutoff value.
+      RealScalar realAkk = numext::real(mat.coeffRef(k,k));
+      if((rs>0) && (abs(realAkk) > RealScalar(0)))
+        A21 /= realAkk;
+
+      if (sign == PositiveSemiDef) {
+        if (realAkk < 0) sign = Indefinite;
+      } else if (sign == NegativeSemiDef) {
+        if (realAkk > 0) sign = Indefinite;
+      } else if (sign == ZeroSign) {
+        if (realAkk > 0) sign = PositiveSemiDef;
+        else if (realAkk < 0) sign = NegativeSemiDef;
       }
     }
 
@@ -399,7 +397,7 @@ template<> struct ldlt_inplace<Lower>
 template<> struct ldlt_inplace<Upper>
 {
   template<typename MatrixType, typename TranspositionType, typename Workspace>
-  static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
+  static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
   {
     Transpose<MatrixType> matt(mat);
     return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
@@ -445,7 +443,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
   m_isInitialized = false;
   m_temporary.resize(size);
 
-  internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
+  internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign);
 
   m_isInitialized = true;
   return *this;
@@ -473,7 +471,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Deri
     for (Index i = 0; i < size; i++)
       m_transpositions.coeffRef(i) = i;
     m_temporary.resize(size);
-    m_sign = sigma>=0 ? 1 : -1;
+    m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
     m_isInitialized = true;
   }
 
@@ -506,14 +504,20 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
     typedef typename LDLTType::MatrixType MatrixType;
     typedef typename LDLTType::Scalar Scalar;
     typedef typename LDLTType::RealScalar RealScalar;
-    const Diagonal<const MatrixType> vectorD = dec().vectorD();
-    RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
-				 RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
+    const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD());
+    // In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
+    // as motivated by LAPACK's xGELSS:
+    // RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
+    // However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
+    // diagonal element is not well justified and to numerical issues in some cases.
+    // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
+    RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
+    
     for (Index i = 0; i < vectorD.size(); ++i) {
       if(abs(vectorD(i)) > tolerance)
-	dst.row(i) /= vectorD(i);
+        dst.row(i) /= vectorD(i);
       else
-	dst.row(i).setZero();
+        dst.row(i).setZero();
     }
 
     // dst = L^-T (D^-1 L^-1 P b)
@@ -566,7 +570,7 @@ MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
   // L^* P
   res = matrixU() * res;
   // D(L^*P)
-  res = vectorD().asDiagonal() * res;
+  res = vectorD().real().asDiagonal() * res;
   // L(DL^*P)
   res = matrixL() * res;
   // P^T (LDL^*P)

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

@@ -58,10 +58,12 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat)
   res.p       = mat.outerIndexPtr();
   res.i       = mat.innerIndexPtr();
   res.x       = mat.valuePtr();
+  res.z       = 0;
   res.sorted  = 1;
   if(mat.isCompressed())
   {
     res.packed  = 1;
+    res.nz = 0;
   }
   else
   {
@@ -170,6 +172,7 @@ class CholmodBase : internal::noncopyable
     CholmodBase()
       : m_cholmodFactor(0), m_info(Success), m_isInitialized(false)
     {
+      m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0);
       cholmod_start(&m_cholmod);
     }
 
@@ -241,7 +244,7 @@ class CholmodBase : internal::noncopyable
       return internal::sparse_solve_retval<CholmodBase, Rhs>(*this, b.derived());
     }
     
-    /** Performs a symbolic decomposition on the sparcity of \a matrix.
+    /** Performs a symbolic decomposition on the sparsity pattern of \a matrix.
       *
       * This function is particularly useful when solving for several problems having the same structure.
       * 
@@ -265,7 +268,7 @@ class CholmodBase : internal::noncopyable
     
     /** Performs a numeric decomposition of \a matrix
       *
-      * The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
+      * The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed.
       *
       * \sa analyzePattern()
       */
@@ -302,7 +305,7 @@ class CholmodBase : internal::noncopyable
       {
         this->m_info = NumericalIssue;
       }
-      // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.)
+      // TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
       dest = Matrix<Scalar,Dest::RowsAtCompileTime,Dest::ColsAtCompileTime>::Map(reinterpret_cast<Scalar*>(x_cd->x),b.rows(),b.cols());
       cholmod_free_dense(&x_cd, &m_cholmod);
     }
@@ -323,7 +326,7 @@ class CholmodBase : internal::noncopyable
       {
         this->m_info = NumericalIssue;
       }
-      // TODO optimize this copy by swapping when possible (be carreful with alignment, etc.)
+      // TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
       dest = viewAsEigen<DestScalar,DestOptions,DestIndex>(*x_cs);
       cholmod_free_sparse(&x_cs, &m_cholmod);
     }
@@ -365,8 +368,8 @@ class CholmodBase : internal::noncopyable
   *
   * This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization
   * using the Cholmod library.
-  * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Thefore, it has little practical interest.
-  * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
+  * This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest.
+  * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
   * X and B can be either dense or sparse.
   *
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@@ -412,8 +415,8 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl
   *
   * This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization
   * using the Cholmod library.
-  * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Thefore, it has little practical interest.
-  * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
+  * This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest.
+  * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
   * X and B can be either dense or sparse.
   *
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@@ -458,7 +461,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp
   * This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization
   * using the Cholmod library.
   * This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM.
-  * The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
+  * The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
   * X and B can be either dense or sparse.
   *
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@@ -501,7 +504,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper
   * \brief A general Cholesky factorization and solver based on Cholmod
   *
   * This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization
-  * using the Cholmod library. The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
+  * using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
   * X and B can be either dense or sparse.
   *
   * This variant permits to change the underlying Cholesky method at runtime.

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

@@ -210,7 +210,7 @@ class Array
       : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
     {
       Base::_check_template_params();
-      Base::resize(other.rows(), other.cols());
+      Base::_resize_to_match(other);
       *this = other;
     }
 

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

@@ -81,7 +81,7 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprTyp
                        && (InnerStrideAtCompileTime == 1)
                         ? PacketAccessBit : 0,
     MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
-    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (traits<XprType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
     FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
     FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
     Flags0 = traits<XprType>::Flags & ( (HereditaryBits & ~RowMajorBit) |

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

@@ -29,9 +29,9 @@ struct all_unroller
 };
 
 template<typename Derived>
-struct all_unroller<Derived, 1>
+struct all_unroller<Derived, 0>
 {
-  static inline bool run(const Derived &mat) { return mat.coeff(0, 0); }
+  static inline bool run(const Derived &/*mat*/) { return true; }
 };
 
 template<typename Derived>
@@ -55,9 +55,9 @@ struct any_unroller
 };
 
 template<typename Derived>
-struct any_unroller<Derived, 1>
+struct any_unroller<Derived, 0>
 {
-  static inline bool run(const Derived &mat) { return mat.coeff(0, 0); }
+  static inline bool run(const Derived & /*mat*/) { return false; }
 };
 
 template<typename Derived>

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

@@ -47,6 +47,17 @@ struct CommaInitializer :
     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)
   {

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

@@ -0,0 +1,154 @@
+// 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/DenseStorage.h

@@ -24,6 +24,14 @@ namespace internal {
 
 struct constructor_without_unaligned_array_assert {};
 
+template<typename T, int Size> void check_static_allocation_size()
+{
+  // if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit
+  #if EIGEN_STACK_ALLOCATION_LIMIT
+  EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+  #endif
+}
+
 /** \internal
   * Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned:
   * to 16 bytes boundary if the total size is a multiple of 16 bytes.
@@ -38,12 +46,12 @@ struct plain_array
 
   plain_array() 
   { 
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 
   plain_array(constructor_without_unaligned_array_assert) 
   { 
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 };
 
@@ -76,12 +84,12 @@ struct plain_array<T, Size, MatrixOrArrayOptions, 16>
   plain_array() 
   { 
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf);
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 
   plain_array(constructor_without_unaligned_array_assert) 
   { 
-    EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
+    check_static_allocation_size<T,Size>();
   }
 };
 

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

@@ -126,36 +126,6 @@ Derived& DenseBase<Derived>::operator-=(const EigenBase<OtherDerived> &other)
   return derived();
 }
 
-/** replaces \c *this by \c *this * \a other.
-  *
-  * \returns a reference to \c *this
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline Derived&
-MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
-{
-  other.derived().applyThisOnTheRight(derived());
-  return derived();
-}
-
-/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
-  */
-template<typename Derived>
-template<typename OtherDerived>
-inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
-{
-  other.derived().applyThisOnTheRight(derived());
-}
-
-/** replaces \c *this by \c *this * \a other. */
-template<typename Derived>
-template<typename OtherDerived>
-inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
-{
-  other.derived().applyThisOnTheLeft(derived());
-}
-
 } // end namespace Eigen
 
 #endif // EIGEN_EIGENBASE_H

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

@@ -589,7 +589,7 @@ struct linspaced_op_impl<Scalar,true>
 
   template<typename Index>
   EIGEN_STRONG_INLINE const Packet packetOp(Index i) const
-  { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(i),m_interPacket))); }
+  { return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(Scalar(i)),m_interPacket))); }
 
   const Scalar m_low;
   const Scalar m_step;
@@ -609,7 +609,7 @@ template <typename Scalar, bool RandomAccess> struct functor_traits< linspaced_o
 template <typename Scalar, bool RandomAccess> struct linspaced_op
 {
   typedef typename packet_traits<Scalar>::type Packet;
-  linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/(num_steps-1))) {}
+  linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/Scalar(num_steps-1))) {}
 
   template<typename Index>
   EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); }

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

@@ -185,21 +185,22 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat&
     explicit_precision = fmt.precision;
   }
 
+  std::streamsize old_precision = 0;
+  if(explicit_precision) old_precision = s.precision(explicit_precision);
+
   bool align_cols = !(fmt.flags & DontAlignCols);
   if(align_cols)
   {
     // compute the largest width
-    for(Index j = 1; j < m.cols(); ++j)
+    for(Index j = 0; j < m.cols(); ++j)
       for(Index i = 0; i < m.rows(); ++i)
       {
         std::stringstream sstr;
-        if(explicit_precision) sstr.precision(explicit_precision);
+        sstr.copyfmt(s);
         sstr << m.coeff(i,j);
         width = std::max<Index>(width, Index(sstr.str().length()));
       }
   }
-  std::streamsize old_precision = 0;
-  if(explicit_precision) old_precision = s.precision(explicit_precision);
   s << fmt.matPrefix;
   for(Index i = 0; i < m.rows(); ++i)
   {

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

@@ -237,6 +237,8 @@ template<typename Derived> class MapBase<Derived, WriteAccessors>
     using Base::Base::operator=;
 };
 
+#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
+
 } // end namespace Eigen
 
 #endif // EIGEN_MAPBASE_H

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

@@ -304,7 +304,7 @@ class Matrix
       : Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
     {
       Base::_check_template_params();
-      Base::resize(other.rows(), other.cols());
+      Base::_resize_to_match(other);
       // FIXME/CHECK: isn't *this = other.derived() more efficient. it allows to
       //              go for pure _set() implementations, right?
       *this = other;

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

@@ -510,6 +510,51 @@ template<typename Derived> class MatrixBase
     {EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
 };
 
+
+/***************************************************************************
+* Implementation of matrix base methods
+***************************************************************************/
+
+/** replaces \c *this by \c *this * \a other.
+  *
+  * \returns a reference to \c *this
+  *
+  * Example: \include MatrixBase_applyOnTheRight.cpp
+  * Output: \verbinclude MatrixBase_applyOnTheRight.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline Derived&
+MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
+{
+  other.derived().applyThisOnTheRight(derived());
+  return derived();
+}
+
+/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
+  *
+  * Example: \include MatrixBase_applyOnTheRight.cpp
+  * Output: \verbinclude MatrixBase_applyOnTheRight.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
+{
+  other.derived().applyThisOnTheRight(derived());
+}
+
+/** replaces \c *this by \a other * \c *this.
+  *
+  * Example: \include MatrixBase_applyOnTheLeft.cpp
+  * Output: \verbinclude MatrixBase_applyOnTheLeft.out
+  */
+template<typename Derived>
+template<typename OtherDerived>
+inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
+{
+  other.derived().applyThisOnTheLeft(derived());
+}
+
 } // end namespace Eigen
 
 #endif // EIGEN_MATRIXBASE_H

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

@@ -553,7 +553,8 @@ struct permut_matrix_product_retval
     template<typename Dest> inline void evalTo(Dest& dst) const
     {
       const Index n = Side==OnTheLeft ? rows() : cols();
-
+      // FIXME we need an is_same for expression that is not sensitive to constness. For instance
+      // is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
       if(is_same<MatrixTypeNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_matrix))
       {
         // apply the permutation inplace

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

@@ -47,7 +47,10 @@ template<> struct check_rows_cols_for_overflow<Dynamic> {
   }
 };
 
-template <typename Derived, typename OtherDerived = Derived, bool IsVector = bool(Derived::IsVectorAtCompileTime)> struct conservative_resize_like_impl;
+template <typename Derived,
+          typename OtherDerived = Derived,
+          bool IsVector = bool(Derived::IsVectorAtCompileTime) && bool(OtherDerived::IsVectorAtCompileTime)>
+struct conservative_resize_like_impl;
 
 template<typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers> struct matrix_swap_impl;
 
@@ -668,8 +671,10 @@ private:
     enum { ThisConstantIsPrivateInPlainObjectBase };
 };
 
+namespace internal {
+
 template <typename Derived, typename OtherDerived, bool IsVector>
-struct internal::conservative_resize_like_impl
+struct conservative_resize_like_impl
 {
   typedef typename Derived::Index Index;
   static void run(DenseBase<Derived>& _this, Index rows, Index cols)
@@ -729,11 +734,14 @@ struct internal::conservative_resize_like_impl
   }
 };
 
-namespace internal {
-
+// Here, the specialization for vectors inherits from the general matrix case
+// to allow calling .conservativeResize(rows,cols) on vectors.
 template <typename Derived, typename OtherDerived>
 struct conservative_resize_like_impl<Derived,OtherDerived,true>
+  : conservative_resize_like_impl<Derived,OtherDerived,false>
 {
+  using conservative_resize_like_impl<Derived,OtherDerived,false>::run;
+  
   typedef typename Derived::Index Index;
   static void run(DenseBase<Derived>& _this, Index size)
   {

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

@@ -94,13 +94,14 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
   typedef _PlainObjectType PlainObjectType;
   typedef _StrideType StrideType;
   enum {
-    Options = _Options
+    Options = _Options,
+    Flags = traits<Map<_PlainObjectType, _Options, _StrideType> >::Flags | NestByRefBit
   };
 
   template<typename Derived> struct match {
     enum {
       HasDirectAccess = internal::has_direct_access<Derived>::ret,
-      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
+      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
       InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic)
                       || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime)
                       || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1),
@@ -111,7 +112,7 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
     };
     typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
   };
-
+  
 };
 
 template<typename Derived>
@@ -171,8 +172,12 @@ protected:
     }
     else
       ::new (static_cast<Base*>(this)) Base(expr.data(), expr.rows(), expr.cols());
-    ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
-                                 StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());    
+    
+    if(Expression::IsVectorAtCompileTime && (!PlainObjectType::IsVectorAtCompileTime) && ((Expression::Flags&RowMajorBit)!=(PlainObjectType::Flags&RowMajorBit)))
+      ::new (&m_stride) StrideBase(expr.innerStride(), StrideType::InnerStrideAtCompileTime==0?0:1);
+    else
+      ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
+                                   StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());    
   }
 
   StrideBase m_stride;

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

@@ -17,16 +17,29 @@ namespace internal {
 template<typename ExpressionType, typename Scalar>
 inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale)
 {
-  Scalar max = bl.cwiseAbs().maxCoeff();
-  if (max>scale)
+  using std::max;
+  Scalar maxCoeff = bl.cwiseAbs().maxCoeff();
+  
+  if (maxCoeff>scale)
   {
-    ssq = ssq * numext::abs2(scale/max);
-    scale = max;
-    invScale = Scalar(1)/scale;
+    ssq = ssq * numext::abs2(scale/maxCoeff);
+    Scalar tmp = Scalar(1)/maxCoeff;
+    if(tmp > NumTraits<Scalar>::highest())
+    {
+      invScale = NumTraits<Scalar>::highest();
+      scale = Scalar(1)/invScale;
+    }
+    else
+    {
+      scale = maxCoeff;
+      invScale = tmp;
+    }
   }
-  // TODO if the max is much much smaller than the current scale,
+  
+  // TODO if the maxCoeff is much much smaller than the current scale,
   // then we can neglect this sub vector
-  ssq += (bl*invScale).squaredNorm();
+  if(scale>Scalar(0)) // if scale==0, then bl is 0 
+    ssq += (bl*invScale).squaredNorm();
 }
 
 template<typename Derived>

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

@@ -284,7 +284,8 @@ struct inplace_transpose_selector<MatrixType,false> { // non square matrix
   * Notice however that this method is only useful if you want to replace a matrix by its own transpose.
   * If you just need the transpose of a matrix, use transpose().
   *
-  * \note if the matrix is not square, then \c *this must be a resizable matrix.
+  * \note if the matrix is not square, then \c *this must be a resizable matrix. 
+  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
   *
   * \sa transpose(), adjoint(), adjointInPlace() */
 template<typename Derived>
@@ -315,6 +316,7 @@ inline void DenseBase<Derived>::transposeInPlace()
   * If you just need the adjoint of a matrix, use adjoint().
   *
   * \note if the matrix is not square, then \c *this must be a resizable matrix.
+  * This excludes (non-square) fixed-size matrices, block-expressions and maps.
   *
   * \sa transpose(), adjoint(), transposeInPlace() */
 template<typename Derived>

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

@@ -278,21 +278,21 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
 
     /** Efficient triangular matrix times vector/matrix product */
     template<typename OtherDerived>
-    TriangularProduct<Mode,true,MatrixType,false,OtherDerived, OtherDerived::IsVectorAtCompileTime>
+    TriangularProduct<Mode, true, MatrixType, false, OtherDerived, OtherDerived::ColsAtCompileTime==1>
     operator*(const MatrixBase<OtherDerived>& rhs) const
     {
       return TriangularProduct
-              <Mode,true,MatrixType,false,OtherDerived,OtherDerived::IsVectorAtCompileTime>
+              <Mode, true, MatrixType, false, OtherDerived, OtherDerived::ColsAtCompileTime==1>
               (m_matrix, rhs.derived());
     }
 
     /** Efficient vector/matrix times triangular matrix product */
     template<typename OtherDerived> friend
-    TriangularProduct<Mode,false,OtherDerived,OtherDerived::IsVectorAtCompileTime,MatrixType,false>
+    TriangularProduct<Mode, false, OtherDerived, OtherDerived::RowsAtCompileTime==1, MatrixType, false>
     operator*(const MatrixBase<OtherDerived>& lhs, const TriangularView& rhs)
     {
       return TriangularProduct
-              <Mode,false,OtherDerived,OtherDerived::IsVectorAtCompileTime,MatrixType,false>
+              <Mode, false, OtherDerived, OtherDerived::RowsAtCompileTime==1, MatrixType, false>
               (lhs.derived(),rhs.m_matrix);
     }
 

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

@@ -50,7 +50,7 @@ struct traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
     MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime,
     Flags0 = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits,
     Flags = (Flags0 & ~RowMajorBit) | (RowsAtCompileTime == 1 ? RowMajorBit : 0),
-    TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime
+    TraversalSize = Direction==Vertical ? MatrixType::RowsAtCompileTime :  MatrixType::ColsAtCompileTime
   };
   #if EIGEN_GNUC_AT_LEAST(3,4)
   typedef typename MemberOp::template Cost<InputScalar,int(TraversalSize)> CostOpType;
@@ -58,7 +58,8 @@ struct traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
   typedef typename MemberOp::template Cost<InputScalar,TraversalSize> CostOpType;
   #endif
   enum {
-    CoeffReadCost = TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value)
+    CoeffReadCost = TraversalSize==Dynamic ? Dynamic
+                  : TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value)
   };
 };
 }

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

@@ -442,8 +442,11 @@ Packet4f pcos<Packet4f>(const Packet4f& _x)
   return _mm_xor_ps(y, sign_bit);
 }
 
+#if EIGEN_FAST_MATH
+
 // This is based on Quake3's fast inverse square root.
 // For detail see here: http://www.beyond3d.com/content/articles/8/
+// It lacks 1 (or 2 bits in some rare cases) of precision, and does not handle negative, +inf, or denormalized numbers correctly.
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f psqrt<Packet4f>(const Packet4f& _x)
 {
@@ -457,6 +460,14 @@ Packet4f psqrt<Packet4f>(const Packet4f& _x)
   return pmul(_x,x);
 }
 
+#else
+
+template<> EIGEN_STRONG_INLINE Packet4f psqrt<Packet4f>(const Packet4f& x) { return _mm_sqrt_ps(x); }
+
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet2d psqrt<Packet2d>(const Packet2d& x) { return _mm_sqrt_pd(x); }
+
 } // end namespace internal
 
 } // end namespace Eigen

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

@@ -83,7 +83,8 @@ template<> struct packet_traits<double> : default_packet_traits
     size=2,
 
     HasDiv  = 1,
-    HasExp  = 1
+    HasExp  = 1,
+    HasSqrt = 1
   };
 };
 template<> struct packet_traits<int>    : default_packet_traits
@@ -507,8 +508,8 @@ template<> EIGEN_STRONG_INLINE int predux_min<Packet4i>(const Packet4i& a)
   // for GCC (eg., it does not like using std::min after the pstore !!)
   EIGEN_ALIGN16 int aux[4];
   pstore(aux, a);
-  register int aux0 = aux[0]<aux[1] ? aux[0] : aux[1];
-  register int aux2 = aux[2]<aux[3] ? aux[2] : aux[3];
+  int aux0 = aux[0]<aux[1] ? aux[0] : aux[1];
+  int aux2 = aux[2]<aux[3] ? aux[2] : aux[3];
   return aux0<aux2 ? aux0 : aux2;
 }
 
@@ -528,8 +529,8 @@ template<> EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a)
   // for GCC (eg., it does not like using std::min after the pstore !!)
   EIGEN_ALIGN16 int aux[4];
   pstore(aux, a);
-  register int aux0 = aux[0]>aux[1] ? aux[0] : aux[1];
-  register int aux2 = aux[2]>aux[3] ? aux[2] : aux[3];
+  int aux0 = aux[0]>aux[1] ? aux[0] : aux[1];
+  int aux2 = aux[2]>aux[3] ? aux[2] : aux[3];
   return aux0>aux2 ? aux0 : aux2;
 }
 

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

@@ -1128,6 +1128,8 @@ EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, Pack1, Pack2, StorageOrder,
   enum { PacketSize = packet_traits<Scalar>::size };
 
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS");
+  EIGEN_UNUSED_VARIABLE(stride)
+  EIGEN_UNUSED_VARIABLE(offset)
   eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
   eigen_assert( (StorageOrder==RowMajor) || ((Pack1%PacketSize)==0 && Pack1<=4*PacketSize) );
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
@@ -1215,6 +1217,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, nr, ColMajor, Conjugate, Pan
   ::operator()(Scalar* blockB, const Scalar* rhs, Index rhsStride, Index depth, Index cols, Index stride, Index offset)
 {
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS COLMAJOR");
+  EIGEN_UNUSED_VARIABLE(stride)
+  EIGEN_UNUSED_VARIABLE(offset)
   eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
   Index packet_cols = (cols/nr) * nr;
@@ -1266,6 +1270,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, nr, RowMajor, Conjugate, Pan
   ::operator()(Scalar* blockB, const Scalar* rhs, Index rhsStride, Index depth, Index cols, Index stride, Index offset)
 {
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS ROWMAJOR");
+  EIGEN_UNUSED_VARIABLE(stride)
+  EIGEN_UNUSED_VARIABLE(offset)
   eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
   Index packet_cols = (cols/nr) * nr;

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

@@ -52,11 +52,7 @@ EIGEN_DONT_INLINE static void run(
   Index rows, Index cols,
   const LhsScalar* lhs, Index lhsStride,
   const RhsScalar* rhs, Index rhsIncr,
-  ResScalar* res, Index
-  #ifdef EIGEN_INTERNAL_DEBUGGING
-    resIncr
-  #endif
-  , RhsScalar alpha);
+  ResScalar* res, Index resIncr, RhsScalar alpha);
 };
 
 template<typename Index, typename LhsScalar, bool ConjugateLhs, typename RhsScalar, bool ConjugateRhs, int Version>
@@ -64,12 +60,9 @@ EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,ColMajor,Co
   Index rows, Index cols,
   const LhsScalar* lhs, Index lhsStride,
   const RhsScalar* rhs, Index rhsIncr,
-  ResScalar* res, Index
-  #ifdef EIGEN_INTERNAL_DEBUGGING
-    resIncr
-  #endif
-  , RhsScalar alpha)
+  ResScalar* res, Index resIncr, RhsScalar alpha)
 {
+  EIGEN_UNUSED_VARIABLE(resIncr)
   eigen_internal_assert(resIncr==1);
   #ifdef _EIGEN_ACCUMULATE_PACKETS
   #error _EIGEN_ACCUMULATE_PACKETS has already been defined
@@ -265,7 +258,7 @@ EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,ColMajor,Co
         // process aligned result's coeffs
         if ((size_t(lhs0+alignedStart)%sizeof(LhsPacket))==0)
           for (Index i = alignedStart;i<alignedSize;i+=ResPacketSize)
-            pstore(&res[i], pcj.pmadd(ploadu<LhsPacket>(&lhs0[i]), ptmp0, pload<ResPacket>(&res[i])));
+            pstore(&res[i], pcj.pmadd(pload<LhsPacket>(&lhs0[i]), ptmp0, pload<ResPacket>(&res[i])));
         else
           for (Index i = alignedStart;i<alignedSize;i+=ResPacketSize)
             pstore(&res[i], pcj.pmadd(ploadu<LhsPacket>(&lhs0[i]), ptmp0, pload<ResPacket>(&res[i])));

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

@@ -79,8 +79,8 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrd
   for (Index j=FirstTriangular ? bound : 0;
        j<(FirstTriangular ? size : bound);j+=2)
   {
-    register const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
-    register const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
+    const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
+    const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
 
     Scalar t0 = cjAlpha * rhs[j];
     Packet ptmp0 = pset1<Packet>(t0);
@@ -147,7 +147,7 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrd
   }
   for (Index j=FirstTriangular ? 0 : bound;j<(FirstTriangular ? bound : size);j++)
   {
-    register const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
+    const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
 
     Scalar t1 = cjAlpha * rhs[j];
     Scalar t2(0);

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

@@ -54,8 +54,25 @@
 #endif
 
 #if defined EIGEN_USE_MKL
+#   include <mkl.h> 
+/*Check IMKL version for compatibility: < 10.3 is not usable with Eigen*/
+#   ifndef INTEL_MKL_VERSION
+#       undef EIGEN_USE_MKL /* INTEL_MKL_VERSION is not even defined on older versions */
+#   elif INTEL_MKL_VERSION < 100305    /* the intel-mkl-103-release-notes say this was when the lapacke.h interface was added*/
+#       undef EIGEN_USE_MKL
+#   endif
+#   ifndef EIGEN_USE_MKL
+    /*If the MKL version is too old, undef everything*/
+#       undef   EIGEN_USE_MKL_ALL
+#       undef   EIGEN_USE_BLAS
+#       undef   EIGEN_USE_LAPACKE
+#       undef   EIGEN_USE_MKL_VML
+#       undef   EIGEN_USE_LAPACKE_STRICT
+#       undef   EIGEN_USE_LAPACKE
+#   endif
+#endif
 
-#include <mkl.h>
+#if defined EIGEN_USE_MKL
 #include <mkl_lapacke.h>
 #define EIGEN_MKL_VML_THRESHOLD 128
 

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 0
+#define EIGEN_MINOR_VERSION 2
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@@ -238,7 +238,12 @@
 #endif
 
 // Suppresses 'unused variable' warnings.
-#define EIGEN_UNUSED_VARIABLE(var) (void)var;
+namespace Eigen {
+  namespace internal {
+    template<typename T> void ignore_unused_variable(const T&) {}
+  }
+}
+#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var);
 
 #if !defined(EIGEN_ASM_COMMENT)
   #if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) )
@@ -284,7 +289,8 @@
 #endif
 
 #ifndef EIGEN_STACK_ALLOCATION_LIMIT
-#define EIGEN_STACK_ALLOCATION_LIMIT 20000
+// 131072 == 128 KB
+#define EIGEN_STACK_ALLOCATION_LIMIT 131072
 #endif
 
 #ifndef EIGEN_DEFAULT_IO_FORMAT

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

@@ -272,12 +272,12 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size)
   // The defined(_mm_free) is just here to verify that this MSVC version
   // implements _mm_malloc/_mm_free based on the corresponding _aligned_
   // functions. This may not always be the case and we just try to be safe.
-  #if defined(_MSC_VER) && defined(_mm_free)
+  #if defined(_MSC_VER) && (!defined(_WIN32_WCE)) && defined(_mm_free)
     result = _aligned_realloc(ptr,new_size,16);
   #else
     result = generic_aligned_realloc(ptr,new_size,old_size);
   #endif
-#elif defined(_MSC_VER)
+#elif defined(_MSC_VER) && (!defined(_WIN32_WCE))
   result = _aligned_realloc(ptr,new_size,16);
 #else
   result = handmade_aligned_realloc(ptr,new_size,old_size);
@@ -578,7 +578,7 @@ template<typename T> class aligned_stack_memory_handler
   */
 #ifdef EIGEN_ALLOCA
 
-  #ifdef __arm__
+  #if defined(__arm__) || defined(_WIN32)
     #define EIGEN_ALIGNED_ALLOCA(SIZE) reinterpret_cast<void*>((reinterpret_cast<size_t>(EIGEN_ALLOCA(SIZE+16)) & ~(size_t(15))) + 16)
   #else
     #define EIGEN_ALIGNED_ALLOCA EIGEN_ALLOCA
@@ -630,11 +630,15 @@ template<typename T> class aligned_stack_memory_handler
       } \
       void operator delete(void * ptr) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
       void operator delete[](void * ptr) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      void operator delete(void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      void operator delete[](void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
       /* in-place new and delete. since (at least afaik) there is no actual   */ \
       /* memory allocated we can safely let the default implementation handle */ \
       /* this particular case. */ \
       static void *operator new(size_t size, void *ptr) { return ::operator new(size,ptr); } \
+      static void *operator new[](size_t size, void* ptr) { return ::operator new[](size,ptr); } \
       void operator delete(void * memory, void *ptr) throw() { return ::operator delete(memory,ptr); } \
+      void operator delete[](void * memory, void *ptr) throw() { return ::operator delete[](memory,ptr); } \
       /* nothrow-new (returns zero instead of std::bad_alloc) */ \
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
       void operator delete(void *ptr, const std::nothrow_t&) throw() { \
@@ -729,15 +733,6 @@ public:
         ::new( p ) T( value );
     }
 
-    // Support for c++11
-#if (__cplusplus >= 201103L)
-    template<typename... Args>
-    void  construct(pointer p, Args&&... args)
-    {
-      ::new(p) T(std::forward<Args>(args)...);
-    }
-#endif
-
     void destroy( pointer p )
     {
         p->~T();
@@ -784,9 +779,9 @@ namespace internal {
 
 #ifdef EIGEN_CPUID
 
-inline bool cpuid_is_vendor(int abcd[4], const char* vendor)
+inline bool cpuid_is_vendor(int abcd[4], const int vendor[3])
 {
-  return abcd[1]==(reinterpret_cast<const int*>(vendor))[0] && abcd[3]==(reinterpret_cast<const int*>(vendor))[1] && abcd[2]==(reinterpret_cast<const int*>(vendor))[2];
+  return abcd[1]==vendor[0] && abcd[3]==vendor[1] && abcd[2]==vendor[2];
 }
 
 inline void queryCacheSizes_intel_direct(int& l1, int& l2, int& l3)
@@ -928,13 +923,16 @@ inline void queryCacheSizes(int& l1, int& l2, int& l3)
 {
   #ifdef EIGEN_CPUID
   int abcd[4];
+  const int GenuineIntel[] = {0x756e6547, 0x49656e69, 0x6c65746e};
+  const int AuthenticAMD[] = {0x68747541, 0x69746e65, 0x444d4163};
+  const int AMDisbetter_[] = {0x69444d41, 0x74656273, 0x21726574}; // "AMDisbetter!"
 
   // identify the CPU vendor
   EIGEN_CPUID(abcd,0x0,0);
   int max_std_funcs = abcd[1];
-  if(cpuid_is_vendor(abcd,"GenuineIntel"))
+  if(cpuid_is_vendor(abcd,GenuineIntel))
     queryCacheSizes_intel(l1,l2,l3,max_std_funcs);
-  else if(cpuid_is_vendor(abcd,"AuthenticAMD") || cpuid_is_vendor(abcd,"AMDisbetter!"))
+  else if(cpuid_is_vendor(abcd,AuthenticAMD) || cpuid_is_vendor(abcd,AMDisbetter_))
     queryCacheSizes_amd(l1,l2,l3);
   else
     // by default let's use Intel's API

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

@@ -512,8 +512,7 @@ template<typename MatrixType>
 template<typename OtherDerived, typename ResultType>
 bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
 {
-  const int rows = m_matU.rows();
-  ei_assert(b.rows() == rows);
+  ei_assert(b.rows() == m_matU.rows());
 
   Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
   for (int j=0; j<b.cols(); ++j)

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

@@ -28,7 +28,7 @@ namespace Eigen {
   *      * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
   * This corresponds to the right-multiply conventions (with right hand side frames).
   * 
-  * The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi].
+  * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
   * 
   * \sa class AngleAxis
   */

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

@@ -150,10 +150,6 @@ public:
   /** \returns the conjugated quaternion */
   Quaternion<Scalar> conjugate() const;
 
-  /** \returns an interpolation for a constant motion between \a other and \c *this
-    * \a t in [0;1]
-    * see http://en.wikipedia.org/wiki/Slerp
-    */
   template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
 
   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
@@ -194,11 +190,11 @@ public:
   * \brief The quaternion class used to represent 3D orientations and rotations
   *
   * \tparam _Scalar the scalar type, i.e., the type of the coefficients
-  * \tparam _Options controls the memory alignement of the coeffecients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
+  * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
   *
   * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
   * orientations and rotations of objects in three dimensions. Compared to other representations
-  * like Euler angles or 3x3 matrices, quatertions offer the following advantages:
+  * like Euler angles or 3x3 matrices, quaternions offer the following advantages:
   * \li \b compact storage (4 scalars)
   * \li \b efficient to compose (28 flops),
   * \li \b stable spherical interpolation
@@ -207,6 +203,8 @@ public:
   * \li \c Quaternionf for \c float
   * \li \c Quaterniond for \c double
   *
+  * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.
+  *
   * \sa  class AngleAxis, class Transform
   */
 
@@ -348,7 +346,7 @@ class Map<const Quaternion<_Scalar>, _Options >
 
     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
       *
-      * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
+      * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
       * \code *coeffs == {x, y, z, w} \endcode
       *
       * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
@@ -385,7 +383,7 @@ class Map<Quaternion<_Scalar>, _Options >
 
     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
       *
-      * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
+      * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
       * \code *coeffs == {x, y, z, w} \endcode
       *
       * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
@@ -399,16 +397,16 @@ class Map<Quaternion<_Scalar>, _Options >
 };
 
 /** \ingroup Geometry_Module
-  * Map an unaligned array of single precision scalar as a quaternion */
+  * Map an unaligned array of single precision scalars as a quaternion */
 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
 /** \ingroup Geometry_Module
-  * Map an unaligned array of double precision scalar as a quaternion */
+  * Map an unaligned array of double precision scalars as a quaternion */
 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
 /** \ingroup Geometry_Module
-  * Map a 16-bits aligned array of double precision scalars as a quaternion */
+  * Map a 16-byte aligned array of single precision scalars as a quaternion */
 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
 /** \ingroup Geometry_Module
-  * Map a 16-bits aligned array of double precision scalars as a quaternion */
+  * Map a 16-byte aligned array of double precision scalars as a quaternion */
 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
 
 /***************************************************************************
@@ -468,7 +466,7 @@ QuaternionBase<Derived>::_transformVector(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.
     // It appears to be much faster than the common algorithm found
-    // in the litterature (30 versus 39 flops). It also requires two
+    // in the literature (30 versus 39 flops). It also requires two
     // Vector3 as temporaries.
     Vector3 uv = this->vec().cross(v);
     uv += uv;
@@ -579,7 +577,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri
   Scalar c = v1.dot(v0);
 
   // if dot == -1, vectors are nearly opposites
-  // => accuraletly compute the rotation axis by computing the
+  // => accurately compute the rotation axis by computing the
   //    intersection of the two planes. This is done by solving:
   //       x^T v0 = 0
   //       x^T v1 = 0
@@ -588,7 +586,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri
   //    which yields a singular value problem
   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
   {
-    c = max<Scalar>(c,-1);
+    c = (max)(c,Scalar(-1));
     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
     Vector3 axis = svd.matrixV().col(2);
@@ -671,14 +669,19 @@ QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& oth
 {
   using std::acos;
   using std::abs;
-  double d = abs(this->dot(other));
-  if (d>=1.0)
+  Scalar d = abs(this->dot(other));
+  if (d>=Scalar(1))
     return Scalar(0);
-  return static_cast<Scalar>(2 * acos(d));
+  return Scalar(2) * acos(d);
 }
 
+ 
+    
 /** \returns the spherical linear interpolation between the two quaternions
-  * \c *this and \a other at the parameter \a t
+  * \c *this and \a other at the parameter \a t in [0;1].
+  * 
+  * This represents an interpolation for a constant motion between \c *this and \a other,
+  * see also http://en.wikipedia.org/wiki/Slerp.
   */
 template <class Derived>
 template <class OtherDerived>

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

@@ -194,9 +194,9 @@ public:
   /** type of the matrix used to represent the linear part of the transformation */
   typedef Matrix<Scalar,Dim,Dim,Options> LinearMatrixType;
   /** type of read/write reference to the linear part of the transformation */
-  typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact)> LinearPart;
+  typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> LinearPart;
   /** type of read reference to the linear part of the transformation */
-  typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact)> ConstLinearPart;
+  typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> ConstLinearPart;
   /** type of read/write reference to the affine part of the transformation */
   typedef typename internal::conditional<int(Mode)==int(AffineCompact),
                               MatrixType&,
@@ -530,9 +530,9 @@ public:
 
   inline Transform& operator=(const UniformScaling<Scalar>& t);
   inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); }
-  inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Isometry)> operator*(const UniformScaling<Scalar>& s) const
+  inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode))> operator*(const UniformScaling<Scalar>& s) const
   {
-    Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Isometry),Options> res = *this;
+    Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode)),Options> res = *this;
     res.scale(s.factor());
     return res;
   }

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

@@ -113,7 +113,7 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
   const Index n = src.cols(); // number of measurements
 
   // required for demeaning ...
-  const RealScalar one_over_n = 1 / static_cast<RealScalar>(n);
+  const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);
 
   // computation of mean
   const VectorType src_mean = src.rowwise().sum() * one_over_n;
@@ -136,16 +136,16 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
 
   // Eq. (39)
   VectorType S = VectorType::Ones(m);
-  if (sigma.determinant()<0) S(m-1) = -1;
+  if (sigma.determinant()<Scalar(0)) S(m-1) = Scalar(-1);
 
   // Eq. (40) and (43)
   const VectorType& d = svd.singularValues();
   Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank;
   if (rank == m-1) {
-    if ( svd.matrixU().determinant() * svd.matrixV().determinant() > 0 ) {
+    if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) {
       Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose();
     } else {
-      const Scalar s = S(m-1); S(m-1) = -1;
+      const Scalar s = S(m-1); S(m-1) = Scalar(-1);
       Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
       S(m-1) = s;
     }
@@ -156,7 +156,7 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
   if (with_scaling)
   {
     // Eq. (42)
-    const Scalar c = 1/src_var * svd.singularValues().dot(S);
+    const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
 
     // Eq. (41)
     Rt.col(m).head(m) = dst_mean;

gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h

@@ -48,7 +48,7 @@ void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vec
   typedef typename MatrixType::Index Index;
   enum { TFactorSize = MatrixType::ColsAtCompileTime };
   Index nbVecs = vectors.cols();
-  Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize> T(nbVecs,nbVecs);
+  Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize, ColMajor> T(nbVecs,nbVecs);
   make_block_householder_triangular_factor(T, vectors, hCoeffs);
 
   const TriangularView<const VectorsType, UnitLower>& V(vectors);

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

@@ -61,6 +61,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
   VectorType s(n), t(n);
 
   RealScalar tol2 = tol*tol;
+  RealScalar eps2 = NumTraits<Scalar>::epsilon()*NumTraits<Scalar>::epsilon();
   int i = 0;
   int restarts = 0;
 
@@ -69,7 +70,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
     Scalar rho_old = rho;
 
     rho = r0.dot(r);
-    if (internal::isMuchSmallerThan(rho,r0_sqnorm))
+    if (abs(rho) < eps2*r0_sqnorm)
     {
       // The new residual vector became too orthogonal to the arbitrarily choosen direction r0
       // Let's restart with a new r0:

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

@@ -20,10 +20,11 @@ namespace Eigen {
   *
   * \param MatrixType the type of the matrix of which we are computing the LU decomposition
   *
-  * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A
-  * is decomposed as A = PLUQ where L is unit-lower-triangular, U is upper-triangular, and P and Q
-  * are permutation matrices. This is a rank-revealing LU decomposition. The eigenvalues (diagonal
-  * coefficients) of U are sorted in such a way that any zeros are at the end.
+  * This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A is
+  * decomposed as \f$ A = P^{-1} L U Q^{-1} \f$ where L is unit-lower-triangular, U is
+  * upper-triangular, and P and Q are permutation matrices. This is a rank-revealing LU
+  * decomposition. The eigenvalues (diagonal coefficients) of U are sorted in such a way that any
+  * zeros are at the end.
   *
   * This decomposition provides the generic approach to solving systems of linear equations, computing
   * the rank, invertibility, inverse, kernel, and determinant.
@@ -511,8 +512,8 @@ typename internal::traits<MatrixType>::Scalar FullPivLU<MatrixType>::determinant
 }
 
 /** \returns the matrix represented by the decomposition,
- * i.e., it returns the product: P^{-1} L U Q^{-1}.
- * This function is provided for debug purpose. */
+ * i.e., it returns the product: \f$ P^{-1} L U Q^{-1} \f$.
+ * This function is provided for debug purposes. */
 template<typename MatrixType>
 MatrixType FullPivLU<MatrixType>::reconstructedMatrix() const
 {

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

@@ -109,7 +109,7 @@ class NaturalOrdering
   * \class COLAMDOrdering
   *
   * Functor computing the \em column \em approximate \em minimum \em degree ordering 
-  * The matrix should be in column-major format
+  * The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
   */
 template<typename Index>
 class COLAMDOrdering
@@ -118,10 +118,14 @@ class COLAMDOrdering
     typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType; 
     typedef Matrix<Index, Dynamic, 1> IndexVector;
     
-    /** Compute the permutation vector form a sparse matrix */
+    /** Compute the permutation vector \a perm form the sparse matrix \a mat
+      * \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
+      */
     template <typename MatrixType>
     void operator() (const MatrixType& mat, PermutationType& perm)
     {
+      eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering");
+      
       Index m = mat.rows();
       Index n = mat.cols();
       Index nnz = mat.nonZeros();
@@ -132,12 +136,12 @@ class COLAMDOrdering
       Index stats [COLAMD_STATS];
       internal::colamd_set_defaults(knobs);
       
-      Index info;
       IndexVector p(n+1), A(Alen); 
       for(Index i=0; i <= n; i++)   p(i) = mat.outerIndexPtr()[i];
       for(Index i=0; i < nnz; i++)  A(i) = mat.innerIndexPtr()[i];
       // Call Colamd routine to compute the ordering 
-      info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats); 
+      Index info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats); 
+      EIGEN_UNUSED_VARIABLE(info);
       eigen_assert( info && "COLAMD failed " );
       
       perm.resize(n);

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

@@ -76,7 +76,8 @@ template<typename _MatrixType> class ColPivHouseholderQR
         m_colsTranspositions(),
         m_temp(),
         m_colSqNorms(),
-        m_isInitialized(false) {}
+        m_isInitialized(false),
+        m_usePrescribedThreshold(false) {}
 
     /** \brief Default Constructor with memory preallocation
       *
@@ -349,7 +350,7 @@ template<typename _MatrixType> class ColPivHouseholderQR
       return m_usePrescribedThreshold ? m_prescribedThreshold
       // this formula comes from experimenting (see "LU precision tuning" thread on the list)
       // and turns out to be identical to Higham's formula used already in LDLt.
-                                      : NumTraits<Scalar>::epsilon() * m_qr.diagonalSize();
+                                      : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
     }
 
     /** \returns the number of nonzero pivots in the QR decomposition.

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

@@ -63,9 +63,10 @@ template<typename _MatrixType> class FullPivHouseholderQR
     typedef typename MatrixType::Index Index;
     typedef internal::FullPivHouseholderQRMatrixQReturnType<MatrixType> MatrixQReturnType;
     typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
-    typedef Matrix<Index, 1, ColsAtCompileTime, RowMajor, 1, MaxColsAtCompileTime> IntRowVectorType;
+    typedef Matrix<Index, 1,
+                   EIGEN_SIZE_MIN_PREFER_DYNAMIC(ColsAtCompileTime,RowsAtCompileTime), RowMajor, 1,
+                   EIGEN_SIZE_MIN_PREFER_FIXED(MaxColsAtCompileTime,MaxRowsAtCompileTime)> IntDiagSizeVectorType;
     typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
-    typedef typename internal::plain_col_type<MatrixType, Index>::type IntColVectorType;
     typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
     typedef typename internal::plain_col_type<MatrixType>::type ColVectorType;
 
@@ -93,10 +94,10 @@ template<typename _MatrixType> class FullPivHouseholderQR
     FullPivHouseholderQR(Index rows, Index cols)
       : m_qr(rows, cols),
         m_hCoeffs((std::min)(rows,cols)),
-        m_rows_transpositions(rows),
-        m_cols_transpositions(cols),
+        m_rows_transpositions((std::min)(rows,cols)),
+        m_cols_transpositions((std::min)(rows,cols)),
         m_cols_permutation(cols),
-        m_temp((std::min)(rows,cols)),
+        m_temp(cols),
         m_isInitialized(false),
         m_usePrescribedThreshold(false) {}
 
@@ -115,10 +116,10 @@ template<typename _MatrixType> class FullPivHouseholderQR
     FullPivHouseholderQR(const MatrixType& matrix)
       : m_qr(matrix.rows(), matrix.cols()),
         m_hCoeffs((std::min)(matrix.rows(), matrix.cols())),
-        m_rows_transpositions(matrix.rows()),
-        m_cols_transpositions(matrix.cols()),
+        m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())),
+        m_cols_transpositions((std::min)(matrix.rows(), matrix.cols())),
         m_cols_permutation(matrix.cols()),
-        m_temp((std::min)(matrix.rows(), matrix.cols())),
+        m_temp(matrix.cols()),
         m_isInitialized(false),
         m_usePrescribedThreshold(false)
     {
@@ -126,11 +127,12 @@ template<typename _MatrixType> class FullPivHouseholderQR
     }
 
     /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
-      * *this is the QR decomposition, if any exists.
+      * \c *this is the QR decomposition.
       *
       * \param b the right-hand-side of the equation to solve.
       *
-      * \returns a solution.
+      * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A,
+      * and an arbitrary solution otherwise.
       *
       * \note The case where b is a matrix is not yet implemented. Also, this
       *       code is space inefficient.
@@ -172,7 +174,7 @@ template<typename _MatrixType> class FullPivHouseholderQR
     }
 
     /** \returns a const reference to the vector of indices representing the rows transpositions */
-    const IntColVectorType& rowsTranspositions() const
+    const IntDiagSizeVectorType& rowsTranspositions() const
     {
       eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
       return m_rows_transpositions;
@@ -344,7 +346,7 @@ template<typename _MatrixType> class FullPivHouseholderQR
       return m_usePrescribedThreshold ? m_prescribedThreshold
       // this formula comes from experimenting (see "LU precision tuning" thread on the list)
       // and turns out to be identical to Higham's formula used already in LDLt.
-                                      : NumTraits<Scalar>::epsilon() * m_qr.diagonalSize();
+                                      : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
     }
 
     /** \returns the number of nonzero pivots in the QR decomposition.
@@ -368,8 +370,8 @@ template<typename _MatrixType> class FullPivHouseholderQR
   protected:
     MatrixType m_qr;
     HCoeffsType m_hCoeffs;
-    IntColVectorType m_rows_transpositions;
-    IntRowVectorType m_cols_transpositions;
+    IntDiagSizeVectorType m_rows_transpositions;
+    IntDiagSizeVectorType m_cols_transpositions;
     PermutationType m_cols_permutation;
     RowVectorType m_temp;
     bool m_isInitialized, m_usePrescribedThreshold;
@@ -415,10 +417,10 @@ FullPivHouseholderQR<MatrixType>& FullPivHouseholderQR<MatrixType>::compute(cons
 
   m_temp.resize(cols);
 
-  m_precision = NumTraits<Scalar>::epsilon() * size;
+  m_precision = NumTraits<Scalar>::epsilon() * RealScalar(size);
 
-  m_rows_transpositions.resize(matrix.rows());
-  m_cols_transpositions.resize(matrix.cols());
+  m_rows_transpositions.resize(size);
+  m_cols_transpositions.resize(size);
   Index number_of_transpositions = 0;
 
   RealScalar biggest(0);
@@ -516,17 +518,6 @@ struct solve_retval<FullPivHouseholderQR<_MatrixType>, Rhs>
                                   dec().hCoeffs().coeff(k), &temp.coeffRef(0));
     }
 
-    if(!dec().isSurjective())
-    {
-      // is c is in the image of R ?
-      RealScalar biggest_in_upper_part_of_c = c.topRows(   dec().rank()     ).cwiseAbs().maxCoeff();
-      RealScalar biggest_in_lower_part_of_c = c.bottomRows(rows-dec().rank()).cwiseAbs().maxCoeff();
-      // FIXME brain dead
-      const RealScalar m_precision = NumTraits<Scalar>::epsilon() * (std::min)(rows,cols);
-      // this internal:: prefix is needed by at least gcc 3.4 and ICC
-      if(!internal::isMuchSmallerThan(biggest_in_lower_part_of_c, biggest_in_upper_part_of_c, m_precision))
-        return;
-    }
     dec().matrixQR()
        .topLeftCorner(dec().rank(), dec().rank())
        .template triangularView<Upper>()
@@ -548,14 +539,14 @@ template<typename MatrixType> struct FullPivHouseholderQRMatrixQReturnType
 {
 public:
   typedef typename MatrixType::Index Index;
-  typedef typename internal::plain_col_type<MatrixType, Index>::type IntColVectorType;
+  typedef typename FullPivHouseholderQR<MatrixType>::IntDiagSizeVectorType IntDiagSizeVectorType;
   typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
   typedef Matrix<typename MatrixType::Scalar, 1, MatrixType::RowsAtCompileTime, RowMajor, 1,
                  MatrixType::MaxRowsAtCompileTime> WorkVectorType;
 
   FullPivHouseholderQRMatrixQReturnType(const MatrixType&       qr,
                                         const HCoeffsType&      hCoeffs,
-                                        const IntColVectorType& rowsTranspositions)
+                                        const IntDiagSizeVectorType& rowsTranspositions)
     : m_qr(qr),
       m_hCoeffs(hCoeffs),
       m_rowsTranspositions(rowsTranspositions)
@@ -595,7 +586,7 @@ public:
 protected:
   typename MatrixType::Nested m_qr;
   typename HCoeffsType::Nested m_hCoeffs;
-  typename IntColVectorType::Nested m_rowsTranspositions;
+  typename IntDiagSizeVectorType::Nested m_rowsTranspositions;
 };
 
 } // end namespace internal

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

@@ -64,7 +64,8 @@ class SPQR
     typedef PermutationMatrix<Dynamic, Dynamic> PermutationType;
   public:
     SPQR() 
-    : m_ordering(SPQR_ORDERING_DEFAULT),
+      : m_isInitialized(false),
+      m_ordering(SPQR_ORDERING_DEFAULT),
       m_allow_tol(SPQR_DEFAULT_TOL),
       m_tolerance (NumTraits<Scalar>::epsilon())
     { 
@@ -72,7 +73,8 @@ class SPQR
     }
     
     SPQR(const _MatrixType& matrix) 
-    : m_ordering(SPQR_ORDERING_DEFAULT),
+    : m_isInitialized(false),
+      m_ordering(SPQR_ORDERING_DEFAULT),
       m_allow_tol(SPQR_DEFAULT_TOL),
       m_tolerance (NumTraits<Scalar>::epsilon())
     {
@@ -82,16 +84,22 @@ class SPQR
     
     ~SPQR()
     {
-      // Calls SuiteSparseQR_free()
+      SPQR_free();
+      cholmod_l_finish(&m_cc);
+    }
+    void SPQR_free()
+    {
       cholmod_l_free_sparse(&m_H, &m_cc);
       cholmod_l_free_sparse(&m_cR, &m_cc);
       cholmod_l_free_dense(&m_HTau, &m_cc);
       std::free(m_E);
       std::free(m_HPinv);
-      cholmod_l_finish(&m_cc);
     }
+
     void compute(const _MatrixType& matrix)
     {
+      if(m_isInitialized) SPQR_free();
+
       MatrixType mat(matrix);
       cholmod_sparse A; 
       A = viewAsCholmod(mat);
@@ -139,7 +147,7 @@ class SPQR
       eigen_assert(b.cols()==1 && "This method is for vectors only");
       
       //Compute Q^T * b
-      Dest y; 
+      typename Dest::PlainObject y;
       y = matrixQ().transpose() * b;
         // Solves with the triangular matrix R
       Index rk = this->rank();

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

@@ -375,14 +375,19 @@ struct svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner, true>
     Scalar z;
     JacobiRotation<Scalar> rot;
     RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p)));
+    
     if(n==0)
     {
       z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
       work_matrix.row(p) *= z;
       if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z);
-      z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
-      work_matrix.row(q) *= z;
-      if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
+      if(work_matrix.coeff(q,q)!=Scalar(0))
+      {
+        z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
+        work_matrix.row(q) *= z;
+        if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
+      }
+      // otherwise the second row is already zero, so we have nothing to do.
     }
     else
     {
@@ -412,6 +417,7 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
                             JacobiRotation<RealScalar> *j_right)
 {
   using std::sqrt;
+  using std::abs;
   Matrix<RealScalar,2,2> m;
   m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)),
        numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q));
@@ -425,9 +431,11 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
   }
   else
   {
-    RealScalar u = d / t;
-    rot1.c() = RealScalar(1) / sqrt(RealScalar(1) + numext::abs2(u));
-    rot1.s() = rot1.c() * u;
+    RealScalar t2d2 = numext::hypot(t,d);
+    rot1.c() = abs(t)/t2d2;
+    rot1.s() = d/t2d2;
+    if(t<RealScalar(0))
+      rot1.s() = -rot1.s();
   }
   m.applyOnTheLeft(0,1,rot1);
   j_right->makeJacobi(m,0,1);
@@ -528,8 +536,9 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD()
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
-        m_rows(-1), m_cols(-1)
+        m_rows(-1), m_cols(-1), m_diagSize(0)
     {}
 
 
@@ -542,6 +551,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD(Index rows, Index cols, unsigned int computationOptions = 0)
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {
@@ -561,6 +571,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0)
       : m_isInitialized(false),
         m_isAllocated(false),
+        m_usePrescribedThreshold(false),
         m_computationOptions(0),
         m_rows(-1), m_cols(-1)
     {
@@ -662,6 +673,69 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
       eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
       return m_nonzeroSingularValues;
     }
+    
+    /** \returns the rank of the matrix of which \c *this is the SVD.
+      *
+      * \note This method has to determine which singular values should be considered nonzero.
+      *       For that, it uses the threshold value that you can control by calling
+      *       setThreshold(const RealScalar&).
+      */
+    inline Index rank() const
+    {
+      using std::abs;
+      eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
+      if(m_singularValues.size()==0) return 0;
+      RealScalar premultiplied_threshold = m_singularValues.coeff(0) * threshold();
+      Index i = m_nonzeroSingularValues-1;
+      while(i>=0 && m_singularValues.coeff(i) < premultiplied_threshold) --i;
+      return i+1;
+    }
+    
+    /** Allows to prescribe a threshold to be used by certain methods, such as rank() and solve(),
+      * which need to determine when singular values are to be considered nonzero.
+      * This is not used for the SVD decomposition itself.
+      *
+      * When it needs to get the threshold value, Eigen calls threshold().
+      * The default is \c NumTraits<Scalar>::epsilon()
+      *
+      * \param threshold The new value to use as the threshold.
+      *
+      * A singular value will be considered nonzero if its value is strictly greater than
+      *  \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
+      *
+      * If you want to come back to the default behavior, call setThreshold(Default_t)
+      */
+    JacobiSVD& setThreshold(const RealScalar& threshold)
+    {
+      m_usePrescribedThreshold = true;
+      m_prescribedThreshold = threshold;
+      return *this;
+    }
+
+    /** Allows to come back to the default behavior, letting Eigen use its default formula for
+      * determining the threshold.
+      *
+      * You should pass the special object Eigen::Default as parameter here.
+      * \code svd.setThreshold(Eigen::Default); \endcode
+      *
+      * See the documentation of setThreshold(const RealScalar&).
+      */
+    JacobiSVD& setThreshold(Default_t)
+    {
+      m_usePrescribedThreshold = false;
+      return *this;
+    }
+
+    /** Returns the threshold that will be used by certain methods such as rank().
+      *
+      * See the documentation of setThreshold(const RealScalar&).
+      */
+    RealScalar threshold() const
+    {
+      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
+      return m_usePrescribedThreshold ? m_prescribedThreshold
+                                      : (std::max<Index>)(1,m_diagSize)*NumTraits<Scalar>::epsilon();
+    }
 
     inline Index rows() const { return m_rows; }
     inline Index cols() const { return m_cols; }
@@ -674,11 +748,12 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
     MatrixVType m_matrixV;
     SingularValuesType m_singularValues;
     WorkMatrixType m_workMatrix;
-    bool m_isInitialized, m_isAllocated;
+    bool m_isInitialized, m_isAllocated, m_usePrescribedThreshold;
     bool m_computeFullU, m_computeThinU;
     bool m_computeFullV, m_computeThinV;
     unsigned int m_computationOptions;
     Index m_nonzeroSingularValues, m_rows, m_cols, m_diagSize;
+    RealScalar m_prescribedThreshold;
 
     template<typename __MatrixType, int _QRPreconditioner, bool _IsComplex>
     friend struct internal::svd_precondition_2x2_block_to_be_real;
@@ -761,6 +836,11 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
     if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols);
     if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize);
   }
+  
+  // Scaling factor to reduce over/under-flows
+  RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff();
+  if(scale==RealScalar(0)) scale = RealScalar(1);
+  m_workMatrix /= scale;
 
   /*** step 2. The main Jacobi SVD iteration. ***/
 
@@ -830,6 +910,8 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
       if(computeV()) m_matrixV.col(pos).swap(m_matrixV.col(i));
     }
   }
+  
+  m_singularValues *= scale;
 
   m_isInitialized = true;
   return *this;
@@ -851,11 +933,11 @@ struct solve_retval<JacobiSVD<_MatrixType, QRPreconditioner>, Rhs>
     // So A^{-1} = V S^{-1} U^*
 
     Matrix<Scalar, Dynamic, Rhs::ColsAtCompileTime, 0, _MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime> tmp;
-    Index nonzeroSingVals = dec().nonzeroSingularValues();
+    Index rank = dec().rank();
     
-    tmp.noalias() = dec().matrixU().leftCols(nonzeroSingVals).adjoint() * rhs();
-    tmp = dec().singularValues().head(nonzeroSingVals).asDiagonal().inverse() * tmp;
-    dst = dec().matrixV().leftCols(nonzeroSingVals) * tmp;
+    tmp.noalias() = dec().matrixU().leftCols(rank).adjoint() * rhs();
+    tmp = dec().singularValues().head(rank).asDiagonal().inverse() * tmp;
+    dst = dec().matrixV().leftCols(rank) * tmp;
   }
 };
 } // end namespace internal

gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h

@@ -37,6 +37,7 @@ class SimplicialCholeskyBase : internal::noncopyable
 {
   public:
     typedef typename internal::traits<Derived>::MatrixType MatrixType;
+    typedef typename internal::traits<Derived>::OrderingType OrderingType;
     enum { UpLo = internal::traits<Derived>::UpLo };
     typedef typename MatrixType::Scalar Scalar;
     typedef typename MatrixType::RealScalar RealScalar;
@@ -240,15 +241,16 @@ class SimplicialCholeskyBase : internal::noncopyable
     RealScalar m_shiftScale;
 };
 
-template<typename _MatrixType, int _UpLo = Lower> class SimplicialLLT;
-template<typename _MatrixType, int _UpLo = Lower> class SimplicialLDLT;
-template<typename _MatrixType, int _UpLo = Lower> class SimplicialCholesky;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialLLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialLDLT;
+template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialCholesky;
 
 namespace internal {
 
-template<typename _MatrixType, int _UpLo> struct traits<SimplicialLLT<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
 {
   typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
   enum { UpLo = _UpLo };
   typedef typename MatrixType::Scalar                         Scalar;
   typedef typename MatrixType::Index                          Index;
@@ -259,9 +261,10 @@ template<typename _MatrixType, int _UpLo> struct traits<SimplicialLLT<_MatrixTyp
   static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
 };
 
-template<typename _MatrixType,int _UpLo> struct traits<SimplicialLDLT<_MatrixType,_UpLo> >
+template<typename _MatrixType,int _UpLo, typename _Ordering> struct traits<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
 {
   typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
   enum { UpLo = _UpLo };
   typedef typename MatrixType::Scalar                             Scalar;
   typedef typename MatrixType::Index                              Index;
@@ -272,9 +275,10 @@ template<typename _MatrixType,int _UpLo> struct traits<SimplicialLDLT<_MatrixTyp
   static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
 };
 
-template<typename _MatrixType, int _UpLo> struct traits<SimplicialCholesky<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
 {
   typedef _MatrixType MatrixType;
+  typedef _Ordering OrderingType;
   enum { UpLo = _UpLo };
 };
 
@@ -294,11 +298,12 @@ template<typename _MatrixType, int _UpLo> struct traits<SimplicialCholesky<_Matr
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
   * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
   *               or Upper. Default is Lower.
+  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
   *
-  * \sa class SimplicialLDLT
+  * \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
   */
-template<typename _MatrixType, int _UpLo>
-    class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+    class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
 {
 public:
     typedef _MatrixType MatrixType;
@@ -382,11 +387,12 @@ public:
   * \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
   * \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
   *               or Upper. Default is Lower.
+  * \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
   *
-  * \sa class SimplicialLLT
+  * \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
   */
-template<typename _MatrixType, int _UpLo>
-    class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+    class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
 {
 public:
     typedef _MatrixType MatrixType;
@@ -467,8 +473,8 @@ public:
   *
   * \sa class SimplicialLDLT, class SimplicialLLT
   */
-template<typename _MatrixType, int _UpLo>
-    class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo> >
+template<typename _MatrixType, int _UpLo, typename _Ordering>
+    class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
 {
 public:
     typedef _MatrixType MatrixType;
@@ -612,15 +618,13 @@ void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, CholMatrixTy
 {
   eigen_assert(a.rows()==a.cols());
   const Index size = a.rows();
-  // TODO allows to configure the permutation
   // Note that amd compute the inverse permutation
   {
     CholMatrixType C;
     C = a.template selfadjointView<UpLo>();
-    // remove diagonal entries:
-    // seems not to be needed
-    // C.prune(keep_diag());
-    internal::minimum_degree_ordering(C, m_Pinv);
+    
+    OrderingType ordering;
+    ordering(C,m_Pinv);
   }
 
   if(m_Pinv.size()>0)