Merge branch 'develop' into combined-imu-factor

.github/scripts/python.sh

@@ -79,7 +79,7 @@ function build()
       -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \
       -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \
       -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \
-      -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \
       -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install
 
 

.github/scripts/unix.sh

@@ -64,14 +64,14 @@ function configure()
       -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
       -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-      -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=${GTSAM_ALLOW_DEPRECATED_SINCE_V42:-OFF} \
+      -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=${GTSAM_ALLOW_DEPRECATED_SINCE_V43:-OFF} \
       -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
       -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
       -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
       -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
       -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-      -DGTSAM_SINGLE_TEST_EXE=ON \
+      -DGTSAM_SINGLE_TEST_EXE=OFF \
       -DBOOST_ROOT=$BOOST_ROOT \
       -DBoost_NO_SYSTEM_PATHS=ON \
       -DBoost_ARCHITECTURE=-x64

.github/workflows/build-linux.yml

@@ -20,19 +20,15 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          ubuntu-20.04-gcc-7,
           ubuntu-20.04-gcc-9,
           ubuntu-20.04-clang-9,
+          ubuntu-22.04-gcc-11,
+          ubuntu-22.04-clang-14,
         ]
 
-        build_type: [Debug, Release]
+        build_type: [Release]
         build_unstable: [ON]
         include:
-          - name: ubuntu-20.04-gcc-7
-            os: ubuntu-20.04
-            compiler: gcc
-            version: "7"
-
           - name: ubuntu-20.04-gcc-9
             os: ubuntu-20.04
             compiler: gcc
@@ -43,14 +39,24 @@ jobs:
             compiler: clang
             version: "9"
 
+          - name: ubuntu-22.04-gcc-11
+            os: ubuntu-22.04
+            compiler: gcc
+            version: "11"
+
+          - name: ubuntu-22.04-clang-14
+            os: ubuntu-22.04
+            compiler: clang
+            version: "14"
+
     steps:
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3
 
       - name: Install Dependencies
         run: |
-          # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
+          # LLVM (clang) 9/14 is not in Bionic's repositories so we add the official LLVM repository.
-          if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
+          if [ "${{ matrix.compiler }}" = "clang" ]; then
             # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
             # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository

.github/workflows/build-macos.yml

@@ -14,7 +14,7 @@ jobs:
       GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }}
 
     strategy:
-      fail-fast: false
+      fail-fast: true
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
@@ -32,20 +32,14 @@ jobs:
 
     steps:
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3
 
       - name: Install Dependencies
         run: |
           brew install cmake ninja
           brew install boost
-          if [ "${{ matrix.compiler }}" = "gcc" ]; then
-            brew install gcc@${{ matrix.version }}
-            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
-          else
           sudo xcode-select -switch /Applications/Xcode.app
           echo "CC=clang" >> $GITHUB_ENV
           echo "CXX=clang++" >> $GITHUB_ENV
-          fi
       - name: Build and Test
         run: bash .github/scripts/unix.sh -t

.github/workflows/build-python.yml

@@ -14,58 +14,45 @@ jobs:
       PYTHON_VERSION: ${{ matrix.python_version }}
 
     strategy:
-      fail-fast: false
+      fail-fast: true
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
-        name: [
+        name:
-          ubuntu-20.04-gcc-7,
+          [
             ubuntu-20.04-gcc-9,
+            ubuntu-20.04-gcc-9-tbb,
             ubuntu-20.04-clang-9,
             macOS-11-xcode-13.4.1,
-          ubuntu-20.04-gcc-7-tbb,
           ]
 
-        build_type: [Debug, Release]
+        build_type: [Release]
         python_version: [3]
         include:
-          - name: ubuntu-20.04-gcc-7
-            os: ubuntu-20.04
-            compiler: gcc
-            version: "7"
-
           - name: ubuntu-20.04-gcc-9
             os: ubuntu-20.04
             compiler: gcc
             version: "9"
 
-          - name: ubuntu-20.04-clang-9
+          - name: ubuntu-20.04-gcc-9-tbb
             os: ubuntu-20.04
-            compiler: clang
+            compiler: gcc
             version: "9"
+            flag: tbb
 
-          # NOTE temporarily added this as it is a required check.
           - name: ubuntu-20.04-clang-9
             os: ubuntu-20.04
             compiler: clang
             version: "9"
-            build_type: Debug
-            python_version: "3"
 
           - name: macOS-11-xcode-13.4.1
             os: macOS-11
             compiler: xcode
             version: "13.4.1"
 
-          - name: ubuntu-20.04-gcc-7-tbb
-            os: ubuntu-20.04
-            compiler: gcc
-            version: "7"
-            flag: tbb
-
     steps:
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3
       - name: Install (Linux)
         if: runner.os == 'Linux'
         run: |
@@ -98,15 +85,9 @@ jobs:
           brew tap ProfFan/robotics
           brew install cmake ninja
           brew install boost
-          if [ "${{ matrix.compiler }}" = "gcc" ]; then
-            brew install gcc@${{ matrix.version }}
-            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
-          else
           sudo xcode-select -switch /Applications/Xcode.app
           echo "CC=clang" >> $GITHUB_ENV
           echo "CXX=clang++" >> $GITHUB_ENV
-          fi
       - name: Set GTSAM_WITH_TBB Flag
         if: matrix.flag == 'tbb'
         run: |

.github/workflows/build-special.yml

@@ -22,71 +22,80 @@ jobs:
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name:
           [
-            ubuntu-gcc-deprecated,
+            ubuntu-clang-deprecated,
-            ubuntu-gcc-quaternions,
+            ubuntu-clang-quaternions,
-            ubuntu-gcc-tbb,
+            ubuntu-clang-tbb,
-            ubuntu-cayleymap,
+            ubuntu-clang-cayleymap,
+            ubuntu-clang-system-libs,
+            ubuntu-no-boost,
           ]
 
         build_type: [Debug, Release]
 
         include:
-          - name: ubuntu-gcc-deprecated
+          - name: ubuntu-clang-deprecated
-            os: ubuntu-20.04
+            os: ubuntu-22.04
-            compiler: gcc
+            compiler: clang
-            version: "9"
+            version: "14"
             flag: deprecated
 
-          - name: ubuntu-gcc-quaternions
+          - name: ubuntu-clang-quaternions
-            os: ubuntu-20.04
+            os: ubuntu-22.04
-            compiler: gcc
+            compiler: clang
-            version: "9"
+            version: "14"
             flag: quaternions
 
-          - name: ubuntu-gcc-tbb
+          - name: ubuntu-clang-tbb
-            os: ubuntu-20.04
+            os: ubuntu-22.04
-            compiler: gcc
+            compiler: clang
-            version: "9"
+            version: "14"
             flag: tbb
 
-          - name: ubuntu-cayleymap
+          - name: ubuntu-clang-cayleymap
-            os: ubuntu-20.04
+            os: ubuntu-22.04
-            compiler: gcc
+            compiler: clang
-            version: "9"
+            version: "14"
             flag: cayley
 
-          - name: ubuntu-system-libs
+          - name: ubuntu-clang-system-libs
-            os: ubuntu-20.04
+            os: ubuntu-22.04
-            compiler: gcc
+            compiler: clang
-            version: "9"
+            version: "14"
-            flag: system-libs
+            flag: system
+
+          - name: ubuntu-no-boost
+            os: ubuntu-22.04
+            compiler: clang
+            version: "14"
+            flag: no_boost
 
     steps:
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3
 
       - name: Install (Linux)
         if: runner.os == 'Linux'
         run: |
-          # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
+          sudo apt-get -y update
-          if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
+          sudo apt-get -y install software-properties-common
+
+          # LLVM (clang) 9/14 is not in 22.04 (jammy)'s repositories so we add the official LLVM repository.
+          if [ "${{ matrix.compiler }}" = "clang" ]; then
+            # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
+            # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
+            # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
+            # This key is not in the keystore by default for Ubuntu so we need to add it.
+            LLVM_KEY=15CF4D18AF4F7421
             gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
-            gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
+            gpg -a --export $LLVM_KEY | sudo apt-key add -
-            sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
+            sudo add-apt-repository "deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main"
           fi
 
-          sudo apt-get -y update
           sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev
 
-          if [ "${{ matrix.compiler }}" = "gcc" ]; then
-            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
-            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
-          else
           sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
           echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
           echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
-          fi
 
       - name: Install Boost
         if: runner.os == 'Linux'
@@ -97,21 +106,15 @@ jobs:
         if: runner.os == 'macOS'
         run: |
           brew install cmake ninja boost
-          if [ "${{ matrix.compiler }}" = "gcc" ]; then
-            brew install gcc@${{ matrix.version }}
-            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
-            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
-          else
           sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
           echo "CC=clang" >> $GITHUB_ENV
           echo "CXX=clang++" >> $GITHUB_ENV
-            fi
 
       - name: Set Allow Deprecated Flag
         if: matrix.flag == 'deprecated'
         run: |
-          echo "GTSAM_ALLOW_DEPRECATED_SINCE_V42=ON" >> $GITHUB_ENV
+          echo "GTSAM_ALLOW_DEPRECATED_SINCE_V43=ON" >> $GITHUB_ENV
-          echo "Allow deprecated since version 4.1"
+          echo "Allow deprecated since version 4.3"
 
       - name: Set Use Quaternions Flag
         if: matrix.flag == 'quaternions'
@@ -135,8 +138,18 @@ jobs:
       - name: Use system versions of 3rd party libraries
         if: matrix.flag == 'system'
         run: |
+          sudo apt-get install libeigen3-dev
           echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV
-          echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV
+          # TODO(dellaert): This does not work yet?
+          # sudo apt-get install metis
+          # echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV
+
+      - name: Turn off boost
+        if: matrix.flag == 'no_boost'
+        run: |
+          echo "GTSAM_ENABLE_BOOST_SERIALIZATION=OFF" >> $GITHUB_ENV
+          echo "GTSAM_USE_BOOST_FEATURES=OFF" >> $GITHUB_ENV
+          echo "GTSAM will not use BOOST"
 
       - name: Build & Test
         run: |

.github/workflows/build-windows.yml

@@ -16,7 +16,7 @@ jobs:
       BOOST_EXE: boost_1_72_0-msvc-14.2
 
     strategy:
-      fail-fast: false
+      fail-fast: true
       matrix:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
@@ -94,7 +94,7 @@ jobs:
           echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
 
       - name: Checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3
 
       - name: Configuration
         run: |

.github/workflows/trigger-packaging.yml

@@ -0,0 +1,21 @@
+# This triggers building of packages
+name: Trigger Package Builds
+on:
+  push:
+    branches:
+      - develop
+jobs:
+  trigger-package-build:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Trigger Package Rebuild
+        uses: actions/github-script@v6
+        with:
+          github-token: ${{ secrets.PACKAGING_REPO_ACCESS_TOKEN }}
+          script: |
+            await github.rest.actions.createWorkflowDispatch({
+              owner: 'borglab-launchpad',
+              repo: 'gtsam-packaging',
+              workflow_id: 'main.yaml',
+              ref: 'master'
+            })

CMakeLists.txt

@@ -8,9 +8,9 @@ endif()
 
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)
-set (GTSAM_VERSION_MINOR 2)
+set (GTSAM_VERSION_MINOR 3)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a7")
+set (GTSAM_PRERELEASE_VERSION "a0")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)
@@ -50,8 +50,24 @@ endif()
 
 include(cmake/HandleGeneralOptions.cmake)   # CMake build options
 
-# Libraries:
+############### Decide on BOOST ######################################
-include(cmake/HandleBoost.cmake)            # Boost
+# Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION
+option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON)
+if(GTSAM_ENABLE_BOOST_SERIALIZATION)
+  add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION)
+endif()
+
+option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON)
+if(GTSAM_USE_BOOST_FEATURES)
+  add_definitions(-DGTSAM_USE_BOOST_FEATURES)
+endif()
+
+if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES)
+include(cmake/HandleBoost.cmake)
+endif()
+######################################################################
+
+# Other Libraries:
 include(cmake/HandleCCache.cmake)           # ccache
 include(cmake/HandleCPack.cmake)            # CPack
 include(cmake/HandleEigen.cmake)            # Eigen3
@@ -101,8 +117,6 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
     # Copy matlab.h to the correct folder.
     configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
                ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
-    # Add the include directories so that matlab.h can be found
-    include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
 
     add_subdirectory(wrap)
     list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")

CppUnitLite/CMakeLists.txt

@@ -6,7 +6,6 @@ file(GLOB cppunitlite_src "*.cpp")
 add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
-target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
 
 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 

CppUnitLite/Test.cpp

@@ -15,8 +15,6 @@
 #include "TestResult.h"
 #include "Failure.h"
 
-#include <boost/lexical_cast.hpp>
-
 Test::Test (const std::string& testName)
   : name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true)
 {
@@ -47,10 +45,10 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri
   result.addFailure (
     Failure (
       name_,
-      boost::lexical_cast<std::string> (__FILE__),
+      std::string(__FILE__),
       __LINE__,
-      boost::lexical_cast<std::string> (expected),
+      std::to_string(expected),
-      boost::lexical_cast<std::string> (actual)));
+      std::to_string(actual)));
 
   return false;
 
@@ -64,7 +62,7 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes
   result.addFailure (
     Failure (
       name_,
-      boost::lexical_cast<std::string> (__FILE__),
+      std::string(__FILE__),
       __LINE__,
       expected,
       actual));

CppUnitLite/Test.h

@@ -23,7 +23,7 @@
 
 
 #include <cmath>
-#include <boost/lexical_cast.hpp>
+#include <string>
 
 class TestResult;
 
@@ -32,7 +32,7 @@ class Test
 public:
   Test (const std::string& testName);
   Test (const std::string& testName, const std::string& filename, long lineNumber, bool safeCheck);
-  virtual ~Test() {};
+  virtual ~Test() {}
 
   virtual void  run (TestResult& result) = 0;
 
@@ -63,7 +63,6 @@ protected:
 #define TEST(testGroup, testName)\
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
-            virtual ~testGroup##testName##Test () {};\
             void run (TestResult& result_) override;} \
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_)
@@ -112,17 +111,17 @@ protected:
 
 #define THROWS_EXCEPTION(condition)\
 { try { condition; \
-    result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
+    result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \
     return; } \
   catch (...) {} }
 
 #define CHECK_EXCEPTION(condition, exception_name)\
 { try { condition; \
-    result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \
+    result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \
     return; } \
   catch (exception_name&) {} \
   catch (...) { \
-  result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast<std::string>(#condition) + boost::lexical_cast<std::string>(", expected: ") + boost::lexical_cast<std::string>(#exception_name))); \
+  result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + std::string(#condition) + std::string(", expected: ") + std::string(#exception_name))); \
   return; } }
 
 #define EQUALITY(expected,actual)\
@@ -130,21 +129,21 @@ protected:
     result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
 
 #define CHECK_EQUAL(expected,actual)\
-{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expected), boost::lexical_cast<std::string>(actual))); }
+{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); }
 
 #define LONGS_EQUAL(expected,actual)\
 { long actualTemp = actual; \
   long expectedTemp = expected; \
   if ((expectedTemp) != (actualTemp)) \
-{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \
+{ result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \
-boost::lexical_cast<std::string>(actualTemp))); return; } }
+std::to_string(actualTemp))); return; } }
 
 #define DOUBLES_EQUAL(expected,actual,threshold)\
 { double actualTemp = actual; \
   double expectedTemp = expected; \
   if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \
 { result_.addFailure (Failure (name_, __FILE__, __LINE__, \
-boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); return; } }
+std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); return; } }
 
 
 /* EXPECTs: tests will continue running after a failure */
@@ -156,15 +155,15 @@ boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std:
 { long actualTemp = actual; \
   long expectedTemp = expected; \
   if ((expectedTemp) != (actualTemp)) \
-{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \
+{ result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \
-boost::lexical_cast<std::string>(actualTemp))); } }
+std::to_string(actualTemp))); } }
 
 #define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\
 { double actualTemp = actual; \
   double expectedTemp = expected; \
   if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \
 { result_.addFailure (Failure (name_, __FILE__, __LINE__, \
-boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); } }
+std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); } }
 
 
 #define FAIL(text) \

CppUnitLite/TestResult.h

@@ -27,7 +27,7 @@ class TestResult
 {
 public:
           TestResult ();
-  virtual ~TestResult() {};
+  virtual ~TestResult() {}
   virtual void  testsStarted ();
   virtual void  addFailure (const Failure& failure);
   virtual void  testsEnded ();

DEVELOP.md

@@ -3,8 +3,8 @@
 ### Coding Conventions
 
 * Classes are Uppercase, methods and functions lowerMixedCase.
-* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs.
+* Apart from those naming conventions, we adopt Google C++ style.
-* Use meaningful variable names, e.g. `measurement` not `msm`.
+* Use meaningful variable names, e.g. `measurement` not `msm`, avoid abbreviations.
 
 
 ### Windows

GTSAM-Concepts.md

@@ -166,7 +166,7 @@ Concept Checks
 
 Boost provides a nice way to check whether a given type satisfies a concept. For example, the following
 
-    BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)
+    GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>)
     
 asserts that Point2 indeed is a model for the VectorSpace concept.
 

INSTALL.md

@@ -50,7 +50,7 @@ will run up to 10x faster in Release mode! See the end of this document for
 additional debugging tips.
 
 3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your
-build directory.
+build directory after setting the `GTSAM_BUILD_DOCS` and `GTSAM_BUILD_[HTML|LATEX]` cmake flags.
 
 4. The instructions below install the library to the default system install path and
 build all components. From a terminal, starting in the root library folder,

README.md

@@ -2,9 +2,9 @@
 
 **Important Note**
 
-As of Dec 2021, the `develop` branch is officially in "Pre 4.2" mode. A great new feature we will be adding in 4.2 is *hybrid inference* a la DCSLAM (Kevin Doherty et al) and we envision several API-breaking changes will happen in the discrete folder.
+**As of January 2023, the `develop` branch is officially in "Pre 4.3" mode. We envision several API-breaking changes as we switch to C++17 and away from boost.**
 
-In addition, features deprecated in 4.1 will be removed. Please use the last [4.1.1 release](https://github.com/borglab/gtsam/releases/tag/4.1.1) if you need those features. However, most (not all, unfortunately) are easily converted and can be tracked down (in 4.1.1) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`.
+In addition, features deprecated in 4.2 will be removed. Please use the last [4.2a8 release](https://github.com/borglab/gtsam/releases/tag/4.2a8) if you need those features. However, most are easily converted and can be tracked down (in 4.2) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`.
 
 ## What is GTSAM?
 
@@ -31,11 +31,11 @@ In the root library folder execute:
 
 ```sh
 #!bash
-$ mkdir build
+mkdir build
-$ cd build
+cd build
-$ cmake ..
+cmake ..
-$ make check (optional, runs unit tests)
+make check (optional, runs unit tests)
-$ make install
+make install
 ```
 
 Prerequisites:
@@ -55,11 +55,6 @@ Optional prerequisites - used automatically if findable by CMake:
 
 GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also 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 also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect.
 
-GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods.
-
-GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile.
-
-
 ## Wrappers
 
 We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
@@ -68,24 +63,39 @@ We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md)
 
 If you are using GTSAM for academic work, please use the following citation:
 
-```
+```bibtex
 @software{gtsam,
-  author       = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle},
+  author       = {Frank Dellaert and GTSAM Contributors},
   title        = {borglab/gtsam},
-  month        = may,
+  month        = May,
   year         = 2022,
-  publisher    = {Zenodo},
+  publisher    = {Georgia Tech Borg Lab},
-  version      = {4.2a7},
+  version      = {4.2a8},
   doi          = {10.5281/zenodo.5794541},
-  url          = {https://doi.org/10.5281/zenodo.5794541}
+  url          = {https://github.com/borglab/gtsam)}}
 }
 ```
 
-You can also get the latest citation available from Zenodo below:
+To cite the `Factor Graphs for Robot Perception` book, please use:
+```bibtex
+@book{factor_graphs_for_robot_perception,
+    author={Frank Dellaert and Michael Kaess},
+    year={2017},
+    title={Factor Graphs for Robot Perception},
+    publisher={Foundations and Trends in Robotics, Vol. 6},
+    url={http://www.cs.cmu.edu/~kaess/pub/Dellaert17fnt.pdf}
+}
+```
 
-[![DOI](https://zenodo.org/badge/86362856.svg)](https://doi.org/10.5281/zenodo.5794541)
+If you are using the IMU preintegration scheme, please cite:
+```bibtex
+@book{imu_preintegration,
+    author={Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza},
+    title={IMU preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation},
+    year={2015}
+}
+```
 
-Specific formats are available in the bottom-right corner of the Zenodo page.
 
 ## The Preintegrated IMU Factor
 

cmake/Config.cmake.in

@@ -21,6 +21,10 @@ else()
 find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
 endif()
 
+if(@GTSAM_USE_SYSTEM_EIGEN@)
+find_dependency(Eigen3 REQUIRED)
+endif()
+
 # Load exports
 include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)
 

cmake/FindEigen3.cmake

@@ -1,81 +0,0 @@
-# - Try to find Eigen3 lib
-#
-# This module supports requiring a minimum version, e.g. you can do
-#   find_package(Eigen3 3.1.2)
-# to require version 3.1.2 or newer of Eigen3.
-#
-# Once done this will define
-#
-#  EIGEN3_FOUND - system has eigen lib with correct version
-#  EIGEN3_INCLUDE_DIR - the eigen include directory
-#  EIGEN3_VERSION - eigen version
-
-# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
-# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
-# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
-
-if(NOT Eigen3_FIND_VERSION)
-  if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 2)
-  endif(NOT Eigen3_FIND_VERSION_MAJOR)
-  if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 91)
-  endif(NOT Eigen3_FIND_VERSION_MINOR)
-  if(NOT Eigen3_FIND_VERSION_PATCH)
-    set(Eigen3_FIND_VERSION_PATCH 0)
-  endif(NOT Eigen3_FIND_VERSION_PATCH)
-
-  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
-endif(NOT Eigen3_FIND_VERSION)
-
-macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
-
-  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
-  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
-
-  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
-  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK FALSE)
-  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK TRUE)
-  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-
-  if(NOT EIGEN3_VERSION_OK)
-
-    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
-                   "but at least version ${Eigen3_FIND_VERSION} is required")
-  endif(NOT EIGEN3_VERSION_OK)
-endmacro(_eigen3_check_version)
-
-if (EIGEN3_INCLUDE_DIR)
-
-  # in cache already
-  _eigen3_check_version()
-  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
-
-else (EIGEN3_INCLUDE_DIR)
-
-  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
-      PATHS
-      ${CMAKE_INSTALL_PREFIX}/include
-      ${KDE4_INCLUDE_DIR}
-      PATH_SUFFIXES eigen3 eigen
-    )
-
-  if(EIGEN3_INCLUDE_DIR)
-    _eigen3_check_version()
-  endif(EIGEN3_INCLUDE_DIR)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
-
-  mark_as_advanced(EIGEN3_INCLUDE_DIR)
-
-endif(EIGEN3_INCLUDE_DIR)
-

cmake/GtsamBuildTypes.cmake

@@ -88,6 +88,9 @@ if(MSVC)
     WINDOWS_LEAN_AND_MEAN
     NOMINMAX
   )
+  list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC
+    _ENABLE_EXTENDED_ALIGNED_STORAGE
+  )
   # Avoid literally hundreds to thousands of warnings:
   list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC
     /wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data
@@ -126,6 +129,8 @@ else()
     -fPIC                                          # ensure proper code generation for shared libraries
     $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
     $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
+    $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables>  # TODO(dellaert): don't know how to resolve
+    $<$<CXX_COMPILER_ID:Clang>:-Wno-weak-vtables>  # TODO(dellaert): don't know how to resolve
     -Wreturn-type  -Werror=return-type             # Error on missing return()
     -Wformat -Werror=format-security               # Error on wrong printf() arguments
     $<$<COMPILE_LANGUAGE:CXX>:${flag_override_}>   # Enforce the use of the override keyword
@@ -138,16 +143,19 @@ else()
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          -g -O3  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
 endif()
 
-# Enable C++11:
+# Enable C++17:
 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.")
+    set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" 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)
+    if (MSVC)
+      # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above?
+      list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++latest)
+    endif()
 else()
   # Old cmake versions:
   if (NOT MSVC)
-    list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-std=c++11>)
+    list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-std=c++17>)
   endif()
 endif()
 
@@ -191,11 +199,35 @@ endif()
 
 if (NOT MSVC)
   option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
-  if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
+  if(GTSAM_BUILD_WITH_MARCH_NATIVE)
-    # Add as public flag so all dependant projects also use it, as required
+    # Check if Apple OS and compiler is [Apple]Clang
-    # by Eigen to avid crashes due to SIMD vectorization:
+    if(APPLE AND (${CMAKE_CXX_COMPILER_ID} MATCHES "^(Apple)?Clang$"))
+      # Check Clang version since march=native is only supported for version 15.0+.
+      if("${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "15.0")
+        if(NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64")
+          # Add as public flag so all dependent projects also use it, as required
+          # by Eigen to avoid crashes due to SIMD vectorization:
           list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")
-  endif()
+        else()
+          message(WARNING "Option GTSAM_BUILD_WITH_MARCH_NATIVE ignored, because native architecture is not supported for Apple silicon and AppleClang version < 15.0.")
+        endif() # CMAKE_SYSTEM_PROCESSOR
+      else()
+        # Add as public flag so all dependent projects also use it, as required
+        # by Eigen to avoid crashes due to SIMD vectorization:
+        list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")
+      endif() # CMAKE_CXX_COMPILER_VERSION
+    else()
+      include(CheckCXXCompilerFlag)
+      CHECK_CXX_COMPILER_FLAG("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE)
+      if(COMPILER_SUPPORTS_MARCH_NATIVE)
+        # Add as public flag so all dependent projects also use it, as required
+        # by Eigen to avoid crashes due to SIMD vectorization:
+        list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")
+      else()
+        message(WARNING "Option GTSAM_BUILD_WITH_MARCH_NATIVE ignored, because native architecture is not supported.")
+      endif() # COMPILER_SUPPORTS_MARCH_NATIVE
+    endif() # APPLE
+  endif() # GTSAM_BUILD_WITH_MARCH_NATIVE
 endif()
 
 # Set up build type library postfixes

cmake/GtsamPrinting.cmake

@@ -51,11 +51,10 @@ function(print_build_options_for_target target_name_)
   # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
   print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
 
-  foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
+  string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper)
-    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/HandleEigen.cmake

@@ -1,10 +1,6 @@
 ###############################################################################
 # Option for using system Eigen or GTSAM-bundled Eigen
-# Default: Use system's Eigen if found automatically:
+option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
-find_package(Eigen3 QUIET)
-set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND})
-option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE})
-unset(USE_SYSTEM_EIGEN_INITIAL_VALUE)
 
 if(NOT GTSAM_USE_SYSTEM_EIGEN)
   # This option only makes sense if using the embedded copy of Eigen, it is
@@ -14,10 +10,14 @@ endif()
 
 # Switch for using system Eigen or GTSAM-bundled Eigen
 if(GTSAM_USE_SYSTEM_EIGEN)
-    find_package(Eigen3 REQUIRED) # need to find again as REQUIRED
+    # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it.
+    find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED
 
-    # Use generic Eigen include paths e.g. <Eigen/Core>
+    # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
+    # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen
+    # versions.  So here I use the target itself to get the proper include
+    # directory (it is generated by cmake, thus has the correct path)
+    get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES)
 
     # check if MKL is also enabled - can have one or the other, but not both!
     # Note: Eigen >= v3.2.5 includes our patches
@@ -30,9 +30,6 @@ if(GTSAM_USE_SYSTEM_EIGEN)
     if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
         message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
     endif()
-
-    # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
 else()
     # Use bundled Eigen include path.
     # Clear any variables set by FindEigen3
@@ -45,7 +42,20 @@ else()
     set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 
     # The actual include directory (for BUILD cmake target interface):
-    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
+    set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen")
+
+    add_library(gtsam_eigen3 INTERFACE)
+
+    target_include_directories(gtsam_eigen3 INTERFACE
+      $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
+      $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
+    )
+    add_library(Eigen3::Eigen ALIAS gtsam_eigen3)
+
+    install(TARGETS gtsam_eigen3 EXPORT GTSAM-exports PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+    list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3)
+    set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}")
 endif()
 
 # Detect Eigen version:
@@ -72,7 +82,7 @@ else()
 endif ()
 
 if (MSVC)
-    if (BUILD_SHARED_LIBS)
+    if (GTSAM_SHARED_LIB)
         # mute eigen static assert to avoid errors in shared lib
         list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
     endif()

cmake/HandleGeneralOptions.cmake

@@ -14,7 +14,8 @@ if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_UNSTABLE_BUILD_PYTHON       "Enable/Disable Python wrapper for libgtsam_unstable" ON)
     option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
 endif()
-option(BUILD_SHARED_LIBS                    "Build shared gtsam library, instead of static" ON)
+option(GTSAM_FORCE_SHARED_LIB               "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" OFF)
+option(GTSAM_FORCE_STATIC_LIB               "Force gtsam to be a static library, overriding BUILD_SHARED_LIBS" OFF)
 option(GTSAM_USE_QUATERNIONS                "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
 option(GTSAM_POSE3_EXPMAP                   "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
 option(GTSAM_ROT3_EXPMAP                    "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
@@ -25,10 +26,29 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP          "Eigen, when using Intel MKL, will a
 option(GTSAM_THROW_CHEIRALITY_EXCEPTION     "Throw exception when a triangulated point is behind a camera" ON)
 option(GTSAM_BUILD_PYTHON                   "Enable/Disable building & installation of Python module with pybind11" OFF)
 option(GTSAM_INSTALL_MATLAB_TOOLBOX         "Enable/Disable installation of matlab toolbox"  OFF)
-option(GTSAM_ALLOW_DEPRECATED_SINCE_V42     "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
+option(GTSAM_ALLOW_DEPRECATED_SINCE_V43     "Allow use of methods/functions deprecated in GTSAM 4.3" ON)
 option(GTSAM_SUPPORT_NESTED_DISSECTION      "Support Metis-based nested dissection" ON)
 option(GTSAM_TANGENT_PREINTEGRATION         "Use new ImuFactor with integration on tangent space" ON)
 option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
+
+if (GTSAM_FORCE_SHARED_LIB)
+    message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB")
+    set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
+    set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
+elseif (GTSAM_FORCE_STATIC_LIB)
+    message(STATUS "GTSAM is a static library due to GTSAM_FORCE_STATIC_LIB")
+    set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
+    set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)
+elseif (BUILD_SHARED_LIBS)
+    message(STATUS "GTSAM is a shared library due to BUILD_SHARED_LIBS is ON")
+    set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE)
+    set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE)
+else()
+    message(STATUS "GTSAM is a static library due to BUILD_SHARED_LIBS is OFF")
+    set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE)
+    set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE)
+endif()
+
 if(NOT MSVC AND NOT XCODE_VERSION)
     option(GTSAM_BUILD_WITH_CCACHE           "Use ccache compiler cache" ON)
 endif()

cmake/HandleGlobalBuildFlags.cmake

@@ -14,11 +14,11 @@ endif()
 # 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)
+if(MSVC AND GTSAM_SHARED_LIB)
     list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT)
 endif()
 
-if (APPLE AND BUILD_SHARED_LIBS)
+if (APPLE AND GTSAM_SHARED_LIB)
     # Set the default install directory on macOS
     set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib")
 endif()

cmake/HandlePrintConfiguration.cmake

@@ -14,7 +14,7 @@ print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS}         "Build timing scripts
 if (DOXYGEN_FOUND)
     print_enabled_config(${GTSAM_BUILD_DOCS}              "Build Docs")
 endif()
-print_enabled_config(${BUILD_SHARED_LIBS}                 "Build shared GTSAM libraries")
+print_enabled_config(${GTSAM_SHARED_LIB}                  "Build shared GTSAM libraries")
 print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES}        "Put build type in library name")
 if(GTSAM_UNSTABLE_AVAILABLE)
     print_enabled_config(${GTSAM_BUILD_UNSTABLE}          "Build libgtsam_unstable        ")
@@ -29,6 +29,8 @@ if(NOT MSVC AND NOT XCODE_VERSION)
     print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}")
 endif()
 
+print_config("Enable Boost serialization" "${GTSAM_ENABLE_BOOST_SERIALIZATION}")
+
 print_build_options_for_target(gtsam)
 
 print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
@@ -87,7 +89,7 @@ print_enabled_config(${GTSAM_USE_QUATERNIONS}             "Quaternions as defaul
 print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency checking    ")
 print_enabled_config(${GTSAM_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
 print_enabled_config(${GTSAM_POSE3_EXPMAP}                "Pose3 retract is full ExpMap    ")
-print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V42}  "Allow features deprecated in GTSAM 4.1")
+print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43}  "Allow features deprecated in GTSAM 4.3")
 print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection   ")
 print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION}      "Use tangent-space preintegration")
 

cmake/HandleTBB.cmake

@@ -7,10 +7,6 @@ if (GTSAM_WITH_TBB)
     if(TBB_FOUND)
         set(GTSAM_USE_TBB 1)  # This will go into config.h
 
-#        if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1"))
-#            message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.")
-#        endif()
-
         if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
             set(TBB_GREATER_EQUAL_2020 1)
         else()

cmake/dllexport.h.in

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

cmake/example_cmake_find_gtsam/CMakeLists.txt

@@ -12,6 +12,6 @@ add_executable(example
 )
 
 # By using CMake exported targets, a simple "link" dependency introduces the
-# include directories (-I) flags, links against Boost, and add any other
+# include directories (-I) flags, and add any other
 # required build flags (e.g. C++11, etc.)
 target_link_libraries(example PRIVATE gtsam)

cmake/obsolete/GtsamTestingObsolete.cmake

@@ -2,7 +2,7 @@
 # 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
+# By default, all tests are linked with CppUnitLite
 # 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)
@@ -32,7 +32,6 @@ endfunction()
 # 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:
 #   - 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)
@@ -51,8 +50,7 @@ macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
 endmacro()
 
 # Macro for adding executables matching a pattern - builds one executable for
-# each file matching the pattern.  These exectuables are automatically linked
+# each file matching the pattern.
-# with boost.
 # Arguments:
 #   - pattern    The glob pattern to match source files
 #   - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
@@ -138,9 +136,9 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
 
 				# Linking and dependendencies
 				if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
-					target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
+					target_link_libraries(${script_bin} ${local_libs})
 				else()
-					target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
+					target_link_libraries(${script_bin} ${full_libs})
 				endif()
 
 				# Add .run target

doc/CMakeLists.txt

@@ -26,6 +26,7 @@ if (GTSAM_BUILD_DOCS)
         gtsam/basis 
         gtsam/discrete
         gtsam/geometry
+        gtsam/hybrid
         gtsam/inference
         gtsam/linear
         gtsam/navigation
@@ -33,7 +34,6 @@ if (GTSAM_BUILD_DOCS)
         gtsam/sam
         gtsam/sfm
         gtsam/slam
-        gtsam/smart
         gtsam/symbolic
         gtsam
     )
@@ -42,10 +42,12 @@ if (GTSAM_BUILD_DOCS)
     set(gtsam_unstable_doc_subdirs
         gtsam_unstable/base
         gtsam_unstable/discrete
+        gtsam_unstable/dynamics
+        gtsam_unstable/geometry
         gtsam_unstable/linear
         gtsam_unstable/nonlinear
+        gtsam_unstable/partition
         gtsam_unstable/slam
-        gtsam_unstable/dynamics 
         gtsam_unstable
     )
     

doc/Code/LocalizationFactor.cpp

@@ -2,11 +2,14 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   double mx_, my_; ///< X and Y measurements
   
 public:
+
+  // Provide access to the Matrix& version of evaluateError:
+  using gtsam::NoiseModelFactor1<Pose2>::evaluateError;
+
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
-  Vector evaluateError(const Pose2& q, 
+  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
-      boost::optional<Matrix&> H = boost::none) const override {
     const Rot2& R = q.rotation();
     if (H) (*H) = (gtsam::Matrix(2, 3) <<
             R.c(), -R.s(), 0.0,

doc/CodingGuidelines.lyx

@@ -271,7 +271,7 @@ color{red}{// Make 'private' any typedefs that must be redefined in derived
 
 \begin_layout Plain Layout
 
-    typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to
+    typedef std::shared_ptr<This> shared_ptr;  ///< Shared pointer to
  this
 \end_layout
 
@@ -304,7 +304,7 @@ color{red}{// Make 'public' the typedefs that will be valid in the derived
 
 \begin_layout Plain Layout
 
-    typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer
+    typedef std::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer
  to a factor
 \end_layout
 

doc/DoxygenLayout.xml

@@ -18,7 +18,6 @@
       <tab type="files" visible="yes" title="" intro=""/>
       <tab type="globals" visible="yes" title="" intro=""/>
     </tab>
-    <tab type="dirs" visible="yes" title="" intro=""/>
     <tab type="examples" visible="yes" title="" intro=""/>  
   </navindex>
 

doc/Hybrid.lyx

@@ -0,0 +1,719 @@
+#LyX 2.3 created this file. For more info see http://www.lyx.org/
+\lyxformat 544
+\begin_document
+\begin_header
+\save_transient_properties true
+\origin unavailable
+\textclass article
+\use_default_options true
+\maintain_unincluded_children false
+\language english
+\language_package default
+\inputencoding auto
+\fontencoding global
+\font_roman "default" "default"
+\font_sans "default" "default"
+\font_typewriter "default" "default"
+\font_math "auto" "auto"
+\font_default_family default
+\use_non_tex_fonts false
+\font_sc false
+\font_osf false
+\font_sf_scale 100 100
+\font_tt_scale 100 100
+\use_microtype false
+\use_dash_ligatures true
+\graphics default
+\default_output_format default
+\output_sync 0
+\bibtex_command default
+\index_command default
+\paperfontsize 11
+\spacing single
+\use_hyperref false
+\papersize default
+\use_geometry true
+\use_package amsmath 1
+\use_package amssymb 1
+\use_package cancel 1
+\use_package esint 1
+\use_package mathdots 1
+\use_package mathtools 1
+\use_package mhchem 1
+\use_package stackrel 1
+\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
+\use_minted 0
+\index Index
+\shortcut idx
+\color #008000
+\end_index
+\leftmargin 1in
+\topmargin 1in
+\rightmargin 1in
+\bottommargin 1in
+\secnumdepth 3
+\tocdepth 3
+\paragraph_separation indent
+\paragraph_indentation default
+\is_math_indent 0
+\math_numbering_side default
+\quotes_style english
+\dynamic_quotes 0
+\papercolumns 1
+\papersides 1
+\paperpagestyle default
+\tracking_changes false
+\output_changes false
+\html_math_output 0
+\html_css_as_file 0
+\html_be_strict false
+\end_header
+
+\begin_body
+
+\begin_layout Title
+Hybrid Inference
+\end_layout
+
+\begin_layout Author
+Frank Dellaert & Varun Agrawal
+\end_layout
+
+\begin_layout Date
+January 2023
+\end_layout
+
+\begin_layout Section
+Hybrid Conditionals
+\end_layout
+
+\begin_layout Standard
+Here we develop a hybrid conditional density, on continuous variables (typically
+ a measurement 
+\begin_inset Formula $x$
+\end_inset
+
+), given a mix of continuous variables 
+\begin_inset Formula $y$
+\end_inset
+
+ and discrete variables 
+\begin_inset Formula $m$
+\end_inset
+
+.
+ We start by reviewing a Gaussian conditional density and its invariants
+ (relationship between density, error, and normalization constant), and
+ then work out what needs to happen for a hybrid version.
+\end_layout
+
+\begin_layout Subsubsection*
+GaussianConditional
+\end_layout
+
+\begin_layout Standard
+A 
+\emph on
+GaussianConditional
+\emph default
+ is a properly normalized, multivariate Gaussian conditional density:
+\begin_inset Formula 
+\[
+P(x|y)=\frac{1}{\sqrt{|2\pi\Sigma|}}\exp\left\{ -\frac{1}{2}\|Rx+Sy-d\|_{\Sigma}^{2}\right\} 
+\]
+
+\end_inset
+
+where 
+\begin_inset Formula $R$
+\end_inset
+
+ is square and upper-triangular.
+ For every 
+\emph on
+GaussianConditional
+\emph default
+, we have the following 
+\series bold
+invariant
+\series default
+,
+\begin_inset Formula 
+\begin{equation}
+\log P(x|y)=K_{gc}-E_{gc}(x,y),\label{eq:gc_invariant}
+\end{equation}
+
+\end_inset
+
+with the 
+\series bold
+log-normalization constant
+\series default
+ 
+\begin_inset Formula $K_{gc}$
+\end_inset
+
+ equal to
+\begin_inset Formula 
+\begin{equation}
+K_{gc}=\log\frac{1}{\sqrt{|2\pi\Sigma|}}\label{eq:log_constant}
+\end{equation}
+
+\end_inset
+
+ and the 
+\series bold
+error
+\series default
+ 
+\begin_inset Formula $E_{gc}(x,y)$
+\end_inset
+
+ equal to the negative log-density, up to a constant: 
+\begin_inset Formula 
+\begin{equation}
+E_{gc}(x,y)=\frac{1}{2}\|Rx+Sy-d\|_{\Sigma}^{2}.\label{eq:gc_error}
+\end{equation}
+
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Subsubsection*
+GaussianMixture
+\end_layout
+
+\begin_layout Standard
+A 
+\emph on
+GaussianMixture
+\emph default
+ (maybe to be renamed to 
+\emph on
+GaussianMixtureComponent
+\emph default
+) just indexes into a number of 
+\emph on
+GaussianConditional
+\emph default
+ instances, that are each properly normalized:
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula 
+\[
+P(x|y,m)=P_{m}(x|y).
+\]
+
+\end_inset
+
+We store one 
+\emph on
+GaussianConditional
+\emph default
+ 
+\begin_inset Formula $P_{m}(x|y)$
+\end_inset
+
+ for every possible assignment 
+\begin_inset Formula $m$
+\end_inset
+
+ to a set of discrete variables.
+ As 
+\emph on
+GaussianMixture
+\emph default
+ is a 
+\emph on
+Conditional
+\emph default
+, it needs to satisfy the a similar invariant to 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gc_invariant"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+:
+\begin_inset Formula 
+\begin{equation}
+\log P(x|y,m)=K_{gm}-E_{gm}(x,y,m).\label{eq:gm_invariant}
+\end{equation}
+
+\end_inset
+
+If we take the log of 
+\begin_inset Formula $P(x|y,m)$
+\end_inset
+
+ we get
+\begin_inset Formula 
+\begin{equation}
+\log P(x|y,m)=\log P_{m}(x|y)=K_{gcm}-E_{gcm}(x,y).\label{eq:gm_log}
+\end{equation}
+
+\end_inset
+
+Equating 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gm_invariant"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+ and 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gm_log"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+ we see that this can be achieved by defining the error 
+\begin_inset Formula $E_{gm}(x,y,m)$
+\end_inset
+
+ as
+\begin_inset Formula 
+\begin{equation}
+E_{gm}(x,y,m)=E_{gcm}(x,y)+K_{gm}-K_{gcm}\label{eq:gm_error}
+\end{equation}
+
+\end_inset
+
+where choose 
+\begin_inset Formula $K_{gm}=\max K_{gcm}$
+\end_inset
+
+, as then the error will always be positive.
+\end_layout
+
+\begin_layout Section
+Hybrid Factors
+\end_layout
+
+\begin_layout Standard
+In GTSAM, we typically condition on known measurements, and factors encode
+ the resulting negative log-likelihood of the unknown variables 
+\begin_inset Formula $y$
+\end_inset
+
+ given the measurements 
+\begin_inset Formula $x$
+\end_inset
+
+.
+ We review how a Gaussian conditional density is converted into a Gaussian
+ factor, and then develop a hybrid version satisfying the correct invariants
+ as well.
+\end_layout
+
+\begin_layout Subsubsection*
+JacobianFactor
+\end_layout
+
+\begin_layout Standard
+A 
+\emph on
+JacobianFactor
+\emph default
+ typically results from a 
+\emph on
+GaussianConditional
+\emph default
+ by having known values 
+\begin_inset Formula $\bar{x}$
+\end_inset
+
+ for the 
+\begin_inset Quotes eld
+\end_inset
+
+measurement
+\begin_inset Quotes erd
+\end_inset
+
+ 
+\begin_inset Formula $x$
+\end_inset
+
+:
+\begin_inset Formula 
+\begin{equation}
+L(y)\propto P(\bar{x}|y)\label{eq:likelihood}
+\end{equation}
+
+\end_inset
+
+In GTSAM factors represent the negative log-likelihood 
+\begin_inset Formula $E_{jf}(y)$
+\end_inset
+
+ and hence we have
+\begin_inset Formula 
+\[
+E_{jf}(y)=-\log L(y)=C-\log P(\bar{x}|y),
+\]
+
+\end_inset
+
+with 
+\begin_inset Formula $C$
+\end_inset
+
+ the log of the proportionality constant in 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:likelihood"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+.
+ Substituting in 
+\begin_inset Formula $\log P(\bar{x}|y)$
+\end_inset
+
+ from the invariant 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gc_invariant"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+ we obtain
+\begin_inset Formula 
+\[
+E_{jf}(y)=C-K_{gc}+E_{gc}(\bar{x},y).
+\]
+
+\end_inset
+
+The 
+\emph on
+likelihood
+\emph default
+ function in 
+\emph on
+GaussianConditional
+\emph default
+ chooses 
+\begin_inset Formula $C=K_{gc}$
+\end_inset
+
+, and the 
+\emph on
+JacobianFactor
+\emph default
+ does not store any constant; it just implements:
+\begin_inset Formula 
+\[
+E_{jf}(y)=E_{gc}(\bar{x},y)=\frac{1}{2}\|R\bar{x}+Sy-d\|_{\Sigma}^{2}=\frac{1}{2}\|Ay-b\|_{\Sigma}^{2}
+\]
+
+\end_inset
+
+with 
+\begin_inset Formula $A=S$
+\end_inset
+
+ and 
+\begin_inset Formula $b=d-R\bar{x}$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Subsubsection*
+GaussianMixtureFactor
+\end_layout
+
+\begin_layout Standard
+Analogously, a 
+\emph on
+GaussianMixtureFactor
+\emph default
+ typically results from a GaussianMixture by having known values 
+\begin_inset Formula $\bar{x}$
+\end_inset
+
+ for the 
+\begin_inset Quotes eld
+\end_inset
+
+measurement
+\begin_inset Quotes erd
+\end_inset
+
+ 
+\begin_inset Formula $x$
+\end_inset
+
+:
+\begin_inset Formula 
+\[
+L(y,m)\propto P(\bar{x}|y,m).
+\]
+
+\end_inset
+
+We will similarly implement the negative log-likelihood 
+\begin_inset Formula $E_{mf}(y,m)$
+\end_inset
+
+:
+\begin_inset Formula 
+\[
+E_{mf}(y,m)=-\log L(y,m)=C-\log P(\bar{x}|y,m).
+\]
+
+\end_inset
+
+Since we know the log-density from the invariant 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gm_invariant"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+, we obtain
+\begin_inset Formula 
+\[
+\log P(\bar{x}|y,m)=K_{gm}-E_{gm}(\bar{x},y,m),
+\]
+
+\end_inset
+
+ and hence
+\begin_inset Formula 
+\[
+E_{mf}(y,m)=C+E_{gm}(\bar{x},y,m)-K_{gm}.
+\]
+
+\end_inset
+
+Substituting in 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gm_error"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+ we finally have an expression where 
+\begin_inset Formula $K_{gm}$
+\end_inset
+
+ canceled out, but we have a dependence on the individual component constants
+ 
+\begin_inset Formula $K_{gcm}$
+\end_inset
+
+:
+\begin_inset Formula 
+\[
+E_{mf}(y,m)=C+E_{gcm}(\bar{x},y)-K_{gcm}.
+\]
+
+\end_inset
+
+Unfortunately, we can no longer choose 
+\begin_inset Formula $C$
+\end_inset
+
+ independently from 
+\begin_inset Formula $m$
+\end_inset
+
+ to make the constant disappear.
+ There are two possibilities:
+\end_layout
+
+\begin_layout Enumerate
+Implement likelihood to yield both a hybrid factor 
+\emph on
+and
+\emph default
+ a discrete factor.
+\end_layout
+
+\begin_layout Enumerate
+Hide the constant inside the collection of JacobianFactor instances, which
+ is the possibility we implement.
+\end_layout
+
+\begin_layout Standard
+In either case, we implement the mixture factor 
+\begin_inset Formula $E_{mf}(y,m)$
+\end_inset
+
+ as a set of 
+\emph on
+JacobianFactor
+\emph default
+ instances 
+\begin_inset Formula $E_{mf}(y,m)$
+\end_inset
+
+, indexed by the discrete assignment 
+\begin_inset Formula $m$
+\end_inset
+
+:
+\begin_inset Formula 
+\[
+E_{mf}(y,m)=E_{jfm}(y)=\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}.
+\]
+
+\end_inset
+
+In GTSAM, we define 
+\begin_inset Formula $A_{m}$
+\end_inset
+
+ and 
+\begin_inset Formula $b_{m}$
+\end_inset
+
+ strategically to make the 
+\emph on
+JacobianFactor
+\emph default
+ compute the constant, as well:
+\begin_inset Formula 
+\[
+\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=C+E_{gcm}(\bar{x},y)-K_{gcm}.
+\]
+
+\end_inset
+
+Substituting in the definition 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gc_error"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+ for 
+\begin_inset Formula $E_{gcm}(\bar{x},y)$
+\end_inset
+
+ we need
+\begin_inset Formula 
+\[
+\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=C+\frac{1}{2}\|R_{m}\bar{x}+S_{m}y-d_{m}\|_{\Sigma_{m}}^{2}-K_{gcm}
+\]
+
+\end_inset
+
+which can achieved by setting
+\begin_inset Formula 
+\[
+A_{m}=\left[\begin{array}{c}
+S_{m}\\
+0
+\end{array}\right],~b_{m}=\left[\begin{array}{c}
+d_{m}-R_{m}\bar{x}\\
+c_{m}
+\end{array}\right],~\Sigma_{mfm}=\left[\begin{array}{cc}
+\Sigma_{m}\\
+ & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+and setting the mode-dependent scalar 
+\begin_inset Formula $c_{m}$
+\end_inset
+
+ such that 
+\begin_inset Formula $c_{m}^{2}=C-K_{gcm}$
+\end_inset
+
+.
+ This can be achieved by 
+\begin_inset Formula $C=\max K_{gcm}=K_{gm}$
+\end_inset
+
+ and 
+\begin_inset Formula $c_{m}=\sqrt{2(C-K_{gcm})}$
+\end_inset
+
+.
+ Note that in case that all constants 
+\begin_inset Formula $K_{gcm}$
+\end_inset
+
+ are equal, we can just use 
+\begin_inset Formula $C=K_{gm}$
+\end_inset
+
+ and
+\begin_inset Formula 
+\[
+A_{m}=S_{m},~b_{m}=d_{m}-R_{m}\bar{x},~\Sigma_{mfm}=\Sigma_{m}
+\]
+
+\end_inset
+
+as before.
+\end_layout
+
+\begin_layout Standard
+In summary, we have
+\begin_inset Formula 
+\begin{equation}
+E_{mf}(y,m)=\frac{1}{2}\|A_{m}y-b_{m}\|_{\Sigma_{mfm}}^{2}=E_{gcm}(\bar{x},y)+K_{gm}-K_{gcm}.\label{eq:mf_invariant}
+\end{equation}
+
+\end_inset
+
+which is identical to the GaussianMixture error 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:gm_error"
+plural "false"
+caps "false"
+noprefix "false"
+
+\end_inset
+
+.
+\end_layout
+
+\end_body
+\end_document

examples/CMakeLists.txt

@@ -1,5 +1,23 @@
 set (excluded_examples
-    elaboratePoint2KalmanFilter.cpp
+    "elaboratePoint2KalmanFilter.cpp"
 )
 
-gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")
+# if GTSAM_ENABLE_BOOST_SERIALIZATION is not set then SolverComparer.cpp will not compile
+if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
+    list (APPEND excluded_examples
+    "SolverComparer.cpp"
+  )
+endif()
+
+# Add examples to exclude if GTSAM_USE_BOOST_FEATURES is not set
+if (NOT GTSAM_USE_BOOST_FEATURES) 
+  # add to excluded examples
+  list (APPEND excluded_examples
+      "CombinedImuFactorsExample.cpp"
+      "ImuFactorsExample.cpp"
+      "ShonanAveragingCLI.cpp"
+      "SolverComparer.cpp"
+      )
+endif()
+
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}")

examples/CameraResectioning.cpp

@@ -20,7 +20,6 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <boost/make_shared.hpp>
 
 using namespace gtsam;
 using namespace gtsam::noiseModel;
@@ -30,8 +29,8 @@ using symbol_shorthand::X;
  * Unary factor on the unknown pose, resulting from meauring the projection of
  * a known 3D point in the image
  */
-class ResectioningFactor: public NoiseModelFactor1<Pose3> {
+class ResectioningFactor: public NoiseModelFactorN<Pose3> {
-  typedef NoiseModelFactor1<Pose3> Base;
+  typedef NoiseModelFactorN<Pose3> Base;
 
   Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
   Point3 P_;              ///< 3D point on the calibration rig
@@ -46,10 +45,9 @@ public:
   }
 
   /// evaluate the error
-  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
+  Vector evaluateError(const Pose3& pose, OptionalMatrixType H) const override {
-      boost::none) const override {
     PinholeCamera<Cal3_S2> camera(pose, *K_);
-    return camera.project(P_, H, boost::none, boost::none) - p_;
+    return camera.project(P_, H, OptionalNone, OptionalNone) - p_;
   }
 };
 
@@ -71,7 +69,7 @@ int main(int argc, char* argv[]) {
   /* 2. add factors to the graph */
   // add measurement factors
   SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
-  boost::shared_ptr<ResectioningFactor> factor;
+  std::shared_ptr<ResectioningFactor> factor;
   graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
           Point2(55, 45), Point3(10, 10, 0));
   graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,

examples/CombinedImuFactorsExample.cpp

@@ -95,7 +95,7 @@ Vector10 readInitialState(ifstream& file) {
   return initial_state;
 }
 
-boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
+std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // We use the sensor specs to build the noise model for the IMU factor.
   double accel_noise_sigma = 0.0003924;
   double gyro_noise_sigma = 0.000205689024915;

examples/CreateSFMExampleData.cpp

@@ -18,9 +18,6 @@
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/slam/dataset.h>
 
-#include <boost/assign/std/vector.hpp>
-
-using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 

examples/Data/randomGrid3D.xml

@@ -1,13 +1,13 @@
 <?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
 <!DOCTYPE boost_serialization>
-<boost_serialization signature="serialization::archive" version="17">
+<boost_serialization signature="serialization::archive" version="19">
-<graph class_id="0" tracking_level="0" version="0">
+<data class_id="0" tracking_level="0" version="0">
 	<Base class_id="1" tracking_level="0" version="0">
 		<factors_ class_id="2" tracking_level="0" version="0">
 			<count>32</count>
 			<item_version>1</item_version>
 			<item class_id="3" tracking_level="0" version="1">
-				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
+				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="1" object_id="_0">
 					<Base class_id="5" tracking_level="0" version="0">
 						<Base class_id="6" tracking_level="0" version="0">
 							<keys_>
@@ -100,9 +100,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_ class_id="11" tracking_level="0" version="1">
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -199,9 +197,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -298,9 +294,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -397,9 +391,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -496,9 +488,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -595,9 +585,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -694,9 +682,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -793,9 +779,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -892,9 +876,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -991,9 +973,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1090,9 +1070,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1189,9 +1167,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1288,9 +1264,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1387,9 +1361,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1486,9 +1458,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1585,9 +1555,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1684,9 +1652,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1783,9 +1749,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1882,9 +1846,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -1981,9 +1943,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2080,9 +2040,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2179,9 +2137,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2278,9 +2234,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2377,9 +2331,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2476,9 +2428,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2575,9 +2525,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2674,9 +2622,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2773,9 +2719,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2872,9 +2816,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -2971,9 +2913,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -3070,9 +3010,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -3402,13 +3340,11 @@
 						<rowEnd_>3</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 		</factors_>
 	</Base>
-</graph>
+</data>
 </boost_serialization>
 

examples/Data/toy3D.xml

@@ -1,13 +1,13 @@
 <?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
 <!DOCTYPE boost_serialization>
-<boost_serialization signature="serialization::archive" version="17">
+<boost_serialization signature="serialization::archive" version="19">
-<graph class_id="0" tracking_level="0" version="0">
+<data class_id="0" tracking_level="0" version="0">
 	<Base class_id="1" tracking_level="0" version="0">
 		<factors_ class_id="2" tracking_level="0" version="0">
 			<count>2</count>
 			<item_version>1</item_version>
 			<item class_id="3" tracking_level="0" version="1">
-				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
+				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="1" object_id="_0">
 					<Base class_id="5" tracking_level="0" version="0">
 						<Base class_id="6" tracking_level="0" version="0">
 							<keys_>
@@ -100,9 +100,7 @@
 						<rowEnd_>9</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_ class_id="11" tracking_level="0" version="1">
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 			<item>
@@ -157,13 +155,11 @@
 						<rowEnd_>3</rowEnd_>
 						<blockStart_>0</blockStart_>
 					</Ab_>
-					<model_>
+					<model_null>1</model_null>
-						<px class_id="-1"></px>
-					</model_>
 				</px>
 			</item>
 		</factors_>
 	</Base>
-</graph>
+</data>
 </boost_serialization>
 

examples/DiscreteBayesNetExample.cpp

@@ -51,8 +51,7 @@ int main(int argc, char **argv) {
   DiscreteFactorGraph fg(asia);
 
   // Create solver and eliminate
-  Ordering ordering;
+  const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7};
-  ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
 
   // solve
   auto mpe = fg.optimize();

examples/FisheyeExample.cpp

@@ -61,7 +61,7 @@ using symbol_shorthand::X;  // for poses
 /* ************************************************************************* */
 int main(int argc, char *argv[]) {
   // Define the camera calibration parameters
-  auto K = boost::make_shared<Cal3Fisheye>(
+  auto K = std::make_shared<Cal3Fisheye>(
       278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035,
       0.020727425669427896, -0.012786476702685545, 0.0025242267320687625);
 

examples/FixedLagSmootherExample.cpp

@@ -23,7 +23,7 @@
  */
 
 // This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable
-#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
@@ -152,7 +152,7 @@ int main(int argc, char** argv) {
   auto &factorGraph = smootherISAM2.getFactors();
 
   // Linearize to a Gaussian factor graph
-  boost::shared_ptr<GaussianFactorGraph> linearGraph = factorGraph.linearize(result);
+  std::shared_ptr<GaussianFactorGraph> linearGraph = factorGraph.linearize(result);
 
   // Converts the linear graph into a Jacobian factor and extracts the Jacobian matrix
   Matrix jacobian = linearGraph->jacobian().first;

examples/ISAM2Example_SmartFactor.cpp

@@ -87,9 +87,8 @@ int main(int argc, char* argv[]) {
     result.print();
 
     cout << "Detailed results:" << endl;
-    for (auto keyedStatus : result.detail->variableStatus) {
+    for (auto& [key, status] : result.detail->variableStatus) {
-      const auto& status = keyedStatus.second;
+      PrintKey(key);
-      PrintKey(keyedStatus.first);
       cout << " {" << endl;
       cout << "reeliminated: " << status.isReeliminated << endl;
       cout << "relinearized above thresh: " << status.isAboveRelinThreshold
@@ -105,7 +104,7 @@ int main(int argc, char* argv[]) {
     Values currentEstimate = isam.calculateEstimate();
     currentEstimate.print("Current estimate: ");
 
-    boost::optional<Point3> pointEstimate = smartFactor->point(currentEstimate);
+    auto pointEstimate = smartFactor->point(currentEstimate);
     if (pointEstimate) {
       cout << *pointEstimate << endl;
     } else {

examples/ImuFactorsExample.cpp

@@ -82,7 +82,7 @@ po::variables_map parseOptions(int argc, char* argv[]) {
   return vm;
 }
 
-boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
+std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
   // We use the sensor specs to build the noise model for the IMU factor.
   double accel_noise_sigma = 0.0003924;
   double gyro_noise_sigma = 0.000205689024915;
@@ -267,7 +267,6 @@ int main(int argc, char* argv[]) {
 
       if (use_isam) {
         isam2->update(*graph, initial_values);
-        isam2->update();
         result = isam2->calculateEstimate();
 
         // reset the graph

examples/ImuFactorsExample2.cpp

@@ -115,7 +115,7 @@ int main(int argc, char* argv[]) {
         Vector6 covvec;
         covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
         auto cov = noiseModel::Diagonal::Variances(covvec);
-        auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
+        auto f = std::make_shared<BetweenFactor<imuBias::ConstantBias> >(
             b1, b2, imuBias::ConstantBias(), cov);
         newgraph.add(f);
         initialEstimate.insert(biasKey, imuBias::ConstantBias());

examples/LocalizationExample.cpp

@@ -62,29 +62,33 @@ using namespace gtsam;
 //
 // The factor will be a unary factor, affect only a single system variable. It will
 // also use a standard Gaussian noise model. Hence, we will derive our new factor from
-// the NoiseModelFactor1.
+// the NoiseModelFactorN.
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
-class UnaryFactor: public NoiseModelFactor1<Pose2> {
+class UnaryFactor: public NoiseModelFactorN<Pose2> {
   // The factor will hold a measurement consisting of an (X,Y) location
   // We could this with a Point2 but here we just use two doubles
   double mx_, my_;
 
  public:
+
+  // Provide access to Matrix& version of evaluateError:
+  using NoiseModelFactor1<Pose2>::evaluateError;
+
   /// shorthand for a smart pointer to a factor
-  typedef boost::shared_ptr<UnaryFactor> shared_ptr;
+  typedef std::shared_ptr<UnaryFactor> shared_ptr;
 
   // The constructor requires the variable key, the (X, Y) measurement value, and the noise model
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
-    NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
+    NoiseModelFactorN<Pose2>(model, j), mx_(x), my_(y) {}
 
   ~UnaryFactor() override {}
 
-  // Using the NoiseModelFactor1 base class there are two functions that must be overridden.
+  // Using the NoiseModelFactorN base class there are two functions that must be overridden.
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const override {
+  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
     // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
     // The error is then simply calculated as E(q) = h(q) - m:
     // error_x = q.x - mx
@@ -101,7 +105,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   // circumstances, the following code that employs the default copy constructor should
   // work fine.
   gtsam::NonlinearFactor::shared_ptr clone() const override {
-    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
 
   // Additionally, we encourage you the use of unit testing your custom factors,

examples/Pose2SLAMExample_g2o.cpp

@@ -48,16 +48,16 @@ int main(const int argc, const char *argv[]) {
   Values::shared_ptr initial;
   bool is3D = false;
   if (kernelType.compare("none") == 0) {
-    boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+    std::tie(graph, initial) = readG2o(g2oFile, is3D);
   }
   if (kernelType.compare("huber") == 0) {
     std::cout << "Using robust kernel: huber " << std::endl;
-    boost::tie(graph, initial) =
+    std::tie(graph, initial) =
         readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
   }
   if (kernelType.compare("tukey") == 0) {
     std::cout << "Using robust kernel: tukey " << std::endl;
-    boost::tie(graph, initial) =
+    std::tie(graph, initial) =
         readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
   }
 
@@ -90,7 +90,7 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
-    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
     writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }

examples/Pose2SLAMExample_graph.cpp

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

examples/Pose2SLAMExample_lago.cpp

@@ -37,7 +37,7 @@ int main(const int argc, const char *argv[]) {
 
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
-  boost::tie(graph, initial) = readG2o(g2oFile);
+  std::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
   auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
@@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
-    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
     writeG2o(*graphNoKernel, estimateLago, outputFile);
     std::cout << "done! " << std::endl;
   }

examples/Pose2SLAMwSPCG.cpp

@@ -69,7 +69,7 @@ int main(int argc, char** argv) {
   // addition, the *type* of the iterativeParams decides on the type of
   // iterative solver, in this case the SPCG (subgraph PCG)
   parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
-  parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
+  parameters.iterativeParams = std::make_shared<SubgraphSolverParameters>();
 
   LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
   Values result = optimizer.optimize();

examples/Pose3Localization.cpp

@@ -37,15 +37,15 @@ int main(const int argc, const char* argv[]) {
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = true;
-  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+  std::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
@@ -67,17 +67,15 @@ int main(const int argc, const char* argv[]) {
     std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
-    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
     writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for (const auto key_value : result) {
+  for (const auto& [key, pose] : result.extract<Pose3>()) {
-    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    std::cout << marginals.marginalCovariance(key) << endl;
-    if (!p) continue;
-    std::cout << marginals.marginalCovariance(key_value.key) << endl;
   }
   return 0;
 }

examples/Pose3SLAMExample_changeKeys.cpp

@@ -36,7 +36,7 @@ int main(const int argc, const char *argv[]) {
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = true;
-  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+  std::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   bool add = false;
   Key firstKey = 8646911284551352320;
@@ -55,27 +55,27 @@ int main(const int argc, const char *argv[]) {
     std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
     // Additional: rewrite input with simplified keys 0,1,...
     Values simpleInitial;
-    for(const auto key_value: *initial) {
+    for (const auto k : initial->keys()) {
       Key key;
       if (add)
-        key = key_value.key + firstKey;
+        key = k + firstKey;
       else
-        key = key_value.key - firstKey;
+        key = k - firstKey;
 
-      simpleInitial.insert(key, initial->at(key_value.key));
+      simpleInitial.insert(key, initial->at(k));
     }
     NonlinearFactorGraph simpleGraph;
-    for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
+    for(const std::shared_ptr<NonlinearFactor>& factor: *graph) {
-      boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+      std::shared_ptr<BetweenFactor<Pose3> > pose3Between =
-          boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+          std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
       if (pose3Between){
         Key key1, key2;
         if(add){
-          key1 = pose3Between->key1() + firstKey;
+          key1 = pose3Between->key<1>() + firstKey;
-          key2 = pose3Between->key2() + firstKey;
+          key2 = pose3Between->key<2>() + firstKey;
         }else{
-          key1 = pose3Between->key1() - firstKey;
+          key1 = pose3Between->key<1>() - firstKey;
-          key2 = pose3Between->key2() - firstKey;
+          key2 = pose3Between->key<2>() - firstKey;
         }
         NonlinearFactor::shared_ptr simpleFactor(
             new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->noiseModel()));

examples/Pose3SLAMExample_g2o.cpp

@@ -36,15 +36,15 @@ int main(const int argc, const char* argv[]) {
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = true;
-  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+  std::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
@@ -66,7 +66,7 @@ int main(const int argc, const char* argv[]) {
     std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
-    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
     writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -36,15 +36,15 @@ int main(const int argc, const char* argv[]) {
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = true;
-  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+  std::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
@@ -60,7 +60,7 @@ int main(const int argc, const char* argv[]) {
     std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
-    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
     writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -36,15 +36,15 @@ int main(const int argc, const char* argv[]) {
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = true;
-  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+  std::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
   auto priorModel = noiseModel::Diagonal::Variances(
       (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for (const auto key_value : *initial) {
+  for (const auto key : initial->keys()) {
     std::cout << "Adding prior to g2o file " << std::endl;
-    firstKey = key_value.key;
+    firstKey = key;
     graph->addPrior(firstKey, Pose3(), priorModel);
     break;
   }
@@ -66,7 +66,7 @@ int main(const int argc, const char* argv[]) {
     std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
-    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
     writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }

examples/RangeISAMExample_plaza2.cpp

@@ -92,7 +92,7 @@ std::list<TimedOdometry> readOdometry() {
 
 // load the ranges from TD
 //    Time (sec)  Sender / Antenna ID Receiver Node ID  Range (m)
-using RangeTriple = boost::tuple<double, size_t, double>;
+using RangeTriple = std::tuple<double, size_t, double>;
 std::vector<RangeTriple> readTriples() {
   std::vector<RangeTriple> triples;
   std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt");
@@ -166,7 +166,7 @@ int main(int argc, char** argv) {
     //--------------------------------- odometry loop --------------------------
     double t;
     Pose2 odometry;
-    boost::tie(t, odometry) = timedOdometry;
+    std::tie(t, odometry) = timedOdometry;
 
     // add odometry factor
     newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(i - 1, i, odometry,
@@ -178,10 +178,10 @@ int main(int argc, char** argv) {
     initial.insert(i, predictedPose);
 
     // Check if there are range factors to be added
-    while (k < K && t >= boost::get<0>(triples[k])) {
+    while (k < K && t >= std::get<0>(triples[k])) {
-      size_t j = boost::get<1>(triples[k]);
+      size_t j = std::get<1>(triples[k]);
       Symbol landmark_key('L', j);
-      double range = boost::get<2>(triples[k]);
+      double range = std::get<2>(triples[k]);
       newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
           i, landmark_key, range, rangeNoise);
       if (initializedLandmarks.count(landmark_key) == 0) {

examples/SFMExampleExpressions_bal.cpp

@@ -31,7 +31,6 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/inference/Symbol.h>
 
-#include <boost/format.hpp>
 #include <vector>
 
 using namespace std;
@@ -50,8 +49,7 @@ int main(int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfmData mydata = SfmData::FromBalFile(filename);
-  cout << boost::format("read %1% tracks on %2% cameras\n") %
+  cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
-              mydata.numberTracks() % mydata.numberCameras();
 
   // Create a factor graph
   ExpressionFactorGraph graph;
@@ -79,9 +77,7 @@ int main(int argc, char* argv[]) {
   for (const SfmTrack& track : mydata.tracks) {
     // Leaf expression for j^th point
     Point3_ point_('p', j);
-    for (const SfmMeasurement& m : track.measurements) {
+    for (const auto& [i, uv] : track.measurements) {
-      size_t i = m.first;
-      Point2 uv = m.second;
       // Leaf expression for i^th camera
       Expression<SfmCamera> camera_(C(i));
       // Below an expression for the prediction of the measurement:

examples/SFMExample_SmartFactor.cpp

@@ -112,12 +112,12 @@ int main(int argc, char* argv[]) {
   for (size_t j = 0; j < points.size(); ++j) {
 
     // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
-    SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
+    SmartFactor::shared_ptr smart = std::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
-      // The output of point() is in boost::optional<Point3>, as sometimes
+      // The output of point() is in std::optional<Point3>, as sometimes
       // the triangulation operation inside smart factor will encounter degeneracy.
-      boost::optional<Point3> point = smart->point(result);
+      auto point = smart->point(result);
-      if (point) // ignore if boost::optional return nullptr
+      if (point) // ignore if std::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_SmartFactorPCG.cpp

@@ -93,9 +93,9 @@ int main(int argc, char* argv[]) {
   parameters.relativeErrorTol = 1e-10;
   parameters.maxIterations = 500;
   PCGSolverParameters::shared_ptr pcg =
-      boost::make_shared<PCGSolverParameters>();
+      std::make_shared<PCGSolverParameters>();
   pcg->preconditioner_ =
-      boost::make_shared<BlockJacobiPreconditionerParameters>();
+      std::make_shared<BlockJacobiPreconditionerParameters>();
   // Following is crucial:
   pcg->setEpsilon_abs(1e-10);
   pcg->setEpsilon_rel(1e-10);
@@ -108,10 +108,10 @@ int main(int argc, char* argv[]) {
   result.print("Final results:\n");
   Values landmark_result;
   for (size_t j = 0; j < points.size(); ++j) {
-    auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
+    auto smart = std::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
-      boost::optional<Point3> point = smart->point(result);
+      std::optional<Point3> point = smart->point(result);
-      if (point)  // ignore if boost::optional return nullptr
+      if (point)  // ignore if std::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_bal.cpp

@@ -23,7 +23,6 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/inference/Symbol.h>
 
-#include <boost/format.hpp>
 #include <vector>
 
 using namespace std;
@@ -45,7 +44,7 @@ int main (int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfmData mydata = SfmData::FromBalFile(filename);
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
+  cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
 
   // Create a factor graph
   NonlinearFactorGraph graph;
@@ -57,9 +56,7 @@ int main (int argc, char* argv[]) {
   // Add measurements to the factor graph
   size_t j = 0;
   for(const SfmTrack& track: mydata.tracks) {
-    for(const SfmMeasurement& m: track.measurements) {
+    for (const auto& [i, uv] : track.measurements) {
-      size_t i = m.first;
-      Point2 uv = m.second;
       graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
     }
     j += 1;

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -26,7 +26,6 @@
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/base/timing.h>
 
-#include <boost/format.hpp>
 #include <vector>
 
 using namespace std;
@@ -47,8 +46,7 @@ int main(int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfmData mydata = SfmData::FromBalFile(filename);
-  cout << boost::format("read %1% tracks on %2% cameras\n") %
+  cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl;
-              mydata.numberTracks() % mydata.numberCameras();
 
   // Create a factor graph
   NonlinearFactorGraph graph;
@@ -59,9 +57,7 @@ int main(int argc, char* argv[]) {
   // Add measurements to the factor graph
   size_t j = 0;
   for (const SfmTrack& track : mydata.tracks) {
-    for (const SfmMeasurement& m : track.measurements) {
+    for (const auto& [i, uv] : track.measurements) {
-      size_t i = m.first;
-      Point2 uv = m.second;
       graph.emplace_shared<MyFactor>(
           uv, noise, C(i), P(j));  // note use of shorthand symbols C and P
     }
@@ -130,9 +126,9 @@ int main(int argc, char* argv[]) {
     cout << endl << endl;
 
     cout << "Time comparison by solving " << filename << " results:" << endl;
-    cout << boost::format("%1% point tracks and %2% cameras\n") %
+
-                mydata.numberTracks() % mydata.numberCameras()
+    cout << mydata.numberTracks() << " point tracks and " << mydata.numberCameras()
-         << endl;
+         << " cameras" << endl;
 
     tictoc_print_();
   }

examples/ShonanAveragingCLI.cpp

@@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
     auto result = shonan.run(initial, pMin);
 
     // Parse file again to set up translation problem, adding a prior
-    boost::tie(inputGraph, posesInFile) = load2D(inputFile);
+    std::tie(inputGraph, posesInFile) = load2D(inputFile);
     auto priorModel = noiseModel::Unit::Create(3);
     inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
 
@@ -119,7 +119,7 @@ int main(int argc, char* argv[]) {
     auto result = shonan.run(initial, pMin);
 
     // Parse file again to set up translation problem, adding a prior
-    boost::tie(inputGraph, posesInFile) = load3D(inputFile);
+    std::tie(inputGraph, posesInFile) = load3D(inputFile);
     auto priorModel = noiseModel::Unit::Create(6);
     inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
 

examples/SolverComparer.cpp

@@ -49,8 +49,6 @@
 #include <boost/archive/binary_oarchive.hpp>
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
-#include <boost/range/adaptor/reversed.hpp>
-#include <boost/serialization/export.hpp>
 
 #include <fstream>
 #include <iostream>
@@ -69,8 +67,8 @@ namespace br = boost::range;
 
 typedef Pose2 Pose;
 
-typedef NoiseModelFactor1<Pose> NM1;
+typedef NoiseModelFactorN<Pose> NM1;
-typedef NoiseModelFactor2<Pose,Pose> NM2;
+typedef NoiseModelFactorN<Pose,Pose> NM2;
 typedef BearingRangeFactor<Pose,Point2> BR;
 
 double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
@@ -79,7 +77,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c
   // the factor graph already includes a factor for the prior/equality constraint.
   //  double dof = graph.size() - config.size();
   int graph_dim = 0;
-  for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
+  for(const std::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
     graph_dim += (int)nlf->dim();
   }
   double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
@@ -259,9 +257,9 @@ void runIncremental()
   while(nextMeasurement < datasetMeasurements.size())
   {
     if(BetweenFactor<Pose>::shared_ptr factor =
-      boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
+      std::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
     {
-      Key key1 = factor->key1(), key2 = factor->key2();
+      Key key1 = factor->key<1>(), key2 = factor->key<2>();
       if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
         // We found an odometry starting at firstStep
         firstPose = std::min(key1, key2);
@@ -310,14 +308,14 @@ void runIncremental()
       NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
 
       if(BetweenFactor<Pose>::shared_ptr factor =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
+        std::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
       {
         // Stop collecting measurements that are for future steps
-        if(factor->key1() > step || factor->key2() > step)
+        if(factor->key<1>() > step || factor->key<2>() > step)
           break;
 
         // Require that one of the nodes is the current one
-        if(factor->key1() != step && factor->key2() != step)
+        if(factor->key<1>() != step && factor->key<2>() != step)
           throw runtime_error("Problem in data file, out-of-sequence measurements");
 
         // Add a new factor
@@ -325,28 +323,28 @@ void runIncremental()
         const auto& measured = factor->measured();
 
         // Initialize the new variable
-        if(factor->key1() > factor->key2()) {
+        if(factor->key<1>() > factor->key<2>()) {
-          if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(factor->key1(), measured.inverse());
+              newVariables.insert(factor->key<1>(), measured.inverse());
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<2>());
-              newVariables.insert(factor->key1(), prevPose * measured.inverse());
+              newVariables.insert(factor->key<1>(), prevPose * measured.inverse());
             }
           }
         } else {
-          if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
+          if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry
             if(step == 1)
-              newVariables.insert(factor->key2(), measured);
+              newVariables.insert(factor->key<2>(), measured);
             else {
-              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
+              Pose prevPose = isam2.calculateEstimate<Pose>(factor->key<1>());
-              newVariables.insert(factor->key2(), prevPose * measured);
+              newVariables.insert(factor->key<2>(), prevPose * measured);
             }
           }
         }
       }
       else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
-        boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
+        std::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
       {
         Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
 
@@ -559,12 +557,12 @@ void runPerturb()
 
   // Perturb values
   VectorValues noise;
-  for(const Values::KeyValuePair key_val: initial)
+  for(const auto& [key, dim]: initial.dims())
   {
-    Vector noisev(key_val.value.dim());
+    Vector noisev(dim);
     for(Vector::Index i = 0; i < noisev.size(); ++i)
       noisev(i) = normal(rng);
-    noise.insert(key_val.key, noisev);
+    noise.insert(key, noisev);
   }
   Values perturbed = initial.retract(noise);
 

examples/StereoVOExample_large.cpp

@@ -27,6 +27,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/utilities.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -113,7 +114,7 @@ int main(int argc, char** argv) {
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;
-  Values pose_values = result.filter<Pose3>();
+  Values pose_values = utilities::allPose3s(result);
   pose_values.print("Final camera poses:\n");
 
   return 0;

examples/TimeTBB.cpp

@@ -18,13 +18,11 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/base/Matrix.h>
 
-#include <boost/assign/list_of.hpp>
 #include <map>
 #include <iostream>
 
 using namespace std;
 using namespace gtsam;
-using boost::assign::list_of;
 
 #ifdef GTSAM_USE_TBB
 
@@ -81,7 +79,7 @@ map<int, double> testWithoutMemoryAllocation(int num_threads)
   // Now call it
   vector<double> results(numberOfProblems);
 
-  const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
+  const vector<size_t> grainSizes = {1, 10, 100, 1000};
   map<int, double> timingResults;
   for(size_t grainSize: grainSizes)
   {
@@ -145,7 +143,7 @@ map<int, double> testWithMemoryAllocation(int num_threads)
   // Now call it
   vector<double> results(numberOfProblems);
 
-  const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
+  const vector<size_t> grainSizes = {1, 10, 100, 1000};
   map<int, double> timingResults;
   for(size_t grainSize: grainSizes)
   {
@@ -172,7 +170,7 @@ int main(int argc, char* argv[])
   cout << "numberOfProblems = " << numberOfProblems << endl;
   cout << "problemSize = " << problemSize << endl;
 
-  const vector<int> numThreads = list_of(1)(4)(8);
+  const vector<int> numThreads = {1, 4, 8};
   Results results;
 
   for(size_t n: numThreads)

examples/TriangulationLOSTExample.cpp

@@ -29,6 +29,7 @@
 #include <chrono>
 #include <iostream>
 #include <random>
+#include <optional>
 
 using namespace std;
 using namespace gtsam;
@@ -121,7 +122,7 @@ int main(int argc, char* argv[]) {
   Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3);
 
   double rank_tol = 1e-9;
-  boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
+  std::shared_ptr<Cal3_S2> calib = std::make_shared<Cal3_S2>();
   std::chrono::nanoseconds durationDLT;
   std::chrono::nanoseconds durationDLTOpt;
   std::chrono::nanoseconds durationLOST;
@@ -131,23 +132,23 @@ int main(int argc, char* argv[]) {
         AddNoiseToMeasurements(measurements, measurementSigma);
 
     auto lostStart = std::chrono::high_resolution_clock::now();
-    boost::optional<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
+    auto estimateLOST = triangulatePoint3<Cal3_S2>(
         cameras, noisyMeasurements, rank_tol, false, measurementNoise, true);
     durationLOST += std::chrono::high_resolution_clock::now() - lostStart;
 
     auto dltStart = std::chrono::high_resolution_clock::now();
-    boost::optional<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
+    auto estimateDLT = triangulatePoint3<Cal3_S2>(
         cameras, noisyMeasurements, rank_tol, false, measurementNoise, false);
     durationDLT += std::chrono::high_resolution_clock::now() - dltStart;
 
     auto dltOptStart = std::chrono::high_resolution_clock::now();
-    boost::optional<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
+    auto estimateDLTOpt = triangulatePoint3<Cal3_S2>(
         cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
     durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
 
-    errorsLOST.row(i) = *estimateLOST - landmark;
+    errorsLOST.row(i) = estimateLOST - landmark;
-    errorsDLT.row(i) = *estimateDLT - landmark;
+    errorsDLT.row(i) = estimateDLT - landmark;
-    errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
+    errorsDLTOpt.row(i) = estimateDLTOpt - landmark;
   }
   PrintCovarianceStats(errorsLOST, "LOST");
   PrintCovarianceStats(errorsDLT, "DLT");

gtsam/3rdparty/Eigen/.gitignore

@@ -1,4 +1,3 @@
-syntax: glob
 qrc_*cxx
 *.orig
 *.pyc
@@ -13,7 +12,7 @@ core
 core.*
 *.bak
 *~
-build*
+*build*
 *.moc.*
 *.moc
 ui_*
@@ -28,7 +27,12 @@ activity.png
 *.rej
 log
 patch
+*.patch
 a
 a.*
 lapack/testing
 lapack/reference
+.*project
+.settings
+Makefile
+!ci/build.gitlab-ci.yml

gtsam/3rdparty/Eigen/.gitlab-ci.yml

@@ -0,0 +1,23 @@
+# This file is part of Eigen, a lightweight C++ template library
+# for linear algebra.
+#
+# Copyright (C) 2020 Arm Ltd. and Contributors
+#
+# This Source Code Form is subject to the terms of the Mozilla
+# Public License v. 2.0. If a copy of the MPL was not distributed
+# with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+stages:
+  - buildsmoketests
+  - smoketests
+  - build
+  - test
+
+variables:
+  BUILDDIR: builddir
+  EIGEN_CI_CMAKE_GENEATOR: "Ninja"
+
+include:
+  - "/ci/smoketests.gitlab-ci.yml"
+  - "/ci/build.gitlab-ci.yml"
+  - "/ci/test.gitlab-ci.yml"

gtsam/3rdparty/Eigen/.gitlab/issue_templates/Bug Report.md

@@ -0,0 +1,69 @@
+<!--
+Please read this!
+
+Before opening a new issue, make sure to search for keywords in the issues
+filtered by "bug::confirmed" or "bug::unconfirmed" and "bugzilla" label:
+
+- https://gitlab.com/libeigen/eigen/-/issues?scope=all&utf8=%E2%9C%93&state=opened&label_name[]=bug%3A%3Aconfirmed
+- https://gitlab.com/libeigen/eigen/-/issues?scope=all&utf8=%E2%9C%93&state=opened&label_name[]=bug%3A%3Aunconfirmed
+- https://gitlab.com/libeigen/eigen/-/issues?scope=all&utf8=%E2%9C%93&state=opened&label_name[]=bugzilla
+
+and verify the issue you're about to submit isn't a duplicate. -->
+
+### Summary
+<!-- Summarize the bug encountered concisely. -->
+
+### Environment
+<!-- Please provide your development environment here -->
+- **Operating System** : Windows/Linux
+- **Architecture** : x64/Arm64/PowerPC ...
+- **Eigen Version** : 3.3.9
+- **Compiler Version** : Gcc7.0
+- **Compile Flags** : -O3 -march=native
+- **Vector Extension** : SSE/AVX/NEON ...
+
+### Minimal Example
+<!-- If possible, please create a minimal example here that exhibits the problematic behavior.
+You can also link to [godbolt](https://godbolt.org). But please note that you need to click 
+the "Share" button in the top right-hand corner of the godbolt page where you reproduce the sample 
+code to get the share link instead of in your browser address bar. 
+
+You can read [the guidelines on stackoverflow](https://stackoverflow.com/help/minimal-reproducible-example)
+on how to create a good minimal example. -->
+
+```cpp
+//show your code here
+```
+
+### Steps to reproduce
+<!-- Describe how one can reproduce the issue - this is very important. Please use an ordered list. -->
+
+1. first step
+2. second step
+3. ... 
+
+### What is the current *bug* behavior?
+<!-- Describe what actually happens. -->
+
+### What is the expected *correct* behavior?
+<!-- Describe what you should see instead. -->
+
+### Relevant logs
+<!-- Add relevant code snippets or program output within blocks marked by " ``` " -->
+
+<!-- OPTIONAL: remove this section if you are not reporting a compilation warning issue.-->
+### Warning Messages
+<!-- Show us the warning messages you got! -->
+
+<!-- OPTIONAL: remove this section if you are not reporting a performance issue. -->
+### Benchmark scripts and results
+<!-- Please share any benchmark scripts - either standalone, or using [Google Benchmark](https://github.com/google/benchmark). -->
+
+### Anything else that might help
+<!-- It will be better to provide us more information to help narrow down the cause. 
+Including but not limited to the following: 
+- lines of code that might help us diagnose the problem. 
+- potential ways to address the issue.
+- last known working/first broken version (release number or commit hash). --> 
+
+- [ ] Have a plan to fix this issue.

gtsam/3rdparty/Eigen/.gitlab/issue_templates/Feature Request.md

@@ -0,0 +1,7 @@
+### Describe the feature you would like to be implemented.
+
+### Would such a feature be useful for other users? Why?
+
+### Any hints on how to implement the requested feature?
+
+### Additional resources

gtsam/3rdparty/Eigen/.gitlab/merge_request_templates/Merge Request Template.md

@@ -0,0 +1,26 @@
+<!-- 
+Thanks for contributing a merge request! Please name and fully describe your MR as you would for a commit message.
+If the MR fixes an issue, please include "Fixes #issue" in the commit message and the MR description.
+
+In addition, we recommend that first-time contributors read our [contribution guidelines](https://eigen.tuxfamily.org/index.php?title=Contributing_to_Eigen) and [git page](https://eigen.tuxfamily.org/index.php?title=Git), which will help you submit a more standardized MR.
+
+Before submitting the MR, you also need to complete the following checks:
+- Make one PR per feature/bugfix (don't mix multiple changes into one PR). Avoid committing unrelated changes.
+- Rebase before committing
+- For code changes, run the test suite (at least the tests that are likely affected by the change).
+  See our [test guidelines](https://eigen.tuxfamily.org/index.php?title=Tests).
+- If possible, add a test (both for bug-fixes as well as new features)
+- Make sure new features are documented
+
+Note that we are a team of volunteers; we appreciate your patience during the review process.
+
+Again, thanks for contributing! -->
+
+### Reference issue
+<!-- You can link to a specific issue using the gitlab syntax #<issue number>  -->
+
+### What does this implement/fix?
+<!--Please explain your changes.-->
+
+### Additional information
+<!--Any additional information you think is important.-->

gtsam/3rdparty/Eigen/.hgtags

@@ -1,33 +0,0 @@
-2db9468678c6480c9633b6272ff0e3599d1e11a3 2.0-beta3
-375224817dce669b6fa31d920d4c895a63fabf32 2.0-beta1
-3b8120f077865e2a072e10f5be33e1d942b83a06 2.0-rc1
-19dfc0e7666bcee26f7a49eb42f39a0280a3485e 2.0-beta5
-7a7d8a9526f003ffa2430dfb0c2c535b5add3023 2.0-beta4
-7d14ad088ac23769c349518762704f0257f6a39b 2.0.1
-b9d48561579fd7d4c05b2aa42235dc9de6484bf2 2.0-beta6
-e17630a40408243cb1a51ad0fe3a99beb75b7450 before-hg-migration
-eda654d4cda2210ce80719addcf854773e6dec5a 2.0.0
-ee9a7c468a9e73fab12f38f02bac24b07f29ed71 2.0-beta2
-d49097c25d8049e730c254a2fed725a240ce4858 after-hg-migration
-655348878731bcb5d9bbe0854077b052e75e5237 actual-start-from-scratch
-12a658962d4e6dfdc9a1c350fe7b69e36e70675c 3.0-beta1
-5c4180ad827b3f869b13b1d82f5a6ce617d6fcee 3.0-beta2
-7ae24ca6f3891d5ac58ddc7db60ad413c8d6ec35 3.0-beta3
-c40708b9088d622567fecc9208ad4a426621d364 3.0-beta4
-b6456624eae74f49ae8683d8e7b2882a2ca0342a 3.0-rc1
-a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1
-304c88ca3affc16dd0b008b1104873986edd77af 3.1.0-alpha2
-920fc730b5930daae0a6dbe296d60ce2e3808215 3.1.0-beta1
-8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1
-bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
-da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
-a8e0d153fc5e239ef8b06e3665f1f9e8cb8d49c8 before-evaluators
-09a8e21866106b49c5dec1d6d543e5794e82efa0 3.3-alpha1
-ce5a455b34c0a0ac3545a1497cb4a16c38ed90e8 3.3-beta1
-69d418c0699907bcd0bf9e0b3ba0a112ed091d85 3.3-beta2
-bef509908b9da05d0d07ffc0da105e2c8c6d3996 3.3-rc1
-04ab5fa4b241754afcf631117572276444c67239 3.3-rc2
-26667be4f70baf4f0d39e96f330714c87b399090 3.3.0
-f562a193118d4f40514e2f4a0ace6e974926ef06 3.3.1
-da9b4e14c2550e0d11078a3c39e6d56eba9905df 3.3.2
-67e894c6cd8f5f1f604b27d37ed47fdf012674ff 3.3.3

gtsam/3rdparty/Eigen/CMakeLists.txt

@@ -1,6 +1,7 @@
-project(Eigen3)
+# cmake_minimum_require must be the first command of the file
+cmake_minimum_required(VERSION 3.5.0)
 
-cmake_minimum_required(VERSION 2.8.5)
+project(Eigen3)
 
 # guard against in-source builds
 
@@ -8,6 +9,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
   message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
 endif()
 
+
 # Alias Eigen_*_DIR to Eigen3_*_DIR:
 
 set(Eigen_SOURCE_DIR ${Eigen3_SOURCE_DIR})
@@ -19,16 +21,9 @@ if (NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE "Release")
 endif()
 
-string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
-if(    NOT cmake_build_type_tolower STREQUAL "debug"
-   AND NOT cmake_build_type_tolower STREQUAL "release"
-   AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
-  message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, RelWithDebInfo (case-insensitive).")
-endif()
-
 
 #############################################################################
-# retrieve version infomation                                               #
+# retrieve version information                                               #
 #############################################################################
 
 # automatically parse the version number
@@ -41,29 +36,28 @@ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_
 set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
 set(EIGEN_VERSION_NUMBER ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
 
-# if we are not in a mercurial clone
+# if we are not in a git clone
-if(IS_DIRECTORY ${CMAKE_SOURCE_DIR}/.hg)
+if(IS_DIRECTORY ${CMAKE_SOURCE_DIR}/.git)
-  # if the mercurial program is absent or this will leave the EIGEN_HG_CHANGESET string empty,
+  # if the git program is absent or this will leave the EIGEN_GIT_REVNUM string empty,
   # but won't stop CMake.
-  execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
+  execute_process(COMMAND git ls-remote --refs -q ${CMAKE_SOURCE_DIR} HEAD OUTPUT_VARIABLE EIGEN_GIT_OUTPUT)
-  execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
 endif()
 
-# if this is the default (aka development) branch, extract the mercurial changeset number from the hg tip output...
+# extract the git rev number from the git output...
-if(EIGEN_BRANCH_OUTPUT MATCHES "default")
+if(EIGEN_GIT_OUTPUT)
-string(REGEX MATCH "^changeset: *[0-9]*:([0-9;a-f]+).*" EIGEN_HG_CHANGESET_MATCH "${EIGEN_HGTIP_OUTPUT}")
+string(REGEX MATCH "^([0-9;a-f]+).*" EIGEN_GIT_CHANGESET_MATCH "${EIGEN_GIT_OUTPUT}")
-set(EIGEN_HG_CHANGESET "${CMAKE_MATCH_1}")
+set(EIGEN_GIT_REVNUM "${CMAKE_MATCH_1}")
-endif(EIGEN_BRANCH_OUTPUT MATCHES "default")
+endif()
 #...and show it next to the version number
-if(EIGEN_HG_CHANGESET)
+if(EIGEN_GIT_REVNUM)
-  set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER} (mercurial changeset ${EIGEN_HG_CHANGESET})")
+  set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER} (git rev ${EIGEN_GIT_REVNUM})")
-else(EIGEN_HG_CHANGESET)
+else()
   set(EIGEN_VERSION "${EIGEN_VERSION_NUMBER}")
-endif(EIGEN_HG_CHANGESET)
+endif()
-
 
 include(CheckCXXCompilerFlag)
 include(GNUInstallDirs)
+include(CMakeDependentOption)
 
 set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
@@ -78,7 +72,7 @@ macro(ei_add_cxx_compiler_flag FLAG)
   if(COMPILER_SUPPORT_${SFLAG})
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
   endif()
-endmacro(ei_add_cxx_compiler_flag)
+endmacro()
 
 check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
 
@@ -94,6 +88,9 @@ else()
   ei_add_cxx_compiler_flag("-std=c++03")
 endif()
 
+# Determine if we should build shared libraries on this platform.
+get_cmake_property(EIGEN_BUILD_SHARED_LIBS TARGET_SUPPORTS_SHARED_LIBS)
+
 #############################################################################
 # find how to link to the standard libraries                                #
 #############################################################################
@@ -134,7 +131,7 @@ if(NOT WIN32 OR NOT CMAKE_HOST_SYSTEM_NAME MATCHES Windows)
   option(EIGEN_BUILD_PKGCONFIG "Build pkg-config .pc file for Eigen" ON)
 endif()
 
-set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_INCLUDE_CURRENT_DIR OFF)
 
 option(EIGEN_SPLIT_LARGE_TESTS "Split large tests into smaller executables" ON)
 
@@ -174,11 +171,7 @@ if(NOT MSVC)
   ei_add_cxx_compiler_flag("-Wdouble-promotion")
 #  ei_add_cxx_compiler_flag("-Wconversion")
 
-  # -Wshadow is insanely too strict with gcc, hopefully it will become usable with gcc 6
-  # if(NOT CMAKE_COMPILER_IS_GNUCXX OR (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER "5.0.0"))
-  if(NOT CMAKE_COMPILER_IS_GNUCXX)
   ei_add_cxx_compiler_flag("-Wshadow")
-  endif()
 
   ei_add_cxx_compiler_flag("-Wno-psabi")
   ei_add_cxx_compiler_flag("-Wno-variadic-macros")
@@ -251,12 +244,30 @@ if(NOT MSVC)
     message(STATUS "Enabling FMA in tests/examples")
   endif()
 
+  option(EIGEN_TEST_AVX2 "Enable/Disable AVX2 in tests/examples" OFF)
+  if(EIGEN_TEST_AVX2)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx2 -mfma")
+    message(STATUS "Enabling AVX2 in tests/examples")
+  endif()
+
   option(EIGEN_TEST_AVX512 "Enable/Disable AVX512 in tests/examples" OFF)
   if(EIGEN_TEST_AVX512)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx512f -fabi-version=6 -DEIGEN_ENABLE_AVX512")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx512f -mfma")
+    if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fabi-version=6")
+    endif()
     message(STATUS "Enabling AVX512 in tests/examples")
   endif()
 
+  option(EIGEN_TEST_AVX512DQ "Enable/Disable AVX512DQ in tests/examples" OFF)
+  if(EIGEN_TEST_AVX512DQ)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx512dq")
+    if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fabi-version=6")
+    endif()
+    message(STATUS "Enabling AVX512DQ in tests/examples")
+  endif()
+
   option(EIGEN_TEST_F16C "Enable/Disable F16C in tests/examples" OFF)
   if(EIGEN_TEST_F16C)
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mf16c")
@@ -275,6 +286,12 @@ if(NOT MSVC)
     message(STATUS "Enabling VSX in tests/examples")
   endif()
 
+  option(EIGEN_TEST_MSA "Enable/Disable MSA in tests/examples" OFF)
+  if(EIGEN_TEST_MSA)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmsa")
+    message(STATUS "Enabling MSA in tests/examples")
+  endif()
+
   option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
   if(EIGEN_TEST_NEON)
     if(EIGEN_TEST_FMA)
@@ -292,12 +309,18 @@ if(NOT MSVC)
     message(STATUS "Enabling NEON in tests/examples")
   endif()
 
-  option(EIGEN_TEST_ZVECTOR "Enable/Disable S390X(zEC13) ZVECTOR in tests/examples" OFF)
+  option(EIGEN_TEST_Z13 "Enable/Disable S390X(zEC13) ZVECTOR in tests/examples" OFF)
-  if(EIGEN_TEST_ZVECTOR)
+  if(EIGEN_TEST_Z13)
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=z13 -mzvector")
     message(STATUS "Enabling S390X(zEC13) ZVECTOR in tests/examples")
   endif()
 
+  option(EIGEN_TEST_Z14 "Enable/Disable S390X(zEC14) ZVECTOR in tests/examples" OFF)
+  if(EIGEN_TEST_Z14)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=z14 -mzvector")
+    message(STATUS "Enabling S390X(zEC13) ZVECTOR in tests/examples")
+  endif()
+
   check_cxx_compiler_flag("-fopenmp" COMPILER_SUPPORT_OPENMP)
   if(COMPILER_SUPPORT_OPENMP)
     option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF)
@@ -307,7 +330,7 @@ if(NOT MSVC)
     endif()
   endif()
 
-else(NOT MSVC)
+else()
 
   # C4127 - conditional expression is constant
   # C4714 - marked as __forceinline not inlined (I failed to deactivate it selectively)
@@ -315,7 +338,7 @@ else(NOT MSVC)
   #         because we are oftentimes returning objects that have a destructor or may
   #         throw exceptions - in particular in the unit tests we are throwing extra many
   #         exceptions to cover indexing errors.
-  # C4505 - unreferenced local function has been removed (impossible to deactive selectively)
+  # C4505 - unreferenced local function has been removed (impossible to deactivate selectively)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /wd4127 /wd4505 /wd4714")
 
   # replace all /Wx by /W4
@@ -335,10 +358,23 @@ else(NOT MSVC)
     if(NOT CMAKE_CL_64)
       # arch is not supported on 64 bit systems, SSE is enabled automatically.
       set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:SSE2")
-    endif(NOT CMAKE_CL_64)
+    endif()
     message(STATUS "Enabling SSE2 in tests/examples")
-  endif(EIGEN_TEST_SSE2)
+  endif()
-endif(NOT MSVC)
+
+  option(EIGEN_TEST_AVX "Enable/Disable AVX in tests/examples" OFF)
+  if(EIGEN_TEST_AVX)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:AVX")
+    message(STATUS "Enabling AVX in tests/examples")
+  endif()
+
+  option(EIGEN_TEST_FMA "Enable/Disable FMA/AVX2 in tests/examples" OFF)
+  if(EIGEN_TEST_FMA AND NOT EIGEN_TEST_NEON)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:AVX2")
+    message(STATUS "Enabling FMA/AVX2 in tests/examples")
+  endif()
+
+endif()
 
 option(EIGEN_TEST_NO_EXPLICIT_VECTORIZATION "Disable explicit vectorization in tests/examples" OFF)
 option(EIGEN_TEST_X87 "Force using X87 instructions. Implies no vectorization." OFF)
@@ -382,7 +418,7 @@ endif()
 
 set(EIGEN_CUDA_COMPUTE_ARCH 30 CACHE STRING "The CUDA compute architecture level to target when compiling CUDA code")
 
-include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 
 # Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR
 if(EIGEN_INCLUDE_INSTALL_DIR)
@@ -391,22 +427,28 @@ endif()
 
 if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR)
   set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR}
-      CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed")
+      CACHE PATH "The directory relative to CMAKE_INSTALL_PREFIX where Eigen header files are installed")
 else()
   set(INCLUDE_INSTALL_DIR
       "${CMAKE_INSTALL_INCLUDEDIR}/eigen3"
-      CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed"
+      CACHE PATH "The directory relative to CMAKE_INSTALL_PREFIX where Eigen header files are installed"
       )
 endif()
 set(CMAKEPACKAGE_INSTALL_DIR
     "${CMAKE_INSTALL_DATADIR}/eigen3/cmake"
-    CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed"
+    CACHE PATH "The directory relative to CMAKE_INSTALL_PREFIX where Eigen3Config.cmake is installed"
     )
 set(PKGCONFIG_INSTALL_DIR
     "${CMAKE_INSTALL_DATADIR}/pkgconfig"
-    CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed"
+    CACHE PATH "The directory relative to CMAKE_INSTALL_PREFIX where eigen3.pc is installed"
     )
 
+foreach(var INCLUDE_INSTALL_DIR CMAKEPACKAGE_INSTALL_DIR PKGCONFIG_INSTALL_DIR)
+  # If an absolute path is specified, make it relative to "{CMAKE_INSTALL_PREFIX}".
+  if(IS_ABSOLUTE "${${var}}")
+    file(RELATIVE_PATH "${var}" "${CMAKE_INSTALL_PREFIX}" "${${var}}")
+  endif()
+endforeach()
 
 # similar to set_target_properties but append the property instead of overwriting it
 macro(ei_add_target_property target prop value)
@@ -415,9 +457,9 @@ macro(ei_add_target_property target prop value)
   # if the property wasn't previously set, ${previous} is now "previous-NOTFOUND" which cmake allows catching with plain if()
   if(NOT previous)
     set(previous "")
-  endif(NOT previous)
+  endif()
   set_target_properties(${target} PROPERTIES ${prop} "${previous} ${value}")
-endmacro(ei_add_target_property)
+endmacro()
 
 install(FILES
   signature_of_eigen3_matrix_library
@@ -431,9 +473,14 @@ if(EIGEN_BUILD_PKGCONFIG)
         )
 endif()
 
-add_subdirectory(Eigen)
+install(DIRECTORY Eigen DESTINATION ${INCLUDE_INSTALL_DIR} COMPONENT Devel)
 
+
+option(EIGEN_BUILD_DOC "Enable creation of Eigen documentation" ON)
+if(EIGEN_BUILD_DOC)
   add_subdirectory(doc EXCLUDE_FROM_ALL)
+endif()
+
 
 option(BUILD_TESTING "Enable creation of Eigen tests." ON)
 if(BUILD_TESTING)
@@ -444,6 +491,8 @@ if(BUILD_TESTING)
   else()
     add_subdirectory(test EXCLUDE_FROM_ALL)
   endif()
+
+  add_subdirectory(failtest)
 endif()
 
 if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
@@ -456,9 +505,32 @@ endif()
 
 # add SYCL
 option(EIGEN_TEST_SYCL "Add Sycl support." OFF)
+option(EIGEN_SYCL_TRISYCL "Use the triSYCL Sycl implementation (ComputeCPP by default)." OFF)
 if(EIGEN_TEST_SYCL)
   set (CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" "cmake/Modules/" "${CMAKE_MODULE_PATH}")
+  find_package(Threads REQUIRED)
+  if(EIGEN_SYCL_TRISYCL)
+    message(STATUS "Using triSYCL")
+    include(FindTriSYCL)
+  else()
+    message(STATUS "Using ComputeCPP SYCL")
     include(FindComputeCpp)
+    set(COMPUTECPP_DRIVER_DEFAULT_VALUE OFF)
+    if (NOT MSVC)
+      set(COMPUTECPP_DRIVER_DEFAULT_VALUE ON)
+    endif()
+    option(COMPUTECPP_USE_COMPILER_DRIVER
+      "Use ComputeCpp driver instead of a 2 steps compilation"
+      ${COMPUTECPP_DRIVER_DEFAULT_VALUE}
+    )
+  endif(EIGEN_SYCL_TRISYCL)
+  option(EIGEN_DONT_VECTORIZE_SYCL "Don't use vectorisation in the SYCL tests." OFF)
+  if(EIGEN_DONT_VECTORIZE_SYCL)
+    message(STATUS "Disabling SYCL vectorization in tests/examples")
+    # When disabling SYCL vectorization, also disable Eigen default vectorization
+    add_definitions(-DEIGEN_DONT_VECTORIZE=1)
+    add_definitions(-DEIGEN_DONT_VECTORIZE_SYCL=1)
+  endif()
 endif()
 
 add_subdirectory(unsupported)
@@ -471,11 +543,11 @@ add_subdirectory(scripts EXCLUDE_FROM_ALL)
 # TODO: consider also replacing EIGEN_BUILD_BTL by a custom target "make btl"?
 if(EIGEN_BUILD_BTL)
   add_subdirectory(bench/btl EXCLUDE_FROM_ALL)
-endif(EIGEN_BUILD_BTL)
+endif()
 
 if(NOT WIN32)
   add_subdirectory(bench/spbench EXCLUDE_FROM_ALL)
-endif(NOT WIN32)
+endif()
 
 configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
 
@@ -487,18 +559,16 @@ message(STATUS "")
 message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")
 message(STATUS "")
 
-option(EIGEN_FAILTEST "Enable failtests." OFF)
-if(EIGEN_FAILTEST)
-  add_subdirectory(failtest)
-endif()
-
 string(TOLOWER "${CMAKE_GENERATOR}" cmake_generator_tolower)
 if(cmake_generator_tolower MATCHES "makefile")
-  message(STATUS "Some things you can do now:")
+  message(STATUS "Available targets (use: make TARGET):")
-  message(STATUS "--------------+--------------------------------------------------------------")
+else()
-  message(STATUS "Command       |   Description")
+  message(STATUS "Available targets (use: cmake --build . --target TARGET):")
-  message(STATUS "--------------+--------------------------------------------------------------")
+endif()
-  message(STATUS "make install  | Install Eigen. Headers will be installed to:")
+message(STATUS "---------+--------------------------------------------------------------")
+message(STATUS "Target   |   Description")
+message(STATUS "---------+--------------------------------------------------------------")
+message(STATUS "install  | Install Eigen. Headers will be installed to:")
 message(STATUS "         |     <CMAKE_INSTALL_PREFIX>/<INCLUDE_INSTALL_DIR>")
 message(STATUS "         |   Using the following values:")
 message(STATUS "         |     CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
@@ -507,17 +577,14 @@ if(cmake_generator_tolower MATCHES "makefile")
 message(STATUS "         |     cmake . -DCMAKE_INSTALL_PREFIX=yourprefix")
 message(STATUS "         |   Or:")
 message(STATUS "         |     cmake . -DINCLUDE_INSTALL_DIR=yourdir")
-  message(STATUS "make doc      | Generate the API documentation, requires Doxygen & LaTeX")
+message(STATUS "doc      | Generate the API documentation, requires Doxygen & LaTeX")
-  message(STATUS "make check    | Build and run the unit-tests. Read this page:")
+if(BUILD_TESTING)
+  message(STATUS "check    | Build and run the unit-tests. Read this page:")
   message(STATUS "         |   http://eigen.tuxfamily.org/index.php?title=Tests")
-  message(STATUS "make blas     | Build BLAS library (not the same thing as Eigen)")
-  message(STATUS "make uninstall| Removes files installed by make install")
-  message(STATUS "--------------+--------------------------------------------------------------")
-else()
-  message(STATUS "To build/run the unit tests, read this page:")
-  message(STATUS "  http://eigen.tuxfamily.org/index.php?title=Tests")
 endif()
-
+message(STATUS "blas     | Build BLAS library (not the same thing as Eigen)")
+message(STATUS "uninstall| Remove files installed by the install target")
+message(STATUS "---------+--------------------------------------------------------------")
 message(STATUS "")
 
 
@@ -529,13 +596,11 @@ set ( EIGEN_DEFINITIONS "")
 set ( EIGEN_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}" )
 set ( EIGEN_ROOT_DIR ${CMAKE_INSTALL_PREFIX} )
 
-# Interface libraries require at least CMake 3.0
-if (NOT CMAKE_VERSION VERSION_LESS 3.0)
 include (CMakePackageConfigHelpers)
 
 # Imported target support
 add_library (eigen INTERFACE)
-
+add_library (Eigen3::Eigen ALIAS eigen)
 target_compile_definitions (eigen INTERFACE ${EIGEN_DEFINITIONS})
 target_include_directories (eigen INTERFACE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
@@ -574,38 +639,6 @@ if (NOT CMAKE_VERSION VERSION_LESS 3.0)
 
 install (EXPORT Eigen3Targets NAMESPACE Eigen3:: DESTINATION ${CMAKEPACKAGE_INSTALL_DIR})
 
-else (NOT CMAKE_VERSION VERSION_LESS 3.0)
-  # Fallback to legacy Eigen3Config.cmake without the imported target
-  
-  # If CMakePackageConfigHelpers module is available (CMake >= 2.8.8)
-  # create a relocatable Config file, otherwise leave the hardcoded paths       
-  include(CMakePackageConfigHelpers OPTIONAL RESULT_VARIABLE CPCH_PATH)
-  
-  if(CPCH_PATH)
-    configure_package_config_file (
-      ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3ConfigLegacy.cmake.in
-      ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
-      PATH_VARS EIGEN_INCLUDE_DIR EIGEN_ROOT_DIR
-      INSTALL_DESTINATION ${CMAKEPACKAGE_INSTALL_DIR}
-      NO_CHECK_REQUIRED_COMPONENTS_MACRO # Eigen does not provide components
-    )
-  else() 
-    # The PACKAGE_* variables are defined by the configure_package_config_file
-    # but without it we define them manually to the hardcoded paths
-    set(PACKAGE_INIT "")
-    set(PACKAGE_EIGEN_INCLUDE_DIR ${EIGEN_INCLUDE_DIR})
-    set(PACKAGE_EIGEN_ROOT_DIR ${EIGEN_ROOT_DIR})
-    configure_file ( ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3ConfigLegacy.cmake.in
-                     ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
-                     @ONLY ESCAPE_QUOTES )
-  endif()
-
-  write_basic_package_version_file( Eigen3ConfigVersion.cmake
-                                    VERSION ${EIGEN_VERSION_NUMBER}
-                                    COMPATIBILITY SameMajorVersion )
-
-endif (NOT CMAKE_VERSION VERSION_LESS 3.0)
-
 install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/cmake/UseEigen3.cmake
                 ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
                 ${CMAKE_CURRENT_BINARY_DIR}/Eigen3ConfigVersion.cmake
@@ -614,3 +647,7 @@ install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/cmake/UseEigen3.cmake
 # Add uninstall target
 add_custom_target ( uninstall
     COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/EigenUninstall.cmake)
+
+if (EIGEN_SPLIT_TESTSUITE)
+  ei_split_testsuite("${EIGEN_SPLIT_TESTSUITE}")
+endif()

gtsam/3rdparty/Eigen/COPYING.APACHE

@@ -0,0 +1,203 @@
+/*
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+*/

gtsam/3rdparty/Eigen/COPYING.MINPACK

@@ -49,4 +49,3 @@ SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT
 (INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE,
 EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE
 POSSIBILITY OF SUCH LOSS OR DAMAGES.
-

gtsam/3rdparty/Eigen/CTestConfig.cmake

@@ -2,12 +2,16 @@
 ## Then modify the CMakeLists.txt file in the root directory of your
 ## project to incorporate the testing dashboard.
 ## # The following are required to uses Dart and the Cdash dashboard
-##   ENABLE_TESTING()
+##   enable_testing()
-##   INCLUDE(CTest)
+##   include(CTest)
-set(CTEST_PROJECT_NAME "Eigen 3.3")
+set(CTEST_PROJECT_NAME "Eigen")
 set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
 
 set(CTEST_DROP_METHOD "http")
-set(CTEST_DROP_SITE "manao.inria.fr")
+set(CTEST_DROP_SITE "my.cdash.org")
-set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen+3.3")
+set(CTEST_DROP_LOCATION "/submit.php?project=Eigen")
 set(CTEST_DROP_SITE_CDASH TRUE)
+#set(CTEST_PROJECT_SUBPROJECTS
+#Official
+#Unsupported
+#)

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

@@ -1,19 +0,0 @@
-include(RegexUtils)
-test_escape_string_as_regex()
-
-file(GLOB Eigen_directory_files "*")
-
-escape_string_as_regex(ESCAPED_CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
-
-foreach(f ${Eigen_directory_files})
-  if(NOT f MATCHES "\\.txt" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/[.].+" AND NOT f MATCHES "${ESCAPED_CMAKE_CURRENT_SOURCE_DIR}/src")
-    list(APPEND Eigen_directory_files_to_install ${f})
-  endif()
-endforeach(f ${Eigen_directory_files})
-
-install(FILES
-  ${Eigen_directory_files_to_install}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel
-  )
-
-install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h")

gtsam/3rdparty/Eigen/Eigen/Cholesky

@@ -43,4 +43,3 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_CHOLESKY_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/Core

@@ -11,251 +11,55 @@
 #ifndef EIGEN_CORE_H
 #define EIGEN_CORE_H
 
-// first thing Eigen does: stop the compiler from committing suicide
+// first thing Eigen does: stop the compiler from reporting useless warnings.
 #include "src/Core/util/DisableStupidWarnings.h"
 
-#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
+// then include this file where all our macros are defined. It's really important to do it first because
-  #define EIGEN_CUDACC __CUDACC__
+// it's where we do all the compiler/OS/arch detections and define most defaults.
-#endif
+#include "src/Core/util/Macros.h"
 
-#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
+// This detects SSE/AVX/NEON/etc. and configure alignment settings
-  #define EIGEN_CUDA_ARCH __CUDA_ARCH__
+#include "src/Core/util/ConfigureVectorization.h"
-#endif
 
-#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
+// We need cuda_runtime.h/hip_runtime.h to ensure that
-#define EIGEN_CUDACC_VER  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
+// the EIGEN_USING_STD macro works properly on the device side
-#elif defined(__CUDACC_VER__)
+#if defined(EIGEN_CUDACC)
-#define EIGEN_CUDACC_VER __CUDACC_VER__
-#else
-#define EIGEN_CUDACC_VER 0
-#endif
-
-// Handle NVCC/CUDA/SYCL
-#if defined(__CUDACC__) || defined(__SYCL_DEVICE_ONLY__)
-  // Do not try asserts on CUDA and SYCL!
-  #ifndef EIGEN_NO_DEBUG
-  #define EIGEN_NO_DEBUG
-  #endif
-
-  #ifdef EIGEN_INTERNAL_DEBUGGING
-  #undef EIGEN_INTERNAL_DEBUGGING
-  #endif
-
-  #ifdef EIGEN_EXCEPTIONS
-  #undef EIGEN_EXCEPTIONS
-  #endif
-
-  // All functions callable from CUDA code must be qualified with __device__
-  #ifdef __CUDACC__
-    // Do not try to vectorize on CUDA and SYCL!
-    #ifndef EIGEN_DONT_VECTORIZE
-    #define EIGEN_DONT_VECTORIZE
-    #endif
-
-    #define EIGEN_DEVICE_FUNC __host__ __device__
-    // We need cuda_runtime.h to ensure that that EIGEN_USING_STD_MATH macro
-    // works properly on the device side
   #include <cuda_runtime.h>
-  #else
+#elif defined(EIGEN_HIPCC)
-    #define EIGEN_DEVICE_FUNC
+  #include <hip/hip_runtime.h>
 #endif
 
-#else
-  #define EIGEN_DEVICE_FUNC
-
-#endif
-
-// When compiling CUDA device code with NVCC, pull in math functions from the
-// global namespace.  In host mode, and when device doee with clang, use the
-// std versions.
-#if defined(__CUDA_ARCH__) && defined(__NVCC__)
-  #define EIGEN_USING_STD_MATH(FUNC) using ::FUNC;
-#else
-  #define EIGEN_USING_STD_MATH(FUNC) using std::FUNC;
-#endif
-
-#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(__CUDA_ARCH__) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL)
-  #define EIGEN_EXCEPTIONS
-#endif
 
 #ifdef EIGEN_EXCEPTIONS
   #include <new>
 #endif
 
-// then include this file where all our macros are defined. It's really important to do it first because
-// it's where we do all the alignment settings (platform detection and honoring the user's will if he
-// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization.
-#include "src/Core/util/Macros.h"
-
 // Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
 // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
-#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6)
+#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6) && EIGEN_GNUC_AT_MOST(5,5)
   #pragma GCC optimize ("-fno-ipa-cp-clone")
 #endif
 
+// Prevent ICC from specializing std::complex operators that silently fail
+// on device. This allows us to use our own device-compatible specializations
+// instead.
+#if defined(EIGEN_COMP_ICC) && defined(EIGEN_GPU_COMPILE_PHASE) \
+    && !defined(_OVERRIDE_COMPLEX_SPECIALIZATION_)
+#define _OVERRIDE_COMPLEX_SPECIALIZATION_ 1
+#endif
 #include <complex>
 
 // this include file manages BLAS and MKL related macros
 // and inclusion of their respective header files
 #include "src/Core/util/MKL_support.h"
 
-// if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into
+
-// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks
+#if defined(EIGEN_HAS_CUDA_FP16) || defined(EIGEN_HAS_HIP_FP16)
-#if EIGEN_MAX_ALIGN_BYTES==0
+  #define EIGEN_HAS_GPU_FP16
-  #ifndef EIGEN_DONT_VECTORIZE
-    #define EIGEN_DONT_VECTORIZE
-  #endif
 #endif
 
-#if EIGEN_COMP_MSVC
+#if defined(EIGEN_HAS_CUDA_BF16) || defined(EIGEN_HAS_HIP_BF16)
-  #include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
+  #define EIGEN_HAS_GPU_BF16
-  #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later
-    // Remember that usage of defined() in a #define is undefined by the standard.
-    // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
-    #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
-      #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
-    #endif
-  #endif
-#else
-  // Remember that usage of defined() in a #define is undefined by the standard
-  #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) )
-    #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
-  #endif
-#endif
-
-#ifndef EIGEN_DONT_VECTORIZE
-
-  #if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
-
-    // Defines symbols for compile-time detection of which instructions are
-    // used.
-    // EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_SSE
-    #define EIGEN_VECTORIZE_SSE2
-
-    // Detect sse3/ssse3/sse4:
-    // gcc and icc defines __SSE3__, ...
-    // there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
-    // want to force the use of those instructions with msvc.
-    #ifdef __SSE3__
-      #define EIGEN_VECTORIZE_SSE3
-    #endif
-    #ifdef __SSSE3__
-      #define EIGEN_VECTORIZE_SSSE3
-    #endif
-    #ifdef __SSE4_1__
-      #define EIGEN_VECTORIZE_SSE4_1
-    #endif
-    #ifdef __SSE4_2__
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #ifdef __AVX__
-      #define EIGEN_VECTORIZE_AVX
-      #define EIGEN_VECTORIZE_SSE3
-      #define EIGEN_VECTORIZE_SSSE3
-      #define EIGEN_VECTORIZE_SSE4_1
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #ifdef __AVX2__
-      #define EIGEN_VECTORIZE_AVX2
-    #endif
-    #ifdef __FMA__
-      #define EIGEN_VECTORIZE_FMA
-    #endif
-    #if defined(__AVX512F__) && defined(EIGEN_ENABLE_AVX512)
-      #define EIGEN_VECTORIZE_AVX512
-      #define EIGEN_VECTORIZE_AVX2
-      #define EIGEN_VECTORIZE_AVX
-      #define EIGEN_VECTORIZE_FMA
-      #ifdef __AVX512DQ__
-        #define EIGEN_VECTORIZE_AVX512DQ
-      #endif
-      #ifdef __AVX512ER__
-        #define EIGEN_VECTORIZE_AVX512ER
-      #endif
-    #endif
-
-    // include files
-
-    // This extern "C" works around a MINGW-w64 compilation issue
-    // https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
-    // In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
-    // However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
-    // with conflicting linkage.  The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
-    // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
-    // notice that since these are C headers, the extern "C" is theoretically needed anyways.
-    extern "C" {
-      // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
-      // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
-      #if EIGEN_COMP_ICC >= 1110
-        #include <immintrin.h>
-      #else
-        #include <mmintrin.h>
-        #include <emmintrin.h>
-        #include <xmmintrin.h>
-        #ifdef  EIGEN_VECTORIZE_SSE3
-        #include <pmmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSSE3
-        #include <tmmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSE4_1
-        #include <smmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSE4_2
-        #include <nmmintrin.h>
-        #endif
-        #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
-        #include <immintrin.h>
-        #endif
-      #endif
-    } // end extern "C"
-  #elif defined __VSX__
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_VSX
-    #include <altivec.h>
-    // We need to #undef all these ugly tokens defined in <altivec.h>
-    // => use __vector instead of vector
-    #undef bool
-    #undef vector
-    #undef pixel
-  #elif defined __ALTIVEC__
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_ALTIVEC
-    #include <altivec.h>
-    // We need to #undef all these ugly tokens defined in <altivec.h>
-    // => use __vector instead of vector
-    #undef bool
-    #undef vector
-    #undef pixel
-  #elif (defined  __ARM_NEON) || (defined __ARM_NEON__)
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_NEON
-    #include <arm_neon.h>
-  #elif (defined __s390x__ && defined __VEC__)
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_ZVECTOR
-    #include <vecintrin.h>
-  #endif
-#endif
-
-#if defined(__F16C__) && !defined(EIGEN_COMP_CLANG)
-  // We can use the optimized fp16 to float and float to fp16 conversion routines
-  #define EIGEN_HAS_FP16_C
-#endif
-
-#if defined __CUDACC__
-  #define EIGEN_VECTORIZE_CUDA
-  #include <vector_types.h>
-  #if EIGEN_CUDACC_VER >= 70500
-    #define EIGEN_HAS_CUDA_FP16
-  #endif
-#endif
-
-#if defined EIGEN_HAS_CUDA_FP16
-  #include <host_defines.h>
-  #include <cuda_fp16.h>
 #endif
 
 #if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
@@ -279,7 +83,10 @@
 #include <cmath>
 #include <cassert>
 #include <functional>
+#include <sstream>
+#ifndef EIGEN_NO_IO
   #include <iosfwd>
+#endif
 #include <cstring>
 #include <string>
 #include <limits>
@@ -287,6 +94,10 @@
 // for min/max:
 #include <algorithm>
 
+#if EIGEN_HAS_CXX11
+#include <array>
+#endif
+
 // for std::is_nothrow_move_assignable
 #ifdef EIGEN_INCLUDE_TYPE_TRAITS
 #include <type_traits>
@@ -302,38 +113,25 @@
   #include <intrin.h>
 #endif
 
-/** \brief Namespace containing all symbols from the %Eigen library. */
+#if defined(EIGEN_USE_SYCL)
-namespace Eigen {
+  #undef min
-
+  #undef max
-inline static const char *SimdInstructionSetsInUse(void) {
+  #undef isnan
-#if defined(EIGEN_VECTORIZE_AVX512)
+  #undef isinf
-  return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+  #undef isfinite
-#elif defined(EIGEN_VECTORIZE_AVX)
+  #include <CL/sycl.hpp>
-  return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+  #include <map>
-#elif defined(EIGEN_VECTORIZE_SSE4_2)
+  #include <memory>
-  return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+  #include <utility>
-#elif defined(EIGEN_VECTORIZE_SSE4_1)
+  #include <thread>
-  return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
+  #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM0
-#elif defined(EIGEN_VECTORIZE_SSSE3)
+  #define EIGEN_SYCL_LOCAL_THREAD_DIM0 16
-  return "SSE, SSE2, SSE3, SSSE3";
+  #endif
-#elif defined(EIGEN_VECTORIZE_SSE3)
+  #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM1
-  return "SSE, SSE2, SSE3";
+  #define EIGEN_SYCL_LOCAL_THREAD_DIM1 16
-#elif defined(EIGEN_VECTORIZE_SSE2)
+  #endif
-  return "SSE, SSE2";
-#elif defined(EIGEN_VECTORIZE_ALTIVEC)
-  return "AltiVec";
-#elif defined(EIGEN_VECTORIZE_VSX)
-  return "VSX";
-#elif defined(EIGEN_VECTORIZE_NEON)
-  return "ARM NEON";
-#elif defined(EIGEN_VECTORIZE_ZVECTOR)
-  return "S390X ZVECTOR";
-#else
-  return "None";
 #endif
-}
 
-} // end namespace Eigen
 
 #if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || defined EIGEN2_SUPPORT
 // This will generate an error message:
@@ -366,58 +164,90 @@ using std::ptrdiff_t;
 #include "src/Core/util/StaticAssert.h"
 #include "src/Core/util/XprHelper.h"
 #include "src/Core/util/Memory.h"
+#include "src/Core/util/IntegralConstant.h"
+#include "src/Core/util/SymbolicIndex.h"
 
 #include "src/Core/NumTraits.h"
 #include "src/Core/MathFunctions.h"
 #include "src/Core/GenericPacketMath.h"
 #include "src/Core/MathFunctionsImpl.h"
 #include "src/Core/arch/Default/ConjHelper.h"
+// Generic half float support
+#include "src/Core/arch/Default/Half.h"
+#include "src/Core/arch/Default/BFloat16.h"
+#include "src/Core/arch/Default/TypeCasting.h"
+#include "src/Core/arch/Default/GenericPacketMathFunctionsFwd.h"
 
 #if defined EIGEN_VECTORIZE_AVX512
   #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/SSE/TypeCasting.h"
+  #include "src/Core/arch/SSE/Complex.h"
   #include "src/Core/arch/AVX/PacketMath.h"
+  #include "src/Core/arch/AVX/TypeCasting.h"
+  #include "src/Core/arch/AVX/Complex.h"
   #include "src/Core/arch/AVX512/PacketMath.h"
+  #include "src/Core/arch/AVX512/TypeCasting.h"
+  #include "src/Core/arch/AVX512/Complex.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/AVX/MathFunctions.h"
   #include "src/Core/arch/AVX512/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_AVX
   // Use AVX for floats and doubles, SSE for integers
   #include "src/Core/arch/SSE/PacketMath.h"
-  #include "src/Core/arch/SSE/Complex.h"
-  #include "src/Core/arch/SSE/MathFunctions.h"
-  #include "src/Core/arch/AVX/PacketMath.h"
-  #include "src/Core/arch/AVX/MathFunctions.h"
-  #include "src/Core/arch/AVX/Complex.h"
-  #include "src/Core/arch/AVX/TypeCasting.h"
   #include "src/Core/arch/SSE/TypeCasting.h"
+  #include "src/Core/arch/SSE/Complex.h"
+  #include "src/Core/arch/AVX/PacketMath.h"
+  #include "src/Core/arch/AVX/TypeCasting.h"
+  #include "src/Core/arch/AVX/Complex.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/AVX/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_SSE
   #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/SSE/TypeCasting.h"
   #include "src/Core/arch/SSE/MathFunctions.h"
   #include "src/Core/arch/SSE/Complex.h"
-  #include "src/Core/arch/SSE/TypeCasting.h"
 #elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
   #include "src/Core/arch/AltiVec/PacketMath.h"
   #include "src/Core/arch/AltiVec/MathFunctions.h"
   #include "src/Core/arch/AltiVec/Complex.h"
 #elif defined EIGEN_VECTORIZE_NEON
   #include "src/Core/arch/NEON/PacketMath.h"
+  #include "src/Core/arch/NEON/TypeCasting.h"
   #include "src/Core/arch/NEON/MathFunctions.h"
   #include "src/Core/arch/NEON/Complex.h"
+#elif defined EIGEN_VECTORIZE_SVE
+  #include "src/Core/arch/SVE/PacketMath.h"
+  #include "src/Core/arch/SVE/TypeCasting.h"
+  #include "src/Core/arch/SVE/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_ZVECTOR
   #include "src/Core/arch/ZVector/PacketMath.h"
   #include "src/Core/arch/ZVector/MathFunctions.h"
   #include "src/Core/arch/ZVector/Complex.h"
+#elif defined EIGEN_VECTORIZE_MSA
+  #include "src/Core/arch/MSA/PacketMath.h"
+  #include "src/Core/arch/MSA/MathFunctions.h"
+  #include "src/Core/arch/MSA/Complex.h"
 #endif
 
-// Half float support
+#if defined EIGEN_VECTORIZE_GPU
-#include "src/Core/arch/CUDA/Half.h"
+  #include "src/Core/arch/GPU/PacketMath.h"
-#include "src/Core/arch/CUDA/PacketMathHalf.h"
+  #include "src/Core/arch/GPU/MathFunctions.h"
-#include "src/Core/arch/CUDA/TypeCasting.h"
+  #include "src/Core/arch/GPU/TypeCasting.h"
+#endif
 
-#if defined EIGEN_VECTORIZE_CUDA
+#if defined(EIGEN_USE_SYCL)
-  #include "src/Core/arch/CUDA/PacketMath.h"
+  #include "src/Core/arch/SYCL/SyclMemoryModel.h"
-  #include "src/Core/arch/CUDA/MathFunctions.h"
+  #include "src/Core/arch/SYCL/InteropHeaders.h"
+#if !defined(EIGEN_DONT_VECTORIZE_SYCL)
+  #include "src/Core/arch/SYCL/PacketMath.h"
+  #include "src/Core/arch/SYCL/MathFunctions.h"
+  #include "src/Core/arch/SYCL/TypeCasting.h"
+#endif
 #endif
 
 #include "src/Core/arch/Default/Settings.h"
+// This file provides generic implementations valid for scalar as well
+#include "src/Core/arch/Default/GenericPacketMathFunctions.h"
 
 #include "src/Core/functors/TernaryFunctors.h"
 #include "src/Core/functors/BinaryFunctors.h"
@@ -428,9 +258,16 @@ using std::ptrdiff_t;
 
 // Specialized functors to enable the processing of complex numbers
 // on CUDA devices
+#ifdef EIGEN_CUDACC
 #include "src/Core/arch/CUDA/Complex.h"
+#endif
 
+#include "src/Core/util/IndexedViewHelper.h"
+#include "src/Core/util/ReshapedHelper.h"
+#include "src/Core/ArithmeticSequence.h"
+#ifndef EIGEN_NO_IO
   #include "src/Core/IO.h"
+#endif
 #include "src/Core/DenseCoeffsBase.h"
 #include "src/Core/DenseBase.h"
 #include "src/Core/MatrixBase.h"
@@ -471,6 +308,8 @@ using std::ptrdiff_t;
 #include "src/Core/Ref.h"
 #include "src/Core/Block.h"
 #include "src/Core/VectorBlock.h"
+#include "src/Core/IndexedView.h"
+#include "src/Core/Reshaped.h"
 #include "src/Core/Transpose.h"
 #include "src/Core/DiagonalMatrix.h"
 #include "src/Core/Diagonal.h"
@@ -507,13 +346,21 @@ using std::ptrdiff_t;
 #include "src/Core/CoreIterators.h"
 #include "src/Core/ConditionEstimator.h"
 
+#if defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
+  #include "src/Core/arch/AltiVec/MatrixProduct.h"
+#elif defined EIGEN_VECTORIZE_NEON
+  #include "src/Core/arch/NEON/GeneralBlockPanelKernel.h"
+#endif
+
 #include "src/Core/BooleanRedux.h"
 #include "src/Core/Select.h"
 #include "src/Core/VectorwiseOp.h"
+#include "src/Core/PartialReduxEvaluator.h"
 #include "src/Core/Random.h"
 #include "src/Core/Replicate.h"
 #include "src/Core/Reverse.h"
 #include "src/Core/ArrayWrapper.h"
+#include "src/Core/StlIterators.h"
 
 #ifdef EIGEN_USE_BLAS
 #include "src/Core/products/GeneralMatrixMatrix_BLAS.h"

gtsam/3rdparty/Eigen/Eigen/Eigenvalues

@@ -10,14 +10,14 @@
 
 #include "Core"
 
-#include "src/Core/util/DisableStupidWarnings.h"
-
 #include "Cholesky"
 #include "Jacobi"
 #include "Householder"
 #include "LU"
 #include "Geometry"
 
+#include "src/Core/util/DisableStupidWarnings.h"
+
 /** \defgroup Eigenvalues_Module Eigenvalues module
   *
   *
@@ -58,4 +58,3 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_EIGENVALUES_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/Geometry

@@ -10,12 +10,12 @@
 
 #include "Core"
 
-#include "src/Core/util/DisableStupidWarnings.h"
-
 #include "SVD"
 #include "LU"
 #include <limits>
 
+#include "src/Core/util/DisableStupidWarnings.h"
+
 /** \defgroup Geometry_Module Geometry module
   *
   * This module provides support for:
@@ -49,14 +49,11 @@
 #include "src/Geometry/AlignedBox.h"
 #include "src/Geometry/Umeyama.h"
 
-// Use the SSE optimized version whenever possible. At the moment the
+// Use the SSE optimized version whenever possible.
-// SSE version doesn't compile when AVX is enabled
+#if (defined EIGEN_VECTORIZE_SSE) || (defined EIGEN_VECTORIZE_NEON)
-#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
+#include "src/Geometry/arch/Geometry_SIMD.h"
-#include "src/Geometry/arch/Geometry_SSE.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_GEOMETRY_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
-

gtsam/3rdparty/Eigen/Eigen/Householder

@@ -27,4 +27,3 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_HOUSEHOLDER_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/Jacobi

@@ -29,5 +29,4 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_JACOBI_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
 

gtsam/3rdparty/Eigen/Eigen/KLUSupport

@@ -0,0 +1,41 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_KLUSUPPORT_MODULE_H
+#define EIGEN_KLUSUPPORT_MODULE_H
+
+#include <Eigen/SparseCore>
+
+#include <Eigen/src/Core/util/DisableStupidWarnings.h>
+
+extern "C" {
+#include <btf.h>
+#include <klu.h>
+   }
+
+/** \ingroup Support_modules
+  * \defgroup KLUSupport_Module KLUSupport module
+  *
+  * This module provides an interface to the KLU library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
+  * It provides the following factorization class:
+  * - class KLU: a sparse LU factorization, well-suited for circuit simulation.
+  *
+  * \code
+  * #include <Eigen/KLUSupport>
+  * \endcode
+  *
+  * In order to use this module, the klu and btf headers must be accessible from the include paths, and your binary must be linked to the klu library and its dependencies.
+  * The dependencies depend on how umfpack has been compiled.
+  * For a cmake based project, you can use our FindKLU.cmake module to help you in this task.
+  *
+  */
+
+#include "src/KLUSupport/KLUSupport.h"
+
+#include <Eigen/src/Core/util/ReenableStupidWarnings.h>
+
+#endif // EIGEN_KLUSUPPORT_MODULE_H

gtsam/3rdparty/Eigen/Eigen/LU

@@ -38,13 +38,10 @@
 #include "src/LU/Determinant.h"
 #include "src/LU/InverseImpl.h"
 
-// Use the SSE optimized version whenever possible. At the moment the
+#if defined EIGEN_VECTORIZE_SSE || defined EIGEN_VECTORIZE_NEON
-// SSE version doesn't compile when AVX is enabled
+  #include "src/LU/arch/InverseSize4.h"
-#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
-  #include "src/LU/arch/Inverse_SSE.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_LU_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/OrderingMethods

@@ -63,10 +63,7 @@
   * \endcode
   */
 
-#ifndef EIGEN_MPL2_ONLY
 #include "src/OrderingMethods/Amd.h"
-#endif
-
 #include "src/OrderingMethods/Ordering.h"
 #include "src/Core/util/ReenableStupidWarnings.h"
 

gtsam/3rdparty/Eigen/Eigen/PaStiXSupport

@@ -36,6 +36,7 @@ extern "C" {
   * \endcode
   *
   * In order to use this module, the PaSTiX headers must be accessible from the include paths, and your binary must be linked to the PaSTiX library and its dependencies.
+  * This wrapper resuires PaStiX version 5.x compiled without MPI support.
   * The dependencies depend on how PaSTiX has been compiled.
   * For a cmake based project, you can use our FindPaSTiX.cmake module to help you in this task.
   *

gtsam/3rdparty/Eigen/Eigen/QR

@@ -10,12 +10,12 @@
 
 #include "Core"
 
-#include "src/Core/util/DisableStupidWarnings.h"
-
 #include "Cholesky"
 #include "Jacobi"
 #include "Householder"
 
+#include "src/Core/util/DisableStupidWarnings.h"
+
 /** \defgroup QR_Module QR module
   *
   *
@@ -48,4 +48,3 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_QR_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc

@@ -37,4 +37,3 @@ void *qRealloc(void *ptr, std::size_t size)
 #endif
 
 #endif // EIGEN_QTMALLOC_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/SVD

@@ -48,4 +48,3 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_SVD_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */

gtsam/3rdparty/Eigen/Eigen/Sparse

@@ -25,9 +25,7 @@
 
 #include "SparseCore"
 #include "OrderingMethods"
-#ifndef EIGEN_MPL2_ONLY
 #include "SparseCholesky"
-#endif
 #include "SparseLU"
 #include "SparseQR"
 #include "IterativeLinearSolvers"

gtsam/3rdparty/Eigen/Eigen/SparseCholesky

@@ -30,16 +30,8 @@
   * \endcode
   */
 
-#ifdef EIGEN_MPL2_ONLY
-#error The SparseCholesky module has nothing to offer in MPL2 only mode
-#endif
-
 #include "src/SparseCholesky/SimplicialCholesky.h"
-
-#ifndef EIGEN_MPL2_ONLY
 #include "src/SparseCholesky/SimplicialCholesky_impl.h"
-#endif
-
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_SPARSECHOLESKY_MODULE_H

gtsam/3rdparty/Eigen/Eigen/SparseLU

@@ -23,6 +23,8 @@
 // Ordering interface
 #include "OrderingMethods"
 
+#include "src/Core/util/DisableStupidWarnings.h"
+
 #include "src/SparseLU/SparseLU_gemm_kernel.h"
 
 #include "src/SparseLU/SparseLU_Structs.h"
@@ -43,4 +45,6 @@
 #include "src/SparseLU/SparseLU_Utils.h"
 #include "src/SparseLU/SparseLU.h"
 
+#include "src/Core/util/ReenableStupidWarnings.h"
+
 #endif // EIGEN_SPARSELU_MODULE_H

gtsam/3rdparty/Eigen/Eigen/SparseQR

@@ -28,7 +28,6 @@
   * 
   */
 
-#include "OrderingMethods"
 #include "src/SparseCore/SparseColEtree.h"
 #include "src/SparseQR/SparseQR.h"
 

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

@@ -16,6 +16,15 @@
 namespace Eigen {
 
 namespace internal {
+  template<typename _MatrixType, int _UpLo> struct traits<LDLT<_MatrixType, _UpLo> >
+   : traits<_MatrixType>
+  {
+    typedef MatrixXpr XprKind;
+    typedef SolverStorage StorageKind;
+    typedef int StorageIndex;
+    enum { Flags = 0 };
+  };
+
   template<typename MatrixType, int UpLo> struct LDLT_Traits;
 
   // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
@@ -36,7 +45,7 @@ namespace internal {
   * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
   * is lower triangular with a unit diagonal and D is a diagonal matrix.
   *
-  * The decomposition uses pivoting to ensure stability, so that L will have
+  * The decomposition uses pivoting to ensure stability, so that D will have
   * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
   * on D also stabilizes the computation.
   *
@@ -48,20 +57,19 @@ namespace internal {
   * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT
   */
 template<typename _MatrixType, int _UpLo> class LDLT
+        : public SolverBase<LDLT<_MatrixType, _UpLo> >
 {
   public:
     typedef _MatrixType MatrixType;
+    typedef SolverBase<LDLT> Base;
+    friend class SolverBase<LDLT>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(LDLT)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
       UpLo = _UpLo
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    typedef typename MatrixType::StorageIndex StorageIndex;
     typedef Matrix<Scalar, RowsAtCompileTime, 1, 0, MaxRowsAtCompileTime, 1> TmpMatrixType;
 
     typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
@@ -180,6 +188,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
       return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
       *
       * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
@@ -191,19 +200,14 @@ template<typename _MatrixType, int _UpLo> class LDLT
       * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
       * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
       * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
-      * computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular.
+      * computes the least-square solution of \f$ A x = b \f$ if \f$ A \f$ is singular.
       *
       * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt()
       */
     template<typename Rhs>
     inline const Solve<LDLT, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
+    solve(const MatrixBase<Rhs>& b) const;
-    {
+    #endif
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      eigen_assert(m_matrix.rows()==b.rows()
-                && "LDLT::solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<LDLT, Rhs>(*this, b.derived());
-    }
 
     template<typename Derived>
     bool solveInPlace(MatrixBase<Derived> &bAndX) const;
@@ -242,12 +246,12 @@ template<typename _MatrixType, int _UpLo> class LDLT
       */
     const LDLT& adjoint() const { return *this; };
 
-    inline Index rows() const { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    inline Index cols() const { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful,
+      * \returns \c Success if computation was successful,
       *          \c NumericalIssue if the factorization failed because of a zero pivot.
       */
     ComputationInfo info() const
@@ -258,8 +262,10 @@ template<typename _MatrixType, int _UpLo> class LDLT
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
@@ -560,14 +566,22 @@ template<typename _MatrixType, int _UpLo>
 template<typename RhsType, typename DstType>
 void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
-  eigen_assert(rhs.rows() == rows());
+  _solve_impl_transposed<true>(rhs, dst);
+}
+
+template<typename _MatrixType,int _UpLo>
+template<bool Conjugate, typename RhsType, typename DstType>
+void LDLT<_MatrixType,_UpLo>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
   // dst = P b
   dst = m_transpositions * rhs;
 
   // dst = L^-1 (P b)
-  matrixL().solveInPlace(dst);
+  // dst = L^-*T (P b)
+  matrixL().template conjugateIf<!Conjugate>().solveInPlace(dst);
 
-  // dst = D^-1 (L^-1 P b)
+  // dst = D^-* (L^-1 P b)
+  // dst = D^-1 (L^-*T P b)
   // more precisely, use pseudo-inverse of D (see bug 241)
   using std::abs;
   const typename Diagonal<const MatrixType>::RealReturnType vecD(vectorD());
@@ -579,7 +593,6 @@ void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) cons
   // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
   // Using numeric_limits::min() gives us more robustness to denormals.
   RealScalar tolerance = (std::numeric_limits<RealScalar>::min)();
-
   for (Index i = 0; i < vecD.size(); ++i)
   {
     if(abs(vecD(i)) > tolerance)
@@ -588,10 +601,12 @@ void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) cons
       dst.row(i).setZero();
   }
 
-  // dst = L^-T (D^-1 L^-1 P b)
+  // dst = L^-* (D^-* L^-1 P b)
-  matrixU().solveInPlace(dst);
+  // dst = L^-T (D^-1 L^-*T P b)
+  matrixL().transpose().template conjugateIf<Conjugate>().solveInPlace(dst);
 
-  // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
+  // dst = P^T (L^-* D^-* L^-1 P b) = A^-1 b
+  // dst = P^-T (L^-T D^-1 L^-*T P b) = A^-1 b
   dst = m_transpositions.transpose() * dst;
 }
 #endif

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

@@ -13,6 +13,16 @@
 namespace Eigen {
 
 namespace internal{
+
+template<typename _MatrixType, int _UpLo> struct traits<LLT<_MatrixType, _UpLo> >
+ : traits<_MatrixType>
+{
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
+  enum { Flags = 0 };
+};
+
 template<typename MatrixType, int UpLo> struct LLT_Traits;
 }
 
@@ -54,18 +64,17 @@ template<typename MatrixType, int UpLo> struct LLT_Traits;
   * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
   */
 template<typename _MatrixType, int _UpLo> class LLT
+        : public SolverBase<LLT<_MatrixType, _UpLo> >
 {
   public:
     typedef _MatrixType MatrixType;
+    typedef SolverBase<LLT> Base;
+    friend class SolverBase<LLT>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(LLT)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    typedef typename MatrixType::StorageIndex StorageIndex;
 
     enum {
       PacketSize = internal::packet_traits<Scalar>::size,
@@ -100,7 +109,7 @@ template<typename _MatrixType, int _UpLo> class LLT
       compute(matrix.derived());
     }
 
-    /** \brief Constructs a LDLT factorization from a given matrix
+    /** \brief Constructs a LLT factorization from a given matrix
       *
       * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
       * \c MatrixType is a Eigen::Ref.
@@ -129,6 +138,7 @@ template<typename _MatrixType, int _UpLo> class LLT
       return Traits::getL(m_matrix);
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
       *
       * Since this LLT class assumes anyway that the matrix A is invertible, the solution
@@ -141,13 +151,8 @@ template<typename _MatrixType, int _UpLo> class LLT
       */
     template<typename Rhs>
     inline const Solve<LLT, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
+    solve(const MatrixBase<Rhs>& b) const;
-    {
+    #endif
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      eigen_assert(m_matrix.rows()==b.rows()
-                && "LLT::solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<LLT, Rhs>(*this, b.derived());
-    }
 
     template<typename Derived>
     void solveInPlace(const MatrixBase<Derived> &bAndX) const;
@@ -180,7 +185,7 @@ template<typename _MatrixType, int _UpLo> class LLT
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful,
+      * \returns \c Success if computation was successful,
       *          \c NumericalIssue if the matrix.appears not to be positive definite.
       */
     ComputationInfo info() const
@@ -194,18 +199,20 @@ template<typename _MatrixType, int _UpLo> class LLT
       * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
       * \code x = decomposition.adjoint().solve(b) \endcode
       */
-    const LLT& adjoint() const { return *this; };
+    const LLT& adjoint() const EIGEN_NOEXCEPT { return *this; };
 
-    inline Index rows() const { return m_matrix.rows(); }
+    inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
-    inline Index cols() const { return m_matrix.cols(); }
+    inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     template<typename VectorType>
-    LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
+    LLT & rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
@@ -459,7 +466,7 @@ LLT<MatrixType,_UpLo>& LLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>
   */
 template<typename _MatrixType, int _UpLo>
 template<typename VectorType>
-LLT<_MatrixType,_UpLo> LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, const RealScalar& sigma)
+LLT<_MatrixType,_UpLo> & LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, const RealScalar& sigma)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType);
   eigen_assert(v.size()==m_matrix.cols());
@@ -476,9 +483,18 @@ LLT<_MatrixType,_UpLo> LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, c
 template<typename _MatrixType,int _UpLo>
 template<typename RhsType, typename DstType>
 void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
+{
+  _solve_impl_transposed<true>(rhs, dst);
+}
+
+template<typename _MatrixType,int _UpLo>
+template<bool Conjugate, typename RhsType, typename DstType>
+void LLT<_MatrixType,_UpLo>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
 {
     dst = rhs;
-  solveInPlace(dst);
+
+    matrixL().template conjugateIf<!Conjugate>().solveInPlace(dst);
+    matrixU().template conjugateIf<!Conjugate>().solveInPlace(dst);
 }
 #endif