Merge branch 'develop' of bitbucket.org:gtborg/gtsam into feature/improvementsIncrementalFilter

* 'develop' of bitbucket.org:gtborg/gtsam: (43 commits)
  metis: rely on global BUILD_SHARED_LIBS"
  missing GTSAM_BUILD_STATIC_LIBRARY flags
  Fix testExpression
  attempt to fix alignment error in expression factors
  Fix alignment crash in numerical derivative with march=native
  Imported TBB targets; update gtsam_unstable cmake
  git ignore qtcreator IDE files
  fixed typo in description
  changed the SFMdata functions so that it allows the passage of function arguments to generate a trajectory; default arguments result in the original behaviour (described in header). In the range bearing examples: fixed weirdo text-artifacts, add newline for readability, added underscore the prediction expression.
  type in filename....
  another comment update
  little typo in a comment
  expression example of estimating trajectory, landmarks and sensor-body-transform simultaneously
  Update LICENSE to enumerate all dependencies in gtsam/3rdparty
  Added extra types included by Jacob Thomson in (declined) PR #269
  Cleaned up Pose3 unit test, added unit test for adjoint.
  Added adjoint operators etc.
  Adding adjoint and adjoint transpose functions
  Switching to METIS ordering fixes out of memory error for large examples.
  Added example by Wenqiang Zhou given in issue #369
  ...

.gitignore

@@ -19,3 +19,5 @@ cython/gtsam_wrapper.pxd
 .env
 /.vs/
 /CMakeSettings.json
+# for QtCreator:
+CMakeLists.txt.user*

CMakeLists.txt

@@ -1,6 +1,6 @@
 
 project(GTSAM CXX C)
-cmake_minimum_required(VERSION 2.6)
+cmake_minimum_required(VERSION 2.8.4)
 
 # new feature to Cmake Version > 2.8.12
 # Mac ONLY. Define Relative Path on Mac OS
@@ -55,7 +55,7 @@ endif()
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)
 endif()
-option(GTSAM_BUILD_STATIC_LIBRARY        "Build a static gtsam library, instead of shared" OFF)
+option(BUILD_SHARED_LIBS                 "Build shared gtsam library, instead of static" ON)
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP 			 	 "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
 option(GTSAM_ROT3_EXPMAP 			 	 "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
@@ -69,6 +69,9 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions depreca
 option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
 option(GTSAM_SUPPORT_NESTED_DISSECTION   "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION      "Use new ImuFactor with integration on tangent space" ON)
+if(NOT MSVC AND NOT XCODE_VERSION)
+    option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
+endif()
 
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
@@ -84,8 +87,8 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
 	message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
 endif()
 
-if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
-	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled.  The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
+	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries.  If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
 endif()
 
 if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
@@ -160,24 +163,14 @@ endif()
 
 ###############################################################################
 # Find TBB
-find_package(TBB)
+find_package(TBB COMPONENTS tbb tbbmalloc)
 
 # Set up variables if we're using TBB
 if(TBB_FOUND AND GTSAM_WITH_TBB)
 	set(GTSAM_USE_TBB 1)  # This will go into config.h
-	include_directories(BEFORE ${TBB_INCLUDE_DIRS})
+  # all definitions and link requisites will go via imported targets:
-	set(GTSAM_TBB_LIBRARIES "")
+  # tbb & tbbmalloc
-	if(TBB_DEBUG_LIBRARIES)
+  list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
-		foreach(lib ${TBB_LIBRARIES})
-			list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}")
-		endforeach()
-		foreach(lib ${TBB_DEBUG_LIBRARIES})
-			list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}")
-		endforeach()
-	else()
-		set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES})
-	endif()
-	list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${GTSAM_TBB_LIBRARIES})
 else()
 	set(GTSAM_USE_TBB 0)  # This will go into config.h
 endif()
@@ -193,6 +186,20 @@ endif()
 # Find Google perftools
 find_package(GooglePerfTools)
 
+###############################################################################
+# Support ccache, if installed
+if(NOT MSVC AND NOT XCODE_VERSION)
+	find_program(CCACHE_FOUND ccache)
+	if(CCACHE_FOUND)
+		if(GTSAM_BUILD_WITH_CCACHE)
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
+		else()
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
+			set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
+		endif()
+	endif(CCACHE_FOUND)
+endif()
 
 ###############################################################################
 # Find MKL
@@ -270,7 +277,7 @@ else()
 endif()
 
 if (MSVC)
-	if (NOT GTSAM_BUILD_STATIC_LIBRARY)
+	if (BUILD_SHARED_LIBS)
 		# mute eigen static assert to avoid errors in shared lib
 		add_definitions(-DEIGEN_NO_STATIC_ASSERT)
 	endif()
@@ -479,7 +486,7 @@ print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS}         "Build timing scripts wit
 if (DOXYGEN_FOUND)
     print_config_flag(${GTSAM_BUILD_DOCS}              "Build Docs                     ")
 endif()
-print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY}        "Build static GTSAM library instead of shared")
+print_config_flag(${BUILD_SHARED_LIBS}                 "Build shared GTSAM libraries   ")
 print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in library name ")
 if(GTSAM_UNSTABLE_AVAILABLE)
     print_config_flag(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
@@ -522,6 +529,15 @@ else()
 endif()
 message(STATUS "  Default allocator              : ${GTSAM_DEFAULT_ALLOCATOR}")
 
+if(NOT MSVC AND NOT XCODE_VERSION)
+	if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
+		message(STATUS "  Build with ccache              : Yes")
+	elseif(CCACHE_FOUND)
+		message(STATUS "  Build with ccache              : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
+	else()
+		message(STATUS "  Build with ccache              : No")
+	endif()
+endif()
 
 message(STATUS "Packaging flags                                               ")
 message(STATUS "  CPack Source Generator         : ${CPACK_SOURCE_GENERATOR}")

LICENSE

@@ -1,18 +1,25 @@
 GTSAM is released under the simplified BSD license, reproduced in the file 
 LICENSE.BSD in this directory.
 
-GTSAM contains two third party libraries, with documentation of licensing and 
+GTSAM contains several third party libraries, with documentation of licensing
-modifications as follows:
+and modifications as follows:
 
-- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree 
+- CCOLAMD 2.9.6: Tim Davis' constrained column approximate minimum degree
 ordering library
-    - Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
+    - Included unmodified in gtsam/3rdparty/CCOLAMD and
+      gtsam/3rdparty/SuiteSparse_config
     - http://faculty.cse.tamu.edu/davis/suitesparse.html
     - Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt
-- Eigen 3.2:  General C++ matrix and linear algebra library
+- ceres: Google's nonlinear least-squares optimization library
-    - Modified with 3 patches that have been contributed back to the Eigen team:
+    - Includes only auto-diff/jet code, with minor modifications to includes
-        - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
+    - http://ceres-solver.org/license.html
-        - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
+- Eigen 3.3.7:  General C++ matrix and linear algebra library
-        - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
     - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README 
-    - Some code that is 3rd-party to Eigen is BSD and LGPL
+    - Some code that is 3rd-party to Eigen is BSD and LGPL
+- GeographicLib 1.35: Charles Karney's geographic conversion utility library
+    - Included unmodified in gtsam/3rdparty/GeographicLib
+    - Licenced under MIT, provided in gtsam/3rdparty/GeographicLib/LICENSE.txt
+- METIS 5.1.0: Graph partitioning and fill-reducing matrix ordering library
+    - Included unmodified in gtsam/3rdparty/metis
+    - Licenced under Apache License v 2.0, provided in
+      gtsam/3rdparty/metis/LICENSE.txt

cmake/FindTBB.cmake

@@ -1,13 +1,6 @@
-# Locate Intel Threading Building Blocks include paths and libraries
+# The MIT License (MIT)
-# FindTBB.cmake can be found at https://code.google.com/p/findtbb/
-# Written by Hannes Hofmann <hannes.hofmann _at_ informatik.uni-erlangen.de>
-# Improvements by Gino van den Bergen <gino _at_ dtecta.com>,
-#   Florian Uhlig <F.Uhlig _at_ gsi.de>,
-#   Jiri Marsik <jiri.marsik89 _at_ gmail.com>
-
-# The MIT License
 #
-# Copyright (c) 2011 Hannes Hofmann
+# Copyright (c) 2015 Justus Calvin
 #
 # Permission is hereby granted, free of charge, to any person obtaining a copy
 # of this software and associated documentation files (the "Software"), to deal
@@ -16,295 +9,306 @@
 # copies of the Software, and to permit persons to whom the Software is
 # furnished to do so, subject to the following conditions:
 #
-# The above copyright notice and this permission notice shall be included in
+# The above copyright notice and this permission notice shall be included in all
-# all copies or substantial portions of the Software.
+# copies or substantial portions of the Software.
 #
 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-# THE SOFTWARE.
+# SOFTWARE.
 
-# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler.
-#   e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21"
-#   TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found
-#   in the TBB installation directory (TBB_INSTALL_DIR).
 #
-# GvdB: Mac OS X distribution places libraries directly in lib directory.
+# FindTBB
+# -------
 #
-# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER.
+# Find TBB include directories and libraries.
-# TBB_ARCHITECTURE [ ia32 | em64t | itanium ]
+#
-#   which architecture to use
+# Usage:
-# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9
+#
-#   which compiler to use (detected automatically on Windows)
+#  find_package(TBB [major[.minor]] [EXACT]
+#               [QUIET] [REQUIRED]
+#               [[COMPONENTS] [components...]]
+#               [OPTIONAL_COMPONENTS components...])
+#
+# where the allowed components are tbbmalloc and tbb_preview. Users may modify
+# the behavior of this module with the following variables:
+#
+# * TBB_ROOT_DIR          - The base directory the of TBB installation.
+# * TBB_INCLUDE_DIR       - The directory that contains the TBB headers files.
+# * TBB_LIBRARY           - The directory that contains the TBB library files.
+# * TBB_<library>_LIBRARY - The path of the TBB the corresponding TBB library.
+#                           These libraries, if specified, override the
+#                           corresponding library search results, where <library>
+#                           may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
+#                           tbb_preview, or tbb_preview_debug.
+# * TBB_USE_DEBUG_BUILD   - The debug version of tbb libraries, if present, will
+#                           be used instead of the release version.
+#
+# Users may modify the behavior of this module with the following environment
+# variables:
+#
+# * TBB_INSTALL_DIR
+# * TBBROOT
+# * LIBRARY_PATH
+#
+# This module will set the following variables:
+#
+# * TBB_FOUND             - Set to false, or undefined, if we haven’t found, or
+#                           don’t want to use TBB.
+# * TBB_<component>_FOUND - If False, optional <component> part of TBB sytem is
+#                           not available.
+# * TBB_VERSION           - The full version string
+# * TBB_VERSION_MAJOR     - The major version
+# * TBB_VERSION_MINOR     - The minor version
+# * TBB_INTERFACE_VERSION - The interface version number defined in
+#                           tbb/tbb_stddef.h.
+# * TBB_<library>_LIBRARY_RELEASE - The path of the TBB release version of
+#                           <library>, where <library> may be tbb, tbb_debug,
+#                           tbbmalloc, tbbmalloc_debug, tbb_preview, or
+#                           tbb_preview_debug.
+# * TBB_<library>_LIBRARY_DEGUG - The path of the TBB release version of
+#                           <library>, where <library> may be tbb, tbb_debug,
+#                           tbbmalloc, tbbmalloc_debug, tbb_preview, or
+#                           tbb_preview_debug.
+#
+# The following varibles should be used to build and link with TBB:
+#
+# * TBB_INCLUDE_DIRS        - The include directory for TBB.
+# * TBB_LIBRARIES           - The libraries to link against to use TBB.
+# * TBB_LIBRARIES_RELEASE   - The release libraries to link against to use TBB.
+# * TBB_LIBRARIES_DEBUG     - The debug libraries to link against to use TBB.
+# * TBB_DEFINITIONS         - Definitions to use when compiling code that uses
+#                             TBB.
+# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
+#                             uses TBB.
+# * TBB_DEFINITIONS_DEBUG   - Definitions to use when compiling debug code that
+#                             uses TBB.
+#
+# This module will also create the "tbb" target that may be used when building
+# executables and libraries.
 
-# This module respects
+include(FindPackageHandleStandardArgs)
-# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR}
 
-# This module defines
+if(NOT TBB_FOUND)
-# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc.
-# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc
-# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug
-# TBB_INSTALL_DIR, the base TBB install directory
-# TBB_LIBRARIES, the libraries to link against to use TBB.
-# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols.
-# TBB_FOUND, If false, don't try to use TBB.
-# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h
 
+  ##################################
+  # Check the build type
+  ##################################
 
-if (WIN32)
+  if(NOT DEFINED TBB_USE_DEBUG_BUILD)
-    # has em64t/vc8 em64t/vc9
+    if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
-    # has ia32/vc7.1 ia32/vc8 ia32/vc9
+      set(TBB_BUILD_TYPE DEBUG)
-    set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB")
+    else()
-    set(_TBB_LIB_NAME "tbb")
+      set(TBB_BUILD_TYPE RELEASE)
-    set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
+    endif()
-    set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
+  elseif(TBB_USE_DEBUG_BUILD)
-    set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
+    set(TBB_BUILD_TYPE DEBUG)
-    if (MSVC71)
+  else()
-        set (_TBB_COMPILER "vc7.1")
+    set(TBB_BUILD_TYPE RELEASE)
-        set (TBB_COMPILER "vc7.1")
+  endif()
-    endif(MSVC71)
-    if (MSVC80)
-        set(_TBB_COMPILER "vc8")
-        set(TBB_COMPILER "vc8")
-    endif(MSVC80)
-    if (MSVC90)
-        set(_TBB_COMPILER "vc9")
-        set(TBB_COMPILER "vc9")
-    endif(MSVC90)
-    if(MSVC10)
-        set(_TBB_COMPILER "vc10")
-        set(TBB_COMPILER "vc10")
-    endif(MSVC10)
-    if(MSVC11)
-        set(_TBB_COMPILER "vc11")
-        set(TBB_COMPILER "vc11")
-    endif(MSVC11)
-    if(MSVC14)
-        set(_TBB_COMPILER "vc14")
-        set(TBB_COMPILER "vc14")
-    endif(MSVC14)
-    # Todo: add other Windows compilers such as ICL.
-	if(TBB_ARCHITECTURE)
-		set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
-	elseif("$ENV{TBB_ARCH_PLATFORM}" STREQUAL "")
-		# Try to guess the architecture
-		if(CMAKE_CL_64)
-			set(_TBB_ARCHITECTURE intel64)
-			set(TBB_ARCHITECTURE intel64)
-		else()
-			set(_TBB_ARCHITECTURE ia32)
-			set(TBB_ARCHITECTURE ia32)
-		endif()
-	endif()
-endif (WIN32)
 
-if (UNIX)
+  ##################################
-    if (APPLE)
+  # Set the TBB search directories
-        # MAC
+  ##################################
-        set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions")
-        # libs: libtbb.dylib, libtbbmalloc.dylib, *_debug
-        set(_TBB_LIB_NAME "tbb")
-        set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
-        set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
-        set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
-        # default flavor on apple: ia32/cc4.0.1_os10.4.9
-        # Jiri: There is no reason to presume there is only one flavor and
-        #       that user's setting of variables should be ignored.
-        if(NOT TBB_COMPILER)
-            set(_TBB_COMPILER "cc4.0.1_os10.4.9")
-        elseif (NOT TBB_COMPILER)
-            set(_TBB_COMPILER ${TBB_COMPILER})
-        endif(NOT TBB_COMPILER)
-        if(NOT TBB_ARCHITECTURE)
-            set(_TBB_ARCHITECTURE "ia32")
-        elseif(NOT TBB_ARCHITECTURE)
-            set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
-        endif(NOT TBB_ARCHITECTURE)
-    else (APPLE)
-        # LINUX
-        set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include")
-        set(_TBB_LIB_NAME "tbb")
-        set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
-        set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
-        set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
-        # has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21
-        # has ia32/*
-        # has itanium/*
-        set(_TBB_COMPILER ${TBB_COMPILER})
-        set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
-    endif (APPLE)
-endif (UNIX)
 
-if (CMAKE_SYSTEM MATCHES "SunOS.*")
+  # Define search paths based on user input and environment variables
-# SUN
+  set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
-# not yet supported
-# has em64t/cc3.4.3_kernel5.10
-# has ia32/*
-endif (CMAKE_SYSTEM MATCHES "SunOS.*")
 
+  # Define the search directories based on the current platform
+  if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
+    set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
+                               "C:/Program Files (x86)/Intel/TBB")
 
-#-- Clear the public variables
+    # Set the target architecture
-set (TBB_FOUND "NO")
+    if(CMAKE_SIZEOF_VOID_P EQUAL 8)
+      set(TBB_ARCHITECTURE "intel64")
+    else()
+      set(TBB_ARCHITECTURE "ia32")
+    endif()
 
+    # Set the TBB search library path search suffix based on the version of VC
+    if(WINDOWS_STORE)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
+    elseif(MSVC14)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
+    elseif(MSVC12)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
+    elseif(MSVC11)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
+    elseif(MSVC10)
+      set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
+    endif()
 
-#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR}
+    # Add the library path search suffix for the VC independent version of TBB
-# first: use CMake variable TBB_INSTALL_DIR
+    list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
-if (TBB_INSTALL_DIR)
-    set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR})
-endif (TBB_INSTALL_DIR)
-# second: use environment variable
-if (NOT _TBB_INSTALL_DIR)
-    if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR})
-    endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
-    # Intel recommends setting TBB21_INSTALL_DIR
-    if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR})
-    endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
-    if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR})
-    endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
-    if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
-        set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR})
-    endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
-endif (NOT _TBB_INSTALL_DIR)
-# third: try to find path automatically
-if (NOT _TBB_INSTALL_DIR)
-    if (_TBB_DEFAULT_INSTALL_DIR)
-        set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR})
-    endif (_TBB_DEFAULT_INSTALL_DIR)
-endif (NOT _TBB_INSTALL_DIR)
-# sanity check
-if (NOT _TBB_INSTALL_DIR)
-    message (STATUS "TBB: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}")
-else (NOT _TBB_INSTALL_DIR)
-# finally: set the cached CMake variable TBB_INSTALL_DIR
-if (NOT TBB_INSTALL_DIR)
-    set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory")
-    mark_as_advanced(TBB_INSTALL_DIR)
-endif (NOT TBB_INSTALL_DIR)
 
+  elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
+    # OS X
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
 
-#-- A macro to rewrite the paths of the library. This is necessary, because
+    # TODO: Check to see which C++ library is being used by the compiler.
-#   find_library() always found the em64t/vc9 version of the TBB libs
+    if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
-macro(TBB_CORRECT_LIB_DIR var_name)
+      # The default C++ library on OS X 10.9 and later is libc++
-#    if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
+      set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
-        string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
+    else()
-#    endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
+      set(TBB_LIB_PATH_SUFFIX "lib")
-    string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
+    endif()
-    string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
+  elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
-    string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
+    # Linux
-    string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
+    set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
-    string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-    string(REPLACE vc11 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
-endmacro(TBB_CORRECT_LIB_DIR var_content)
 
+    # TODO: Check compiler version to see the suffix should be <arch>/gcc4.1 or
+    #       <arch>/gcc4.1. For now, assume that the compiler is more recent than
+    #       gcc 4.4.x or later.
+    if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
+      set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
+    elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
+      set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
+    endif()
+  endif()
 
-#-- Look for include directory and set ${TBB_INCLUDE_DIR}
+  ##################################
-set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include)
+  # Find the TBB include dir
-# Jiri: tbbvars now sets the CPATH environment variable to the directory
+  ##################################
-#       containing the headers.
-find_path(TBB_INCLUDE_DIR
-    tbb/task_scheduler_init.h
-    PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH
-)
-mark_as_advanced(TBB_INCLUDE_DIR)
 
+  find_path(TBB_INCLUDE_DIRS tbb/tbb.h
+      HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
+      PATHS ${TBB_DEFAULT_SEARCH_DIR}
+      PATH_SUFFIXES include)
 
-#-- Look for libraries
+  ##################################
-# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh]
+  # Set version strings
-if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
+  ##################################
-    set (_TBB_LIBRARY_DIR 
-         ${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM}
-         ${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib
-        )
-endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
-# Jiri: This block isn't mutually exclusive with the previous one
-#       (hence no else), instead I test if the user really specified
-#       the variables in question.
-if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
-    # HH: deprecated
-    message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).")
-    # Jiri: It doesn't hurt to look in more places, so I store the hints from
-    #       ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER
-    #       variables and search them both.
-    set (
-		_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}
-		_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/lib/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}" ${_TBB_LIBRARY_DIR}
-		)
-endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
 
-# GvdB: Mac OS X distribution places libraries directly in lib directory.
+  if(TBB_INCLUDE_DIRS)
-list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib)
+    file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
+    string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
+        TBB_VERSION_MAJOR "${_tbb_version_file}")
+    string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
+        TBB_VERSION_MINOR "${_tbb_version_file}")
+    string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
+        TBB_INTERFACE_VERSION "${_tbb_version_file}")
+    set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
+  endif()
 
-# Jiri: No reason not to check the default paths. From recent versions,
+  ##################################
-#       tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH
+  # Find TBB components
-#       variables, which now point to the directories of the lib files.
+  ##################################
-#       It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS
-#       argument instead of the implicit PATHS as it isn't hard-coded
-#       but computed by system introspection. Searching the LIBRARY_PATH
-#       and LD_LIBRARY_PATH environment variables is now even more important
-#       that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates
-#       the use of TBB built from sources.
-find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
-find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
 
-#Extract path from TBB_LIBRARY name
+  if(TBB_VERSION VERSION_LESS 4.3)
-get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH)
+    set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
+  else()
+    set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
+  endif()
 
-#TBB_CORRECT_LIB_DIR(TBB_LIBRARY)
+  # Find each component
-#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY)
+  foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
-mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY)
+    if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
 
-#-- Look for debug libraries
+      # Search for the libraries
-# Jiri: Changed the same way as for the release libraries.
+      find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
-find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
+          HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
+          PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
-find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
+          PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
-        PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
 
-# Jiri: Self-built TBB stores the debug libraries in a separate directory.
+      find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
-#       Extract path from TBB_LIBRARY_DEBUG name
+          HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
-get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH)
+          PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
+          PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
 
-#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG)
+      if(TBB_${_comp}_LIBRARY_DEBUG)
-#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG)
+        list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
-mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG)
+      endif()
+      if(TBB_${_comp}_LIBRARY_RELEASE)
+        list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
+      endif()
+      if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
+        set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
+      endif()
 
+      if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
+        set(TBB_${_comp}_FOUND TRUE)
+      else()
+        set(TBB_${_comp}_FOUND FALSE)
+      endif()
 
-if (TBB_INCLUDE_DIR)
+      # Mark internal variables as advanced
-    if (TBB_LIBRARY)
+      mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
-        set (TBB_FOUND "YES")
+      mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
-        set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES})
+      mark_as_advanced(TBB_${_comp}_LIBRARY)
-        set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES})
-        set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE)
-        set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE)
-        # Jiri: Self-built TBB stores the debug libraries in a separate directory.
-        set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE)
-        mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES)
-        message(STATUS "Found Intel TBB")
-    endif (TBB_LIBRARY)
-endif (TBB_INCLUDE_DIR)
 
-if (NOT TBB_FOUND)
+    endif()
-    message(STATUS "TBB: Intel TBB NOT found!")
+  endforeach()
-    message(STATUS "TBB: Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}")
-    # do only throw fatal, if this pkg is REQUIRED
-    if (TBB_FIND_REQUIRED)
-        message(FATAL_ERROR "Could NOT find TBB library.")
-    endif (TBB_FIND_REQUIRED)
-endif (NOT TBB_FOUND)
 
-endif (NOT _TBB_INSTALL_DIR)
+  ##################################
+  # Set compile flags and libraries
+  ##################################
 
-if (TBB_FOUND)
+  set(TBB_DEFINITIONS_RELEASE "")
-	set(TBB_INTERFACE_VERSION 0)
+  set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
-	FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS)
+
-	STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}")
+  if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
-	set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}")
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
-endif (TBB_FOUND)
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
+  elseif(TBB_LIBRARIES_RELEASE)
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
+  elseif(TBB_LIBRARIES_DEBUG)
+    set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
+    set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
+  endif()
+
+  find_package_handle_standard_args(TBB
+      REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
+      HANDLE_COMPONENTS
+      VERSION_VAR TBB_VERSION)
+
+  ##################################
+  # Create targets
+  ##################################
+
+  if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
+    # Start fix to support different targets for tbb, tbbmalloc, etc.
+    # (Jose Luis Blanco, Jan 2019)
+    # Iterate over tbb, tbbmalloc, etc.
+    foreach(libname ${TBB_SEARCH_COMPOMPONENTS})
+      if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG))
+        continue()
+      endif()
+
+      add_library(${libname} SHARED IMPORTED)
+
+      set_target_properties(${libname} PROPERTIES
+            INTERFACE_INCLUDE_DIRECTORIES  ${TBB_INCLUDE_DIRS}
+            IMPORTED_LOCATION              ${TBB_${libname}_LIBRARY_RELEASE})
+      if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG)
+        set_target_properties(${libname} PROPERTIES
+            INTERFACE_COMPILE_DEFINITIONS "$<$<OR:$<CONFIG:Debug>,$<CONFIG:RelWithDebInfo>>:TBB_USE_DEBUG=1>"
+            IMPORTED_LOCATION_DEBUG          ${TBB_${libname}_LIBRARY_DEBUG}
+            IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG}
+            IMPORTED_LOCATION_RELEASE        ${TBB_${libname}_LIBRARY_RELEASE}
+            IMPORTED_LOCATION_MINSIZEREL     ${TBB_${libname}_LIBRARY_RELEASE}
+            )
+      elseif(TBB_${libname}_LIBRARY_RELEASE)
+        set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
+      else()
+        set_target_properties(${libname} PROPERTIES
+            INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
+            IMPORTED_LOCATION              ${TBB_${libname}_LIBRARY_DEBUG}
+            )
+      endif()
+    endforeach()
+    # End of fix to support different targets
+  endif()
+
+  mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
+
+  unset(TBB_ARCHITECTURE)
+  unset(TBB_BUILD_TYPE)
+  unset(TBB_LIB_PATH_SUFFIX)
+  unset(TBB_DEFAULT_SEARCH_DIR)
+
+endif()

cmake/dllexport.h.in

@@ -31,10 +31,10 @@
 
 // Whether GTSAM is compiled as static or DLL in windows. 
 // This will be used to decide whether include __declspec(dllimport) or not in headers
-#cmakedefine GTSAM_BUILD_STATIC_LIBRARY
+#cmakedefine BUILD_SHARED_LIBS
 
 #ifdef _WIN32
-#  ifdef @library_name@_BUILD_STATIC_LIBRARY
+#  ifndef BUILD_SHARED_LIBS
 #    define @library_name@_EXPORT
 #    define @library_name@_EXTERN_EXPORT extern
 #  else
@@ -50,3 +50,6 @@
 #  define @library_name@_EXPORT
 #  define @library_name@_EXTERN_EXPORT extern
 #endif
+
+#undef BUILD_SHARED_LIBS
+

cython/gtsam/tests/test_Pose3.py

@@ -1,25 +1,32 @@
+"""Pose3 unit tests."""
 import math
 import unittest
 
-from gtsam import Point3, Rot3, Pose3
+import numpy as np
+
+from gtsam import Point3, Pose3, Rot3
 
 
 class TestPose3(unittest.TestCase):
+    """Test selected Pose3 methods."""
 
-    def test__between(self):
+    def test_between(self):
-        T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2))
+        """Test between method."""
+        T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
         T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3))
         expected = T2.inverse().compose(T3)
         actual = T2.between(T3)
         self.assertTrue(actual.equals(expected, 1e-6))
 
     def test_transform_to(self):
-        transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0))
+        """Test transform_to method."""
-        actual = transform.transform_to(Point3(3,2,10))
+        transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
-        expected = Point3 (2,1,10)
+        actual = transform.transform_to(Point3(3, 2, 10))
+        expected = Point3(2, 1, 10)
         self.assertTrue(actual.equals(expected, 1e-6))
 
     def test_range(self):
+        """Test range method."""
         l1 = Point3(1, 0, 0)
         l2 = Point3(1, 1, 0)
         x1 = Pose3()
@@ -28,16 +35,23 @@ class TestPose3(unittest.TestCase):
         xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
 
         # establish range is indeed zero
-        self.assertEqual(1,x1.range(point=l1))
+        self.assertEqual(1, x1.range(point=l1))
 
         # establish range is indeed sqrt2
-        self.assertEqual(math.sqrt(2.0),x1.range(point=l2))
+        self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
 
         # establish range is indeed zero
-        self.assertEqual(1,x1.range(pose=xl1))
+        self.assertEqual(1, x1.range(pose=xl1))
-        
+
         # establish range is indeed sqrt2
-        self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2))
+        self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
+
+    def test_adjoint(self):
+        """Test adjoint method."""
+        xi = np.array([1, 2, 3, 4, 5, 6])
+        expected = np.dot(Pose3.adjointMap(xi), xi)
+        actual = Pose3.adjoint(xi, xi)
+        np.testing.assert_array_equal(actual, expected)
 
 
 if __name__ == "__main__":

examples/ISAM2_SmartFactorStereo_IMU.cpp

@@ -15,6 +15,7 @@
 #include <fstream>
 #include <iostream>
 #include <sstream>
+#include <string>
 #include <vector>
 
 using namespace std;
@@ -224,7 +225,7 @@ int main(int argc, char* argv[]) {
 
       smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
     } else {
-      throw runtime_error("unexpected data type: " + type);
+      throw runtime_error("unexpected data type: " + string(1, type));
     }
 
     lastFrame = frame;

examples/Pose2SLAMStressTest.cpp

@@ -0,0 +1,84 @@
+/**
+ * @file Pose2SLAMStressTest.cpp
+ * @brief Test GTSAM on large open-loop chains
+ * @date May 23, 2018
+ * @author Wenqiang Zhou
+ */
+
+// Create N 3D poses, add relative motion between each consecutive poses. (The
+// relative motion is simply a unit translation(1, 0, 0), no rotation). For each
+// each pose, add some random noise to the x value of the translation part.
+// Use gtsam to create a prior factor for the first pose and N-1 between factors
+// and run optimization.
+
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/StereoFactor.h>
+
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+void testGtsam(int numberNodes) {
+  std::random_device rd;
+  std::mt19937 e2(rd());
+  std::uniform_real_distribution<> dist(0, 1);
+
+  vector<Pose3> poses;
+  for (int i = 0; i < numberNodes; ++i) {
+    Matrix4 M;
+    double r = dist(e2);
+    r = (r - 0.5) / 10 + i;
+    M << 1, 0, 0, r, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+    poses.push_back(Pose3(M));
+  }
+
+  // prior factor for the first pose
+  auto priorModel = noiseModel::Isotropic::Variance(6, 1e-4);
+  Matrix4 first_M;
+  first_M << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+  Pose3 first = Pose3(first_M);
+
+  NonlinearFactorGraph graph;
+  graph.add(PriorFactor<Pose3>(0, first, priorModel));
+
+  // vo noise model
+  auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);
+
+  // relative VO motion
+  Matrix4 vo_M;
+  vo_M << 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+  Pose3 relativeMotion(vo_M);
+  for (int i = 0; i < numberNodes - 1; ++i) {
+    graph.add(
+        BetweenFactor<Pose3>(i, i + 1, relativeMotion, VOCovarianceModel));
+  }
+
+  // inital values
+  Values initial;
+  for (int i = 0; i < numberNodes; ++i) {
+    initial.insert(i, poses[i]);
+  }
+
+  LevenbergMarquardtParams params;
+  params.setVerbosity("ERROR");
+  params.setOrderingType("METIS");
+  params.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
+  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
+  auto result = optimizer.optimize();
+}
+
+int main(int args, char* argv[]) {
+  int numberNodes = stoi(argv[1]);
+  cout << "number of_nodes: " << numberNodes << endl;
+  testGtsam(numberNodes);
+  return 0;
+}

examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp

@@ -0,0 +1,102 @@
+/**
+ * @file    Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
+ * @brief   A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
+ * @author  Thomas Horstink
+ * @date    January 4th, 2019
+ */
+
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/BearingRange.h>
+#include <gtsam/slam/expressions.h>
+#include <gtsam/nonlinear/ExpressionFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <examples/SFMdata.h>
+
+using namespace gtsam;
+
+typedef BearingRange<Pose3, Point3> BearingRange3D;
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+
+  // Move around so the whole state (including the sensor tf) is observable
+  Pose3 init_pose = Pose3();
+  Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
+  Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0));
+  Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0));
+
+  int steps = 4;
+  auto poses  = createPoses(init_pose, delta_pose1, steps);
+  auto poses2 = createPoses(init_pose, delta_pose2, steps);
+  auto poses3 = createPoses(init_pose, delta_pose3, steps);
+
+  // Concatenate poses to create trajectory
+  poses.insert( poses.end(), poses2.begin(), poses2.end() );
+  poses.insert( poses.end(), poses3.begin(), poses3.end() );  // std::vector of Pose3
+  auto points = createPoints();                               // std::vector of Point3 
+
+  // (ground-truth) sensor pose in body frame, further an unknown variable
+  Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  
+  // The graph
+  ExpressionFactorGraph graph;
+  
+  // Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
+  auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
+  
+  // Uncertainty bearing range measurement;
+  auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
+  
+  // Expressions for body-frame at key 0 and sensor-tf
+  Pose3_ x_('x', 0);
+  Pose3_ body_T_sensor_('T', 0);
+  
+  // Add a prior on the body-pose
+  graph.addExpressionFactor(x_, poses[0], poseNoise); 
+  
+  // Simulated measurements from pose
+  for (size_t i = 0; i < poses.size(); ++i) {
+    auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
+    for (size_t j = 0; j < points.size(); ++j) {
+      
+      // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
+      auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
+      
+      // Create a *perfect* measurement
+      auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
+      
+      // Add factor
+      graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
+    }
+    
+    // and add a between factor to the graph
+    if (i > 0)
+    {
+      // And also we have a *perfect* measurement for the between factor.
+      graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); 
+    }
+  }
+
+  // Create perturbed initial
+  Values initial;
+  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  for (size_t i = 0; i < poses.size(); ++i)
+    initial.insert(Symbol('x', i), poses[i].compose(delta)); 
+  for (size_t j = 0; j < points.size(); ++j)
+    initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+  
+  // Initialize body_T_sensor wrongly (because we do not know!)
+  initial.insert<Pose3>(Symbol('T',0), Pose3()); 
+
+  std::cout << "initial error: " << graph.error(initial) << std::endl;
+  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+  std::cout << "final error: " << graph.error(result) << std::endl;
+
+  initial.at<Pose3>(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
+  result.at<Pose3>(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n");    /* optimized sensor_P_body estimate */
+  body_T_sensor_gt.print("\nGround truth body_T_sensor\n");                     /* sensor_P_body ground truth */
+
+  return 0;
+}
+/* ************************************************************************* */

examples/SFMdata.h

@@ -16,9 +16,10 @@
  */
 
 /**
- * A structure-from-motion example with landmarks
+ * A structure-from-motion example with landmarks, default function arguments give
  *  - The landmarks form a 10 meter cube
  *  - The robot rotates around the landmarks, always facing towards the cube
+ * Passing function argument allows to specificy an initial position, a pose increment and step count.
  */
 
 // As this is a full 3D problem, we will use Pose3 variables to represent the camera
@@ -49,20 +50,19 @@ std::vector<gtsam::Point3> createPoints() {
 }
 
 /* ************************************************************************* */
-std::vector<gtsam::Pose3> createPoses() {
+std::vector<gtsam::Pose3> createPoses(
-
+            const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
+            const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), 
+            int steps = 8) {
+  
   // Create the set of ground-truth poses
+  // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
   std::vector<gtsam::Pose3> poses;
-  double radius = 30.0;
+  int i = 1;
-  int i = 0;
+  poses.push_back(init);
-  double theta = 0.0;
+  for(; i < steps; ++i) {
-  gtsam::Point3 up(0,0,1);
+    poses.push_back(poses[i-1].compose(delta));
-  gtsam::Point3 target(0,0,0);
-  for(; i < 8; ++i, theta += 2*M_PI/8) {
-    gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0);
-    gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up);
-    poses.push_back(camera.pose());
   }
+  
   return poses;
-}
+}
-/* ************************************************************************* */

gtsam.h

@@ -228,6 +228,12 @@ virtual class Value {
   size_t dim() const;
 };
 
+#include <gtsam/base/GenericValue.h>
+template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+virtual class GenericValue : gtsam::Value {
+  void serializable() const;
+};
+
 #include <gtsam/base/deprecated/LieScalar.h>
 class LieScalar {
   // Standard constructors
@@ -567,8 +573,13 @@ class Pose2 {
   // Lie Group
   static gtsam::Pose2 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose2& p);
+  static Matrix ExpmapDerivative(Vector v);
+  static Matrix LogmapDerivative(const gtsam::Pose2& v);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
+  static Matrix adjointMap(Vector v);
+  Vector adjoint(Vector xi, Vector y);
+  Vector adjointTranspose(Vector xi, Vector y);
   static Matrix wedge(double vx, double vy, double w);
 
   // Group Actions on Point2
@@ -617,6 +628,11 @@ class Pose3 {
   static Vector Logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
+  static Matrix adjointMap(Vector xi);
+  static Vector adjoint(Vector xi, Vector y);
+  static Vector adjointTranspose(Vector xi, Vector y);
+  static Matrix ExpmapDerivative(Vector xi);
+  static Matrix LogmapDerivative(const gtsam::Pose3& xi);
   static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
 
   // Group Action on Point3
@@ -2259,10 +2275,13 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
 template<POSE, POINT>
 virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
-typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
@@ -2275,10 +2294,13 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactor
 template<POSE, POINT>
 virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
   RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor);
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransformPosePoint2;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransform2D;
-typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransformPosePoint3;
+typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransform3D;
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2> RangeFactorWithTransformPose2;
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3> RangeFactorWithTransformPose3;
 
@@ -2292,6 +2314,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 };
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
+typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
 
 #include <gtsam/sam/BearingRangeFactor.h>
 template<POSE, POINT, BEARING, RANGE>
@@ -2305,6 +2328,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
 };
 
 typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double> BearingRangeFactorPose2;
 
 
 #include <gtsam/slam/ProjectionFactor.h>

gtsam/3rdparty/metis/CMakeLists.txt

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

gtsam/CMakeLists.txt

@@ -97,48 +97,48 @@ message(STATUS "GTSAM Version: ${gtsam_version}")
 message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
 
 # build shared and static versions of the library
-if (GTSAM_BUILD_STATIC_LIBRARY)
+message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
-    message(STATUS "Building GTSAM - static")
+
-    add_library(gtsam STATIC ${gtsam_srcs})
+# BUILD_SHARED_LIBS automatically defines static/shared libs:
-	target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
+add_library(gtsam ${gtsam_srcs})
-    set_target_properties(gtsam PROPERTIES
+target_link_libraries(gtsam
-        OUTPUT_NAME         gtsam
+    PUBLIC
-        CLEAN_DIRECT_OUTPUT 1
+    ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
-    	VERSION             ${gtsam_version}
+set_target_properties(gtsam PROPERTIES
-     	SOVERSION           ${gtsam_soversion})
+    OUTPUT_NAME         gtsam
-	if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+    CLEAN_DIRECT_OUTPUT 1
+    VERSION             ${gtsam_version}
+    SOVERSION           ${gtsam_soversion})
+
+if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+	if (NOT BUILD_SHARED_LIBS)
 		set_target_properties(gtsam PROPERTIES
 			PREFIX "lib"
 			COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
-	endif()
+	else()
-    install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
-    list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
-    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-else()
-    message(STATUS "Building GTSAM - shared")
-    add_library(gtsam SHARED ${gtsam_srcs})
-    target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
-    set_target_properties(gtsam PROPERTIES
-        OUTPUT_NAME         gtsam
-        CLEAN_DIRECT_OUTPUT 1
-    	VERSION             ${gtsam_version}
-     	SOVERSION           ${gtsam_soversion})
-	if(WIN32)
 		set_target_properties(gtsam PROPERTIES
 			PREFIX ""
 			DEFINE_SYMBOL GTSAM_EXPORTS
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
 	endif()
-	if (APPLE)
-		set_target_properties(gtsam PROPERTIES
-			INSTALL_NAME_DIR
-			"${CMAKE_INSTALL_PREFIX}/lib")
-	endif()
-    install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-    list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
-    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 endif()
 
+if (APPLE AND BUILD_SHARED_LIBS)
+	set_target_properties(gtsam PROPERTIES
+		INSTALL_NAME_DIR
+		"${CMAKE_INSTALL_PREFIX}/lib")
+endif()
+
+install(
+	TARGETS gtsam
+	EXPORT GTSAM-exports
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	RUNTIME DESTINATION bin)
+
+list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+
 # make sure that ccolamd compiles even in face of warnings
 if(WIN32)
     set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
@@ -153,11 +153,10 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 
     # Generate, build and install toolbox
     set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	if(GTSAM_BUILD_STATIC_LIBRARY)
+        if(NOT BUILD_SHARED_LIBS)
-		list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
+                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-	endif()
+        endif()
 
     # Wrap
     wrap_and_install_library(../gtsam.h "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
 endif ()
-

gtsam/base/GenericValue.h

@@ -164,11 +164,11 @@ public:
 
   protected:
 
-    // implicit assignment operator for (const GenericValue& rhs) works fine here
     /// Assignment operator, protected because only the Value or DERIVED
     /// assignment operators should be used.
     GenericValue<T>& operator=(const GenericValue<T>& rhs) {
-      // Nothing to do, do not call base class assignment operator
+      Value::operator=(static_cast<Value const&>(rhs));
+      value_ = rhs.value_;
       return *this;
     }
 

gtsam/base/numericalDerivative.h

@@ -177,7 +177,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -202,7 +202,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::func
 //       "Template argument X1 must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
        "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1), x2, delta);
 }
 
 /** use a raw C++ function pointer */
@@ -230,7 +230,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
@@ -258,7 +258,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
@@ -286,7 +286,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
-  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1), x3, delta);
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3>
@@ -314,7 +314,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -341,7 +341,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
       "Template argument X2 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -368,7 +368,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
       "Template argument X3 must be a manifold type.");
-  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta);
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -395,7 +395,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
       "Template argument X4 must be a manifold type.");
-  return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta);
+  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4>
@@ -423,7 +423,7 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4, x5), x1, delta);
+  return numericalDerivative11<Y, X1>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -451,7 +451,7 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4, x5), x2, delta);
+  return numericalDerivative11<Y, X2>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -479,7 +479,7 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4, x5), x3, delta);
+  return numericalDerivative11<Y, X3>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -507,7 +507,7 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1, x5), x4, delta);
+  return numericalDerivative11<Y, X4>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -535,7 +535,7 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
       "Template argument Y must be a manifold type.");
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
       "Template argument X1 must be a manifold type.");
-  return numericalDerivative11<Y, X5>(boost::bind(h, x1, x2, x3, x4, _1), x5, delta);
+  return numericalDerivative11<Y, X5>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
 }
 
 template<class Y, class X1, class X2, class X3, class X4, class X5>
@@ -587,7 +587,7 @@ public:
       f_(f), x1_(x1), delta_(delta) {
   }
   Vector operator()(const X2& x2) {
-    return numericalGradient<X1>(boost::bind(f_, _1, x2), x1_, delta_);
+    return numericalGradient<X1>(boost::bind(f_, _1, boost::cref(x2)), x1_, delta_);
   }
 };
 
@@ -618,7 +618,7 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
 
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
+  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2)));
 
   return numericalDerivative11<Vector, X1>(
       boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -639,7 +639,7 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(f, x1, _1));
+  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1));
 
   return numericalDerivative11<Vector, X2>(
       boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -664,7 +664,7 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
   typedef typename internal::FixedSizeMatrix<X1>::type Vector;
   Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
       double) = &numericalGradient<X1>;
-  boost::function<double(const X1&)> f2(boost::bind(f, _1, x2, x3));
+  boost::function<double(const X1&)> f2(boost::bind(f, _1, boost::cref(x2), boost::cref(x3)));
 
   return numericalDerivative11<Vector, X1>(
       boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -687,7 +687,7 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
   typedef typename internal::FixedSizeMatrix<X2>::type Vector;
   Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
       double) = &numericalGradient<X2>;
-  boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
+  boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), _1, boost::cref(x3)));
 
   return numericalDerivative11<Vector, X2>(
       boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -710,7 +710,7 @@ inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
   typedef typename internal::FixedSizeMatrix<X3>::type Vector;
   Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
       double) = &numericalGradient<X3>;
-  boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
+  boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), _1));
 
   return numericalDerivative11<Vector, X3>(
       boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, _1, delta)),
@@ -731,7 +731,7 @@ inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X2>(
-      boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, x3)),
+      boost::function<double(const X1&, const X2&)>(boost::bind(f, _1, _2, boost::cref(x3))),
       x1, x2, delta);
 }
 
@@ -740,7 +740,7 @@ inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X1, X3>(
-      boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, x2, _2)),
+      boost::function<double(const X1&, const X3&)>(boost::bind(f, _1, boost::cref(x2), _2)),
       x1, x3, delta);
 }
 
@@ -749,7 +749,7 @@ inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
     boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
     const X2& x2, const X3& x3, double delta = 1e-5) {
   return numericalHessian212<X2, X3>(
-      boost::function<double(const X2&, const X3&)>(boost::bind(f, x1, _1, _2)),
+      boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), _1, _2)),
       x2, x3, delta);
 }
 

gtsam/base/serialization.h

@@ -24,7 +24,6 @@
 #include <string>
 
 // includes for standard serialization types
-#include <boost/serialization/export.hpp>
 #include <boost/serialization/optional.hpp>
 #include <boost/serialization/shared_ptr.hpp>
 #include <boost/serialization/vector.hpp>
@@ -39,6 +38,7 @@
 #include <boost/archive/xml_oarchive.hpp>
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/archive/binary_oarchive.hpp>
+#include <boost/serialization/export.hpp>
 
 namespace gtsam {
 

gtsam/geometry/Pose2.h

@@ -132,6 +132,8 @@ public:
    * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
    */
   Matrix3 AdjointMap() const;
+
+  /// Apply AdjointMap to twist xi
   inline Vector3 Adjoint(const Vector3& xi) const {
     return AdjointMap()*xi;
   }
@@ -141,6 +143,20 @@ public:
    */
   static Matrix3 adjointMap(const Vector3& v);
 
+  /**
+   * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
+   */
+  Vector3 adjoint(const Vector3& xi, const Vector3& y) {
+    return adjointMap(xi) * y;
+  }
+
+  /**
+   * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
+   */
+  Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
+    return adjointMap(xi).transpose() * y;
+  }
+
   /**
    * wedge for SE(2):
    * @param xi 3-dim twist (v,omega) where

gtsam/navigation/PreintegrationBase.h

@@ -27,6 +27,8 @@
 #include <gtsam/linear/NoiseModel.h>
 
 #include <iosfwd>
+#include <string>
+#include <utility>
 
 namespace gtsam {
 
@@ -61,7 +63,6 @@ class GTSAM_EXPORT PreintegrationBase {
   typedef PreintegrationParams Params;
 
  protected:
-
   /// Parameters. Declared mutable only for deprecated predict method.
   /// TODO(frank): make const once deprecated method is removed
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@@ -78,7 +79,10 @@ class GTSAM_EXPORT PreintegrationBase {
   /// Default constructor for serialization
   PreintegrationBase() {}
 
-public:
+  /// Virtual destructor for serialization
+  virtual ~PreintegrationBase() {}
+
+ public:
   /// @name Constructors
   /// @{
 
@@ -95,7 +99,7 @@ public:
   /// @name Basic utilities
   /// @{
   /// Re-initialize PreintegratedMeasurements
-  virtual void resetIntegration()=0;
+  virtual void resetIntegration() = 0;
 
   /// @name Basic utilities
   /// @{
@@ -129,10 +133,10 @@ public:
   const imuBias::ConstantBias& biasHat() const { return biasHat_; }
   double deltaTij() const { return deltaTij_; }
 
-  virtual Vector3  deltaPij() const=0;
+  virtual Vector3  deltaPij() const = 0;
-  virtual Vector3  deltaVij() const=0;
+  virtual Vector3  deltaVij() const = 0;
-  virtual Rot3     deltaRij() const=0;
+  virtual Rot3     deltaRij() const = 0;
-  virtual NavState deltaXij() const=0;
+  virtual NavState deltaXij() const = 0;
 
   // Exposed for MATLAB
   Vector6 biasHatVector() const { return biasHat_.vector(); }
@@ -147,20 +151,24 @@ public:
   /// @name Main functionality
   /// @{
 
-  /// Subtract estimate and correct for sensor pose
+  /** 
-  /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
+   * Subtract estimate and correct for sensor pose
-  /// Ignore D_correctedOmega_measuredAcc as it is trivially zero
+   * Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
+   * Ignore D_correctedOmega_measuredAcc as it is trivially zero
+   */
   std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
       const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
       OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
       OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
       OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
 
-  /// Update preintegrated measurements and get derivatives
+  /**
-  /// It takes measured quantities in the j frame
+   *  Update preintegrated measurements and get derivatives
-  /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
+   * It takes measured quantities in the j frame
+   * Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
+   */
   virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
-      const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
+      const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0;
 
   /// Version without derivatives
   virtual void integrateMeasurement(const Vector3& measuredAcc,
@@ -169,7 +177,7 @@ public:
   /// Given the estimate of the bias, return a NavState tangent vector
   /// summarizing the preintegrated IMU measurements so far
   virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
-      OptionalJacobian<9, 6> H = boost::none) const=0;
+      OptionalJacobian<9, 6> H = boost::none) const = 0;
 
   /// Predict state at time j
   NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
@@ -182,7 +190,10 @@ public:
                        OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
                        OptionalJacobian<9, 6> H3) const;
 
-  /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
+  /** 
+   * Compute errors w.r.t. preintegrated measurements and jacobians 
+   * wrt pose_i, vel_i, bias_i, pose_j, bias_j
+   */
   Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
       const Pose3& pose_j, const Vector3& vel_j,
       const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
@@ -202,8 +213,8 @@ public:
   /// @}
 #endif
 
-public:
+ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-} /// namespace gtsam
+}  /// namespace gtsam

gtsam/navigation/Scenario.h

@@ -24,12 +24,15 @@ namespace gtsam {
 /// Simple trajectory simulator.
 class Scenario {
  public:
+  /// virtual destructor
+  virtual ~Scenario() {}
+
   // Quantities a Scenario needs to specify:
 
-  virtual Pose3 pose(double t) const = 0;
+  virtual Pose3 pose(double t) const = 0;  ///< pose at time t
-  virtual Vector3 omega_b(double t) const = 0;
+  virtual Vector3 omega_b(double t) const = 0;  ///< angular velocity in body frame
-  virtual Vector3 velocity_n(double t) const = 0;
+  virtual Vector3 velocity_n(double t) const = 0;  ///< velocity at time t, in nav frame
-  virtual Vector3 acceleration_n(double t) const = 0;
+  virtual Vector3 acceleration_n(double t) const = 0;  ///< acceleration in nav frame
 
   // Derived quantities:
 

gtsam/nonlinear/ExpressionFactor.h

@@ -266,6 +266,13 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
   virtual Expression<T> expression() const {
     return expression(this->keys_[0], this->keys_[1]);
   }
+
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
+  }
 };
 // ExpressionFactor2
 

gtsam/nonlinear/Values.h

@@ -385,6 +385,17 @@ namespace gtsam {
     ConstFiltered<ValueType>
     filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
 
+    // Count values of given type \c ValueType
+    template<class ValueType>
+    size_t count() const {
+      size_t i = 0;
+      for (const auto& key_value : *this) {
+        if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
+          ++i;
+      }
+      return i;
+    }
+
   private:
     // Filters based on ValueType (if not Value) and also based on the user-
     // supplied \c filter function.

gtsam/nonlinear/internal/ExecutionTrace.h

@@ -35,7 +35,7 @@ template<int T> struct CallRecord;
 /// Storage type for the execution trace.
 /// It enforces the proper alignment in a portable way.
 /// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
-static const unsigned TraceAlignment = 16;
+static const unsigned TraceAlignment = 32;
 typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
 
 template<bool UseBlock, typename Derived>

gtsam/nonlinear/tests/testExpression.cpp

@@ -196,6 +196,7 @@ TEST(Expression, BinaryDimensions) {
 TEST(Expression, BinaryTraceSize) {
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t expectedTraceSize = sizeof(Binary::Record);
+  internal::upAlign(expectedTraceSize);
   EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize());
 }
 

gtsam/nonlinear/tests/testValues.cpp

@@ -383,6 +383,8 @@ TEST(Values, filter) {
     ++ i;
   }
   EXPECT_LONGS_EQUAL(2, (long)i);
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose3>());
+  EXPECT_LONGS_EQUAL(2, (long)values.count<Pose2>());
 
   // construct a values with the view
   Values actualSubValues2(pose_filtered);

gtsam/sam/BearingFactor.h

@@ -60,6 +60,14 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
     std::cout << s << "BearingFactor" << std::endl;
     Base::print(s, kf);
   }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
 };  // BearingFactor
 
 /// traits

gtsam/sam/BearingRangeFactor.h

@@ -73,6 +73,14 @@ class BearingRangeFactor
     Base::print(s, kf);
   }
 
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
 };  // BearingRangeFactor
 
 /// traits

gtsam/sam/RangeFactor.h

@@ -65,6 +65,14 @@ class RangeFactor : public ExpressionFactor2<T, A1, A2> {
     std::cout << s << "RangeFactor" << std::endl;
     Base::print(s, kf);
   }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
 };  // \ RangeFactor
 
 /// traits

gtsam/slam/dataset.cpp

@@ -432,23 +432,27 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
   fstream stream(filename.c_str(), fstream::out);
 
   // save 2D & 3D poses
-  Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
+  for (const auto& key_value : estimate) {
-  for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
+    auto p = dynamic_cast<const GenericValue<Pose2>*>(&key_value.value);
-    stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
+    if (!p) continue;
-        << key_value.value.y() << " " << key_value.value.theta() << endl;
+    const Pose2& pose = p->value();
+    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
+        << pose.y() << " " << pose.theta() << endl;
   }
 
-  Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
+  for(const auto& key_value: estimate) {
-  for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
+      auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
-      Point3 p = key_value.value.translation();
+      if (!p) continue;
-      Rot3 R = key_value.value.rotation();
+      const Pose3& pose = p->value();
-      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
+      Point3 t = pose.translation();
+      Rot3 R = pose.rotation();
+      stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "  << t.y() << " " << t.z()
         << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
         << " " << R.toQuaternion().w() << endl;
   }
 
   // save edges (2D or 3D)
-  for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
+  for(const auto& factor_: graph) {
     boost::shared_ptr<BetweenFactor<Pose2> > factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
     if (factor){
@@ -857,48 +861,47 @@ bool writeBAL(const string& filename, SfM_data &data) {
 
 bool writeBALfromValues(const string& filename, const SfM_data &data,
     Values& values) {
-
+  using Camera = PinholeCamera<Cal3Bundler>;
   SfM_data dataValues = data;
 
   // Store poses or cameras in SfM_data
-  Values valuesPoses = values.filter<Pose3>();
+  size_t nrPoses = values.count<Pose3>();
-  if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
+  if (nrPoses == dataValues.number_cameras()) { // we only estimated camera poses
     for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
       Key poseKey = symbol('x', i);
       Pose3 pose = values.at<Pose3>(poseKey);
       Cal3Bundler K = dataValues.cameras[i].calibration();
-      PinholeCamera<Cal3Bundler> camera(pose, K);
+      Camera camera(pose, K);
       dataValues.cameras[i] = camera;
     }
   } else {
-    Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
+    size_t nrCameras = values.count<Camera>();
-    if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
+    if (nrCameras == dataValues.number_cameras()) { // we only estimated camera poses and calibration
-      for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
+      for (size_t i = 0; i < nrCameras; i++) { // for each camera
         Key cameraKey = i; // symbol('c',i);
-        PinholeCamera<Cal3Bundler> camera =
+        Camera camera = values.at<Camera>(cameraKey);
-            values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
         dataValues.cameras[i] = camera;
       }
     } else {
       cout
-          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
+          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras "
           << dataValues.number_cameras() << ") and values (#cameras "
-          << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
+          << nrPoses << ", #poses " << nrCameras << ")!!"
           << endl;
       return false;
     }
   }
 
   // Store 3D points in SfM_data
-  Values valuesPoints = values.filter<Point3>();
+  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.number_tracks();
-  if (valuesPoints.size() != dataValues.number_tracks()) {
+  if (nrPoints != nrTracks) {
     cout
         << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
-        << dataValues.number_tracks() << ") and values (#points "
+        << nrTracks << ") and values (#points "
-        << valuesPoints.size() << ")!!" << endl;
+        << nrPoints << ")!!" << endl;
   }
 
-  for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point
+  for (size_t j = 0; j < nrTracks; j++) { // for each point
     Key pointKey = P(j);
     if (values.exists(pointKey)) {
       Point3 point = values.at<Point3>(pointKey);

gtsam_unstable/CMakeLists.txt

@@ -65,44 +65,40 @@ set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR})
 message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}")
 message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
 
-# build shared or static versions of the library
+# BUILD_SHARED_LIBS automatically defines static/shared libs:
-if (GTSAM_BUILD_STATIC_LIBRARY)
+add_library(gtsam_unstable ${gtsam_unstable_srcs})
-	message(STATUS "Building GTSAM_UNSTABLE - static")
+set_target_properties(gtsam_unstable PROPERTIES
-	add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs})
+  OUTPUT_NAME         gtsam_unstable
-	set_target_properties(gtsam_unstable PROPERTIES
+  CLEAN_DIRECT_OUTPUT 1
-		OUTPUT_NAME         gtsam_unstable
+  VERSION             ${gtsam_unstable_version}
-		CLEAN_DIRECT_OUTPUT 1
+  SOVERSION           ${gtsam_unstable_soversion})
-		VERSION             ${gtsam_unstable_version}
+target_link_libraries(gtsam_unstable
-		SOVERSION           ${gtsam_unstable_soversion})
+    PUBLIC
-	if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+    gtsam
+    ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
+
+if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
+	if (NOT BUILD_SHARED_LIBS)
 		set_target_properties(gtsam_unstable PROPERTIES
 			PREFIX "lib"
 			COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC)
-	endif()
+	else()
-	target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
-	install(TARGETS gtsam_unstable EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
-	list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
-	set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-else()
-    message(STATUS "Building GTSAM_UNSTABLE - shared")
-    add_library(gtsam_unstable SHARED ${gtsam_unstable_srcs})
-    set_target_properties(gtsam_unstable PROPERTIES
-        OUTPUT_NAME         gtsam_unstable
-        CLEAN_DIRECT_OUTPUT 1
-    	VERSION             ${gtsam_unstable_version}
-     	SOVERSION           ${gtsam_unstable_soversion})
-	if(WIN32)
 		set_target_properties(gtsam_unstable PROPERTIES
 			PREFIX ""
 			DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS
 			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
 	endif()
-    target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
-    install(TARGETS gtsam_unstable EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
-    list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
-    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 endif()
 
+install(
+	TARGETS gtsam_unstable
+	EXPORT GTSAM-exports
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	RUNTIME DESTINATION bin)
+list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
+set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
+
 # Wrap version for gtsam_unstable
 if (GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Set up codegen
@@ -110,9 +106,9 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
 
     # Generate, build and install toolbox
     set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
-	if(GTSAM_BUILD_STATIC_LIBRARY)
+        if(NOT BUILD_SHARED_LIBS)
-		list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
+                list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
-	endif()
+        endif()
 
     # Wrap
     wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")

gtsam_unstable/partition/GenericGraph.cpp

@@ -47,15 +47,15 @@ namespace gtsam { namespace partition {
           toErase.push_back(itFactor);  nrFactors--; continue;
         }
 
-        size_t label1 = dsf.findSet(key1.index);
+        size_t label1 = dsf.find(key1.index);
-        size_t label2 = dsf.findSet(key2.index);
+        size_t label2 = dsf.find(key2.index);
         if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }
 
         // merge two trees if the connection is strong enough, otherwise cache it
         // an odometry factor always merges two islands
         if (key1.type == NODE_POSE_2D && key2.type  == NODE_POSE_2D) {
           toErase.push_back(itFactor); nrFactors--;
-          dsf.makeUnionInPlace(label1, label2);
+          dsf.merge(label1, label2);
           succeed = true;
           break;
         }
@@ -64,7 +64,7 @@ namespace gtsam { namespace partition {
         if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
             (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
           toErase.push_back(itFactor); nrFactors--;
-          dsf.makeUnionInPlace(label1, label2);
+          dsf.merge(label1, label2);
           succeed = true;
           break;
         }
@@ -87,7 +87,7 @@ namespace gtsam { namespace partition {
           } else {
             toErase.push_back(itFactor); nrFactors--;
             toErase.push_back(itCached->second); nrFactors--;
-            dsf.makeUnionInPlace(label1, label2);
+            dsf.merge(label1, label2);
             connections.erase(itCached);
             succeed = true;
             break;
@@ -150,8 +150,8 @@ namespace gtsam { namespace partition {
         }
 
         if (graph.size() == 178765) cout << "kai22" << endl;
-        size_t label1 = dsf.findSet(key1.index);
+        size_t label1 = dsf.find(key1.index);
-        size_t label2 = dsf.findSet(key2.index);
+        size_t label2 = dsf.find(key2.index);
         if (label1 == label2) {  toErase.push_back(itFactor);  nrFactors--; continue; }
 
         if (graph.size() == 178765) cout << "kai23" << endl;
@@ -160,7 +160,7 @@ namespace gtsam { namespace partition {
         if ((key1.type == NODE_POSE_3D && key2.type  == NODE_LANDMARK_3D) ||
             (key1.type == NODE_POSE_3D && key2.type  == NODE_POSE_3D)) {
           toErase.push_back(itFactor); nrFactors--;
-          dsf.makeUnionInPlace(label1, label2);
+          dsf.merge(label1, label2);
           succeed = true;
           break;
         }

gtsam_unstable/slam/serialization.cpp

@@ -70,8 +70,8 @@ typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
 typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
 typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactorPosePoint2;
+typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
-typedef RangeFactor<Pose3, Point3>                      RangeFactorPosePoint3;
+typedef RangeFactor<Pose3, Point3>                      RangeFactor3D;
 typedef RangeFactor<Pose2, Pose2>                       RangeFactorPose2;
 typedef RangeFactor<Pose3, Pose3>                       RangeFactorPose3;
 typedef RangeFactor<CalibratedCamera, Point3>           RangeFactorCalibratedCameraPoint;
@@ -172,8 +172,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
 
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2");
+BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3");
+BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");

gtsampy.h

@@ -2258,8 +2258,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
   RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
 };
 
-typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
+typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
-typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
+typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
 typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;

matlab/+gtsam/Contents.m

@@ -142,8 +142,8 @@
 %   RangeFactorCalibratedCameraPoint  - class RangeFactorCalibratedCameraPoint, see Doxygen page for details
 %   RangeFactorPose2                  - class RangeFactorPose2, see Doxygen page for details
 %   RangeFactorPose3                  - class RangeFactorPose3, see Doxygen page for details
-%   RangeFactorPosePoint2             - class RangeFactorPosePoint2, see Doxygen page for details
+%   RangeFactor2D                     - class RangeFactor2D, see Doxygen page for details
-%   RangeFactorPosePoint3             - class RangeFactorPosePoint3, see Doxygen page for details
+%   RangeFactor3D                     - class RangeFactor3D, see Doxygen page for details
 %   RangeFactorSimpleCamera           - class RangeFactorSimpleCamera, see Doxygen page for details
 %   RangeFactorSimpleCameraPoint      - class RangeFactorSimpleCameraPoint, see Doxygen page for details
 %   VisualISAMGenerateData            - VisualISAMGenerateData creates data for viusalSLAM::iSAM examples

matlab/gtsam_examples/RangeISAMExample_plaza.m

@@ -125,7 +125,7 @@ for i=1:M % M
     j = TD(k,3);
     range = TD(k,4);
     if addRange
-      factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+      factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range);
       % Throw out obvious outliers based on current landmark estimates
       error=factor.unwhitenedError(landmarkEstimates);
       if k<=minK || abs(error)<5
@@ -146,14 +146,14 @@ for i=1:M % M
     end
     isam.update(newFactors, initial);
     result = isam.calculateEstimate();
-    lastPose = result.at(i);
+    lastPose = result.atPose2(i);
     % update landmark estimates
     if addRange
       landmarkEstimates = Values;
       for jj=1:size(TL,1)
         j=TL(jj,1);
         key = symbol('L',j);
-        landmarkEstimates.insert(key,result.at(key));
+        landmarkEstimates.insert(key,result.atPoint2(key));
       end
     end
     newFactors = NonlinearFactorGraph;

matlab/gtsam_examples/RangeSLAMExample_plaza.m

@@ -76,7 +76,7 @@ for i=1:M
   while k<=K && t>=TD(k,1)
     j = TD(k,3);
     range = TD(k,4);
-    factor = RangeFactorPosePoint2(i, symbol('L',j), range, noiseModels.range);
+    factor = RangeFactor2D(i, symbol('L',j), range, noiseModels.range);
     graph.add(factor);
     k=k+1;
   end

matlab/gtsam_examples/SBAExample.m

@@ -64,7 +64,7 @@ for i=1:size(truth.cameras,2)
     initialEstimate.insert(symbol('c',i), camera_i);
 end
 for j=1:size(truth.points,2)
-    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
     initialEstimate.insert(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));

matlab/gtsam_examples/SFMExample.m

@@ -58,7 +58,7 @@ for i=1:size(truth.cameras,2)
     initialEstimate.insert(symbol('x',i), pose_i);
 end
 for j=1:size(truth.points,2)
-    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
     initialEstimate.insert(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));

matlab/gtsam_tests/testJacobianFactor.m

@@ -54,7 +54,8 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13);
+unit2 = noiseModel.Unit.Create(2);
+expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,unit2);
 % check if the result matches
 CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4));
 

matlab/gtsam_tests/testSerialization.m

@@ -52,14 +52,26 @@ priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
-% Between Factors - FAIL: unregistered class
+% Between Factors
 odometry = Pose2(2.0, 0.0, 0.0);
 odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
  
-% BearingRange Factors - FAIL: unregistered class
+% Range Factors
+rNoise = noiseModel.Diagonal.Sigmas([0.2]);
+graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise));
+graph.add(RangeFactor2D(i2, j1, 2, rNoise));
+graph.add(RangeFactor2D(i3, j2, 2, rNoise));
+
+% Bearing Factors
 degrees = pi/180;
+bNoise = noiseModel.Diagonal.Sigmas([0.1]);
+graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
+graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
+graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));
+
+% BearingRange Factors
 brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));

matlab/unstable_examples/SmartRangeFactorExample.m

@@ -103,9 +103,9 @@ for ind_pose = 2:7
       r2 = curr_pose.range(lmk1); % range of lmk1 wrt x2
       srf1.addRange(key_curr, r2);
       
-      rangef1 = RangeFactorPosePoint2(key_prev, lmkKey(1), r1, noiseRange);
+      rangef1 = RangeFactor2D(key_prev, lmkKey(1), r1, noiseRange);
       fullGraph.add(rangef1); 
-      rangef2 = RangeFactorPosePoint2(key_curr, lmkKey(1), r2, noiseRange);
+      rangef2 = RangeFactor2D(key_curr, lmkKey(1), r2, noiseRange);
       fullGraph.add(rangef2); 
       
       if goodInitFlag_lmk1==1
@@ -123,9 +123,9 @@ for ind_pose = 2:7
       r4 = curr_pose.range(lmk2); % range of lmk2 wrt x3
       srf2.addRange(key_curr, r4);    
       
-      rangef3 = RangeFactorPosePoint2(key_curr, lmkKey(1), r3, noiseRange);
+      rangef3 = RangeFactor2D(key_curr, lmkKey(1), r3, noiseRange);
       fullGraph.add(rangef3); 
-      rangef4 = RangeFactorPosePoint2(key_curr, lmkKey(2), r4, noiseRange);
+      rangef4 = RangeFactor2D(key_curr, lmkKey(2), r4, noiseRange);
       % IF WE ADD FACTOR HERE IT CRASHES: fullGraph.add(rangef4);  
     %====================================================================  
     case 4
@@ -138,9 +138,9 @@ for ind_pose = 2:7
       
       % DELAYED INITIALIZATION: 
       fullGraph.add(rangef4); 
-      rangef5 = RangeFactorPosePoint2(key_curr, lmkKey(2), r5, noiseRange);
+      rangef5 = RangeFactor2D(key_curr, lmkKey(2), r5, noiseRange);
       fullGraph.add(rangef5); 
-      rangef6 = RangeFactorPosePoint2(key_curr, lmkKey(3), r6, noiseRange);
+      rangef6 = RangeFactor2D(key_curr, lmkKey(3), r6, noiseRange);
       % IF WE ADD FACTOR HERE IT CRASHES:  fullGraph.add(rangef6); 
       
       if goodInitFlag_lmk2==1
@@ -160,9 +160,9 @@ for ind_pose = 2:7
 
       % DELAYED INITIALIZATION: 
       fullGraph.add(rangef6); 
-      rangef7 = RangeFactorPosePoint2(key_curr, lmkKey(2), r7, noiseRange);
+      rangef7 = RangeFactor2D(key_curr, lmkKey(2), r7, noiseRange);
       fullGraph.add(rangef7); 
-      rangef8 = RangeFactorPosePoint2(key_curr, lmkKey(3), r8, noiseRange);
+      rangef8 = RangeFactor2D(key_curr, lmkKey(3), r8, noiseRange);
       fullGraph.add(rangef8); 
       
       if goodInitFlag_lmk3==1
@@ -176,7 +176,7 @@ for ind_pose = 2:7
       r9 = curr_pose.range(lmk3); % range of lmk3 wrt x6
       srf3.addRange(key_curr, r9);
       
-      rangef9 = RangeFactorPosePoint2(key_curr, lmkKey(3), r9, noiseRange);
+      rangef9 = RangeFactor2D(key_curr, lmkKey(3), r9, noiseRange);
       fullGraph.add(rangef9);
     case 7
       % x6-lmk3
@@ -184,7 +184,7 @@ for ind_pose = 2:7
       srf3.addRange(key_curr, r10);
       smartGraph.add(srf3);
       
-      rangef10 = RangeFactorPosePoint2(key_curr, lmkKey(3), r10, noiseRange);
+      rangef10 = RangeFactor2D(key_curr, lmkKey(3), r10, noiseRange);
       fullGraph.add(rangef10);
   end
   

tests/testSerializationSLAM.cpp

@@ -96,8 +96,8 @@ typedef NonlinearEquality<CalibratedCamera>  NonlinearEqualityCalibratedCamera;
 typedef NonlinearEquality<SimpleCamera>      NonlinearEqualitySimpleCamera;
 typedef NonlinearEquality<StereoCamera>      NonlinearEqualityStereoCamera;
 
-typedef RangeFactor<Pose2, Point2>                      RangeFactorPosePoint2;
+typedef RangeFactor<Pose2, Point2>                      RangeFactor2D;
-typedef RangeFactor<Pose3, Point3>                      RangeFactorPosePoint3;
+typedef RangeFactor<Pose3, Point3>                      RangeFactor3D;
 typedef RangeFactor<Pose2, Pose2>                       RangeFactorPose2;
 typedef RangeFactor<Pose3, Pose3>                       RangeFactorPose3;
 typedef RangeFactor<CalibratedCamera, Point3>           RangeFactorCalibratedCameraPoint;
@@ -204,8 +204,8 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqua
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
 
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2");
+BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
-BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3");
+BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
 BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
 BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
@@ -378,8 +378,8 @@ TEST (testSerializationSLAM, factors) {
   NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera);
   NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera);
 
-  RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1);
+  RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1);
-  RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1);
+  RangeFactor3D rangeFactor3D(a09, a05, 2.0, model1);
   RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
   RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
   RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
@@ -439,8 +439,8 @@ TEST (testSerializationSLAM, factors) {
   graph += nonlinearEqualitySimpleCamera;
   graph += nonlinearEqualityStereoCamera;
 
-  graph += rangeFactorPosePoint2;
+  graph += rangeFactor2D;
-  graph += rangeFactorPosePoint3;
+  graph += rangeFactor3D;
   graph += rangeFactorPose2;
   graph += rangeFactorPose3;
   graph += rangeFactorCalibratedCameraPoint;
@@ -505,8 +505,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsObj<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsObj<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
-  EXPECT(equalsObj<RangeFactorPosePoint2>(rangeFactorPosePoint2));
+  EXPECT(equalsObj<RangeFactor2D>(rangeFactor2D));
-  EXPECT(equalsObj<RangeFactorPosePoint3>(rangeFactorPosePoint3));
+  EXPECT(equalsObj<RangeFactor3D>(rangeFactor3D));
   EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
@@ -571,8 +571,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsXML<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsXML<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
-  EXPECT(equalsXML<RangeFactorPosePoint2>(rangeFactorPosePoint2));
+  EXPECT(equalsXML<RangeFactor2D>(rangeFactor2D));
-  EXPECT(equalsXML<RangeFactorPosePoint3>(rangeFactorPosePoint3));
+  EXPECT(equalsXML<RangeFactor3D>(rangeFactor3D));
   EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
@@ -637,8 +637,8 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<NonlinearEqualitySimpleCamera>(nonlinearEqualitySimpleCamera));
   EXPECT(equalsBinary<NonlinearEqualityStereoCamera>(nonlinearEqualityStereoCamera));
 
-  EXPECT(equalsBinary<RangeFactorPosePoint2>(rangeFactorPosePoint2));
+  EXPECT(equalsBinary<RangeFactor2D>(rangeFactor2D));
-  EXPECT(equalsBinary<RangeFactorPosePoint3>(rangeFactorPosePoint3));
+  EXPECT(equalsBinary<RangeFactor3D>(rangeFactor3D));
   EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
   EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
   EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));

tests/testVisualISAM2.cpp

@@ -0,0 +1,127 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testVisualISAM2.cpp
+ * @brief   Test convergence of visualSLAM example.
+ * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <examples/SFMdata.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(testVisualISAM2, all)
+{
+    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+    auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+    // Create ground truth data
+    vector<Point3> points = createPoints();
+    vector<Pose3> poses = createPoses();
+
+    // Set the parameters
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.01;
+    parameters.relinearizeSkip = 1;
+    ISAM2 isam(parameters);
+
+    // Create a Factor Graph and Values to hold the new data
+    NonlinearFactorGraph graph;
+    Values initialEstimate;
+
+    // Loop over the poses, adding the observations to iSAM incrementally
+    for (size_t i = 0; i < poses.size(); ++i)
+    {
+        // Add factors for each landmark observation
+        for (size_t j = 0; j < points.size(); ++j)
+        {
+            SimpleCamera camera(poses[i], *K);
+            Point2 measurement = camera.project(points[j]);
+            graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
+                measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+        }
+
+        // Add an initial guess for the current pose
+        // Intentionally initialize the variables off from the ground truth
+        static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                Point3(0.05, -0.10, 0.20));
+        initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
+
+        // Treat first iteration as special case
+        if (i == 0)
+        {
+            // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
+            static auto kPosePrior = noiseModel::Diagonal::Sigmas(
+                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                    .finished());
+            graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
+                                                     kPosePrior);
+
+            // Add a prior on landmark l0
+            static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+            graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
+                                                      kPointPrior);
+
+            // Add initial guesses to all observed landmarks
+            // Intentionally initialize the variables off from the ground truth
+            static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+            for (size_t j = 0; j < points.size(); ++j)
+                initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
+        }
+        else
+        {
+            // Update iSAM with the new factors
+            isam.update(graph, initialEstimate);
+
+            // Do an extra update to converge withing these 8 iterations
+            isam.update();
+
+            // Optimize
+            Values currentEstimate = isam.calculateEstimate();
+
+            // reset for next iteration
+            graph.resize(0);
+            initialEstimate.clear();
+        }
+    } // for loop
+
+    auto result = isam.calculateEstimate();
+    EXPECT_LONGS_EQUAL(16, result.size());
+    for (size_t j = 0; j < points.size(); ++j)
+    {
+        Symbol key('l', j);
+        EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
+    }
+}
+
+/* ************************************************************************* */
+int main()
+{
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

wrap/Module.cpp

@@ -127,7 +127,7 @@ void Module::parseMarkup(const std::string& data) {
   TemplateInstantiationTypedef singleInstantiation, singleInstantiation0;
   TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList);
 
-  // typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
+  // typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
   TypeGrammar instantiationClass_g(singleInstantiation.class_);
   Rule templateSingleInstantiation_p =
     (str_p("typedef") >> instantiationClass_g >>
@@ -273,9 +273,9 @@ void Module::generate_matlab_wrapper(const string& toolboxPath) const {
 
   // Include boost.serialization archive headers before other class headers
   if (hasSerialiable) {
-    wrapperFile.oss << "#include <boost/serialization/export.hpp>\n";
     wrapperFile.oss << "#include <boost/archive/text_iarchive.hpp>\n";
-    wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n\n";
+    wrapperFile.oss << "#include <boost/archive/text_oarchive.hpp>\n";
+    wrapperFile.oss << "#include <boost/serialization/export.hpp>\n\n";
   }
 
   // Generate includes while avoiding redundant includes

wrap/tests/expected/geometry_wrapper.cpp

@@ -1,9 +1,9 @@
 #include <wrap/matlab.h>
 #include <map>
 
-#include <boost/serialization/export.hpp>
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
+#include <boost/serialization/export.hpp>
 
 #include <folder/path/to/Test.h>
 #include <gtsam/geometry/Point2.h>