Merge pull request #119 from borglab/release/4.0.2

Release version 4.0.2

.github/ISSUE_TEMPLATE/bug-report.md

@@ -0,0 +1,35 @@
+---
+name: "Bug Report"
+about: Submit a bug report to help us improve GTSAM
+---
+
+<!--Please only submit issues/bug reports that come with enough information to reproduce them, ideally a unit test that fails, and possible ideas on what might be wrong. -->
+
+<!-- Even better yet, fix the bug and/or documentation, add a unit test, and create a pull request! -->
+
+<!-- This is a channel to report bugs/issues, not a support channel to help install/use/debug your own code. We'd love to help, but just don't have the bandwidth. Please post questions in the GTSAM Google group (https://groups.google.com/forum/#!forum/gtsam-users) -->
+
+## Description
+
+<!-- A clear description of the bug -->
+
+## Steps to reproduce
+
+1. 
+2.
+
+<!-- If you have a code sample, unit test, error messages, stack traces, etc., please provide it here as well -->
+
+## Expected behavior
+
+<!-- A clear and concise description of what you expected to happen. -->
+
+## Environment
+
+<!-- Please provide information about your code environment, things such as OS, language of use (C++, Matlab or Python), version of dependent libraries if using a custom build etc., anything to provide us more information. -->
+
+<!-- The more the information, the faster we can help resolve the issue -->
+
+## Additional information
+
+<!-- Add any other infor or context about the problem here. -->

.github/ISSUE_TEMPLATE/feature-request.md

@@ -0,0 +1,24 @@
+---
+name: "Feature Request"
+about: Submit a proposal/request for a new GTSAM feature
+---
+
+## Feature
+
+<!-- A clear and concise description of the feature proposal -->
+
+## Motivation
+
+<!-- Please outline the motivation for the proposal. Is your feature request related to a problem? e.g., I'm always frustrated when [...]. If this is related to another GitHub issue, please link here too. -->
+
+## Pitch
+
+<!-- A clear and concise description of what you want to happen. -->
+
+## Alternatives
+
+<!-- A clear and concise description of any alternative solutions or features you've considered, if any. -->
+
+## Additional context
+
+<!-- Add any other context or screenshots about the feature request here. -->

.github/ISSUE_TEMPLATE/questions-help-support.md

@@ -0,0 +1,5 @@
+---
+name: "Questions/Help/Support"
+---
+
+Please post questions and support requests in the [GTSAM Google group](https://groups.google.com/forum/#!forum/gtsam-users) and not on Github.

.travis.sh

@@ -0,0 +1,85 @@
+#!/bin/bash
+
+# common tasks before either build or test
+function prepare ()
+{
+  set -e   # Make sure any error makes the script to return an error code
+  set -x   # echo
+
+  SOURCE_DIR=`pwd`
+  BUILD_DIR=build
+
+  #env
+  git clean -fd || true
+  rm -fr $BUILD_DIR || true
+  mkdir $BUILD_DIR && cd $BUILD_DIR
+
+  if [ -z "$CMAKE_BUILD_TYPE" ]; then
+    CMAKE_BUILD_TYPE=Debug
+  fi
+
+  if [ -z "$GTSAM_ALLOW_DEPRECATED_SINCE_V4" ]; then
+    GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
+  fi
+
+  if [ ! -z "$GCC_VERSION" ]; then
+    sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-$GCC_VERSION 60 \
+                         --slave /usr/bin/g++ g++ /usr/bin/g++-$GCC_VERSION
+    sudo update-alternatives --set gcc /usr/bin/gcc-$GCC_VERSION
+  fi
+}
+
+# common tasks after either build or test
+function finish ()
+{
+  # Print ccache stats
+  ccache -s
+
+  cd $SOURCE_DIR
+}
+
+# compile the code with the intent of populating the cache
+function build ()
+{
+  prepare
+
+  cmake $SOURCE_DIR \
+      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
+      -DGTSAM_BUILD_TESTS=OFF \
+      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=ON \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4
+
+  # Actual build:
+  VERBOSE=1 make -j2
+
+  finish
+}
+
+# run the tests
+function test ()
+{
+  prepare
+
+  cmake $SOURCE_DIR \
+      -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE \
+      -DGTSAM_BUILD_TESTS=ON \
+      -DGTSAM_BUILD_UNSTABLE=$GTSAM_BUILD_UNSTABLE \
+      -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
+
+  # Actual build:
+  make -j2 check
+
+  finish
+}
+
+# select between build or test
+case $1 in
+  -b)
+    build
+    ;;
+  -t)                      
+    test
+    ;;
+esac

.travis.yml

@@ -0,0 +1,113 @@
+language: cpp
+cache: ccache
+sudo: required
+dist: xenial
+
+addons:
+  apt:
+    sources:
+    - ubuntu-toolchain-r-test
+    packages:
+    - g++-8
+    - clang-3.8
+    - build-essential
+    - pkg-config
+    - cmake
+    - libpython-dev python-numpy
+    - libboost-all-dev
+
+# before_install:
+#  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update          ; fi
+
+install:
+  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache  ; fi
+  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi
+
+# We first do the compile stage specified below, then the matrix expansion specified after.
+stages:
+  - compile
+  - test
+
+# Compile stage without building examples/tests to populate the caches.
+jobs:
+  include:
+# on Mac, GCC
+  - stage: compile
+    os: osx
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
+  - stage: compile
+    os: osx
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
+# on Mac, CLANG
+  - stage: compile
+    os: osx
+    compiler: clang
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
+  - stage: compile
+    os: osx
+    compiler: clang
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
+# on Linux, GCC
+  - stage: compile
+    os: linux
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
+  - stage: compile
+    os: linux
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
+# on Linux, CLANG
+  - stage: compile
+    os: linux
+    compiler: clang
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    script: bash .travis.sh -b
+  - stage: compile
+    os: linux
+    compiler: clang
+    env: CMAKE_BUILD_TYPE=Release
+    script: bash .travis.sh -b
+# on Linux, with deprecated ON to make sure that path still compiles
+  - stage: compile
+    os: linux
+    compiler: clang
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
+    script: bash .travis.sh -b
+
+# Matrix configuration:
+os:
+  - osx
+  - linux
+compiler:
+  - gcc
+  - clang
+env:
+  global:
+    - MAKEFLAGS="-j2"
+    - CCACHE_SLOPPINESS=pch_defines,time_macros
+    - GTSAM_ALLOW_DEPRECATED_SINCE_V4=OFF
+    - GTSAM_BUILD_UNSTABLE=ON
+  matrix:
+    - CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    - CMAKE_BUILD_TYPE=Release
+script:
+  - bash .travis.sh -t
+
+matrix:
+  exclude:
+    # Exclude g++ debug on Linux as it consistently times out
+    - os: linux
+      compiler: gcc
+      env : CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
+    # Exclude clang on Linux/clang in release until issue #57 is solved
+    - os: linux
+      compiler: clang
+      env : CMAKE_BUILD_TYPE=Release

CMakeLists.txt

@@ -1,4 +1,3 @@
-
 project(GTSAM CXX C)
 cmake_minimum_required(VERSION 3.0)
 
@@ -47,6 +46,17 @@ else()
     set(GTSAM_UNSTABLE_AVAILABLE 0)
 endif()
 
+# ----------------------------------------------------------------------------
+#   Uninstall target, for "make uninstall"
+# ----------------------------------------------------------------------------
+configure_file(
+  "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
+  "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
+  IMMEDIATE @ONLY)
+
+add_custom_target(uninstall
+  "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")
+
 
 ###############################################################################
 # Set up options
@@ -65,13 +75,26 @@ option(GTSAM_WITH_EIGEN_MKL              "Eigen will use Intel MKL if available"
 option(GTSAM_WITH_EIGEN_MKL_OPENMP       "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_ALLOW_DEPRECATED_SINCE_V4   "Allow use of methods/functions deprecated in GTSAM 4" ON)
-option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
+option(GTSAM_TYPEDEF_POINTS_TO_VECTORS   "Typedef 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()
 
+if(NOT MSVC AND NOT XCODE_VERSION)
+  # Set the build type to upper case for downstream use
+  string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER)
+
+  # Set the GTSAM_BUILD_TAG variable.
+  # If build type is Release, set to blank (""), else set to the build type.
+  if("${CMAKE_BUILD_TYPE_UPPER}" STREQUAL "RELEASE")
+   set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory
+  else()
+   set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}")
+  endif()
+endif()
+
 # Options relating to MATLAB wrapper
 # TODO: Check for matlab mex binary before handling building of binaries
 option(GTSAM_INSTALL_MATLAB_TOOLBOX      "Enable/Disable installation of matlab toolbox"  OFF)
@@ -84,10 +107,7 @@ if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_
 	message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
 endif()
 if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES)
-    set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE}_POSTFIX})
+		set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
-    if(NOT "${CURRENT_POSTFIX}" STREQUAL "")
-        message(FATAL_ERROR "Cannot use executable postfixes with the matlab or cython wrappers. Please disable GTSAM_BUILD_TYPE_POSTFIXES")
-    endif()
 endif()
 if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
 	message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
@@ -123,20 +143,20 @@ if(MSVC)
 	# If we use Boost shared libs, disable auto linking.
 	# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
 	if(NOT Boost_USE_STATIC_LIBS)
-		list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
+		list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
 	endif()
 	# Virtual memory range for PCH exceeded on VS2015
 	if(MSVC_VERSION LESS 1910) # older than VS2017
-	  list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
+	  list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
 	endif()
 endif()
 
-# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() 
+# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT()
-# or explicit instantiation will generate build errors. 
+# or explicit instantiation will generate build errors.
 # See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017
 #
 if(MSVC AND BUILD_SHARED_LIBS)
-	list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
+	list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
 endif()
 
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
@@ -159,34 +179,30 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
 # so we downgraded this to classic filenames-based variables, and manually adding
 # the target_include_directories(xxx ${Boost_INCLUDE_DIR})
 set(GTSAM_BOOST_LIBRARIES
-  optimized
+  optimized ${Boost_SERIALIZATION_LIBRARY_RELEASE}
-    ${Boost_SERIALIZATION_LIBRARY_RELEASE}
+  optimized ${Boost_SYSTEM_LIBRARY_RELEASE}
-    ${Boost_SYSTEM_LIBRARY_RELEASE}
+  optimized ${Boost_FILESYSTEM_LIBRARY_RELEASE}
-    ${Boost_FILESYSTEM_LIBRARY_RELEASE}
+  optimized ${Boost_THREAD_LIBRARY_RELEASE}
-    ${Boost_THREAD_LIBRARY_RELEASE}
+  optimized ${Boost_DATE_TIME_LIBRARY_RELEASE}
-    ${Boost_DATE_TIME_LIBRARY_RELEASE}
+  optimized ${Boost_REGEX_LIBRARY_RELEASE}
-    ${Boost_REGEX_LIBRARY_RELEASE}
+  debug ${Boost_SERIALIZATION_LIBRARY_DEBUG}
-  debug
+  debug ${Boost_SYSTEM_LIBRARY_DEBUG}
-    ${Boost_SERIALIZATION_LIBRARY_DEBUG}
+  debug ${Boost_FILESYSTEM_LIBRARY_DEBUG}
-    ${Boost_SYSTEM_LIBRARY_DEBUG}
+  debug ${Boost_THREAD_LIBRARY_DEBUG}
-    ${Boost_FILESYSTEM_LIBRARY_DEBUG}
+  debug ${Boost_DATE_TIME_LIBRARY_DEBUG}
-    ${Boost_THREAD_LIBRARY_DEBUG}
+  debug ${Boost_REGEX_LIBRARY_DEBUG}
-    ${Boost_DATE_TIME_LIBRARY_DEBUG}
-    ${Boost_REGEX_LIBRARY_DEBUG}
 )
 message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}")
 if (GTSAM_DISABLE_NEW_TIMERS)
     message("WARNING:  GTSAM timing instrumentation manually disabled")
-    list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
+    list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
 else()
     if(Boost_TIMER_LIBRARY)
       list(APPEND GTSAM_BOOST_LIBRARIES
-        optimized
+        optimized ${Boost_TIMER_LIBRARY_RELEASE}
-          ${Boost_TIMER_LIBRARY_RELEASE}
+        optimized ${Boost_CHRONO_LIBRARY_RELEASE}
-          ${Boost_CHRONO_LIBRARY_RELEASE}
+        debug ${Boost_TIMER_LIBRARY_DEBUG}
-        debug
+        debug ${Boost_CHRONO_LIBRARY_DEBUG}
-          ${Boost_TIMER_LIBRARY_DEBUG}
-          ${Boost_CHRONO_LIBRARY_DEBUG}
         )
     else()
       list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
@@ -197,7 +213,7 @@ endif()
 
 if(NOT (${Boost_VERSION} LESS 105600))
 	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
-	list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
+	list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
 endif()
 
 ###############################################################################
@@ -265,7 +281,7 @@ find_package(OpenMP)  # do this here to generate correct message if disabled
 if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
     if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
         set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+        list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS})
     endif()
 endif()
 
@@ -277,6 +293,7 @@ endif()
 ### See:  http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
 ###       http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
 option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
+option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF)
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
@@ -314,12 +331,35 @@ else()
 	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
 endif()
 
+# Detect Eigen version:
+set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h")
+if (EXISTS ${EIGEN_VER_H})
+	file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION)
+
+	# Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION  XX", etc...
+
+	string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}")
+	string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}")
+
+	string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}")
+	string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}")
+
+	string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}")
+	string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}")
+
+	set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}")
+
+	message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}")
+else()
+	message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`")
+endif ()
+
 if (MSVC)
 	if (BUILD_SHARED_LIBS)
 		# mute eigen static assert to avoid errors in shared lib
-		list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT)
+		list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
 	endif()
-	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
+	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 endif()
 
 ###############################################################################
@@ -361,28 +401,28 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
 endif()
 
 if(MSVC)
-	list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
+	list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
-	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
+	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
-	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
+	list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
 endif()
 
 # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
-    list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
   endif()
 endif()
 
 # As of XCode 7, clang also complains about this
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
   if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
-    list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
   endif()
 endif()
 
 if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
   # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h
-  list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS)
+  list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()
 
 ###############################################################################
@@ -480,6 +520,9 @@ message(STATUS "==============================================================="
 message(STATUS "================  Configuration Options  ======================")
 message(STATUS "  CMAKE_CXX_COMPILER_ID type     : ${CMAKE_CXX_COMPILER_ID}")
 message(STATUS "  CMAKE_CXX_COMPILER_VERSION     : ${CMAKE_CXX_COMPILER_VERSION}")
+message(STATUS "  CMake version                  : ${CMAKE_VERSION}")
+message(STATUS "  CMake generator                : ${CMAKE_GENERATOR}")
+message(STATUS "  CMake build tool               : ${CMAKE_BUILD_TOOL}")
 message(STATUS "Build flags                                               ")
 print_config_flag(${GTSAM_BUILD_TESTS}                 "Build Tests                    ")
 print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS}       "Build examples with 'make all' ")
@@ -492,18 +535,18 @@ print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in librar
 if(GTSAM_UNSTABLE_AVAILABLE)
     print_config_flag(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
 endif()
-string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
+
-print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE}     "Build for native architecture  ")
 if(NOT MSVC AND NOT XCODE_VERSION)
+    print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE}     "Build for native architecture  ")
     message(STATUS "  Build type                     : ${CMAKE_BUILD_TYPE}")
-    message(STATUS "  C compilation flags            : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
+    message(STATUS "  C compilation flags            : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
-    message(STATUS "  C++ compilation flags          : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
+    message(STATUS "  C++ compilation flags          : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
-endif()
-if(GTSAM_USE_SYSTEM_EIGEN)
-    message(STATUS "  Use System Eigen               : Yes")
-else()
-    message(STATUS "  Use System Eigen               : No")
 endif()
+
+print_build_options_for_target(gtsam)
+
+message(STATUS "  Use System Eigen               : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
+
 if(GTSAM_USE_TBB)
 	message(STATUS "  Use Intel TBB                  : Yes")
 elseif(TBB_FOUND)

DEVELOP

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

DEVELOP.md

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

README.md

@@ -1,8 +1,8 @@
-README - Georgia Tech Smoothing and Mapping library
+[![Build Status](https://travis-ci.com/borglab/gtsam.svg?branch=develop)](https://travis-ci.com/borglab/gtsam/)
-===================================================
 
-What is GTSAM?
+# README - Georgia Tech Smoothing and Mapping library
---------------
+
+## What is GTSAM?
 
 GTSAM is a library of C++ classes that implement smoothing and
 mapping (SAM) in robotics and vision, using factor graphs and Bayes
@@ -13,12 +13,11 @@ On top of the C++ library, GTSAM includes a MATLAB interface (enable
 GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
 is under development.
 
-Quickstart
+## Quickstart
-----------
 
 In the root library folder execute:
 
-```
+```sh
 #!bash
 $ mkdir build
 $ cd build
@@ -40,32 +39,33 @@ Optional prerequisites - used automatically if findable by CMake:
     - See [INSTALL.md](INSTALL.md) for more installation information
     - Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL.
 
-GTSAM 4 Compatibility
+## GTSAM 4 Compatibility
----------------------
 
 GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
 
 Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
 
-The Preintegrated IMU Factor
+## Wrappers
-----------------------------
+
+We provide support for [MATLAB](matlab/README.md) and [Python](cython/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
+
+## The Preintegrated IMU Factor
 
 GTSAM includes a state of the art IMU handling scheme based on
 
-- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
+- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. [[link]](https://ieeexplore.ieee.org/document/6092505)
 
 Our implementation improves on this using integration on the manifold, as detailed in
 
-- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
+- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. [[link]](https://ieeexplore.ieee.org/abstract/document/6907483)
-- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015.
+- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. [[link]](http://www.roboticsproceedings.org/rss11/p06.pdf)
 
 If you are using the factor in academic work, please cite the publications above.
 
 In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
 
 
-Additional Information
+## Additional Information
-----------------------
 
 There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
 

USAGE.md

@@ -1,48 +1,42 @@
-USAGE - Georgia Tech Smoothing and Mapping library
+# GTSAM USAGE
-===================================
-What is this file?
 
-	This file explains how to make use of the library for common SLAM tasks, 
+This file explains how to make use of the library for common SLAM tasks, using a visual SLAM implementation as an example.
-	using a visual SLAM implementation as an example.
+
+## Getting Started
+
+### Install
 	
-	
+Follow the installation instructions in the README file to build and install gtsam, as well as running tests to ensure the library is working properly.
-Getting Started
----------------------------------------------------
-Install:
-	Follow the installation instructions in the README file to build and 
-	install gtsam, as well as running tests to ensure the library is working
-	properly.
 
-Compiling/Linking with gtsam:
+### Compiling/Linking with GTSAM
-  	The installation creates a binary "libgtsam" at the installation prefix,
-  	and an include folder "gtsam".  These are the only required includes, but 
-  	the library has also been designed to make use of XML serialization through
-  	the Boost.serialization library, which requires the the Boost.serialization
-  	headers and binaries to be linked.  
-  	
-  	If you use CMake for your project, you can use the CMake scripts in the 
-  	cmake folder for finding GTSAM, CppUnitLite, and Wrap.  
 
-Examples:
+The installation creates a binary `libgtsam` at the installation prefix, and an include folder `gtsam`.  These are the only required includes, but the library has also been designed to make use of XML serialization through the `Boost.serialization` library, which requires the the Boost.serialization headers and binaries to be linked.  
-	To see how the library works, examine the unit tests provided.  
+
+If you use CMake for your project, you can use the CMake scripts in the cmake folder for finding `GTSAM`, `CppUnitLite`, and `Wrap`.  
+
+### Examples
+
+To see how the library works, examine the unit tests provided.  
  
+## Overview
 
-Overview
+The GTSAM library has three primary components necessary for the construction of factor graph representation and optimization which users will need to adapt to their particular problem.  
----------------------------------------------------
+
-The GTSAM library has three primary components necessary for the construction
+* FactorGraph
-of factor graph representation and optimization which users will need to 
+
-adapt to their particular problem.  
+	A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.
+
+* Values:
+
+	Values is a single object containing labeled values for all of the variables.  Currently, all variables are labeled with strings, but the type or organization of the variables can change.
+
+* Factors
 
-* FactorGraph:
-	A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.  
-* Values: 
-	Values is a single object containing labeled values for all of the variables.  Currently, all variables are labeled with strings, but the type or organization of the variables can change
-* Factors:
 	A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry.
 
 The library is organized according to the following directory structure:
 
-    3rdparty      local copies of third party libraries - Eigen3 and CCOLAMD
+    3rdparty      local copies of third party libraries e.g. Eigen3 and CCOLAMD
     base          provides some base Math and data structures, as well as test-related utilities
     geometry      points, poses, tensors, etc
     inference     core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees 

Using-GTSAM-EXPORT.md

@@ -0,0 +1,37 @@
+# Using GTSAM_EXPORT
+
+To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL.  However, there are several intricacies that make this more difficult than it sounds.  In general, if you follow the following three rules, GTSAM_EXPORT should work properly.  The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules.
+
+## Usage Rules
+1.  Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file.
+2.  Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if:
+    * At least one of the functions inside that class is declared in a .cpp file and not just the .h file.
+    * You can `GTSAM_EXPORT` any class it inherits from as well.  (Note that this implictly requires the class does not derive from a "header-only" class.  Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) 
+3.  If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations.  (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
+
+## When is GTSAM_EXPORT being used incorrectly
+Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library.  For example, an error in gtsam/base will often show up when compiling the check_base_program or the MATLAB wrapper, but not when compiling/linking gtsam itself.  The most common errors will say something like:
+
+```
+Error	LNK2019	unresolved external symbol "public: void __cdecl gtsam::SO3::print(class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)const " (?print@SO3@gtsam@@QEBAXAEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z) referenced in function "public: static void __cdecl gtsam::Testable<class gtsam::SO3>::Print(class gtsam::SO3 const &,class std::basic_string<char,struct std::char_traits<char>,class std::allocator<char> > const &)" (?Print@?$Testable@VSO3@gtsam@@@gtsam@@SAXAEBVSO3@2@AEBV?$basic_string@DU?$char_traits@D@std@@V?$allocator@D@2@@std@@@Z)	check_geometry_program	C:\AFIT\lib\gtsam\build\gtsam\geometry\tests\testSO3.obj
+```
+
+Let's analyze this error statement.  First, there is an unresolved symbol `gtsam::SO3::print`.  This can occur because _either_ `GTSAM_EXPORT` was not added to the print function definition when it should have been, _OR_ because `GTSAM_EXPORT` was added to the print function definition when it is fully declared in the header.  This error was detected while compiling `check_geometry_program` and pulling in `...\testSO3.obj`.  Specifically, within the function call `gtsam::Testable<class gtsam::SO3>::Print (...)`.  Note that this error did _not_ occur when compiling the library that actually has SO3 inside of it.
+
+## But Why?
+I believe that how the compiler/linker interacts with GTSAM_EXPORT can be explained by understanding the rules that MSVC operates under.  
+
+But first, we need to understand exactly what `GTSAM_EXPORT` is.  `GTSAM_EXPORT` is a `#define` macro that is created by CMAKE when GTSAM is being compiled on a Windows machine.  Inside the GTSAM project, GTSAM export will be set to a "dllexport" command.  A "dllexport" command tells the compiler that this function should go into the DLL and be visible externally.  In any other library, `GTSAM_EXPORT`  will be set to a "dllimport" command, telling the linker it should find this function in a DLL somewhere.  This leads to the first rule the compiler uses.  (Note that I say "compiler rules" when the rules may actually be in the linker, but I am conflating the two terms here when I speak of the "compiler rules".) 
+
+***Compiler Rule #1*** If a `dllimport` command is used in defining a function or class, that function or class _must_ be found in a DLL.
+
+Rule #1 doesn't seem very bad, until you combine it with rule #2
+
+***Compiler Rule #2*** Anything declared in a header file is not included in a DLL.
+
+When these two rules are combined, you get some very confusing results.  For example, a class which is completely defined in a header (e.g. LieMatrix) cannot use `GTSAM_EXPORT` in its definition.  If LieMatrix is defined with `GTSAM_EXPORT`, then the compiler _must_ find LieMatrix in a DLL.  Because LieMatrix is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error.  Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.
+
+Also note that when a class that you want to export inherits from another class that is not exportable, this can cause significant issues.  According to this [MSVC Warning page](https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warning-level-2-c4275?view=vs-2019), it may not strictly be a rule, but we have seen several linker errors when a class that is defined with `GTSAM_EXPORT` extended an Eigen class.  In general, it appears that any inheritance of non-exportable class by an exportable class is a bad idea.
+
+## Conclusion
+Hopefully, this little document clarifies when `GTSAM_EXPORT` should and should not be used whenever future GTSAM code is being written.  Following the usage rules above, we have been able to get all of the libraries, together with their test and wrapping libraries, to compile/link successfully.

bitbucket-pipelines.yml

@@ -1,15 +0,0 @@
-# Built from sample configuration for C++ – Make.
-# Check https://confluence.atlassian.com/x/5Q4SMw for more examples.
-# -----
-# Our custom docker image from Docker Hub as the build environment.
-image: dellaert/ubuntu-boost-tbb-eigen3:bionic
-
-pipelines:
-  default:
-    - step:
-        script: # Modify the commands below to build your repository.
-          - mkdir build
-          - cd build
-          - cmake -DGTSAM_USE_SYSTEM_EIGEN=OFF -DGTSAM_USE_EIGEN_MKL=OFF .. 
-          - make -j2
-          - make -j2 check

cmake/GtsamBuildTypes.cmake

@@ -1,7 +1,45 @@
 
+# function:  list_append_cache(var [new_values ...])
+# Like "list(APPEND ...)" but working for CACHE variables.
+# -----------------------------------------------------------
+function(list_append_cache var)
+  set(cur_value ${${var}})
+  list(APPEND cur_value ${ARGN})
+  get_property(MYVAR_DOCSTRING CACHE ${var} PROPERTY HELPSTRING)
+  set(${var} "${cur_value}" CACHE STRING "${MYVAR_DOCSTRING}" FORCE)
+endfunction()
+
+# function:  append_config_if_not_empty(TARGET_VARIABLE build_type)
+# Auxiliary function used to merge configuration-specific flags into the
+# global variables that will actually be send to cmake targets.
+# -----------------------------------------------------------
+function(append_config_if_not_empty TARGET_VARIABLE_ build_type)
+  string(TOUPPER "${build_type}" build_type_toupper)
+  set(flags_variable_name "${TARGET_VARIABLE_}_${build_type_toupper}")
+  set(flags_ ${${flags_variable_name}})
+  if (NOT "${flags_}" STREQUAL "")
+    if (${build_type_toupper} STREQUAL "COMMON")
+      # Special "COMMON" configuration type, just append without CMake expression:
+      list_append_cache(${TARGET_VARIABLE_} "${flags_}")
+    else()
+      # Regular configuration type:
+      list_append_cache(${TARGET_VARIABLE_} "$<$<CONFIG:${build_type}>:${flags_}>")
+    endif()
+  endif()
+endfunction()
+
+
 # Add install prefix to search path
 list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
 
+
+# Set up build types for MSVC and XCode
+set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
+      CACHE STRING "Build types available to MSVC and XCode")
+mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
+set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES} CACHE STRING "Build configurations" FORCE)
+
+
 # Default to Release mode
 if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
     set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
@@ -13,39 +51,79 @@ endif()
 # Add option for using build type postfixes to allow installing multiple build modes
 option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build type to the name of compiled libraries" ON)
 
-# Set custom compilation flags.
+# Define all cache variables, to be populated below depending on the OS/compiler:
-# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
+set(GTSAM_COMPILE_OPTIONS_PRIVATE        "" CACHE STRING "(Do not edit) Private compiler flags for all build configurations." FORCE)
-# so that we don't "pollute" the global variable namespace in the cmake cache.
+set(GTSAM_COMPILE_OPTIONS_PUBLIC         "" CACHE STRING "(Do not edit) Public compiler flags (exported to user projects) for all build configurations."  FORCE)
-  # Set all CMAKE_BUILD_TYPE flags:
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE    "" CACHE STRING "(Do not edit) Private preprocessor macros for all build configurations." FORCE)
-  # (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools)
+set(GTSAM_COMPILE_DEFINITIONS_PUBLIC     "" CACHE STRING "(Do not edit) Public preprocessor macros for all build configurations." FORCE)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PUBLIC)
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
+
+foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
+  string(TOUPPER "${build_type}" build_type_toupper)
+
+  # Define empty cache variables for "public". "private" are creaed below.
+  set(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}      "" CACHE STRING "(User editable) Public compiler flags (exported to user projects) for `${build_type_toupper}` configuration.")
+  set(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}  "" CACHE STRING "(User editable) Public preprocessor macros for `${build_type_toupper}` configuration.")
+endforeach()
+
+# Common preprocessor macros for each configuration:
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG           "_DEBUG;EIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "(User editable) Private preprocessor macros for Debug configuration.")
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO  "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for RelWithDebInfo configuration.")
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE         "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.")
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING       "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.")
+set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING          "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.")
 if(MSVC)
-    set(GTSAM_CMAKE_C_FLAGS                  "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
+  # Common to all configurations:
-    set(GTSAM_CMAKE_CXX_FLAGS                "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
+  list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE
-    set(GTSAM_CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
+    WINDOWS_LEAN_AND_MEAN
-    set(GTSAM_CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
+    NOMINMAX
-    set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO   "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
+	)
-    set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(GTSAM_CMAKE_C_FLAGS_RELEASE          "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_RELEASE        "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(GTSAM_CMAKE_C_FLAGS_PROFILING        "${GTSAM_CMAKE_C_FLAGS_RELEASE}   /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_PROFILING      "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(GTSAM_CMAKE_C_FLAGS_TIMING           "${GTSAM_CMAKE_C_FLAGS_RELEASE}   /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_TIMING         "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-else()
-    set(GTSAM_CMAKE_C_FLAGS                  "-std=c11   -Wall" CACHE STRING "Flags used by the compiler for all builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS                "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
-    set(GTSAM_CMAKE_C_FLAGS_DEBUG            "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_DEBUG          "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
-    set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO   "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
-    set(GTSAM_CMAKE_C_FLAGS_RELEASE          "   -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_RELEASE        "   -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
-    set(GTSAM_CMAKE_C_FLAGS_PROFILING        "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_PROFILING      "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
-    set(GTSAM_CMAKE_C_FLAGS_TIMING           "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
-    set(GTSAM_CMAKE_CXX_FLAGS_TIMING         "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
 endif()
 
+# Other (non-preprocessor macros) compiler flags:
+if(MSVC)
+  # Common to all configurations, next for each configuration:
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          /W3 /GR /EHsc /MP  CACHE STRING "(User editable) Private compiler flags for all configurations.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           /MDd /Zi /Ob0 /Od /RTC1  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  /MD /O2 /D /Zi /d2Zi+  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         /MD /O2  CACHE STRING "(User editable) Private compiler flags for Release configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING       /MD /O2  /Zi  CACHE STRING "(User editable) Private compiler flags for Profiling configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          /MD /O2  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
+else()
+  # Common to all configurations, next for each configuration:
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          -Wall CACHE STRING "(User editable) Private compiler flags for all configurations.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           -g -fno-inline  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  -g -O3  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         -O3  CACHE STRING "(User editable) Private compiler flags for Release configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING       -O3  CACHE STRING "(User editable) Private compiler flags for Profiling configuration.")
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          -g -O3  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
+endif()
+
+# Enable C++11:
+if (NOT CMAKE_VERSION VERSION_LESS 3.8)
+    set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_11" CACHE STRING "CMake compile features property for all gtsam targets.")
+    # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html
+    # This is to enable -std=c++11 instead of -std=g++11
+    set(CMAKE_CXX_EXTENSIONS OFF)
+else()
+  # Old cmake versions:
+  if (NOT MSVC)
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-std=c++11>)
+  endif()
+endif()
+
+# Merge all user-defined flags into the variables that are to be actually used by CMake:
+foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES})
+  append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PRIVATE ${build_type})
+  append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PUBLIC  ${build_type})
+  append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PRIVATE  ${build_type})
+  append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC   ${build_type})
+endforeach()
+
+# Linker flags:
 set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING  "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
 set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING  "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
 set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING     "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
@@ -54,26 +132,11 @@ set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEA
 set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
 set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING    "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
 
-mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING 
+mark_as_advanced(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING
                  GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING
-                 GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING 
+                 GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_ GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
                  GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
 
-# Apply the gtsam specific build flags as normal variables. This makes it so that they only
-# apply to the gtsam part of the build if gtsam is built as a subproject
-set(CMAKE_C_FLAGS                  "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}")
-set(CMAKE_CXX_FLAGS                "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}")
-set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}")
-set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}")
-set(CMAKE_C_FLAGS_RELWITHDEBINFO   "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}")
-set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
-set(CMAKE_C_FLAGS_RELEASE          "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}")
-set(CMAKE_CXX_FLAGS_RELEASE        "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}")
-set(CMAKE_C_FLAGS_PROFILING        "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}")
-set(CMAKE_CXX_FLAGS_PROFILING      "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}")
-set(CMAKE_C_FLAGS_TIMING           "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}")
-set(CMAKE_CXX_FLAGS_TIMING         "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}")
-
 set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
 set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})
 set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING})
@@ -86,14 +149,17 @@ set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING})
 if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
     # Apple Clang before 5.0 does not support -ftemplate-depth.
     if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0"))
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
+        list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-ftemplate-depth=1024")
     endif()
 endif()
 
-option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
+if (NOT MSVC)
-if(GTSAM_BUILD_WITH_MARCH_NATIVE)
+  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON)
-  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native")
+  if(GTSAM_BUILD_WITH_MARCH_NATIVE)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
+    # Add as public flag so all dependant projects also use it, as required
+    # by Eigen to avid crashes due to SIMD vectorization:
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")
+  endif()
 endif()
 
 # Set up build type library postfixes
@@ -118,12 +184,6 @@ if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
   endif()
 endif()
 
-# Set up build types for MSVC and XCode
-set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
-      CACHE STRING "Build types available to MSVC and XCode")
-mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
-set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
-
 # Check build types
 string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
 if(    NOT cmake_build_type_tolower STREQUAL ""
@@ -159,3 +219,20 @@ function(gtsam_assign_all_source_folders)
   gtsam_assign_source_folders("${all_c_srcs};${all_cpp_srcs};${all_headers}")
 endfunction()
 
+# Applies the per-config build flags to the given target (e.g. gtsam, wrap_lib)
+function(gtsam_apply_build_flags target_name_)
+  # To enable C++11: the use of target_compile_features() is preferred since
+  # it will be not in conflict with a more modern C++ standard, if used in a
+  # client program.
+  if (NOT "${GTSAM_COMPILE_FEATURES_PUBLIC}" STREQUAL "")
+  	target_compile_features(${target_name_} PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC})
+  endif()
+
+  target_compile_definitions(${target_name_} PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
+  target_compile_definitions(${target_name_} PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
+  if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "")
+    target_compile_options(${target_name_} PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC})
+  endif()
+  target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE})
+
+endfunction(gtsam_apply_build_flags)

cmake/GtsamCythonWrap.cmake

@@ -86,8 +86,13 @@ function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
   if(APPLE)
     set(link_flags "-undefined dynamic_lookup")
   endif()
-  set_target_properties(${target} PROPERTIES COMPILE_FLAGS "-w" LINK_FLAGS "${link_flags}"
+  set_target_properties(${target}
-    OUTPUT_NAME ${output_lib_we} PREFIX "" LIBRARY_OUTPUT_DIRECTORY ${output_dir})
+      PROPERTIES COMPILE_FLAGS "-w"
+      LINK_FLAGS "${link_flags}"
+      OUTPUT_NAME ${output_lib_we}
+      PREFIX ""
+      ${CMAKE_BUILD_TYPE_UPPER}_POSTFIX ""
+      LIBRARY_OUTPUT_DIRECTORY ${output_dir})
 endfunction()
 
 # Cythonize a pyx from the command line as described at
@@ -161,9 +166,13 @@ endfunction()
 function(install_cython_wrapped_library interface_header generated_files_path install_path)
   get_filename_component(module_name "${interface_header}" NAME_WE)
 
-    # NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name
+  # NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name
   # here prevents creating the top-level module name directory in the destination.
-  message(STATUS "Installing Cython Toolbox to ${install_path}") #${GTSAM_CYTHON_INSTALL_PATH}")
+  # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH/subdirectory if there is one
+  get_filename_component(location "${install_path}" PATH)
+  get_filename_component(name "${install_path}" NAME)
+  message(STATUS "Installing Cython Toolbox to ${location}${GTSAM_BUILD_TAG}/${name}") #${GTSAM_CYTHON_INSTALL_PATH}"
+
   if(GTSAM_BUILD_TYPE_POSTFIXES)
     foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
       string(TOUPPER "${build_type}" build_type_upper)
@@ -172,10 +181,8 @@ function(install_cython_wrapped_library interface_header generated_files_path in
       else()
         set(build_type_tag "${build_type}")
       endif()
-      # Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
+
-      get_filename_component(location "${install_path}" PATH)
+      install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}${build_type_tag}/${name}"
-      get_filename_component(name "${install_path}" NAME)
-      install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}/${name}${build_type_tag}"
           CONFIGURATIONS "${build_type}"
           PATTERN "build" EXCLUDE
           PATTERN "CMakeFiles" EXCLUDE

cmake/GtsamMatlabWrap.cmake

@@ -35,7 +35,11 @@ mark_as_advanced(FORCE MEX_COMMAND)
 # Now that we have mex, trace back to find the Matlab installation root
 get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
 get_filename_component(mex_path "${MEX_COMMAND}" PATH)
-get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
+if(mex_path MATCHES ".*/win64$")
+	get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
+else()
+	get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
+endif()
 set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
 
 
@@ -78,28 +82,29 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 			set(mexModuleExt mexw32)
 		endif()
 	endif()
-		
+
 	# Wrap codegen interface
     #usage: wrap interfacePath moduleName toolboxPath headerPath
     #  interfacePath : *absolute* path to directory of module interface file
     #  moduleName    : the name of the module, interface file must be called moduleName.h
     #  toolboxPath   : the directory in which to generate the wrappers
-    #  headerPath    : path to matlab.h 
+    #  headerPath    : path to matlab.h
-	
+
 	# Extract module name from interface header file name
 	get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
 	get_filename_component(modulePath "${interfaceHeader}" PATH)
 	get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-	
+
 	# Paths for generated files
 	set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
 	set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
 	set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
-	
+
 	message(STATUS "Building wrap module ${moduleName}")
-	
+
 	# Find matlab.h in GTSAM
-	if("${PROJECT_NAME}" STREQUAL "GTSAM")
+	if(("${PROJECT_NAME}" STREQUAL "gtsam") OR
+		("${PROJECT_NAME}" STREQUAL "gtsam_unstable"))
 		set(matlab_h_path "${PROJECT_SOURCE_DIR}")
 	else()
 		if(NOT GTSAM_INCLUDE_DIR)
@@ -221,6 +226,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 	string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
 	add_library(${moduleName}_matlab_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
 	target_link_libraries(${moduleName}_matlab_wrapper ${correctedOtherLibraries})
+	target_link_libraries(${moduleName}_matlab_wrapper ${moduleName})
 	set_target_properties(${moduleName}_matlab_wrapper PROPERTIES
 		OUTPUT_NAME              "${moduleName}_wrapper"
 		PREFIX                   ""

cmake/GtsamPrinting.cmake

@@ -7,4 +7,45 @@ function(print_config_flag flag msg)
     else ()
         message(STATUS "  ${msg}: Disabled")
     endif ()
-endfunction(print_config_flag)
+endfunction()
+
+# Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake
+function(string_pad RESULT_NAME DESIRED_LENGTH VALUE)
+    string(LENGTH "${VALUE}" VALUE_LENGTH)
+    math(EXPR REQUIRED_PADS "${DESIRED_LENGTH} - ${VALUE_LENGTH}")
+    set(PAD ${VALUE})
+    if(REQUIRED_PADS GREATER 0)
+        math(EXPR REQUIRED_MINUS_ONE "${REQUIRED_PADS} - 1")
+        foreach(FOO RANGE ${REQUIRED_MINUS_ONE})
+            set(PAD "${PAD} ")
+        endforeach()
+    endif()
+    set(${RESULT_NAME} "${PAD}" PARENT_SCOPE)
+endfunction()
+
+set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.")
+mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH)
+
+# Print "  var: ${var}" padding with spaces as needed
+function(print_padded variable_name)
+  string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}")
+  message(STATUS "${padded_prop}: ${${variable_name}}")
+endfunction()
+
+
+# Prints all the relevant CMake build options for a given target:
+function(print_build_options_for_target target_name_)
+  print_padded(GTSAM_COMPILE_FEATURES_PUBLIC)
+  print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE)
+  print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC)
+  print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
+  print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
+
+  foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
+    string(TOUPPER "${build_type}" build_type_toupper)
+    print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper})
+    print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper})
+    print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper})
+    print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper})
+  endforeach()
+endfunction()

cmake/GtsamTesting.cmake

@@ -8,7 +8,7 @@
 # Macro:
 #
 # gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)
-# 
+#
 # Add a group of unit tests.  A list of unit test .cpp files or glob patterns specifies the
 # tests to create.  Tests are assigned into a group name so they can easily by run
 # independently with a make target.  Running 'make check' builds and runs all tests.
@@ -35,7 +35,7 @@ endmacro()
 # Macro:
 #
 # gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)
-# 
+#
 # Add scripts that will serve as examples of how to use the library.  A list of files or
 # glob patterns is specified, and one executable will be created for each matching .cpp
 # file.  These executables will not be installed.  They are built with 'make all' if
@@ -60,7 +60,7 @@ endmacro()
 # Macro:
 #
 # gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries)
-# 
+#
 # Add scripts that time aspects of the library.  A list of files or
 # glob patterns is specified, and one executable will be created for each matching .cpp
 # file.  These executables will not be installed.  They are not built with 'make all',
@@ -88,29 +88,36 @@ enable_testing()
 
 option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)
 option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
-option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
+	option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
 
-# Add option for combining unit tests
+		# Add option for combining unit tests
-if(MSVC OR XCODE_VERSION)
+		if(MSVC OR XCODE_VERSION)
-	option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
+			option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
-else()
+		else()
-	option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
+			option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
-endif()
+		endif()
-mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
+		mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
 
-# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
+		# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
-if(GTSAM_BUILD_TESTS)
+		if(GTSAM_BUILD_TESTS)
-    add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+			add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
-    
+			# Also add alternative checks using valgrind.
-    # Add target to build tests without running
+			# We don't look for valgrind being installed in the system, since these
-	add_custom_target(all.tests)
+			# targets are not invoked unless directly instructed by the user.
-endif()
+			if (UNIX)
+				# Run all tests using valgrind:
+				add_custom_target(check_valgrind)
+			endif()
 
-# Add examples target
+			# Add target to build tests without running
-add_custom_target(examples)
+			add_custom_target(all.tests)
+		endif()
 
-# Add timing target
+		# Add examples target
-add_custom_target(timing)
+		add_custom_target(examples)
+
+		# Add timing target
+		add_custom_target(timing)
 
 
 # Implementations of this file's macros:
@@ -118,23 +125,24 @@ add_custom_target(timing)
 macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 	if(GTSAM_BUILD_TESTS)
 		# Add group target if it doesn't already exist
-	    if(NOT TARGET check.${groupName})
+		if(NOT TARGET check.${groupName})
 			add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
+			set_property(TARGET check.${groupName} PROPERTY FOLDER "Unit tests")
 		endif()
-	
-	    # Get all script files
-        file(GLOB script_files ${globPatterns})
 
-	    # Remove excluded scripts from the list
+		# Get all script files
-	    if(NOT "${excludedFiles}" STREQUAL "")
+		file(GLOB script_files ${globPatterns})
+
+		# Remove excluded scripts from the list
+		if(NOT "${excludedFiles}" STREQUAL "")
 			file(GLOB excludedFilePaths ${excludedFiles})
 			if("${excludedFilePaths}" STREQUAL "")
 				message(WARNING "The pattern '${excludedFiles}' for excluding tests from group ${groupName} did not match any files")
 			else()
-		    	list(REMOVE_ITEM script_files ${excludedFilePaths})
+				list(REMOVE_ITEM script_files ${excludedFilePaths})
 			endif()
-	    endif()
+		endif()
-	
+
 		# Separate into source files and headers (allows for adding headers to show up in
 		# MSVC and Xcode projects).
 		set(script_srcs "")
@@ -147,7 +155,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 				list(APPEND script_srcs ${script_file})
 			endif()
 		endforeach()
-	
+
 		# Don't put test files in folders in MSVC and Xcode because they're already grouped
 		source_group("" FILES ${script_srcs} ${script_headers})
 
@@ -156,26 +164,41 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			foreach(script_src IN ITEMS ${script_srcs})
 				# Get test base name
 				get_filename_component(script_name ${script_src} NAME_WE)
-			
+
 				# Add executable
 				add_executable(${script_name} ${script_src} ${script_headers})
 				target_link_libraries(${script_name} CppUnitLite ${linkLibraries})
-				
+
+				# Apply user build flags from CMake cache variables:
+				gtsam_apply_build_flags(${script_name})
+
 				# Add target dependencies
 				add_test(NAME ${script_name} COMMAND ${script_name})
 				add_dependencies(check.${groupName} ${script_name})
 				add_dependencies(check ${script_name})
-                add_dependencies(all.tests ${script_name})
+				add_dependencies(all.tests ${script_name})
 				if(NOT MSVC AND NOT XCODE_VERSION)
-				  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
+					# Regular test run:
+					add_custom_target(${script_name}.run
+						COMMAND ${EXECUTABLE_OUTPUT_PATH}${script_name}
+						DEPENDS ${script_name}
+					)
+
+					# Run with valgrind:
+					set(GENERATED_EXE "$<TARGET_FILE:${script_name}>")
+					add_custom_target(${script_name}.valgrind
+						COMMAND "valgrind" "--error-exitcode=1" ${GENERATED_EXE}
+						DEPENDS ${script_name}
+					)
+					add_dependencies(check_valgrind ${script_name}.valgrind)
 				endif()
-			
+
 				# Add TOPSRCDIR
-				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
-			
+
 				# Exclude from 'make all' and 'make install'
 				set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
-			
+
 				# Configure target folder (for MSVC and Xcode)
 				set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
 			endforeach()
@@ -188,16 +211,21 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 
 			# Default on MSVC and XCode - combine test group into a single exectuable
 			set(target_name check_${groupName}_program)
-		
+
 			# Add executable
 			add_executable(${target_name} "${script_srcs}" ${script_headers})
 			target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
-		
+
+			# Apply user build flags from CMake cache variables:
+			gtsam_apply_build_flags(${target_name})
+
+			set_property(TARGET check_${groupName}_program PROPERTY FOLDER "Unit tests")
+
 			# Only have a main function in one script - use preprocessor
 			set(rest_script_srcs ${script_srcs})
 			list(REMOVE_AT rest_script_srcs 0)
 			set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=inline no_main")
-		
+
 			# Add target dependencies
 			add_test(NAME ${target_name} COMMAND ${target_name})
 			add_dependencies(check.${groupName} ${target_name})
@@ -205,10 +233,10 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
 			if(NOT XCODE_VERSION)
 				add_dependencies(all.tests ${target_name})
 			endif()
-		
+
 			# Add TOPSRCDIR
-			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+			set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
-		
+
 			# Exclude from 'make all' and 'make install'
 			set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
 
@@ -220,18 +248,18 @@ endmacro()
 
 
 macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll)
-    # Get all script files
+	# Get all script files
-    file(GLOB script_files ${globPatterns})
+	file(GLOB script_files ${globPatterns})
 
-    # Remove excluded scripts from the list
+	# Remove excluded scripts from the list
-    if(NOT "${excludedFiles}" STREQUAL "")
+	if(NOT "${excludedFiles}" STREQUAL "")
 		file(GLOB excludedFilePaths ${excludedFiles})
 		if("${excludedFilePaths}" STREQUAL "")
 			message(WARNING "The script exclusion pattern '${excludedFiles}' did not match any files")
 		else()
-	    	list(REMOVE_ITEM script_files ${excludedFilePaths})
+			list(REMOVE_ITEM script_files ${excludedFilePaths})
 		endif()
-    endif()
+	endif()
 
 	# Separate into source files and headers (allows for adding headers to show up in
 	# MSVC and Xcode projects).
@@ -257,18 +285,21 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
 		# Add executable
 		add_executable(${script_name} ${script_src} ${script_headers})
 		target_link_libraries(${script_name} ${linkLibraries})
-	
+
+		# Apply user build flags from CMake cache variables:
+		gtsam_apply_build_flags(${script_name})
+
 		# Add target dependencies
 		add_dependencies(${groupName} ${script_name})
 		if(NOT MSVC AND NOT XCODE_VERSION)
-		  add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
+			add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
 		endif()
 
 		# Add TOPSRCDIR
-		set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+		set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
 
-        # Exclude from all or not - note weird variable assignment because we're in a macro	
+		# Exclude from all or not - note weird variable assignment because we're in a macro
-	    set(buildWithAll_on ${buildWithAll})
+		set(buildWithAll_on ${buildWithAll})
 		if(NOT buildWithAll_on)
 			# Exclude from 'make all' and 'make install'
 			set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON)

cmake/README.md

@@ -1,5 +1,4 @@
-GTSAMCMakeTools
+# GTSAMCMakeTools
-===============
 
 This is the collection of GTSAM CMake tools that may be useful in external projects.  The way to use this collection is by first making a find_package call:
 
@@ -7,8 +6,7 @@ This is the collection of GTSAM CMake tools that may be useful in external proje
 
 which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable.  After that, you may include the files you would like to use.  These files and the functions they define are explained below.
 
-GtsamBuildTypes
+## GtsamBuildTypes
----------------
 
     include(GtsamBuildTypes)
 
@@ -17,8 +15,8 @@ Including this file immediately sets up the following build types and a drop-dow
 *   `Debug`
 *   `Release`
 *   `RelWithDebInfo`
-*   `Profiling`:  All optimizations enabled and minimal debug symbols
+*   `Profiling`: All optimizations enabled and minimal debug symbols
-*   `Timing`:  Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation
+*   `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation
 
 It also configures several minor details, as follows:
 
@@ -30,8 +28,7 @@ It defines the following functions:
 *   `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files.  Folders will be determined relative to the current source folder when this function is called.
 *   `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder.
 
-GtsamTesting
+## GtsamTesting
-------------
 
     include(GtsamTesting)
 
@@ -70,8 +67,7 @@ Defines two useful functions for creating CTest unit tests.  Also immediately cr
                        an empty string "" if nothing needs to be excluded.
         linkLibraries: The list of libraries to link to.
 
-GtsamMatlabWrap
+## GtsamMatlabWrap
----------------
 
     include(GtsamMatlabWrap)
     
@@ -97,8 +93,7 @@ Defines functions for generating MATLAB wrappers.  Also immediately creates seve
         extraMexFlags:    Any *additional* flags to pass to the compiler when building
                           the wrap code.  Normally, leave this empty.
 
-GtsamMakeConfigFile
+## GtsamMakeConfigFile
--------------------
 
     include(GtsamMakeConfigFile)
      

cmake/cmake_uninstall.cmake.in

@@ -0,0 +1,27 @@
+# -----------------------------------------------
+# File that provides "make uninstall" target
+#  We use the file 'install_manifest.txt'
+# -----------------------------------------------
+if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+  message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+foreach(file ${files})
+  message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
+  if(EXISTS "$ENV{DESTDIR}${file}")
+    exec_program(
+      "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\""
+      OUTPUT_VARIABLE rm_out
+      RETURN_VALUE rm_retval
+      )
+    if(NOT "${rm_retval}" STREQUAL 0)
+      message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
+    endif(NOT "${rm_retval}" STREQUAL 0)
+  else(EXISTS "$ENV{DESTDIR}${file}")
+    message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
+  endif(EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)
+
+

cmake/example_cmake_find_gtsam/CMakeLists.txt

@@ -0,0 +1,17 @@
+# This file shows how to build and link a user project against GTSAM using CMake
+###################################################################################
+# To create your own project, replace "example" with the actual name of your project
+cmake_minimum_required(VERSION 3.0)
+project(example CXX)
+
+# Find GTSAM, either from a local build, or from a Debian/Ubuntu package.
+find_package(GTSAM REQUIRED)
+
+add_executable(example
+  main.cpp
+)
+
+# By using CMake exported targets, a simple "link" dependency introduces the
+# include directories (-I) flags, links against Boost, and add any other
+# required build flags (e.g. C++11, etc.)
+target_link_libraries(example PRIVATE gtsam)

cmake/example_cmake_find_gtsam/main.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 Pose2SLAMExample.cpp
+ * @brief A 2D Pose SLAM example
+ * @date Oct 21, 2010
+ * @author Yong Dian Jian
+ */
+
+/**
+ * A simple 2D pose slam example
+ *  - The robot moves in a 2 meter square
+ *  - The robot moves 2 meters each step, turning 90 degrees after each step
+ *  - The robot initially faces along the X axis (horizontal, to the right in 2D)
+ *  - We have full odometry between pose
+ *  - We have a loop closure constraint when the robot returns to the first position
+ */
+
+// In planar SLAM example we use Pose2 variables (x, y, theta) to represent the robot poses
+#include <gtsam/geometry/Pose2.h>
+
+// We will use simple integer Keys to refer to the robot poses.
+#include <gtsam/inference/Key.h>
+
+// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
+// Here we will use Between factors for the relative motion described by odometry measurements.
+// We will also use a Between Factor to encode the loop closure constraint
+// Also, we will initialize the robot at the origin using a Prior factor.
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+// When the factors are created, we will add them to a Factor Graph. As the factors we are using
+// are nonlinear factors, we will need a Nonlinear Factor Graph.
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+// Finally, once all of the factors have been added to our factor graph, we will want to
+// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
+// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
+// a Gauss-Newton solver
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+
+// Once the optimized values have been calculated, we can also calculate the marginal covariance
+// of desired variables
+#include <gtsam/nonlinear/Marginals.h>
+
+// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
+// nonlinear functions around an initial linearization point, then solve the linear system
+// to update the linearization point. This happens repeatedly until the solver converges
+// to a consistent set of variable values. This requires us to specify an initial guess
+// for each variable, held in a Values container.
+#include <gtsam/nonlinear/Values.h>
+
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv) {
+
+  // 1. Create a factor graph container and add factors to it
+  NonlinearFactorGraph graph;
+
+  // 2a. Add a prior on the first pose, setting it to the origin
+  // A prior factor consists of a mean and a noise model (covariance matrix)
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
+
+  // For simplicity, we will use the same noise model for odometry and loop closures
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+
+  // 2b. Add odometry factors
+  // Create odometry (Between) factors between consecutive poses
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
+
+  // 2c. Add the loop closure constraint
+  // This factor encodes the fact that we have returned to the same pose. In real systems,
+  // these constraints may be identified in many ways, such as appearance-based techniques
+  // with camera images. We will use another Between Factor to enforce this constraint:
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
+  graph.print("\nFactor Graph:\n"); // print
+
+  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+  // For illustrative purposes, these have been deliberately set to incorrect values
+  Values initialEstimate;
+  initialEstimate.insert(1, Pose2(0.5, 0.0,  0.2   ));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
+  initialEstimate.print("\nInitial Estimate:\n"); // print
+
+  // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
+  // The optimizer accepts an optional set of configuration parameters,
+  // controlling things like convergence criteria, the type of linear
+  // system solver to use, and the amount of information displayed during
+  // optimization. We will set a few parameters as a demonstration.
+  GaussNewtonParams parameters;
+  // Stop iterating once the change in error between steps is less than this value
+  parameters.relativeErrorTol = 1e-5;
+  // Do not perform more than N iteration steps
+  parameters.maxIterations = 100;
+  // Create the optimizer ...
+  GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
+  // ... and optimize
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+
+  // 5. Calculate and print marginal covariances for all variables
+  cout.precision(3);
+  Marginals marginals(graph, result);
+  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
+  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
+  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
+  cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
+  cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;
+
+  return 0;
+}

cmake/obsolete/GtsamTestingObsolete.cmake

@@ -1,22 +1,22 @@
 
-# Macro for adding categorized tests in a "tests" folder, with 
+# Macro for adding categorized tests in a "tests" folder, with
 # optional exclusion of tests and convenience library linking options
-#  
+#
 # By default, all tests are linked with CppUnitLite and boost
-# Arguments: 
+# Arguments:
 #   - subdir    The name of the category for this test
 #   - local_libs  A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
 #   - full_libs   The main library to link against if not using convenience libraries
-#   - excluded_tests  A list of test files that should not be compiled - use for debugging 
+#   - excluded_tests  A list of test files that should not be compiled - use for debugging
 function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
     # Subdirectory target for tests
     add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
     set(is_test TRUE)
 
 	# Put check target in Visual Studio solution folder
-	file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
+	file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
 	set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
-    
+
 	# Link with CppUnitLite - pulled from gtsam installation
 	list(APPEND local_libs CppUnitLite)
 	list(APPEND full_libs CppUnitLite)
@@ -29,15 +29,15 @@ function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
     ${is_test})                                         # Set all as tests
 endfunction()
 
-# Macro for adding categorized timing scripts in a "tests" folder, with 
+# Macro for adding categorized timing scripts in a "tests" folder, with
 # optional exclusion of tests and convenience library linking options
-#  
+#
 # By default, all tests are linked with boost
-# Arguments: 
+# Arguments:
 #   - subdir    The name of the category for this timing script
 #   - local_libs  A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
 #   - full_libs   The main library to link against if not using convenience libraries
-#   - excluded_srcs  A list of timing files that should not be compiled - use for debugging 
+#   - excluded_srcs  A list of timing files that should not be compiled - use for debugging
 macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
     # Subdirectory target for timing - does not actually execute the scripts
     add_custom_target(timing.${subdir})
@@ -60,11 +60,11 @@ endmacro()
 #   - excluded_srcs  A list of timing files that should not be compiled - use for debugging
 function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
     set(is_test FALSE)
-    
+
     if(NOT excluded_srcs)
         set(excluded_srcs "")
     endif()
-    
+
     # Build executables
     gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
 endfunction()
@@ -73,7 +73,7 @@ endfunction()
 macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test)
 	# Print warning about using this obsolete function
 	message(AUTHOR_WARNING "Warning:  Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called.  This will be removed in the future.  The new macros are much easier anyway!!")
-	
+
     # Get all script files
     set(script_files "")
     foreach(one_pattern ${pattern})
@@ -102,20 +102,20 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
 			list(APPEND script_srcs ${script_file})
 		endif()
 	endforeach()
-		
+
-    
+
     # Add targets and dependencies for each script
     if(NOT "${group}" STREQUAL "")
         message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
     endif()
-	
+
 	# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
 	if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
 		foreach(script_src ${script_srcs})
 			get_filename_component(script_base ${script_src} NAME_WE)
 			if (script_base) # Check for null filenames and headers
 				set( script_bin ${script_base} )
-				message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") 
+				message(STATUS "Adding ${pretty_prefix_name} ${script_bin}")
 				add_executable(${script_bin} ${script_src} ${script_headers})
 				if(NOT "${target_prefix}" STREQUAL "")
 					if(NOT "${group}" STREQUAL "")
@@ -123,37 +123,37 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
 					endif()
 					add_dependencies(${target_prefix} ${script_bin})
 				endif()
-				
+
 				# Add TOPSRCDIR
-				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+				set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
 
 				# Disable building during make all/install
 				if (GTSAM_DISABLE_TESTS_ON_INSTALL)
 					set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
 				endif()
-				
+
 				if (is_test)
 					add_test(NAME ${script_base} COMMAND ${script_bin})
 				endif()
-				
+
 				# Linking and dependendencies
 				if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
 					target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
 				else()
 					target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
 				endif()
-				
+
 				# Add .run target
 				if(NOT MSVC AND NOT XCODE_VERSION)
 				  add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
 				endif()
-				
+
 				# Set up Visual Studio folders
-				file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
+				file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
 				set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
 			endif()
 		endforeach(script_src)
-		
+
 		if(MSVC)
 			source_group("" FILES ${script_srcs} ${script_headers})
 		endif()
@@ -166,28 +166,28 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
 		else()
 			target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
 		endif()
-		
+
 		# Only have a main function in one script
 		set(rest_script_srcs ${script_srcs})
 		list(REMOVE_AT rest_script_srcs 0)
 		set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
-			
+
 		# Add TOPSRCDIR
-		set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
+		set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"")
-			
+
 		# Add test
 		add_dependencies(${target_prefix}.${group} ${script_bin})
 		add_dependencies(${target_prefix} ${script_bin})
 		add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
-		
+
 		# Disable building during make all/install
 		if (GTSAM_DISABLE_TESTS_ON_INSTALL)
 			set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
 		endif()
-		
+
 		# Set up Visual Studio folders
 		if(MSVC)
-		    file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
+		    file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
 			set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
 			source_group("" FILES ${script_srcs} ${script_headers})
 		endif()

cython/README.md

@@ -1,7 +1,9 @@
+# Python Wrapper
+
 This is the Cython/Python wrapper around the GTSAM C++ library.
 
-INSTALL
+## Install
-=======
+
 - if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used.
     - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
 - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
@@ -27,8 +29,8 @@ export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
     - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`.
       Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package.
 
-UNIT TESTS
+## Unit Tests
-==========
+
 The Cython toolbox also has a small set of unit tests located in the
 test directory. To run them:
 
@@ -37,11 +39,11 @@ test directory. To run them:
  python -m unittest discover
 ```
 
-WRITING YOUR OWN SCRIPTS
+## Writing Your Own Scripts
-========================
+
 See the tests for examples.
 
-## Some important notes:
+### Some Important Notes:
 
 - Vector/Matrix:
   + GTSAM expects double-precision floating point vectors and matrices.
@@ -66,8 +68,8 @@ Examples:
       noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
 ```
 
-WRAPPING YOUR OWN PROJECT THAT USES GTSAM
+## Wrapping Your Own Project That Uses GTSAM
-=========================================
+
 - Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH}
   + so that it can find gtsam Cython header: gtsam/gtsam.pxd
 
@@ -88,8 +90,8 @@ wrap_and_install_library_cython("your_project_interface.h"
 #Optional: install_cython_scripts and install_cython_files. See GtsamCythonWrap.cmake.
 ```
 
-KNOWN ISSUES
+## KNOWN ISSUES
-============
+
   - Doesn't work with python3 installed from homebrew
     - size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
     - Guess: 64 vs 32b? disutils Compiler flags?
@@ -99,63 +101,55 @@ KNOWN ISSUES
     - support these constructors by default and declare "delete" for special classes?
 
 
-TODO
+### TODO
-=====
+
-☐ allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
+- [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
-☐ a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?)
+- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?)
-☐ inner namespaces ==> inner packages?
+- [ ] inner namespaces ==> inner packages?
-☐ Wrap fixed-size Matrices/Vectors?
+- [ ] Wrap fixed-size Matrices/Vectors?
 
 
-Completed/Cancelled:
+### Completed/Cancelled:
-=====
+
-✔ Fix Python tests: don't use " import <package> * ": Bad style!!! @done (18-03-17 19:50)
+- [x] Fix Python tests: don't use " import <package> * ": Bad style!!! (18-03-17 19:50)
-✔ Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
+- [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
-✔ Wrap unstable @done (18-03-17 15:30)
+- [x] Wrap unstable @done (18-03-17 15:30)
-✔ Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30)
+- [x] Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30)
-  ✔ 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
+- [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
-  ✔ 06-03-17: manage to remove the requirements for default and copy constructors
+- [x] 06-03-17: manage to remove the requirements for default and copy constructors
-  ✘ 25-11-16:
+- [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
-    Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet.
+- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;```  Users don't have to wrap this constructor, however.
-    Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
+- [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
-✘ Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;```  Users don't have to wrap this constructor, however.
+- [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
-✔ Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
+- [x] CMake install script @done (25-11-16 02:30)
-✔ Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
+- [ ] [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy
-✔ CMake install script @done (25-11-16 02:30)
+- [ ] forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work?
-✘ [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy
+- [x] wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00)
-✘ forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work?
+- [x] Global functions @done (22-11-16 21:00)
-✔ wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00)
+- [x] [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00)
-✔ Global functions @done (22-11-16 21:00)
+- [x] Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00)
-✔ [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00)
+- [x] Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00)
-✔ Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00)
+- [x] Casting from parent and grandparents @done (16-11-16 17:00)
-✔ Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00)
+- [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
-✔ Casting from parent and grandparents @done (16-11-16 17:00)
+- [x] Support "print obj" @done (16-11-16 17:00)
-✔ Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
+- [x] methods for FastVector: at, [], ...  @done (16-11-16 17:00)
-✔ Support "print obj" @done (16-11-16 17:00)
+- [x] Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
-✔ methods for FastVector: at, [], ...  @done (16-11-16 17:00)
+- [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
-✔ Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
+- [x] [Nice to have] parse typedef @done (16-09-13 17:19)
-✔ KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
+- [x] ctypedef at correct places @done (16-09-12 18:34)
-✔ [Nice to have] parse typedef @done (16-09-13 17:19)
+- [x] expand template variable type in constructor/static methods? @done (16-09-12 18:34)
-✔ ctypedef at correct places @done (16-09-12 18:34)
+- [x] NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20)
-✔ expand template variable type in constructor/static methods? @done (16-09-12 18:34)
+- [x] Value: no default constructor @done (16-09-13 17:20)
-✔ NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20)
+- [x] ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25)
-✔ Value: no default constructor @done (16-09-13 17:20)
+- [x] Delete duplicate methods in derived class @done (16-09-12 13:38)
-✔ ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25)
+- [x] Fix return properly @done (16-09-11 17:14)
-✔ Delete duplicate methods in derived class @done (16-09-12 13:38)
+- [x] handle pair @done (16-09-11 17:14)
-✔ Fix return properly @done (16-09-11 17:14)
+- [x] Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59)
- ✔ handle pair @done (16-09-11 17:14)
+- [x] Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59)
-✔ Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59)
+- [x] Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22)
-✔ Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59)
+- [x] Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05)
-✔ Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22)
+- [ ] Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20)
-✔ Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05)
+- [ ] Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28)
-✘ Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20)
-✘ Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28)
 
 - inference before symbolic/linear
 - what's the purpose of "virtual" ??
-
-Installation:
-  ☐ Prerequisite:
-    - Users create venv and pip install requirements before compiling
-    - Wrap cython script in gtsam/cython folder
-  ☐ Install built module into venv?

cython/gtsam_eigency/CMakeLists.txt

@@ -39,11 +39,11 @@ add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigen
 
 # install
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-        DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}
+        DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}"
         PATTERN "CMakeLists.txt" EXCLUDE
         PATTERN "__init__.py.in" EXCLUDE)
 install(TARGETS cythonize_eigency_core cythonize_eigency_conversions
-        DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency")
+        DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency")
-install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
+install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency)
 configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
-install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
+install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency)

cython/setup.py.in

@@ -7,7 +7,7 @@ except ImportError:
 
 if 'SETUP_PY_NO_CHECK' not in os.environ:
     script_path = os.path.abspath(os.path.realpath(__file__))
-    install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py')))
+    install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}', 'setup.py')))
     if script_path != install_path:
         print('setup.py is being run from an unexpected location: "{}"'.format(script_path))
         print('please run `make install` and run the script from there')

debian/README.md

@@ -0,0 +1,12 @@
+# How to build a GTSAM debian package
+
+To use the ``debuild`` command, install the ``devscripts`` package
+
+    sudo apt install devscripts
+
+Change into the gtsam directory, then run:
+
+    debuild -us -uc -j4
+
+Adjust the ``-j4`` depending on how many CPUs you want to build on in
+parallel. 

debian/changelog

@@ -0,0 +1,5 @@
+gtsam (4.0.0-1berndpfrommer) bionic; urgency=medium
+
+  * initial release
+
+ -- Bernd Pfrommer <bernd.pfrommer@gmail.com>  Wed, 18 Jul 2018 20:36:44 -0400

debian/compat

@@ -0,0 +1 @@
+9

debian/control

@@ -0,0 +1,15 @@
+Source: gtsam
+Section: libs
+Priority: optional
+Maintainer: Frank Dellaert <frank@cc.gatech.edu>
+Uploaders: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>, Bernd Pfrommer <bernd.pfrommer@gmail.com>
+Build-Depends: cmake, libboost-all-dev (>= 1.58), libeigen3-dev, libtbb-dev, debhelper (>=9)
+Standards-Version: 3.9.7
+Homepage: https://github.com/borglab/gtsam
+Vcs-Browser: https://github.com/borglab/gtsam
+
+Package: libgtsam-dev
+Architecture: any
+Depends: ${shlibs:Depends}, ${misc:Depends}
+Description: Georgia Tech Smoothing and Mapping Library
+ gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications

debian/copyright

@@ -0,0 +1,15 @@
+Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
+Upstream-Name: gtsam
+Source: https://bitbucket.org/gtborg/gtsam.git
+
+Files: *
+Copyright: 2017, Frank Dellaert
+License: BSD
+
+Files: gtsam/3rdparty/CCOLAMD/*
+Copyright: 2005-2011, Univ. of Florida.  Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore.  Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. http://www.cise.ufl.edu/research/sparse
+License: GNU LESSER GENERAL PUBLIC LICENSE
+
+Files: gtsam/3rdparty/Eigen/*
+Copyright: 2017, Multiple Authors
+License: MPL2

debian/rules

@@ -0,0 +1,25 @@
+#!/usr/bin/make -f
+# See debhelper(7) (uncomment to enable)
+# output every command that modifies files on the build system.
+export DH_VERBOSE = 1
+
+
+# see FEATURE AREAS in dpkg-buildflags(1)
+#export DEB_BUILD_MAINT_OPTIONS = hardening=+all
+
+# see ENVIRONMENT in dpkg-buildflags(1)
+# package maintainers to append CFLAGS
+#export DEB_CFLAGS_MAINT_APPEND  = -Wall -pedantic
+# package maintainers to append LDFLAGS
+#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed
+
+
+%:
+	dh $@ --parallel
+
+
+# dh_make generated override targets
+# This is example for Cmake (See https://bugs.debian.org/641051 )
+override_dh_auto_configure:
+	dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF
+

debian/source/format

@@ -0,0 +1 @@
+3.0 (quilt)

doc/ImuFactor.lyx

@@ -398,7 +398,7 @@ The local coordinates
  and note that 
 \begin_inset Formula 
 \[
-\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega}
+\frac{d\Phi_{R_{0}}\left(\omega t\right)}{dt}\biggr\vert_{t=0}=\frac{dR_{0}\exp\left(\Skew{\omega t}\right)}{dt}\biggr\vert_{t=0}=R_{0}\Skew{\omega t}
 \]
 
 \end_inset

doc/trustregion.bib

@@ -8,7 +8,6 @@
 %% Saved with string encoding Unicode (UTF-8) 
 
 
-
 @webpage{Hauser06lecture,
 	Author = {Raphael Hauser},
 	Date-Added = {2011-10-10 15:21:22 +0000},
@@ -16,11 +15,5 @@
 	Title = {Lecture Notes on Unconstrained Optimization},
 	Url = {http://www.numerical.rl.ac.uk/nimg/oupartc/lectures/raphael/},
 	Year = {2006},
-	Bdsk-Url-1 = {http://www.numerical.rl.ac.uk/nimg/oupartc/lectures/raphael/},
+	howpublished = {\href{http://www.numerical.rl.ac.uk/nimg/oupartc/lectures/raphael/}{link}},
-	Bdsk-File-1 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmUxAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+Sv+qSlgAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2OYAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmUxAAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQAxAA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUxAAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUx0hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA},
+}
-	Bdsk-File-2 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmUyAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+Xv+qSowAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PMAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmUyAAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQAyAA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUyAAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUy0hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA},
-	Bdsk-File-3 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmUzAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+mv+qSpQAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PUAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmUzAAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQAzAA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUzAAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmUz0hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA},
-	Bdsk-File-4 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU0AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+vv+qSpwAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PcAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU0AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA0AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU0AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU00hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA},
-	Bdsk-File-5 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU1AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn+5v+qSqAAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PgAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU1AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA1AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU1AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU10hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA},
-	Bdsk-File-6 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU2AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn/Fv+qSqgAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PoAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU2AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA2AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU2AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU20hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA},
-	Bdsk-File-7 = {YnBsaXN0MDDUAQIDBAUIJidUJHRvcFgkb2JqZWN0c1gkdmVyc2lvblkkYXJjaGl2ZXLRBgdUcm9vdIABqAkKFRYXGyIjVSRudWxs0wsMDQ4RFFpOUy5vYmplY3RzV05TLmtleXNWJGNsYXNzog8QgASABqISE4ACgAOAB1lhbGlhc0RhdGFccmVsYXRpdmVQYXRo0hgNGRpXTlMuZGF0YU8RAYwAAAAAAYwAAgAADE1hY2ludG9zaCBIRAAAAAAAAAAAAAAAAAAAAMpAsaxIKwAAAD/T8xBIYXVzZXIwNmxlY3R1cmU3AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQn/Pv+qSrAAAAAAAAAAAAAMAAgAACSAAAAAAAAAAAAAAAAAAAAAKTGl0ZXJhdHVyZQAQAAgAAMpA6ewAAAARAAgAAL/q2PwAAAABAAwAP9PzAAUCJwAAvuwAAgA5TWFjaW50b3NoIEhEOlVzZXJzOgByaWNoYXJkOgBMaXRlcmF0dXJlOgBIYXVzZXIwNmxlY3R1cmU3AAAOACIAEABIAGEAdQBzAGUAcgAwADYAbABlAGMAdAB1AHIAZQA3AA8AGgAMAE0AYQBjAGkAbgB0AG8AcwBoACAASABEABIAKVVzZXJzL3JpY2hhcmQvTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU3AAATAAEvAAAVAAIADv//AACABdIcHR4fWCRjbGFzc2VzWiRjbGFzc25hbWWjHyAhXU5TTXV0YWJsZURhdGFWTlNEYXRhWE5TT2JqZWN0XxAkLi4vLi4vLi4vTGl0ZXJhdHVyZS9IYXVzZXIwNmxlY3R1cmU30hwdJCWiJSFcTlNEaWN0aW9uYXJ5EgABhqBfEA9OU0tleWVkQXJjaGl2ZXIACAARABYAHwAoADIANQA6ADwARQBLAFIAXQBlAGwAbwBxAHMAdgB4AHoAfACGAJMAmACgAjACMgI3AkACSwJPAl0CZAJtApQCmQKcAqkCrgAAAAAAAAIBAAAAAAAAACgAAAAAAAAAAAAAAAAAAALA}}

doc/trustregion.lyx

@@ -1,10 +1,11 @@
-#LyX 2.0 created this file. For more info see http://www.lyx.org/
+#LyX 2.1 created this file. For more info see http://www.lyx.org/
-\lyxformat 413
+\lyxformat 474
 \begin_document
 \begin_header
 \textclass article
 \begin_preamble
-\usepackage{amssymb}
+\usepackage{url}
+\usepackage{hyperref}
 \end_preamble
 \use_default_options true
 \maintain_unincluded_children false
@@ -15,13 +16,13 @@
 \font_roman default
 \font_sans default
 \font_typewriter default
+\font_math auto
 \font_default_family default
 \use_non_tex_fonts false
 \font_sc false
 \font_osf false
 \font_sf_scale 100
 \font_tt_scale 100
-
 \graphics default
 \default_output_format default
 \output_sync 0
@@ -32,15 +33,24 @@
 \use_hyperref false
 \papersize default
 \use_geometry false
-\use_amsmath 1
+\use_package amsmath 1
-\use_esint 1
+\use_package amssymb 2
-\use_mhchem 1
+\use_package cancel 1
-\use_mathdots 1
+\use_package esint 1
+\use_package mathdots 1
+\use_package mathtools 1
+\use_package mhchem 1
+\use_package stackrel 0
+\use_package stmaryrd 1
+\use_package undertilde 1
 \cite_engine basic
+\cite_engine_type default
+\biblio_style plain
 \use_bibtopic false
 \use_indices false
 \paperorientation portrait
 \suppress_date false
+\justification true
 \use_refstyle 1
 \index Index
 \shortcut idx
@@ -231,7 +241,7 @@ key "Hauser06lecture"
 
 \end_inset
 
- (in our /net/hp223/borg/Literature folder).
+.
 \end_layout
 
 \begin_layout Standard
@@ -465,22 +475,39 @@ where
 \end_inset
 
 .
- A typical update rule [
+ A typical update rule, as per Lec.
-\color blue
+ 7-1.2 of 
-see where this came from in paper
+\begin_inset CommandInset citation
-\color inherit
+LatexCommand cite
-] is
+key "Hauser06lecture"
+
+\end_inset
+
+ is:
 \begin_inset Formula 
 \[
-\Delta\leftarrow\begin{cases}
+\Delta_{k+1}\leftarrow\begin{cases}
-\max\left(\Delta,3\norm{\delta x_{d}}\right)\text{,} & \rho>0.75\\
+\Delta_{k}/4 & \rho<0.25\\
-\Delta & 0.75>\rho>0.25\\
+\min\left(2\Delta_{k},\Delta_{max}\right)\text{,} & \rho>0.75\\
-\Delta/2 & 0.25>\rho
+\Delta_{k} & 0.75>\rho>0.25
 \end{cases}
 \]
 
 \end_inset
 
+where 
+\begin_inset Formula $\Delta_{k}\triangleq\norm{\delta x_{d}}$
+\end_inset
+
+.
+ Note that the rule is designed to ensure that 
+\begin_inset Formula $\Delta_{k}$
+\end_inset
+
+never exceeds the maximum trust region size 
+\begin_inset Formula $\Delta_{max}.$
+\end_inset
+
 
 \end_layout
 
@@ -593,9 +620,9 @@ To find the intersection of the line between
 \begin{align*}
 \Delta & =\norm{\left(1-\tau\right)\delta x_{u}+\tau\delta x_{n}}\\
 \Delta^{2} & =\left(1-\tau\right)^{2}\delta x_{u}^{\t}\delta x_{u}+2\tau\left(1-\tau\right)\delta x_{u}^{\t}\delta x_{n}+\tau^{2}\delta x_{n}^{\t}\delta x_{n}\\
-0 & =uu-2\tau uu+\tau^{2}uu+2\tau un-2\tau^{2}un+\tau^{2}nn-\Delta^{2}\\
+0 & =\delta x_{u}^{\t}\delta x_{u}-2\tau\delta x_{u}^{\t}\delta x_{u}+\tau^{2}\delta x_{u}^{\t}\delta x_{u}+2\tau\delta x_{u}^{\t}\delta x_{n}-2\tau^{2}\delta x_{u}^{\t}\delta x_{n}+\tau^{2}\delta x_{n}^{\t}\delta x_{n}-\Delta^{2}\\
-0 & =\left(uu-2un+nn\right)\tau^{2}+\left(2un-2uu\right)\tau-\Delta^{2}+uu\\
+0 & =\left(\delta x_{u}^{\t}\delta x_{u}-2\delta x_{u}^{\t}\delta x_{n}+\delta x_{n}^{\t}\delta x_{n}\right)\tau^{2}+\left(2\delta x_{u}^{\t}\delta x_{n}-2\delta x_{u}^{\t}\delta x_{u}\right)\tau-\Delta^{2}+\delta x_{u}^{\t}\delta x_{u}\\
-\tau & =\frac{-\left(2un-2uu\right)\pm\sqrt{\left(2un-2uu\right)^{2}-4\left(uu-2un+nn\right)\left(uu-\Delta^{2}\right)}}{2\left(uu-un+nn\right)}
+\tau & =\frac{-\left(2\delta x_{u}^{\t}\delta x_{n}-2\delta x_{u}^{\t}\delta x_{u}\right)\pm\sqrt{\left(2\delta x_{u}^{\t}\delta x_{n}-2\delta x_{u}^{\t}\delta x_{u}\right)^{2}-4\left(\delta x_{u}^{\t}\delta x_{u}-2\delta x_{u}^{\t}\delta x_{n}+\delta x_{n}^{\t}\delta x_{n}\right)\left(\delta x_{u}^{\t}\delta x_{u}-\Delta^{2}\right)}}{2\left(\delta x_{u}^{\t}\delta x_{u}-\delta x_{u}^{\t}\delta x_{n}+\delta x_{n}^{\t}\delta x_{n}\right)}
 \end{align*}
 
 \end_inset
@@ -641,7 +668,7 @@ Thus, mathematically, we can write the dogleg update
 \begin_inset Formula 
 \[
 \delta x_{d}^{\left(k\right)}=\begin{cases}
--\frac{\Delta}{\norm{g^{\left(k\right)}}}g^{\left(k\right)}\text{,} & \Delta<\norm{\delta x_{u}^{\left(k\right)}}\\
+-\frac{\Delta}{\norm{\delta x_{u}^{\left(k\right)}}}\delta x_{u}^{\left(k\right)}\text{,} & \Delta<\norm{\delta x_{u}^{\left(k\right)}}\\
 \left(1-\tau^{\left(k\right)}\right)\delta x_{u}^{\left(k\right)}+\tau^{\left(k\right)}\delta x_{n}^{\left(k\right)}\text{,} & \norm{\delta x_{u}^{\left(k\right)}}<\Delta<\norm{\delta x_{n}^{\left(k\right)}}\\
 \delta x_{n}^{\left(k\right)}\text{,} & \norm{\delta x_{n}^{\left(k\right)}}<\Delta
 \end{cases}

examples/RangeISAMExample_plaza2.cpp

@@ -108,7 +108,7 @@ int main (int argc, char** argv) {
 
   // Set Noise parameters
   Vector priorSigmas = Vector3(1,1,M_PI);
-  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
+  Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
   double sigmaR = 100; // range standard deviation
   const NM::Base::shared_ptr // all same type
   priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
@@ -157,7 +157,7 @@ int main (int argc, char** argv) {
     boost::tie(t, odometry) = timedOdometry;
 
     // add odometry factor
-    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
+    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry, odoNoise));
 
     // predict pose and add as initial estimate
     Pose2 predictedPose = lastPose.compose(odometry);

gtsam.h

@@ -248,10 +248,13 @@ class FactorIndices {
 
 /** gtsam namespace functions */
 
+#include <gtsam/base/debug.h>
+bool isDebugVersion();
+
 #include <gtsam/base/DSFMap.h>
-class IndexPair { 
+class IndexPair {
-  IndexPair(); 
+  IndexPair();
-  IndexPair(size_t i, size_t j); 
+  IndexPair(size_t i, size_t j);
   size_t i() const;
   size_t j() const;
 };
@@ -1385,6 +1388,15 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
   void serializable() const;
 };
 
+virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
+  Welsch(double k);
+  static gtsam::noiseModel::mEstimator::Welsch* Create(double k);
+
+  // enabling serialization functionality
+  void serializable() const;
+};
+
+
 }///\namespace mEstimator
 
 virtual class Robust : gtsam::noiseModel::Base {
@@ -1886,6 +1898,7 @@ class NonlinearFactorGraph {
   gtsam::KeyVector keyVector() const;
 
   // NonlinearFactorGraph
+  void printErrors(const gtsam::Values& values) const;
   double error(const gtsam::Values& values) const;
   double probPrime(const gtsam::Values& values) const;
   gtsam::Ordering orderingCOLAMD() const;
@@ -2495,6 +2508,9 @@ virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
   SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
       const CALIBRATION* K,
       const gtsam::Pose3& body_P_sensor);
+  SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
+      const CALIBRATION* K,
+      const gtsam::SmartProjectionParams& params);
   SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
       const CALIBRATION* K,
       const gtsam::Pose3& body_P_sensor,

gtsam/3rdparty/CMakeLists.txt

@@ -16,6 +16,21 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 		endif()
 	endforeach(eigen_dir)
 
+	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
+		message("-- Installing Eigen Unsupported modules")
+		# do the same for the unsupported eigen folder
+		file(GLOB_RECURSE  unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
+
+		file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
+		foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
+			get_filename_component(filename ${unsupported_eigen_dir} NAME)
+			if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES "CXX11") OR (${filename} MATCHES ".svn")))
+				list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
+				install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
+			endif()
+		endforeach(unsupported_eigen_dir)
+	endif()
+
 	# Add to project source
 	set(eigen_headers ${eigen_headers} PARENT_SCOPE)
 	# set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE)
@@ -24,6 +39,13 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
 	install(DIRECTORY Eigen/Eigen
 		DESTINATION include/gtsam/3rdparty/Eigen
 		FILES_MATCHING PATTERN "*.h")
+
+	if(GTSAM_WITH_EIGEN_UNSUPPORTED)
+		install(DIRECTORY Eigen/unsupported/Eigen
+			DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
+			FILES_MATCHING PATTERN "*.h")
+	endif()
+
 endif()
 
 option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)

gtsam/CMakeLists.txt

@@ -1,3 +1,5 @@
+project(gtsam LANGUAGES CXX)
+
 # We split the library in to separate subfolders, each containing
 # tests, timing, and an optional convenience library.
 # The following variable is the master list of subdirs to add
@@ -82,9 +84,9 @@ ENDIF(MSVC)
 # Generate and install config and dllexport files
 configure_file(config.h.in config.h)
 set(library_name GTSAM) # For substitution in dllexport.h.in
-configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
+configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
-list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h")
+list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h")
-install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam)
+install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam)
 
 if(GTSAM_SUPPORT_NESTED_DISSECTION)
     list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
@@ -101,17 +103,16 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 
 # BUILD_SHARED_LIBS automatically defines static/shared libs:
 add_library(gtsam ${gtsam_srcs})
+
 # Boost:
 target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
 target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR})
 # Other deps:
 target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
-target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
+
-target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
+# Apply build flags:
-if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "")
+gtsam_apply_build_flags(gtsam)
-  target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC})
+
-endif()
-target_compile_options(gtsam PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE})
 set_target_properties(gtsam PROPERTIES
     OUTPUT_NAME         gtsam
     CLEAN_DIRECT_OUTPUT 1
@@ -171,7 +172,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
 		set_target_properties(gtsam PROPERTIES
 			PREFIX ""
 			DEFINE_SYMBOL GTSAM_EXPORTS
-			RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
+    RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin")
 	endif()
 endif()
 

gtsam/base/Matrix.h

@@ -90,7 +90,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
     for(size_t j=0; j<n1; j++) {
       if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
         return false;
-      else if(fabs(A(i,j) - B(i,j)) > tol)
+      else if(std::abs(A(i,j) - B(i,j)) > tol)
         return false;
     }
   return true;
@@ -299,7 +299,7 @@ GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
  * @param A is the input matrix, and is the output
  * @param clear_below_diagonal enables zeroing out below diagonal
  */
-void inplace_QR(Matrix& A);
+GTSAM_EXPORT void inplace_QR(Matrix& A);
 
 /**
  * Imperative algorithm for in-place full elimination with

gtsam/base/Vector.cpp

@@ -84,7 +84,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
   for(size_t i=0; i<m; ++i) {
     if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
       return false;
-    if(fabs(vec1[i] - vec2[i]) > tol)
+    if(std::abs(vec1[i] - vec2[i]) > tol)
       return false;
   }
   return true;
@@ -97,7 +97,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol
   for(size_t i=0; i<m; ++i) {
     if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
       return false;
-    if(fabs(vec1[i] - vec2[i]) > tol)
+    if(std::abs(vec1[i] - vec2[i]) > tol)
       return false;
   }
   return true;
@@ -145,14 +145,14 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
   bool flag = false;   double scale = 1.0;
   size_t m = vec1.size();
   for(size_t i=0; i<m; i++) {
-    if((fabs(vec1[i])>tol&&fabs(vec2[i])<tol) || (fabs(vec1[i])<tol&&fabs(vec2[i])>tol))
+    if((std::abs(vec1[i])>tol && std::abs(vec2[i])<tol) || (std::abs(vec1[i])<tol && std::abs(vec2[i])>tol))
       return false;
     if(vec1[i] == 0 && vec2[i] == 0) continue;
     if (!flag) {
       scale = vec1[i] / vec2[i];
       flag = true ;
     }
-    else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false;
+    else if (std::abs(vec1[i] - vec2[i]*scale) > tol) return false;
   }
   return flag;
 }
@@ -213,7 +213,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
 
   // Check once for zero entries of a. TODO: really needed ?
   vector<bool> isZero;
-  for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9);
+  for (size_t i = 0; i < m; ++i) isZero.push_back(std::abs(a[i]) < 1e-9);
 
   // If there is a valid (a!=0) constraint (sigma==0) return the first one
   for (size_t i = 0; i < m; ++i) {

gtsam/base/Vector.h

@@ -63,6 +63,18 @@ GTSAM_MAKE_VECTOR_DEFS(12);
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
 
+/**
+  * Ensure we are not including a different version of Eigen in user code than
+  * while compiling gtsam, since it can lead to hard-to-understand runtime
+  * crashes.
+  */
+#if defined(GTSAM_EIGEN_VERSION_WORLD)
+static_assert(
+    GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION &&
+    GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION,
+  "Error: GTSAM was built against a different version of Eigen");
+#endif
+
 /**
  * print without optional string, must specify cout yourself
  */

gtsam/base/cholesky.cpp

@@ -111,7 +111,7 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
     return true;
 
   assert(ABC.cols() == ABC.rows());
-  assert(ABC.rows() >= topleft);
+  assert(size_t(ABC.rows()) >= topleft);
   const size_t n = static_cast<size_t>(ABC.rows() - topleft);
   assert(nFrontal <= size_t(n));
 

gtsam/base/debug.cpp

@@ -47,4 +47,15 @@ void guardedSetDebug(const std::string& s, const bool v) {
   gtsam::debugFlags[s] = v;
 }
 
+bool isDebugVersion() {
+#ifdef NDEBUG
+  // nondebug
+  return false;
+#else
+  // debug
+  return true;
+#endif
+
+}
+
 }

gtsam/base/debug.h

@@ -47,6 +47,9 @@ namespace gtsam {
   // Non-guarded use led to crashes, and solved in commit cd35db2
   bool GTSAM_EXPORT guardedIsDebug(const std::string& s);
   void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v);
+
+  // function to check if compiled version has debug information
+  bool GTSAM_EXPORT isDebugVersion();
 }
 
 #undef ISDEBUG

gtsam/base/deprecated/LieMatrix.h

@@ -29,7 +29,7 @@ namespace gtsam {
  * we can directly add double, Vector, and Matrix into values now, because of
  * gtsam::traits.
  */
-struct GTSAM_EXPORT LieMatrix : public Matrix {
+struct LieMatrix : public Matrix {
 
   /// @name Constructors
   /// @{

gtsam/base/deprecated/LieScalar.h

@@ -28,7 +28,7 @@ namespace gtsam {
    * we can directly add double, Vector, and Matrix into values now, because of
    * gtsam::traits.
    */
-  struct GTSAM_EXPORT LieScalar {
+  struct LieScalar {
 
     enum { dimension = 1 };
 
@@ -53,7 +53,7 @@ namespace gtsam {
       std::cout << name << ": " << d_ << std::endl;
     }
     bool equals(const LieScalar& expected, double tol = 1e-5) const {
-      return fabs(expected.d_ - d_) <= tol;
+      return std::abs(expected.d_ - d_) <= tol;
     }
     /// @}
 

gtsam/base/deprecated/LieVector.h

@@ -27,7 +27,7 @@ namespace gtsam {
  * we can directly add double, Vector, and Matrix into values now, because of
  * gtsam::traits.
  */
-struct GTSAM_EXPORT LieVector : public Vector {
+struct LieVector : public Vector {
 
   enum { dimension = Eigen::Dynamic };
 

gtsam/base/tests/testVector.cpp

@@ -165,7 +165,7 @@ TEST(Vector, weightedPseudoinverse )
 
   // verify
   EXPECT(assert_equal(expected,actual));
-  EXPECT(fabs(expPrecision-precision) < 1e-5);
+  EXPECT(std::abs(expPrecision-precision) < 1e-5);
 }
 
 /* ************************************************************************* */

gtsam/base/timing.h

@@ -142,7 +142,7 @@ namespace gtsam {
     /**
      * Timing Entry, arranged in a tree
      */
-    class GTSAM_EXPORT TimingOutline {
+    class TimingOutline {
     protected:
       size_t id_;
       size_t t_;
@@ -174,21 +174,21 @@ namespace gtsam {
 
     public:
       /// Constructor
-      TimingOutline(const std::string& label, size_t myId);
+      GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId);
-      size_t time() const; ///< time taken, including children
+      GTSAM_EXPORT size_t time() const; ///< time taken, including children
       double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children
       double self() const { return double(t_)     / 1000000.0;} ///< self time only, in seconds
       double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds
       double min()  const { return double(tMin_)  / 1000000.0;} ///< min time, in seconds
       double max()  const { return double(tMax_)  / 1000000.0;} ///< max time, in seconds
       double mean() const { return self() / double(n_); } ///< mean self time, in seconds
-      void print(const std::string& outline = "") const;
+      GTSAM_EXPORT void print(const std::string& outline = "") const;
-      void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
+      GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
-      const boost::shared_ptr<TimingOutline>&
+      GTSAM_EXPORT const boost::shared_ptr<TimingOutline>&
         child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
-      void tic();
+      GTSAM_EXPORT void tic();
-      void toc();
+      GTSAM_EXPORT void toc();
-      void finishedIteration();
+      GTSAM_EXPORT void finishedIteration();
 
       GTSAM_EXPORT friend void toc(size_t id, const char *label);
     }; // \TimingOutline

gtsam/config.h.in

@@ -25,7 +25,7 @@
 #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@"
 
 // Paths to example datasets distributed with GTSAM
-#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data"
+#define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data"
 #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data"
 
 // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices)
@@ -52,6 +52,12 @@
 // Whether Eigen with MKL will use OpenMP (if OpenMP was found, Eigen uses MKL, and GTSAM_WITH_EIGEN_MKL_OPENMP is enabled in CMake)
 #cmakedefine GTSAM_USE_EIGEN_MKL_OPENMP
 
+// Eigen library version (needed to avoid mixing versions, which often leads
+// to segfaults)
+#cmakedefine GTSAM_EIGEN_VERSION_WORLD @GTSAM_EIGEN_VERSION_WORLD@
+#cmakedefine GTSAM_EIGEN_VERSION_MAJOR @GTSAM_EIGEN_VERSION_MAJOR@
+#cmakedefine GTSAM_EIGEN_VERSION_MINOR @GTSAM_EIGEN_VERSION_MINOR@
+
 // The default allocator to use
 #cmakedefine GTSAM_ALLOCATOR_BOOSTPOOL
 #cmakedefine GTSAM_ALLOCATOR_TBB

gtsam/discrete/DecisionTree-inl.h

@@ -79,7 +79,7 @@ namespace gtsam {
     bool equals(const Node& q, double tol) const {
       const Leaf* other = dynamic_cast<const Leaf*> (&q);
       if (!other) return false;
-      return fabs(double(this->constant_ - other->constant_)) < tol;
+      return std::abs(double(this->constant_ - other->constant_)) < tol;
     }
 
     /** print */

gtsam/geometry/Cal3Bundler.cpp

@@ -59,9 +59,9 @@ void Cal3Bundler::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
-  if (fabs(f_ - K.f_) > tol || fabs(k1_ - K.k1_) > tol
+  if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
-      || fabs(k2_ - K.k2_) > tol || fabs(u0_ - K.u0_) > tol
+      || std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
-      || fabs(v0_ - K.v0_) > tol)
+      || std::abs(v0_ - K.v0_) > tol)
     return false;
   return true;
 }

gtsam/geometry/Cal3DS2.cpp

@@ -30,9 +30,9 @@ void Cal3DS2::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
-  if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
+  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
-      fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
+      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
+      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
     return false;
   return true;
 }

gtsam/geometry/Cal3DS2_Base.cpp

@@ -49,9 +49,9 @@ void Cal3DS2_Base::print(const std::string& s_) const {
 
 /* ************************************************************************* */
 bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
-  if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
+  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
-      fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
+      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
+      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
     return false;
   return true;
 }

gtsam/geometry/Cal3Unified.cpp

@@ -43,10 +43,10 @@ void Cal3Unified::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
-  if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
+  if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol || std::abs(s_ - K.s_) > tol ||
-      fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
+      std::abs(u0_ - K.u0_) > tol || std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
-      fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol ||
+      std::abs(k2_ - K.k2_) > tol || std::abs(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
-      fabs(xi_ - K.xi_) > tol)
+      std::abs(xi_ - K.xi_) > tol)
     return false;
   return true;
 }

gtsam/geometry/Cal3_S2.cpp

@@ -64,15 +64,15 @@ void Cal3_S2::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
-  if (fabs(fx_ - K.fx_) > tol)
+  if (std::abs(fx_ - K.fx_) > tol)
     return false;
-  if (fabs(fy_ - K.fy_) > tol)
+  if (std::abs(fy_ - K.fy_) > tol)
     return false;
-  if (fabs(s_ - K.s_) > tol)
+  if (std::abs(s_ - K.s_) > tol)
     return false;
-  if (fabs(u0_ - K.u0_) > tol)
+  if (std::abs(u0_ - K.u0_) > tol)
     return false;
-  if (fabs(v0_ - K.v0_) > tol)
+  if (std::abs(v0_ - K.v0_) > tol)
     return false;
   return true;
 }

gtsam/geometry/Cal3_S2Stereo.cpp

@@ -30,7 +30,7 @@ void Cal3_S2Stereo::print(const std::string& s) const {
 
 /* ************************************************************************* */
 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
-  if (fabs(b_ - other.b_) > tol) return false;
+  if (std::abs(b_ - other.b_) > tol) return false;
   return K_.equals(other.K_,tol);
 }
 

gtsam/geometry/CalibratedCamera.h

@@ -29,8 +29,7 @@
 
 namespace gtsam {
 
-class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
+class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
-    CheiralityException> {
 public:
   CheiralityException()
     : CheiralityException(std::numeric_limits<Key>::max()) {}

gtsam/geometry/EssentialMatrix.h

@@ -23,7 +23,7 @@ namespace gtsam {
  * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
  * We can then non-linearly optimize immediately on this 5-dimensional manifold.
  */
-class GTSAM_EXPORT EssentialMatrix {
+class EssentialMatrix {
  private:
   Rot3 R_;     ///< Rotation
   Unit3 t_;    ///< Translation
@@ -48,12 +48,12 @@ class GTSAM_EXPORT EssentialMatrix {
   }
 
   /// Named constructor with derivatives
-  static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
+  GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
                                                   OptionalJacobian<5, 3> H1 = boost::none,
                                                   OptionalJacobian<5, 2> H2 = boost::none);
 
   /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
-  static EssentialMatrix FromPose3(const Pose3& _1P2_,
+  GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_,
       OptionalJacobian<5, 6> H = boost::none);
 
   /// Random, using Rot3::Random and Unit3::Random
@@ -70,7 +70,7 @@ class GTSAM_EXPORT EssentialMatrix {
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const;
+  GTSAM_EXPORT void print(const std::string& s = "") const;
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
@@ -138,7 +138,7 @@ class GTSAM_EXPORT EssentialMatrix {
    * @param Dpoint optional 3*3 Jacobian wrpt point
    * @return point in pose coordinates
    */
-  Point3 transformTo(const Point3& p,
+  GTSAM_EXPORT Point3 transformTo(const Point3& p,
       OptionalJacobian<3, 5> DE = boost::none,
       OptionalJacobian<3, 3> Dpoint = boost::none) const;
 
@@ -147,7 +147,7 @@ class GTSAM_EXPORT EssentialMatrix {
    * @param cRb rotation from body frame to camera frame
    * @param E essential matrix E in camera frame C
    */
-  EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
+  GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
       boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
 
   /**
@@ -160,7 +160,7 @@ class GTSAM_EXPORT EssentialMatrix {
   }
 
   /// epipolar error, algebraic
-  double error(const Vector3& vA, const Vector3& vB,
+  GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
       OptionalJacobian<1, 5> H = boost::none) const;
 
   /// @}

gtsam/geometry/OrientedPlane3.h

@@ -78,7 +78,7 @@ public:
 
   /// The equals function with tolerance
   bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
-    return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol));
+    return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol));
   }
 
   /// @}

gtsam/geometry/Point2.cpp

@@ -27,7 +27,7 @@ namespace gtsam {
 double norm2(const Point2& p, OptionalJacobian<1,2> H) {
   double r = std::sqrt(p.x() * p.x() + p.y() * p.y());
   if (H) {
-    if (fabs(r) > 1e-10)
+    if (std::abs(r) > 1e-10)
       *H << p.x() / r, p.y() / r;
     else
       *H << 1, 1;  // really infinity, why 1 ?
@@ -59,7 +59,7 @@ void Point2::print(const string& s) const {
 
 /* ************************************************************************* */
 bool Point2::equals(const Point2& q, double tol) const {
-  return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol);
+  return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol);
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point2.h

@@ -37,7 +37,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Point2 : public Vector2 {
+class Point2 : public Vector2 {
 private:
 
 public:
@@ -66,10 +66,10 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const;
+  GTSAM_EXPORT void print(const std::string& s = "") const;
 
   /// equals with an tolerance, prints out message if unequal
-  bool equals(const Point2& q, double tol = 1e-9) const;
+  GTSAM_EXPORT bool equals(const Point2& q, double tol = 1e-9) const;
 
   /// @}
   /// @name Group
@@ -86,10 +86,10 @@ public:
   Point2 unit() const { return *this/norm(); }
 
   /** norm of point, with derivative */
-  double norm(OptionalJacobian<1,2> H = boost::none) const;
+  GTSAM_EXPORT double norm(OptionalJacobian<1,2> H = boost::none) const;
 
   /** distance between two points */
-  double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
+  GTSAM_EXPORT double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
       OptionalJacobian<1,2> H2 = boost::none) const;
 
   /// @}
@@ -124,9 +124,9 @@ public:
   static Vector2 Logmap(const Point2& p) { return p;}
   static Point2 Expmap(const Vector2& v) { return Point2(v);}
   inline double dist(const Point2& p2) const {return distance(p2);}
-  static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
+  GTSAM_EXPORT static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
-  static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
+  GTSAM_EXPORT static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
-  static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
+  GTSAM_EXPORT static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
   /// @}
 #endif
 

gtsam/geometry/Point3.cpp

@@ -24,8 +24,8 @@ namespace gtsam {
 
 #ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
 bool Point3::equals(const Point3 &q, double tol) const {
-  return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
+  return (std::abs(x() - q.x()) < tol && std::abs(y() - q.y()) < tol &&
-          fabs(z() - q.z()) < tol);
+          std::abs(z() - q.z()) < tol);
 }
 
 void Point3::print(const string& s) const {
@@ -98,7 +98,7 @@ double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
 double norm3(const Point3 &p, OptionalJacobian<1, 3> H) {
   double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
   if (H) {
-    if (fabs(r) > 1e-10)
+    if (std::abs(r) > 1e-10)
       *H << p.x() / r, p.y() / r, p.z() / r;
     else
       *H << 1, 1, 1;  // really infinity, why 1 ?

gtsam/geometry/Point3.h

@@ -42,7 +42,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-class GTSAM_EXPORT Point3 : public Vector3 {
+class Point3 : public Vector3 {
 
   public:
 
@@ -63,10 +63,10 @@ class GTSAM_EXPORT Point3 : public Vector3 {
     /// @{
 
     /** print with optional string */
-    void print(const std::string& s = "") const;
+	GTSAM_EXPORT void print(const std::string& s = "") const;
 
     /** equals with an tolerance */
-    bool equals(const Point3& p, double tol = 1e-9) const;
+	GTSAM_EXPORT bool equals(const Point3& p, double tol = 1e-9) const;
 
     /// @}
     /// @name Group
@@ -80,21 +80,21 @@ class GTSAM_EXPORT Point3 : public Vector3 {
     /// @{
 
     /** distance between two points */
-    double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
+	GTSAM_EXPORT double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
                     OptionalJacobian<1, 3> H2 = boost::none) const;
 
     /** Distance of the point from the origin, with Jacobian */
-    double norm(OptionalJacobian<1,3> H = boost::none) const;
+	GTSAM_EXPORT double norm(OptionalJacobian<1,3> H = boost::none) const;
 
     /** normalize, with optional Jacobian */
-    Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
+	GTSAM_EXPORT Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
 
     /** cross product @return this x q */
-    Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
+	GTSAM_EXPORT Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
                                   OptionalJacobian<3, 3> H_q = boost::none) const;
 
     /** dot product @return this * q*/
-    double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
+	GTSAM_EXPORT double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
                                 OptionalJacobian<1, 3> H_q = boost::none) const;
 
     /// @}
@@ -130,9 +130,9 @@ class GTSAM_EXPORT Point3 : public Vector3 {
     static Point3 Expmap(const Vector3& v) { return Point3(v);}
     inline double dist(const Point3& q) const { return (q - *this).norm(); }
     Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
-    Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
+	GTSAM_EXPORT Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
                OptionalJacobian<3, 3> H2 = boost::none) const;
-    Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
+	GTSAM_EXPORT Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
                OptionalJacobian<3, 3> H2 = boost::none) const;
   /// @}
 #endif

gtsam/geometry/Pose2.cpp

@@ -142,7 +142,7 @@ Matrix3 Pose2::adjointMap(const Vector3& v) {
 Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
   double alpha = v[2];
   Matrix3 J;
-  if (fabs(alpha) > 1e-5) {
+  if (std::abs(alpha) > 1e-5) {
     // Chirikjian11book2, pg. 36
     /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26
      * Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation
@@ -174,7 +174,7 @@ Matrix3 Pose2::LogmapDerivative(const Pose2& p) {
   Vector3 v = Logmap(p);
   double alpha = v[2];
   Matrix3 J;
-  if (fabs(alpha) > 1e-5) {
+  if (std::abs(alpha) > 1e-5) {
     double alphaInv = 1/alpha;
     double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
     double v1 = v[0], v2 = v[1];

gtsam/geometry/Pose2.h

@@ -33,7 +33,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
+class Pose2: public LieGroup<Pose2, 3> {
 
 public:
 
@@ -97,10 +97,10 @@ public:
   /// @{
 
   /** print with optional string */
-  void print(const std::string& s = "") const;
+  GTSAM_EXPORT void print(const std::string& s = "") const;
 
   /** assert equality up to a tolerance */
-  bool equals(const Pose2& pose, double tol = 1e-9) const;
+  GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
 
   /// @}
   /// @name Group
@@ -110,7 +110,7 @@ public:
   inline static Pose2 identity() { return Pose2(); }
 
   /// inverse
-  Pose2 inverse() const;
+  GTSAM_EXPORT Pose2 inverse() const;
 
   /// compose syntactic sugar
   inline Pose2 operator*(const Pose2& p2) const {
@@ -122,16 +122,16 @@ public:
   /// @{
 
   ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
-  static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
+  GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
 
   ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
-  static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
+  GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
 
   /**
    * Calculate Adjoint map
    * 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;
+  GTSAM_EXPORT Matrix3 AdjointMap() const;
 
   /// Apply AdjointMap to twist xi
   inline Vector3 Adjoint(const Vector3& xi) const {
@@ -141,7 +141,7 @@ public:
   /**
    * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
    */
-  static Matrix3 adjointMap(const Vector3& v);
+  GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
 
   /**
    * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
@@ -177,15 +177,15 @@ public:
   }
 
   /// Derivative of Expmap
-  static Matrix3 ExpmapDerivative(const Vector3& v);
+  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
 
   /// Derivative of Logmap
-  static Matrix3 LogmapDerivative(const Pose2& v);
+  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
 
   // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
   struct ChartAtOrigin {
-    static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
+	GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
-    static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
+	GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
   };
 
   using LieGroup<Pose2, 3>::inverse; // version with derivative
@@ -195,12 +195,12 @@ public:
   /// @{
 
   /** Return point coordinates in pose coordinate frame */
-  Point2 transformTo(const Point2& point,
+  GTSAM_EXPORT Point2 transformTo(const Point2& point,
       OptionalJacobian<2, 3> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
   /** Return point coordinates in global frame */
-  Point2 transformFrom(const Point2& point,
+  GTSAM_EXPORT Point2 transformFrom(const Point2& point,
       OptionalJacobian<2, 3> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
@@ -233,14 +233,14 @@ public:
   inline const Rot2&   rotation() const { return r_; }
 
   //// return transformation matrix
-  Matrix3 matrix() const;
+  GTSAM_EXPORT Matrix3 matrix() const;
 
   /**
    * Calculate bearing to a landmark
    * @param point 2D location of landmark
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  Rot2 bearing(const Point2& point,
+  GTSAM_EXPORT Rot2 bearing(const Point2& point,
                OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
 
   /**
@@ -248,7 +248,7 @@ public:
    * @param point SO(2) location of other pose
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  Rot2 bearing(const Pose2& pose,
+  GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
                OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
 
   /**
@@ -256,7 +256,7 @@ public:
    * @param point 2D location of landmark
    * @return range (double)
    */
-  double range(const Point2& point,
+  GTSAM_EXPORT double range(const Point2& point,
       OptionalJacobian<1, 3> H1=boost::none,
       OptionalJacobian<1, 2> H2=boost::none) const;
 
@@ -265,7 +265,7 @@ public:
    * @param point 2D location of other pose
    * @return range (double)
    */
-  double range(const Pose2& point,
+  GTSAM_EXPORT double range(const Pose2& point,
       OptionalJacobian<1, 3> H1=boost::none,
       OptionalJacobian<1, 3> H2=boost::none) const;
 

gtsam/geometry/Pose3.cpp

@@ -221,7 +221,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
 #else
   // The closed-form formula in Barfoot14tro eq. (102)
   double phi = w.norm();
-  if (fabs(phi)>1e-5) {
+  if (std::abs(phi)>1e-5) {
     const double sinPhi = sin(phi), cosPhi = cos(phi);
     const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
     // Invert the sign of odd-order terms to have the right Jacobian

gtsam/geometry/Rot2.cpp

@@ -25,7 +25,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Rot2 Rot2::fromCosSin(double c, double s) {
-  if (fabs(c * c + s * s - 1.0) > 1e-9) {
+  if (std::abs(c * c + s * s - 1.0) > 1e-9) {
     double norm_cs = sqrt(c*c + s*s);
     c = c/norm_cs;
     s = s/norm_cs;
@@ -46,13 +46,13 @@ void Rot2::print(const string& s) const {
 
 /* ************************************************************************* */
 bool Rot2::equals(const Rot2& R, double tol) const {
-  return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
+  return std::abs(c_ - R.c_) <= tol && std::abs(s_ - R.s_) <= tol;
 }
 
 /* ************************************************************************* */
 Rot2& Rot2::normalize() {
   double scale = c_*c_ + s_*s_;
-  if(fabs(scale-1.0)>1e-10) {
+  if(std::abs(scale-1.0)>1e-10) {
     scale = pow(scale, -0.5);
     c_ *= scale;
     s_ *= scale;
@@ -115,7 +115,7 @@ Point2 Rot2::unrotate(const Point2& p,
 /* ************************************************************************* */
 Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
-  if(fabs(n) > 1e-5) {
+  if(std::abs(n) > 1e-5) {
     if (H) {
       *H << -y / d2, x / d2;
     }

gtsam/geometry/SO3.h

@@ -62,13 +62,13 @@ public:
   }
 
   /// Static, named constructor TODO think about relation with above
-  static SO3 AxisAngle(const Vector3& axis, double theta);
+  GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta);
 
   /// @}
   /// @name Testable
   /// @{
 
-  void print(const std::string& s) const;
+  GTSAM_EXPORT void print(const std::string& s) const;
 
   bool equals(const SO3 & R, double tol) const {
     return equal_with_abs_tol(*this, R, tol);
@@ -96,19 +96,19 @@ public:
    * Exponential map at identity - create a rotation from canonical coordinates
    * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
    */
-  static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
+  GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
 
   /// Derivative of Expmap
-  static Matrix3 ExpmapDerivative(const Vector3& omega);
+  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& omega);
 
   /**
    * Log map at identity - returns the canonical coordinates
    * \f$ [R_x,R_y,R_z] \f$ of this rotation
    */
-  static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
+  GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
 
   /// Derivative of Logmap
-  static Matrix3 LogmapDerivative(const Vector3& omega);
+  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Vector3& omega);
 
   Matrix3 AdjointMap() const {
     return *this;
@@ -156,14 +156,14 @@ class GTSAM_EXPORT ExpmapFunctor {
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
+class DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
   double a, b;
   Matrix3 dexp_;
 
  public:
   /// Constructor with element of Lie algebra so(3)
-  DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
+  GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@@ -174,11 +174,11 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Matrix3& dexp() const { return dexp_; }
 
   /// Multiplies with dexp(), with optional derivatives
-  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
+  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
                     OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /// Multiplies with dexp().inverse(), with optional derivatives
-  Vector3 applyInvDexp(const Vector3& v,
+  GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
                        OptionalJacobian<3, 3> H1 = boost::none,
                        OptionalJacobian<3, 3> H2 = boost::none) const;
 };

gtsam/geometry/StereoPoint2.h

@@ -62,8 +62,8 @@ public:
 
   /** equals */
   bool equals(const StereoPoint2& q, double tol = 1e-9) const {
-    return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol
+    return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol
-        && fabs(v_ - q.v_) < tol);
+        && std::abs(v_ - q.v_) < tol);
   }
 
   /// @}

gtsam/geometry/Unit3.cpp

@@ -68,7 +68,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
 /* ************************************************************************* */
 // Get the axis of rotation with the minimum projected length of the point
 static Point3 CalculateBestAxis(const Point3& n) {
-  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
+  double mx = std::abs(n.x()), my = std::abs(n.y()), mz = std::abs(n.z());
   if ((mx <= my) && (mx <= mz)) {
     return Point3(1.0, 0.0, 0.0);
   } else if ((my <= mx) && (my <= mz)) {

gtsam/geometry/Unit3.h

@@ -39,7 +39,7 @@
 namespace gtsam {
 
 /// Represents a 3D point on a unit sphere.
-class GTSAM_EXPORT Unit3 {
+class Unit3 {
 
 private:
 
@@ -94,11 +94,11 @@ public:
   }
 
   /// Named constructor from Point3 with optional Jacobian
-  static Unit3 FromPoint3(const Point3& point, //
+  GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, //
       OptionalJacobian<2, 3> H = boost::none);
 
   /// Random direction, using boost::uniform_on_sphere
-  static Unit3 Random(boost::mt19937 & rng);
+  GTSAM_EXPORT static Unit3 Random(boost::mt19937 & rng);
 
   /// @}
 
@@ -108,7 +108,7 @@ public:
   friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
 
   /// The print fuction
-  void print(const std::string& s = std::string()) const;
+  GTSAM_EXPORT void print(const std::string& s = std::string()) const;
 
   /// The equals function with tolerance
   bool equals(const Unit3& s, double tol = 1e-9) const {
@@ -125,16 +125,16 @@ public:
    * tangent to the sphere at the current direction.
    * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
    */
-  const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
+  GTSAM_EXPORT const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
-  Matrix3 skew() const;
+  GTSAM_EXPORT Matrix3 skew() const;
 
   /// Return unit-norm Point3
-  Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
+  GTSAM_EXPORT Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
 
   /// Return unit-norm Vector
-  Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
+  GTSAM_EXPORT Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
 
   /// Return scaled direction as Point3
   friend Point3 operator*(double s, const Unit3& d) {
@@ -142,20 +142,20 @@ public:
   }
 
   /// Return dot product with q
-  double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
+  GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
                              OptionalJacobian<1,2> H2 = boost::none) const;
 
   /// Signed, vector-valued error between two directions
   /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
-  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
+  GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
 
   /// Signed, vector-valued error between two directions
   /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
-  Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
+  GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
                       OptionalJacobian<2, 2> H_q = boost::none) const;
 
   /// Distance between two directions
-  double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
+  GTSAM_EXPORT double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
 
   /// Cross-product between two Unit3s
   Unit3 cross(const Unit3& q) const {
@@ -188,10 +188,10 @@ public:
   };
 
   /// The retract function
-  Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
+  GTSAM_EXPORT Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
 
   /// The local coordinates function
-  Vector2 localCoordinates(const Unit3& s) const;
+  GTSAM_EXPORT Vector2 localCoordinates(const Unit3& s) const;
 
   /// @}
 

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -206,7 +206,7 @@ TEST( CalibratedCamera, DBackprojectFromCamera)
 static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) {
   return CalibratedCamera(pose).backproject(point, depth);
 }
-TEST( PinholePose, Dbackproject)
+TEST( PinholePose, DbackprojectCalibCamera)
 {
   Matrix36 Dpose;
   Matrix31 Ddepth;

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -218,10 +218,10 @@ TEST (EssentialMatrix, epipoles) {
   }
 
   // check rank 2 constraint
-  CHECK(fabs(S(2))<1e-10);
+  CHECK(std::abs(S(2))<1e-10);
 
   // check epipoles not at infinity
-  CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10);
+  CHECK(std::abs(U(2,2))>1e-10 && std::abs(V(2,2))>1e-10);
 
   // Check epipoles
 

gtsam/geometry/tests/testPinholePose.cpp

@@ -58,6 +58,7 @@ TEST( PinholePose, constructor)
 }
 
 //******************************************************************************
+/* Already in testPinholeCamera??? 
 TEST(PinholeCamera, Pose) {
 
   Matrix actualH;
@@ -69,6 +70,7 @@ TEST(PinholeCamera, Pose) {
   Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
+*/
 
 /* ************************************************************************* */
 TEST( PinholePose, lookat)
@@ -207,7 +209,7 @@ static Point3 backproject(const Pose3& pose, const Cal3_S2& cal,
   return Camera(pose, cal.vector()).backproject(p, depth);
 }
 
-TEST( PinholePose, Dbackproject)
+TEST( PinholePose, DbackprojectRegCamera)
 {
   Matrix36 Dpose;
   Matrix31 Ddepth;

gtsam/geometry/tests/testQuaternion.cpp

@@ -81,13 +81,14 @@ TEST(Quaternion , Compose) {
 }
 
 //******************************************************************************
-Vector3 z_axis(0, 0, 1);
+Vector3 Q_z_axis(0, 0, 1);
-Q id(Eigen::AngleAxisd(0, z_axis));
+Q id(Eigen::AngleAxisd(0, Q_z_axis));
-Q R1(Eigen::AngleAxisd(1, z_axis));
+Q R1(Eigen::AngleAxisd(1, Q_z_axis));
 Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
 
 //******************************************************************************
 TEST(Quaternion , Between) {
+  Vector3 z_axis(0, 0, 1);
   Q q1(Eigen::AngleAxisd(0.2, z_axis));
   Q q2(Eigen::AngleAxisd(0.1, z_axis));
 
@@ -98,6 +99,7 @@ TEST(Quaternion , Between) {
 
 //******************************************************************************
 TEST(Quaternion , Inverse) {
+  Vector3 z_axis(0, 0, 1);
   Q q1(Eigen::AngleAxisd(0.1, z_axis));
   Q expected(Eigen::AngleAxisd(-0.1, z_axis));
 

gtsam/geometry/triangulation.h

@@ -386,7 +386,7 @@ private:
 /**
  * TriangulationResult is an optional point, along with the reasons why it is invalid.
  */
-class GTSAM_EXPORT TriangulationResult: public boost::optional<Point3> {
+class TriangulationResult: public boost::optional<Point3> {
   enum Status {
     VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
   };

gtsam/inference/BayesTree-inst.h

@@ -36,19 +36,20 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CLIQUE>
   BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
-    BayesTreeCliqueData data;
+    BayesTreeCliqueData stats;
-    for(const sharedClique& root: roots_)
+    for (const sharedClique& root : roots_) getCliqueData(root, &stats);
-      getCliqueData(data, root);
+    return stats;
-    return data;
   }
 
   /* ************************************************************************* */
-  template<class CLIQUE>
+  template <class CLIQUE>
-  void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
+  void BayesTree<CLIQUE>::getCliqueData(sharedClique clique,
-    data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
+                                        BayesTreeCliqueData* stats) const {
-    data.separatorSizes.push_back(clique->conditional()->nrParents());
+    const auto conditional = clique->conditional();
-    for(sharedClique c: clique->children) {
+    stats->conditionalSizes.push_back(conditional->nrFrontals());
-      getCliqueData(data, c);
+    stats->separatorSizes.push_back(conditional->nrParents());
+    for (sharedClique c : clique->children) {
+      getCliqueData(c, stats);
     }
   }
 
@@ -133,34 +134,26 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  // TODO: Clean up
   namespace {
-    template<class FACTOR, class CLIQUE>
+  template <class FACTOR, class CLIQUE>
-    int _pushClique(FactorGraph<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) {
+  struct _pushCliqueFunctor {
-      fg.push_back(clique->conditional_);
+    _pushCliqueFunctor(FactorGraph<FACTOR>* graph_) : graph(graph_) {}
+    FactorGraph<FACTOR>* graph;
+    int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
+      graph->push_back(clique->conditional_);
       return 0;
     }
-
+  };
-    template<class FACTOR, class CLIQUE>
+  }  // namespace
-    struct _pushCliqueFunctor {
-      _pushCliqueFunctor(FactorGraph<FACTOR>& graph_) : graph(graph_) {}
-      FactorGraph<FACTOR>& graph;
-      int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
-        graph.push_back(clique->conditional_);
-        return 0;
-      }
-    };
-  }
 
   /* ************************************************************************* */
-  template<class CLIQUE>
+  template <class CLIQUE>
-  void BayesTree<CLIQUE>::addFactorsToGraph(FactorGraph<FactorType>& graph) const
+  void BayesTree<CLIQUE>::addFactorsToGraph(
-  {
+      FactorGraph<FactorType>* graph) const {
     // Traverse the BayesTree and add all conditionals to this graph
-    int data = 0; // Unused
+    int data = 0;  // Unused
-    _pushCliqueFunctor<FactorType,CLIQUE> functor(graph);
+    _pushCliqueFunctor<FactorType, CLIQUE> functor(graph);
-    treeTraversal::DepthFirstForest(*this, data, functor); // FIXME: sort of works?
+    treeTraversal::DepthFirstForest(*this, data, functor);
-//    treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique<FactorType,CLIQUE>, boost::ref(graph), _1));
   }
 
   /* ************************************************************************* */
@@ -434,50 +427,51 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<class CLIQUE>
+  template <class CLIQUE>
-  void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
+  void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
-  {
+                                     Cliques* orphans) {
     // base case is NULL, if so we do nothing and return empties above
     if (clique) {
-
       // remove the clique from orphans in case it has been added earlier
-      orphans.remove(clique);
+      orphans->remove(clique);
 
       // remove me
       this->removeClique(clique);
 
       // remove path above me
-      this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
+      this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn,
+                       orphans);
 
-      // add children to list of orphans (splice also removed them from clique->children_)
+      // add children to list of orphans (splice also removed them from
-      orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end());
+      // clique->children_)
+      orphans->insert(orphans->begin(), clique->children.begin(),
+                      clique->children.end());
       clique->children.clear();
 
-      bn.push_back(clique->conditional_);
+      bn->push_back(clique->conditional_);
-
     }
   }
 
-  /* ************************************************************************* */
+  /* *************************************************************************
-  template<class CLIQUE>
+   */
-  void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans)
+  template <class CLIQUE>
-  {
+  void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType* bn,
+                                    Cliques* orphans) {
+    gttic(removetop);
     // process each key of the new factor
-    for(const Key& j: keys)
+    for (const Key& j : keys) {
-    {
       // get the clique
-      // TODO: Nodes will be searched again in removeClique
+      // TODO(frank): Nodes will be searched again in removeClique
       typename Nodes::const_iterator node = nodes_.find(j);
-      if(node != nodes_.end()) {
+      if (node != nodes_.end()) {
         // remove path from clique to root
         this->removePath(node->second, bn, orphans);
       }
     }
 
     // Delete cachedShortcuts for each orphan subtree
-    //TODO: Consider Improving
+    // TODO(frank): Consider Improving
-    for(sharedClique& orphan: orphans)
+    for (sharedClique& orphan : *orphans) orphan->deleteCachedShortcuts();
-      orphan->deleteCachedShortcuts();
   }
 
   /* ************************************************************************* */

gtsam/inference/BayesTree.h

@@ -89,14 +89,14 @@ namespace gtsam {
     /** Map from keys to Clique */
     typedef ConcurrentMap<Key, sharedClique> Nodes;
 
+    /** Root cliques */
+    typedef FastVector<sharedClique> Roots;
+
   protected:
 
     /** Map from indices to Clique */
     Nodes nodes_;
 
-    /** Root cliques */
-    typedef FastVector<sharedClique> Roots;
-
     /** Root cliques */
     Roots roots_;
 
@@ -208,13 +208,13 @@ namespace gtsam {
      * Remove path from clique to root and return that path as factors
      * plus a list of orphaned subtree roots. Used in removeTop below.
      */
-    void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans);
+    void removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans);
 
     /**
      * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
      * Factors and orphans are added to the in/out arguments.
      */
-    void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans);
+    void removeTop(const KeyVector& keys, BayesNetType* bn, Cliques* orphans);
 
     /**
      * Remove the requested subtree. */
@@ -229,7 +229,7 @@ namespace gtsam {
     void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
 
     /** Add all cliques in this BayesTree to the specified factor graph */
-    void addFactorsToGraph(FactorGraph<FactorType>& graph) const;
+    void addFactorsToGraph(FactorGraph<FactorType>* graph) const;
 
   protected:
 
@@ -238,7 +238,7 @@ namespace gtsam {
         int parentnum = 0) const;
 
     /** Gather data on a single clique */
-    void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const;
+    void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const;
 
     /** remove a clique: warning, can result in a forest */
     void removeClique(sharedClique clique);
@@ -249,7 +249,26 @@ namespace gtsam {
     // Friend JunctionTree because it directly fills roots and nodes index.
     template<class BAYESRTEE, class GRAPH> friend class EliminatableClusterTree;
 
-  private:
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+   public:
+    /// @name Deprecated
+    /// @{
+    void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) {
+      removePath(clique, &bn, &orphans);
+    }
+    void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) {
+      removeTop(keys, &bn, &orphans);
+    }
+    void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const {
+      getCliqueData(clique, &stats);
+    }
+    void addFactorsToGraph(FactorGraph<FactorType>& graph) const{
+      addFactorsToGraph(& graph);
+    }
+    /// @}
+#endif
+
+   private:
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

gtsam/inference/EliminationTree-inst.h

@@ -98,7 +98,7 @@ namespace gtsam {
       for (size_t j = 0; j < n; j++)
       {
         // Retrieve the factors involving this variable and create the current node
-        const VariableIndex::Factors& factors = structure[order[j]];
+        const FactorIndices& factors = structure[order[j]];
         const sharedNode node = boost::make_shared<Node>();
         node->key = order[j];
 

gtsam/inference/FactorGraph-inst.h

@@ -26,74 +26,105 @@
 #include <boost/bind.hpp>
 
 #include <stdio.h>
+#include <algorithm>
+#include <iostream>  // for cout :-(
 #include <sstream>
-#include <iostream> // for cout :-(
+#include <string>
 
 namespace gtsam {
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  template<class FACTOR>
+template <class FACTOR>
-  void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
+void FactorGraph<FACTOR>::print(const std::string& s,
-    std::cout << s << std::endl;
+                                const KeyFormatter& formatter) const {
-    std::cout << "size: " << size() << std::endl;
+  std::cout << s << std::endl;
-    for (size_t i = 0; i < factors_.size(); i++) {
+  std::cout << "size: " << size() << std::endl;
-      std::stringstream ss;
+  for (size_t i = 0; i < factors_.size(); i++) {
-      ss << "factor " << i << ": ";
+    std::stringstream ss;
-      if (factors_[i])
+    ss << "factor " << i << ": ";
-        factors_[i]->print(ss.str(), formatter);
+    if (factors_[i]) factors_[i]->print(ss.str(), formatter);
+  }
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
+  // check whether the two factor graphs have the same number of factors.
+  if (factors_.size() != fg.size()) return false;
+
+  // check whether the factors are the same, in same order.
+  for (size_t i = 0; i < factors_.size(); i++) {
+    sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
+    if (f1 == NULL && f2 == NULL) continue;
+    if (f1 == NULL || f2 == NULL) return false;
+    if (!f1->equals(*f2, tol)) return false;
+  }
+  return true;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+size_t FactorGraph<FACTOR>::nrFactors() const {
+  size_t size_ = 0;
+  for (const sharedFactor& factor : factors_)
+    if (factor) size_++;
+  return size_;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+KeySet FactorGraph<FACTOR>::keys() const {
+  KeySet keys;
+  for (const sharedFactor& factor : this->factors_) {
+    if (factor) keys.insert(factor->begin(), factor->end());
+  }
+  return keys;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+KeyVector FactorGraph<FACTOR>::keyVector() const {
+  KeyVector keys;
+  keys.reserve(2 * size());  // guess at size
+  for (const sharedFactor& factor : factors_)
+    if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
+  std::sort(keys.begin(), keys.end());
+  auto last = std::unique(keys.begin(), keys.end());
+  keys.erase(last, keys.end());
+  return keys;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+template <typename CONTAINER, typename>
+FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
+                                               bool useEmptySlots) {
+  const size_t num_factors = factors.size();
+  FactorIndices newFactorIndices(num_factors);
+  if (useEmptySlots) {
+    size_t i = 0;
+    for (size_t j = 0; j < num_factors; ++j) {
+      // Loop to find the next available factor slot
+      do {
+        if (i >= size())
+          // Make room for remaining factors, happens only once.
+          resize(size() + num_factors - j);
+        else if (at(i))
+          ++i;  // Move on to the next slot or past end.
+        else
+          break;  // We found an empty slot, break to fill it.
+      } while (true);
+
+      // Use the current slot, updating graph and newFactorSlots.
+      at(i) = factors[j];
+      newFactorIndices[j] = i;
     }
+  } else {
+    // We're not looking for unused slots, so just add the factors at the end.
+    for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
+    push_back(factors);
   }
+  return newFactorIndices;
+}
 
-  /* ************************************************************************* */
+}  // namespace gtsam
-  template<class FACTOR>
-  bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
-    /** check whether the two factor graphs have the same number of factors_ */
-    if (factors_.size() != fg.size()) return false;
-
-    /** check whether the factors_ are the same */
-    for (size_t i = 0; i < factors_.size(); i++) {
-      // TODO: Doesn't this force order of factor insertion?
-      sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
-      if (f1 == NULL && f2 == NULL) continue;
-      if (f1 == NULL || f2 == NULL) return false;
-      if (!f1->equals(*f2, tol)) return false;
-    }
-    return true;
-  }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  size_t FactorGraph<FACTOR>::nrFactors() const {
-    size_t size_ = 0;
-    for(const sharedFactor& factor: factors_)
-      if (factor) size_++;
-    return size_;
-  }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  KeySet FactorGraph<FACTOR>::keys() const {
-    KeySet keys;
-    for(const sharedFactor& factor: this->factors_) {
-      if(factor)
-        keys.insert(factor->begin(), factor->end());
-    }
-    return keys;
-  }
-
-  /* ************************************************************************* */
-  template <class FACTOR>
-  KeyVector FactorGraph<FACTOR>::keyVector() const {
-    KeyVector keys;
-    keys.reserve(2 * size());  // guess at size
-    for (const sharedFactor& factor: factors_)
-      if (factor)
-        keys.insert(keys.end(), factor->begin(), factor->end());
-    std::sort(keys.begin(), keys.end());
-    auto last = std::unique(keys.begin(), keys.end());
-    keys.erase(last, keys.end());
-    return keys;
-  }
-
-  /* ************************************************************************* */
-} // namespace gtsam

gtsam/inference/FactorGraph.h

@@ -22,350 +22,382 @@
 
 #pragma once
 
-#include <gtsam/base/Testable.h>
 #include <gtsam/base/FastVector.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
 
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/vector.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
 
+#include <string>
 #include <type_traits>
 #include <utility>
 
 namespace gtsam {
+/// Define collection type:
+typedef FastVector<FactorIndex> FactorIndices;
 
-  // Forward declarations
+// Forward declarations
-  template<class CLIQUE> class BayesTree;
+template <class CLIQUE>
+class BayesTree;
 
-  /** Helper */
+/** Helper */
-  template<class C>
+template <class C>
-  class CRefCallPushBack
+class CRefCallPushBack {
-  {
+  C& obj;
-    C& obj;
-  public:
-    CRefCallPushBack(C& obj) : obj(obj) {}
-    template<typename A>
-    void operator()(const A& a) { obj.push_back(a); }
-  };
 
-  /** Helper */
+ public:
-  template<class C>
+  explicit CRefCallPushBack(C& obj) : obj(obj) {}
-  class RefCallPushBack
+  template <typename A>
-  {
+  void operator()(const A& a) {
-    C& obj;
+    obj.push_back(a);
-  public:
+  }
-    RefCallPushBack(C& obj) : obj(obj) {}
+};
-    template<typename A>
-    void operator()(A& a) { obj.push_back(a); }
-  };
 
-  /** Helper */
+/** Helper */
-  template<class C>
+template <class C>
-  class CRefCallAddCopy
+class RefCallPushBack {
-  {
+  C& obj;
-    C& obj;
+
-  public:
+ public:
-    CRefCallAddCopy(C& obj) : obj(obj) {}
+  explicit RefCallPushBack(C& obj) : obj(obj) {}
-    template<typename A>
+  template <typename A>
-    void operator()(const A& a) { obj.addCopy(a); }
+  void operator()(A& a) {
-  };
+    obj.push_back(a);
+  }
+};
+
+/** Helper */
+template <class C>
+class CRefCallAddCopy {
+  C& obj;
+
+ public:
+  explicit CRefCallAddCopy(C& obj) : obj(obj) {}
+  template <typename A>
+  void operator()(const A& a) {
+    obj.addCopy(a);
+  }
+};
+
+/**
+ * A factor graph is a bipartite graph with factor nodes connected to variable
+ * nodes. In this class, however, only factor nodes are kept around.
+ * \nosubgrouping
+ */
+template <class FACTOR>
+class FactorGraph {
+ public:
+  typedef FACTOR FactorType;  ///< factor type
+  typedef boost::shared_ptr<FACTOR>
+      sharedFactor;  ///< Shared pointer to a factor
+  typedef sharedFactor value_type;
+  typedef typename FastVector<sharedFactor>::iterator iterator;
+  typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
+
+ private:
+  typedef FactorGraph<FACTOR> This;  ///< Typedef for this class
+  typedef boost::shared_ptr<This>
+      shared_ptr;  ///< Shared pointer for this class
+
+  /// Check if a DERIVEDFACTOR is in fact derived from FactorType.
+  template <typename DERIVEDFACTOR>
+  using IsDerived = typename std::enable_if<
+      std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
+
+  /// Check if T has a value_type derived from FactorType.
+  template <typename T>
+  using HasDerivedValueType = typename std::enable_if<
+      std::is_base_of<FactorType, typename T::value_type>::value>::type;
+
+  /// Check if T has a value_type derived from FactorType.
+  template <typename T>
+  using HasDerivedElementType = typename std::enable_if<std::is_base_of<
+      FactorType, typename T::value_type::element_type>::value>::type;
+
+ protected:
+  /** concept check, makes sure FACTOR defines print and equals */
+  GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
+
+  /** Collection of factors */
+  FastVector<sharedFactor> factors_;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** Default constructor */
+  FactorGraph() {}
+
+  /** Constructor from iterator over factors (shared_ptr or plain objects) */
+  template <typename ITERATOR>
+  FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
+    push_back(firstFactor, lastFactor);
+  }
+
+  /** Construct from container of factors (shared_ptr or plain objects) */
+  template <class CONTAINER>
+  explicit FactorGraph(const CONTAINER& factors) {
+    push_back(factors);
+  }
+
+  /// @}
+
+ public:
+  /// @name Adding Single Factors
+  /// @{
 
   /**
-   * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
+   * Reserve space for the specified number of factors if you know in
-   * In this class, however, only factor nodes are kept around.
+   * advance how many there will be (works like FastVector::reserve).
-   * \nosubgrouping
    */
-  template<class FACTOR>
+  void reserve(size_t size) { factors_.reserve(size); }
-  class FactorGraph {
 
-  public:
+  /// Add a factor directly using a shared_ptr.
-    typedef FACTOR FactorType;  ///< factor type
+  template <class DERIVEDFACTOR>
-    typedef boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
+  IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
-    typedef sharedFactor value_type;
+    factors_.push_back(boost::shared_ptr<FACTOR>(factor));
-    typedef typename FastVector<sharedFactor>::iterator iterator;
+  }
-    typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
 
-  private:
+  /// Emplace a shared pointer to factor of given type.
-    typedef FactorGraph<FACTOR> This;  ///< Typedef for this class
+  template <class DERIVEDFACTOR, class... Args>
-    typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer for this class
+  IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
+    factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
+        Eigen::aligned_allocator<DERIVEDFACTOR>(),
+        std::forward<Args>(args)...));
+  }
 
-  protected:
+  /**
-    /** concept check, makes sure FACTOR defines print and equals */
+   * Add a factor by value, will be copy-constructed (use push_back with a
-    GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
+   * shared_ptr to avoid the copy).
+   */
+  template <class DERIVEDFACTOR>
+  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
+    factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
+        Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
+  }
 
-    /** Collection of factors */
+  /// `add` is a synonym for push_back.
-    FastVector<sharedFactor> factors_;
+  template <class DERIVEDFACTOR>
+  IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
+    push_back(factor);
+  }
 
-    /// @name Standard Constructors
+  /// `+=` works well with boost::assign list inserter.
-    /// @{
+  template <class DERIVEDFACTOR>
+  typename std::enable_if<
+      std::is_base_of<FactorType, DERIVEDFACTOR>::value,
+      boost::assign::list_inserter<RefCallPushBack<This>>>::type
+  operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
+    return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
+        factor);
+  }
 
-    /** Default constructor */
+  /// @}
-    FactorGraph() {}
+  /// @name Adding via iterators
+  /// @{
 
-    /** Constructor from iterator over factors (shared_ptr or plain objects) */
+  /**
-    template<typename ITERATOR>
+   * Push back many factors with an iterator over shared_ptr (factors are not
-    FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
+   * copied)
+   */
+  template <typename ITERATOR>
+  HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
+                                            ITERATOR lastFactor) {
+    factors_.insert(end(), firstFactor, lastFactor);
+  }
 
-    /** Construct from container of factors (shared_ptr or plain objects) */
+  /// Push back many factors with an iterator (factors are copied)
-    template<class CONTAINER>
+  template <typename ITERATOR>
-    explicit FactorGraph(const CONTAINER& factors) { push_back(factors); }
+  HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
+                                          ITERATOR lastFactor) {
+    for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
+  }
 
-    /// @}
+  /// @}
-    /// @name Advanced Constructors
+  /// @name Adding via container
-    /// @{
+  /// @{
 
-    // TODO: are these needed?
+  /**
+   * Push back many factors as shared_ptr's in a container (factors are not
+   * copied)
+   */
+  template <typename CONTAINER>
+  HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
+    push_back(container.begin(), container.end());
+  }
 
-    ///**
+  /// Push back non-pointer objects in a container (factors are copied).
-    // * @brief Constructor from a Bayes net
+  template <typename CONTAINER>
-    // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
+  HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
-    // * @return a factor graph with all the conditionals, as factors
+    push_back(container.begin(), container.end());
-    // */
+  }
-    //template<class CONDITIONAL>
-    //FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
 
-    ///** convert from Bayes tree */
+  /**
-    //template<class CONDITIONAL, class CLIQUE>
+   * Add a factor or container of factors, including STL collections,
-    //FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
+   * BayesTrees, etc.
+   */
+  template <class FACTOR_OR_CONTAINER>
+  void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
+    push_back(factorOrContainer);
+  }
 
-    ///** convert from a derived type */
+  /**
-    //template<class DERIVEDFACTOR>
+   * Add a factor or container of factors, including STL collections,
-    //FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
+   * BayesTrees, etc.
-    //  factors_.assign(factors.begin(), factors.end());
+   */
-    //}
+  template <class FACTOR_OR_CONTAINER>
+  boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
+      const FACTOR_OR_CONTAINER& factorOrContainer) {
+    return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
+        factorOrContainer);
+  }
 
-    /// @}
+  /// @}
+  /// @name Specialized versions
+  /// @{
 
-  public:
+  /**
-    /// @name Adding Factors
+   * Push back a BayesTree as a collection of factors.
-    /// @{
+   * NOTE: This should be hidden in derived classes in favor of a
+   * type-specialized version that calls this templated function.
+   */
+  template <class CLIQUE>
+  typename std::enable_if<
+      std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
+  push_back(const BayesTree<CLIQUE>& bayesTree) {
+    bayesTree.addFactorsToGraph(this);
+  }
 
-    /**
+  /**
-     * Reserve space for the specified number of factors if you know in
+   * Add new factors to a factor graph and returns a list of new factor indices,
-     * advance how many there will be (works like FastVector::reserve).
+   * optionally finding and reusing empty factor slots.
-     */
+   */
-    void reserve(size_t size) { factors_.reserve(size); }
+  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
+  FactorIndices add_factors(const CONTAINER& factors,
+                            bool useEmptySlots = false);
 
-    // TODO: are these needed?
+  /// @}
+  /// @name Testable
+  /// @{
 
-    /** Add a factor directly using a shared_ptr */
+  /** print out graph */
-    template<class DERIVEDFACTOR>
+  void print(const std::string& s = "FactorGraph",
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
+             const KeyFormatter& formatter = DefaultKeyFormatter) const;
-    push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
-      factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
 
-    /** Add a factor directly using a shared_ptr */
+  /** Check equality */
-    void push_back(const sharedFactor& factor) {
+  bool equals(const This& fg, double tol = 1e-9) const;
-      factors_.push_back(factor); }
+  /// @}
 
-    /** Emplace a factor */
+ public:
-	template<class DERIVEDFACTOR, class... Args>
+  /// @name Standard Interface
-	typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
+  /// @{
-		emplace_shared(Args&&... args) {
-		factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
-	}
 
-    /** push back many factors with an iterator over shared_ptr (factors are not copied) */
+  /** return the number of factors (including any null factors set by remove()
-    template<typename ITERATOR>
+   * ). */
-    typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
+  size_t size() const { return factors_.size(); }
-      push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
-        factors_.insert(end(), firstFactor, lastFactor); }
 
-    /** push back many factors as shared_ptr's in a container (factors are not copied) */
+  /** Check if the graph is empty (null factors set by remove() will cause
-    template<typename CONTAINER>
+   * this to return false). */
-    typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
+  bool empty() const { return factors_.empty(); }
-      push_back(const CONTAINER& container) {
-        push_back(container.begin(), container.end());
-    }
 
-    /** push back a BayesTree as a collection of factors.  NOTE: This should be hidden in derived
+  /** Get a specific factor by index (this checks array bounds and may throw
-     *  classes in favor of a type-specialized version that calls this templated function. */
+   * an exception, as opposed to operator[] which does not).
-    template<class CLIQUE>
+   */
-    typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
+  const sharedFactor at(size_t i) const { return factors_.at(i); }
-    push_back(const BayesTree<CLIQUE>& bayesTree) {
-      bayesTree.addFactorsToGraph(*this);
-    }
 
-//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  /** Get a specific factor by index (this checks array bounds and may throw
-    /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
+   * an exception, as opposed to operator[] which does not).
-    *  the copy). */
+   */
-    template<class DERIVEDFACTOR>
+  sharedFactor& at(size_t i) { return factors_.at(i); }
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-      push_back(const DERIVEDFACTOR& factor) {
-        factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
-    }
-//#endif
 
-    /** push back many factors with an iterator over plain factors (factors are copied) */
+  /** Get a specific factor by index (this does not check array bounds, as
-    template<typename ITERATOR>
+   * opposed to at() which does).
-    typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
+   */
-      push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
+  const sharedFactor operator[](size_t i) const { return at(i); }
-        for (ITERATOR f = firstFactor; f != lastFactor; ++f)
-          push_back(*f);
-    }
 
-    /** push back many factors as non-pointer objects in a container (factors are copied) */
+  /** Get a specific factor by index (this does not check array bounds, as
-    template<typename CONTAINER>
+   * opposed to at() which does).
-    typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
+   */
-      push_back(const CONTAINER& container) {
+  sharedFactor& operator[](size_t i) { return at(i); }
-        push_back(container.begin(), container.end());
-    }
 
-    /** Add a factor directly using a shared_ptr */
+  /** Iterator to beginning of factors. */
-    template<class DERIVEDFACTOR>
+  const_iterator begin() const { return factors_.begin(); }
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
-      boost::assign::list_inserter<RefCallPushBack<This> > >::type
-      operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
-        return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
-    }
 
-    /** Add a factor directly using a shared_ptr */
+  /** Iterator to end of factors. */
-    boost::assign::list_inserter<CRefCallPushBack<This> >
+  const_iterator end() const { return factors_.end(); }
-      operator+=(const sharedFactor& factor) {
-        return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factor);
-    }
 
-    /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */
+  /** Get the first factor */
-    template<class FACTOR_OR_CONTAINER>
+  sharedFactor front() const { return factors_.front(); }
-    boost::assign::list_inserter<CRefCallPushBack<This> >
-      operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
-        return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factorOrContainer);
-    }
 
-    /** Add a factor directly using a shared_ptr */
+  /** Get the last factor */
-    template<class DERIVEDFACTOR>
+  sharedFactor back() const { return factors_.back(); }
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-      add(boost::shared_ptr<DERIVEDFACTOR> factor) {
-        push_back(factor);
-    }
 
-    /** Add a factor directly using a shared_ptr */
+  /// @}
-    void add(const sharedFactor& factor) {
+  /// @name Modifying Factor Graphs (imperative, discouraged)
-      push_back(factor);
+  /// @{
-    }
 
-    /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */
+  /** non-const STL-style begin() */
-    template<class FACTOR_OR_CONTAINER>
+  iterator begin() { return factors_.begin(); }
-    void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
-      push_back(factorOrContainer);
-    }
 
-    /// @}
+  /** non-const STL-style end() */
-    /// @name Testable
+  iterator end() { return factors_.end(); }
-    /// @{
 
-    /** print out graph */
+  /** Directly resize the number of factors in the graph. If the new size is
-    void print(const std::string& s = "FactorGraph",
+   * less than the original, factors at the end will be removed.  If the new
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+   * size is larger than the original, null factors will be appended.
+   */
+  void resize(size_t size) { factors_.resize(size); }
 
-    /** Check equality */
+  /** delete factor without re-arranging indexes by inserting a NULL pointer
-    bool equals(const This& fg, double tol = 1e-9) const;
+   */
-    /// @}
+  void remove(size_t i) { factors_[i].reset(); }
 
-  public:
+  /** replace a factor by index */
-    /// @name Standard Interface
+  void replace(size_t index, sharedFactor factor) { at(index) = factor; }
-    /// @{
 
-    /** return the number of factors (including any null factors set by remove() ). */
+  /** Erase factor and rearrange other factors to take up the empty space */
-    size_t size() const { return factors_.size(); }
+  iterator erase(iterator item) { return factors_.erase(item); }
 
-    /** Check if the graph is empty (null factors set by remove() will cause this to return false). */
+  /** Erase factors and rearrange other factors to take up the empty space */
-    bool empty() const { return factors_.empty(); }
+  iterator erase(iterator first, iterator last) {
+    return factors_.erase(first, last);
+  }
 
-    /** Get a specific factor by index (this checks array bounds and may throw an exception, as
+  /// @}
-     *  opposed to operator[] which does not).
+  /// @name Advanced Interface
-     */
+  /// @{
-    const sharedFactor at(size_t i) const { return factors_.at(i); }
 
-    /** Get a specific factor by index (this checks array bounds and may throw an exception, as
+  /** return the number of non-null factors */
-     *  opposed to operator[] which does not).
+  size_t nrFactors() const;
-     */
-    sharedFactor& at(size_t i) { return factors_.at(i); }
 
-    /** Get a specific factor by index (this does not check array bounds, as opposed to at() which
+  /** Potentially slow function to return all keys involved, sorted, as a set
-     *  does).
+   */
-     */
+  KeySet keys() const;
-    const sharedFactor operator[](size_t i) const { return at(i); }
 
-    /** Get a specific factor by index (this does not check array bounds, as opposed to at() which
+  /** Potentially slow function to return all keys involved, sorted, as a
-     *  does).
+   * vector
-     */
+   */
-    sharedFactor& operator[](size_t i) { return at(i); }
+  KeyVector keyVector() const;
 
-    /** Iterator to beginning of factors. */
+  /** MATLAB interface utility: Checks whether a factor index idx exists in
-    const_iterator begin() const { return factors_.begin();}
+   * the graph and is a live pointer */
+  inline bool exists(size_t idx) const { return idx < size() && at(idx); }
 
-    /** Iterator to end of factors. */
+ private:
-    const_iterator end()   const { return factors_.end();  }
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(factors_);
+  }
 
-    /** Get the first factor */
+  /// @}
-    sharedFactor front() const { return factors_.front(); }
+};  // FactorGraph
-
+}  // namespace gtsam
-    /** Get the last factor */
-    sharedFactor back() const { return factors_.back(); }
-
-    /// @}
-    /// @name Modifying Factor Graphs (imperative, discouraged)
-    /// @{
-
-    /** non-const STL-style begin() */
-    iterator begin()       { return factors_.begin();}
-
-    /** non-const STL-style end() */
-    iterator end()         { return factors_.end();  }
-
-    /** Directly resize the number of factors in the graph. If the new size is less than the
-     * original, factors at the end will be removed.  If the new size is larger than the original,
-     * null factors will be appended.
-     */
-    void resize(size_t size) { factors_.resize(size); }
-
-    /** delete factor without re-arranging indexes by inserting a NULL pointer */
-    void remove(size_t i) { factors_[i].reset();}
-
-    /** replace a factor by index */
-    void replace(size_t index, sharedFactor factor) { at(index) = factor; }
-
-    /** Erase factor and rearrange other factors to take up the empty space */
-    iterator erase(iterator item) { return factors_.erase(item); }
-
-    /** Erase factors and rearrange other factors to take up the empty space */
-    iterator erase(iterator first, iterator last) { return factors_.erase(first, last); }
-
-    /// @}
-    /// @name Advanced Interface
-    /// @{
-
-    /** return the number of non-null factors */
-    size_t nrFactors() const;
-
-    /** Potentially slow function to return all keys involved, sorted, as a set */
-    KeySet keys() const;
-
-    /** Potentially slow function to return all keys involved, sorted, as a vector */
-    KeyVector keyVector() const;
-
-    /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
-    inline bool exists(size_t idx) const { return idx < size() && at(idx); }
-
-  private:
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(factors_);
-    }
-
-    /// @}
-
-  }; // FactorGraph
-
-} // namespace gtsam
 
 #include <gtsam/inference/FactorGraph-inst.h>

gtsam/inference/ISAM-inst.h

@@ -24,14 +24,14 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class BAYESTREE>
-void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
+void ISAM<BAYESTREE>::updateInternal(const FactorGraphType& newFactors,
-    Cliques& orphans, const Eliminate& function) {
+    Cliques* orphans, const Eliminate& function) {
   // Remove the contaminated part of the Bayes tree
   BayesNetType bn;
   const KeySet newFactorKeys = newFactors.keys();
   if (!this->empty()) {
     KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
-    this->removeTop(keyVector, bn, orphans);
+    this->removeTop(keyVector, &bn, orphans);
   }
 
   // Add the removed top and the new factors
@@ -40,7 +40,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
   factors += newFactors;
 
   // Add the orphaned subtrees
-  for (const sharedClique& orphan : orphans)
+  for (const sharedClique& orphan : *orphans)
     factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
 
   // Get an ordering where the new keys are eliminated last
@@ -62,7 +62,7 @@ template<class BAYESTREE>
 void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
     const Eliminate& function) {
   Cliques orphans;
-  this->update_internal(newFactors, orphans, function);
+  this->updateInternal(newFactors, &orphans, function);
 }
 
 }

gtsam/inference/ISAM.h

@@ -22,56 +22,67 @@
 
 namespace gtsam {
 
+/**
+ * A Bayes tree with an update methods that implements the iSAM algorithm.
+ * Given a set of new factors, it re-eliminates the invalidated part of the
+ * tree. \nosubgrouping
+ */
+template <class BAYESTREE>
+class ISAM : public BAYESTREE {
+ public:
+  typedef BAYESTREE Base;
+  typedef typename Base::BayesNetType BayesNetType;
+  typedef typename Base::FactorGraphType FactorGraphType;
+  typedef typename Base::Clique Clique;
+  typedef typename Base::sharedClique sharedClique;
+  typedef typename Base::Cliques Cliques;
+
+ private:
+  typedef typename Base::Eliminate Eliminate;
+  typedef typename Base::EliminationTraitsType EliminationTraitsType;
+
+ public:
+  /// @name Standard Constructors
+  /// @{
+
+  /** Create an empty Bayes Tree */
+  ISAM() {}
+
+  /** Copy constructor */
+  explicit ISAM(const Base& bayesTree) : Base(bayesTree) {}
+
+  /// @}
+  /// @name Advanced Interface Interface
+  /// @{
+
   /**
-   * A Bayes tree with an update methods that implements the iSAM algorithm.
+   * update the Bayes tree with a set of new factors, typically derived from
-   * Given a set of new factors, it re-eliminates the invalidated part of the tree.
+   * measurements
-   * \nosubgrouping
+   * @param newFactors is a factor graph that contains the new factors
+   * @param function an elimination routine
    */
-  template<class BAYESTREE>
+  void update(
-  class ISAM: public BAYESTREE
+      const FactorGraphType& newFactors,
-  {
-  public:
-
-    typedef BAYESTREE Base;
-    typedef typename Base::BayesNetType BayesNetType;
-    typedef typename Base::FactorGraphType FactorGraphType;
-    typedef typename Base::Clique Clique;
-    typedef typename Base::sharedClique sharedClique;
-    typedef typename Base::Cliques Cliques;
-
-  private:
-
-    typedef typename Base::Eliminate Eliminate;
-    typedef typename Base::EliminationTraitsType EliminationTraitsType;
-
-  public:
-
-    /// @name Standard Constructors
-    /// @{
-
-    /** Create an empty Bayes Tree */
-    ISAM() {}
-
-    /** Copy constructor */
-    ISAM(const Base& bayesTree) : Base(bayesTree) {}
-
-    /// @}
-    /// @name Advanced Interface Interface
-    /// @{
-
-    /**
-     * update the Bayes tree with a set of new factors, typically derived from measurements
-     * @param newFactors is a factor graph that contains the new factors
-     * @param function an elimination routine
-     */
-    void update(const FactorGraphType& newFactors, const Eliminate& function = EliminationTraitsType::DefaultEliminate);
-
-    /** update_internal provides access to list of orphans for drawing purposes */
-    void update_internal(const FactorGraphType& newFactors, Cliques& orphans,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate);
 
-    /// @}
+  /** updateInternal provides access to list of orphans for drawing purposes
+   */
+  void updateInternal(
+      const FactorGraphType& newFactors, Cliques* orphans,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate);
 
-  };
+  /// @}
 
-}/// namespace gtsam
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  /// @name Deprecated
+  /// @{
+  void update_internal(
+      const FactorGraphType& newFactors, Cliques& orphans,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate) {
+    updateInternal(newFactors, &orphans, function);
+  }
+  /// @}
+#endif
+};
+
+}  // namespace gtsam

gtsam/inference/Key.cpp

@@ -56,7 +56,7 @@ string _multirobotKeyFormatter(Key key) {
 
 /* ************************************************************************* */
 template<class CONTAINER>
-static void Print(const CONTAINER& keys, const string& s,
+void Print(const CONTAINER& keys, const string& s,
     const KeyFormatter& keyFormatter) {
   cout << s << " ";
   if (keys.empty())
@@ -83,6 +83,44 @@ void PrintKeySet(const KeySet& keys, const string& s,
     const KeyFormatter& keyFormatter) {
   Print(keys, s, keyFormatter);
 }
+
+/* ************************************************************************* */
+// Access to custom stream property.
+void *&key_formatter::property(ios_base &s) {
+  static int kUniqueIndex = ios_base::xalloc();
+  return s.pword(kUniqueIndex);
+}
+
+/* ************************************************************************* */
+// Store pointer to formatter in property.
+void key_formatter::set_property(ios_base &s, const KeyFormatter &f) {
+  property(s) = (void *)(&f);
+}
+
+/* ************************************************************************* */
+// Get pointer to formatter from property.
+KeyFormatter *key_formatter::get_property(ios_base &s) {
+  return (KeyFormatter *)(property(s));
+}
+
+/* ************************************************************************* */
+// Stream operator that will take a key_formatter and set the stream property.
+ostream &operator<<(ostream &os, const key_formatter &m) {
+  key_formatter::set_property(os, m.formatter_);
+  return os;
+}
+
+/* ************************************************************************* */
+// Stream operator that takes a StreamedKey and properly formats it
+ostream &operator<<(ostream &os, const StreamedKey &streamedKey) {
+  const KeyFormatter *formatter = key_formatter::get_property(os);
+  if (formatter == nullptr) {
+    formatter = &DefaultKeyFormatter;
+  }
+  os << (*formatter)(streamedKey.key_);
+  return (os);
+}
+
 /* ************************************************************************* */
 
 } // \namespace gtsam

gtsam/inference/Key.h

@@ -27,6 +27,8 @@
 
 #include <boost/function.hpp>
 
+#include <iosfwd>
+
 namespace gtsam {
 
 /// Typedef for a function to format a key, i.e. to convert it to a string
@@ -52,6 +54,34 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
 static const gtsam::KeyFormatter MultiRobotKeyFormatter =
     &_multirobotKeyFormatter;
 
+/// To use the key_formatter on Keys, they must be wrapped in a StreamedKey.
+struct StreamedKey {
+  const Key &key_;
+  explicit StreamedKey(const Key &key) : key_(key) {}
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &);
+};
+
+/**
+ * Output stream manipulator that will format gtsam::Keys according to the given
+ * KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey.
+ * LabeledSymbol and Symbol values do not have to be wrapped.
+ * usage:
+ *   Key key = LabeledSymbol('x', 'A', 5); // cast to key type
+ *   cout << key_formatter(MultiRobotKeyFormatter) << StreamedKey(key);
+ */
+class key_formatter {
+ public:
+  explicit key_formatter(KeyFormatter v) : formatter_(v) {}
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const key_formatter &);
+  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &);
+
+ private:
+  KeyFormatter formatter_;
+  static void *&property(std::ios_base &s);
+  static void set_property(std::ios_base &s, const KeyFormatter &f);
+  static KeyFormatter *get_property(std::ios_base &s);
+};
+
 /// Define collection type once and for all - also used in wrappers
 typedef FastVector<Key> KeyVector;
 

gtsam/inference/LabeledSymbol.cpp

@@ -121,6 +121,13 @@ boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c,
   return boost::bind(&LabeledSymbol::chr,   boost::bind(make, _1)) == c &&
       boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
 }
+
+/* ************************************************************************* */
+std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) {
+  os << StreamedKey(symbol);
+  return os;
+}
+
 /* ************************************************************************* */
 
 } // \namespace gtsam

gtsam/inference/LabeledSymbol.h

@@ -100,10 +100,15 @@ public:
   LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }
   LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); }
 
-  // Create a new symbol with a different value
+  // Create a new symbol with a different character.
   LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); }
+
+  // Create a new symbol with a different label.
   LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); }
 
+  /// Output stream operator that can be used with key_formatter (see Key.h).
+  friend std::ostream &operator<<(std::ostream &, const LabeledSymbol &);
+
 private:
 
   /** Serialization function */

gtsam/inference/Ordering.cpp

@@ -79,7 +79,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
   size_t index = 0;
   for (auto key_factors: variableIndex) {
     // Arrange factor indices into COLAMD format
-    const VariableIndex::Factors& column = key_factors.second;
+    const FactorIndices& column = key_factors.second;
     for(size_t factorIndex: column) {
       A[count++] = (int) factorIndex; // copy sparse column
     }

gtsam/inference/Symbol.cpp

@@ -66,5 +66,10 @@ boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
   return bind(&Symbol::chr, bind(make, _1)) == c;
 }
 
+std::ostream &operator<<(std::ostream &os, const Symbol &symbol) {
+  os << StreamedKey(symbol);
+  return os;
+}
+
 } // namespace gtsam
 

gtsam/inference/Symbol.h

@@ -112,6 +112,9 @@ public:
    */
   static boost::function<bool(Key)> ChrTest(unsigned char c);
 
+  /// Output stream operator that can be used with key_formatter (see Key.h).
+  friend std::ostream &operator<<(std::ostream &, const Symbol &);
+
 private:
 
   /** Serialization function */

gtsam/inference/VariableIndex-inl.h

@@ -67,8 +67,8 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
           "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
     if (factors[i]) {
       for(Key j: *factors[i]) {
-        Factors& factorEntries = internalAt(j);
+        FactorIndices& factorEntries = internalAt(j);
-        Factors::iterator entry = std::find(factorEntries.begin(),
+        auto entry = std::find(factorEntries.begin(),
             factorEntries.end(), *factorIndex);
         if (entry == factorEntries.end())
           throw std::invalid_argument(

gtsam/inference/VariableIndex.h

@@ -41,26 +41,22 @@ namespace gtsam {
  * \nosubgrouping
  */
 class GTSAM_EXPORT VariableIndex {
-public:
+ public:
-
   typedef boost::shared_ptr<VariableIndex> shared_ptr;
-  typedef FactorIndices Factors;
+  typedef FactorIndices::iterator Factor_iterator;
-  typedef Factors::iterator Factor_iterator;
+  typedef FactorIndices::const_iterator Factor_const_iterator;
-  typedef Factors::const_iterator Factor_const_iterator;
 
-protected:
+ protected:
-  typedef FastMap<Key,Factors> KeyMap;
+  typedef FastMap<Key, FactorIndices> KeyMap;
   KeyMap index_;
-  size_t nFactors_; // Number of factors in the original factor graph.
+  size_t nFactors_;  // Number of factors in the original factor graph.
-  size_t nEntries_; // Sum of involved variable counts of each factor.
+  size_t nEntries_;  // Sum of involved variable counts of each factor.
 
-public:
+ public:
   typedef KeyMap::const_iterator const_iterator;
   typedef KeyMap::const_iterator iterator;
   typedef KeyMap::value_type value_type;
 
-public:
-
   /// @name Standard Constructors
   /// @{
 
@@ -71,8 +67,10 @@ public:
    * Create a VariableIndex that computes and stores the block column structure
    * of a factor graph.
    */
-  template<class FG>
+  template <class FG>
-  VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) { augment(factorGraph); }
+  explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) {
+    augment(factorGraph);
+  }
 
   /// @}
   /// @name Standard Interface
@@ -88,7 +86,7 @@ public:
   size_t nEntries() const { return nEntries_; }
 
   /// Access a list of factors by variable
-  const Factors& operator[](Key variable) const {
+  const FactorIndices& operator[](Key variable) const {
     KeyMap::const_iterator item = index_.find(variable);
     if(item == index_.end())
       throw std::invalid_argument("Requested non-existent variable from VariableIndex");
@@ -96,6 +94,11 @@ public:
     return item->second;
   }
 
+  /// Return true if no factors associated with a variable
+  bool empty(Key variable) const {
+    return (*this)[variable].empty();
+  }
+
   /// @}
   /// @name Testable
   /// @{
@@ -166,16 +169,18 @@ protected:
   Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
 
   /// Internal version of 'at' that asserts existence
-  const Factors& internalAt(Key variable) const {
+  const FactorIndices& internalAt(Key variable) const {
     const KeyMap::const_iterator item = index_.find(variable);
     assert(item != index_.end());
-    return item->second; }
+    return item->second; 
+  }
 
   /// Internal version of 'at' that asserts existence
-  Factors& internalAt(Key variable) {
+  FactorIndices& internalAt(Key variable) {
     const KeyMap::iterator item = index_.find(variable);
     assert(item != index_.end());
-    return item->second; }
+    return item->second; 
+  }
 
   /// @}
 };

gtsam/inference/tests/testKey.cpp

@@ -22,6 +22,9 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/list.hpp> // for operator +=
+
+#include <sstream>
+
 using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
@@ -41,17 +44,15 @@ TEST(Key, KeySymbolConversion) {
 template<int KeySize>
 Key KeyTestValue();
 
-template<>
+template <>
-Key KeyTestValue<8>()
+Key KeyTestValue<8>() {
-{
   return 0x6100000000000005;
-};
+}
 
-template<>
+template <>
-Key KeyTestValue<4>()
+Key KeyTestValue<4>() {
-{
   return 0x61000005;
-};
+}
 
 /* ************************************************************************* */
 TEST(Key, KeySymbolEncoding) {
@@ -68,12 +69,41 @@ TEST(Key, KeySymbolEncoding) {
 
 /* ************************************************************************* */
 TEST(Key, ChrTest) {
-  Key key = Symbol('c',3);
+  Symbol key('c', 3);
   EXPECT(Symbol::ChrTest('c')(key));
   EXPECT(!Symbol::ChrTest('d')(key));
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+// A custom (nonsensical) formatter.
+string keyMyFormatter(Key key) {
+  return "special";
+}
+
+TEST(Key, Formatting) {
+  Symbol key('c', 3);
+  EXPECT("c3" == DefaultKeyFormatter(key));
+
+  // Try streaming keys, should be default-formatted.
+  stringstream ss;
+  ss << StreamedKey(key);
+  EXPECT("c3" == ss.str());
+
+  // use key_formatter with a function pointer
+  stringstream ss2;
+  ss2 << key_formatter(keyMyFormatter) << StreamedKey(key);
+  EXPECT("special" == ss2.str());
+
+  // use key_formatter with a function object.
+  stringstream ss3;
+  ss3 << key_formatter(DefaultKeyFormatter) << StreamedKey(key);
+  EXPECT("c3" == ss3.str());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
 

gtsam/inference/tests/testLabeledSymbol.cpp

@@ -80,6 +80,29 @@ TEST(LabeledSymbol, ChrTest) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+// A custom (nonsensical) formatter.
+string labeledSymbolMyFormatter(Key key) {
+  return "special";
+}
+
+TEST(LabeledSymbol, Formatting) {
+  LabeledSymbol symbol('c', 'A', 3);
+
+  // use key_formatter with a function pointer
+  stringstream ss2;
+  ss2 << key_formatter(labeledSymbolMyFormatter) << symbol;
+  EXPECT("special" == ss2.str());
+
+  // use key_formatter with a function object.
+  stringstream ss3;
+  ss3 << key_formatter(MultiRobotKeyFormatter) << symbol;
+  EXPECT("cA3" == ss3.str());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */