Merge pull request #1167 from borglab/release/4.2a6

Finish release 4.2a6

.github/scripts/unix.sh

@@ -71,6 +71,7 @@ function configure()
       -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 \
       -DBOOST_ROOT=$BOOST_ROOT \
       -DBoost_NO_SYSTEM_PATHS=ON \
       -DBoost_ARCHITECTURE=-x64
@@ -95,7 +96,11 @@ function build ()
   configure
 
   if [ "$(uname)" == "Linux" ]; then
-    make -j$(nproc)
+    if (($(nproc) > 2)); then
+      make -j$(nproc)
+    else
+      make -j2
+    fi
   elif [ "$(uname)" == "Darwin" ]; then
     make -j$(sysctl -n hw.physicalcpu)
   fi
@@ -113,7 +118,11 @@ function test ()
 
   # Actual testing
   if [ "$(uname)" == "Linux" ]; then
-    make -j$(nproc) check
+    if (($(nproc) > 2)); then
+      make -j$(nproc) check
+    else
+      make -j2 check
+    fi
   elif [ "$(uname)" == "Darwin" ]; then
     make -j$(sysctl -n hw.physicalcpu) check
   fi

.github/workflows/build-windows.yml

@@ -48,7 +48,9 @@ jobs:
       - name: Install Dependencies
         shell: powershell
         run: |
-          Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh')
+          iwr -useb get.scoop.sh -outfile 'install_scoop.ps1'
+          .\install_scoop.ps1 -RunAsAdmin
+
           scoop install cmake --global  # So we don't get issues with CMP0074 policy
           scoop install ninja --global
 
@@ -106,6 +108,21 @@ jobs:
           cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam
           cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable
           cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
+
+          # Run GTSAM tests
           cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete
+          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference
           cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation
+          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear
+          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm
+          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam
+          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic
+
+          # Run GTSAM_UNSTABLE tests
+          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
+          

CMakeLists.txt

@@ -1,4 +1,3 @@
-project(GTSAM CXX C)
 cmake_minimum_required(VERSION 3.0)
 
 # new feature to Cmake Version > 2.8.12
@@ -11,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a5")
+set (GTSAM_PRERELEASE_VERSION "a6")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)
@@ -19,6 +18,11 @@ if (${GTSAM_VERSION_PATCH} EQUAL 0)
 else()
     set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}${GTSAM_PRERELEASE_VERSION}")
 endif()
+
+project(GTSAM
+        LANGUAGES CXX C
+        VERSION "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
+
 message(STATUS "GTSAM Version: ${GTSAM_VERSION_STRING}")
 
 set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})

DEVELOP.md

@@ -15,7 +15,7 @@ For example:
 ```cpp
 class GTSAM_EXPORT MyClass { ... };
 
-GTSAM_EXPORT myFunction();
+GTSAM_EXPORT return_type myFunction();
 ```
 
 More details [here](Using-GTSAM-EXPORT.md).

INSTALL.md

@@ -13,7 +13,7 @@ $ make install
 ## Important Installation Notes
 
 1. GTSAM requires the following libraries to be installed on your system:
-    - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes).
+    - BOOST version 1.65 or greater (install through Linux repositories or MacPorts). Please see [Boost Notes](#boost-notes) for version recommendations based on your compiler.
 
     - Cmake version 3.0 or higher
     - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
@@ -72,7 +72,7 @@ execute commands as follows for an out-of-source build:
 Versions of Boost prior to 1.65 have a known bug that prevents proper "deep" serialization of objects, which means that objects encapsulated inside other objects don't get serialized.
 This is particularly seen when using `clang` as the C++ compiler.
 
-For this reason we require Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager.
+For this reason we recommend Boost>=1.65, and recommend installing it through alternative channels when it is not available through your operating system's primary package manager.
 
 ## Known Issues
 

Using-GTSAM-EXPORT.md

@@ -8,6 +8,7 @@ To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and need
     * At least one of the functions inside that class is declared in a .cpp file and not just the .h file.
     * You can `GTSAM_EXPORT` any class it inherits from as well.  (Note that this implictly requires the class does not derive from a "header-only" class.  Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!) 
 3.  If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations.  (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
+4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization.
 
 ## When is GTSAM_EXPORT being used incorrectly
 Unfortunately, using `GTSAM_EXPORT` incorrectly often does not cause a compiler or linker error in the library that is being compiled, but only when you try to use that DLL in a different library.  For example, an error in `gtsam/base` will often show up when compiling the `check_base_program` or the MATLAB wrapper, but not when compiling/linking gtsam itself.  The most common errors will say something like:

cmake/GtsamBuildTypes.cmake

@@ -93,6 +93,10 @@ if(MSVC)
 	/wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data
   )
 
+  add_compile_options(/wd4005)
+  add_compile_options(/wd4101)
+  add_compile_options(/wd4834)
+
 endif()
 
 # Other (non-preprocessor macros) compiler flags:
@@ -187,7 +191,7 @@ 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!)" ON)
-  if(GTSAM_BUILD_WITH_MARCH_NATIVE)
+  if(GTSAM_BUILD_WITH_MARCH_NATIVE AND (APPLE AND NOT CMAKE_SYSTEM_PROCESSOR STREQUAL "arm64"))
     # Add as public flag so all dependant projects also use it, as required
     # by Eigen to avid crashes due to SIMD vectorization:
     list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")

cmake/HandleMetis.cmake

@@ -21,7 +21,12 @@ if(GTSAM_USE_SYSTEM_METIS)
     mark_as_advanced(METIS_LIBRARY)
 
     add_library(metis-gtsam-if INTERFACE)
-    target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR})
+    target_include_directories(metis-gtsam-if BEFORE INTERFACE ${METIS_INCLUDE_DIR}
+      # gtsam_unstable/partition/FindSeparator-inl.h uses internal metislib.h API
+      # via extern "C"
+      $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
+      $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
+    )
     target_link_libraries(metis-gtsam-if INTERFACE ${METIS_LIBRARY})
   endif()
 else()
@@ -30,10 +35,12 @@ else()
   add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis)
 
   target_include_directories(metis-gtsam BEFORE PUBLIC
+    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
     $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/include>
+    # gtsam_unstable/partition/FindSeparator-inl.h uses internal metislib.h API
+    # via extern "C"
     $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
     $<BUILD_INTERFACE:${GTSAM_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
-    $<INSTALL_INTERFACE:include/gtsam/3rdparty/metis/>
   )
 
   add_library(metis-gtsam-if INTERFACE)

doc/Code/LocalizationExample2.cpp

@@ -1,7 +1,7 @@
 // add unary measurement factors, like GPS, on all three poses
-noiseModel::Diagonal::shared_ptr unaryNoise =
+auto unaryNoise =
  noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
-graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
-graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
-graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
 

doc/Code/LocalizationFactor.cpp

@@ -6,8 +6,7 @@ public:
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
   Vector evaluateError(const Pose2& q, 
-                       boost::optional<Matrix&> H = boost::none) const
+      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/Code/OdometryExample.cpp

@@ -3,13 +3,11 @@ NonlinearFactorGraph graph;
 
 // Add a Gaussian prior on pose x_1
 Pose2 priorMean(0.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr priorNoise =
+auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
-graph.addPrior(1, priorMean, priorNoise);
 
 // Add two odometry factors
 Pose2 odometry(2.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr odometryNoise =
+auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

doc/Code/OdometryOutput1.txt

@@ -1,11 +1,14 @@
 Factor Graph:
 size: 3
-factor 0: PriorFactor on 1
+
-  prior mean: (0, 0, 0)
+Factor 0: PriorFactor on 1
+  prior mean:  (0, 0, 0)
   noise model: diagonal sigmas [0.3; 0.3; 0.1];
-factor 1: BetweenFactor(1,2)
+
-  measured: (2, 0, 0)
+Factor 1: BetweenFactor(1,2)
+  measured:  (2, 0, 0)
   noise model: diagonal sigmas [0.2; 0.2; 0.1];
-factor 2: BetweenFactor(2,3)
+
-  measured: (2, 0, 0)
+Factor 2: BetweenFactor(2,3)
+  measured:  (2, 0, 0)
   noise model: diagonal sigmas [0.2; 0.2; 0.1];

doc/Code/OdometryOutput2.txt

@@ -1,11 +1,23 @@
 Initial Estimate:
+
 Values with 3 values:
-Value 1: (0.5, 0, 0.2)
+Value 1: (gtsam::Pose2)
-Value 2: (2.3, 0.1, -0.2)
+(0.5, 0, 0.2)
-Value 3: (4.1, 0.1, 0.1)
+
+Value 2: (gtsam::Pose2)
+(2.3, 0.1, -0.2)
+
+Value 3: (gtsam::Pose2)
+(4.1, 0.1, 0.1)
 
 Final Result:
+
 Values with 3 values:
-Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
+Value 1: (gtsam::Pose2)
-Value 2: (2, 7.4e-18, -2.5e-18)
+(7.5-16, -5.3-16, -1.8-16)
-Value 3: (4, -1.8e-18, -3.1e-18)
+
+Value 2: (gtsam::Pose2)
+(2, -1.1-15, -2.5-16)
+
+Value 3: (gtsam::Pose2)
+(4, -1.7-15, -2.5-16)

doc/Code/OdometryOutput3.txt

@@ -1,12 +1,12 @@
 x1 covariance:
-       0.09     1.1e-47     5.7e-33
+   0.09 1.7e-33 2.8e-33
-    1.1e-47        0.09     1.9e-17
+1.7e-33    0.09 2.6e-17
-    5.7e-33     1.9e-17        0.01
+2.8e-33 2.6e-17    0.01
 x2 covariance:
-       0.13     4.7e-18     2.4e-18
+   0.13 1.2e-18 6.1e-19
-    4.7e-18        0.17        0.02
+1.2e-18    0.17    0.02
-    2.4e-18        0.02        0.02
+6.1e-19    0.02    0.02
 x3 covariance:
-       0.17     2.7e-17     8.4e-18
+   0.17 8.6e-18 2.7e-18
-    2.7e-17        0.37        0.06
+8.6e-18    0.37    0.06
-    8.4e-18        0.06        0.03
+2.7e-18    0.06    0.03

doc/gtsam.lyx

@@ -134,14 +134,10 @@ A Hands-on Introduction
 
 \begin_layout Author
 Frank Dellaert
-\begin_inset Newline newline
-\end_inset
-
-Technical Report number GT-RIM-CP&R-2014-XXX
 \end_layout
 
 \begin_layout Date
-September 2014
+Updated Last March 2022
 \end_layout
 
 \begin_layout Standard
@@ -154,18 +150,14 @@ filename "common_macros.tex"
 
 \end_layout
 
-\begin_layout Section*
+\begin_layout Abstract
-Overview
-\end_layout
-
-\begin_layout Standard
 In this document I provide a hands-on introduction to both factor graphs
  and GTSAM.
  This is an updated version from the 2012 TR that is tailored to our GTSAM
- 3.0 library and beyond.
+ 4.0 library and beyond.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
 
 \series bold
 Factor graphs
@@ -206,7 +198,7 @@ ts or prior knowledge.
  robotics and vision.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
 The GTSAM toolbox (GTSAM stands for 
 \begin_inset Quotes eld
 \end_inset
@@ -221,11 +213,13 @@ Georgia Tech Smoothing and Mapping
  It provides state of the art solutions to the SLAM and SFM problems, but
  can also be used to model and solve both simpler and more complex estimation
  problems.
- It also provides a MATLAB interface which allows for rapid prototype developmen
+ It also provides MATLAB and Python wrappers which allow for rapid prototype
-t, visualization, and user interaction.
+ development, visualization, and user interaction.
+ In addition, it is easy to use in Jupyter notebooks and/or Google's coLaborator
+y.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
 GTSAM exploits sparsity to be computationally efficient.
  Typically measurements only provide information on the relationship between
  a handful of variables, and hence the resulting factor graph will be sparsely
@@ -236,14 +230,17 @@ l complexity.
  GTSAM provides iterative methods that are quite efficient regardless.
 \end_layout
 
-\begin_layout Standard
+\begin_layout Abstract
-You can download the latest version of GTSAM at 
+You can download the latest version of GTSAM from GitHub at
+\end_layout
+
+\begin_layout Abstract
 \begin_inset Flex URL
 status open
 
 \begin_layout Plain Layout
 
-http://tinyurl.com/gtsam
+https://github.com/borglab/gtsam
 \end_layout
 
 \end_inset
@@ -741,7 +738,7 @@ noindent
 \begin_inset Formula $f_{0}(x_{1})$
 \end_inset
 
- on lines 5-8 as an instance of 
+ on lines 5-7 as an instance of 
 \series bold
 \emph on
 PriorFactor<T>
@@ -773,7 +770,7 @@ Pose2,
 noiseModel::Diagonal
 \series default
 \emph default
- by specifying three standard deviations in line 7, respectively 30 cm.
+ by specifying three standard deviations in line 6, respectively 30 cm.
 \begin_inset space ~
 \end_inset
 
@@ -795,7 +792,7 @@ Similarly, odometry measurements are specified as
 Pose2
 \series default
 \emph default
- on line 11, with a slightly different noise model defined on line 12-13.
+ on line 10, with a slightly different noise model defined on line 11.
  We then add the two factors 
 \begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$
 \end_inset
@@ -804,7 +801,7 @@ Pose2
 \begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$
 \end_inset
 
- on lines 14-15, as instances of yet another templated class, 
+ on lines 12-13, as instances of yet another templated class, 
 \series bold
 \emph on
 BetweenFactor<T>
@@ -875,7 +872,7 @@ smoothing and mapping
 
 .
  Later in this document we will talk about how we can also use GTSAM to
- do filtering (which you often do 
+ do filtering (which often you do 
 \emph on
 not
 \emph default
@@ -928,7 +925,11 @@ Values
 \begin_layout Standard
 The latter point is often a point of confusion with beginning users of GTSAM.
  It helps to remember that when designing GTSAM we took a functional approach
- of classes corresponding to mathematical objects, which are usually immutable.
+ of classes corresponding to mathematical objects, which are usually 
+\emph on
+immutable
+\emph default
+.
  You should think of a factor graph as a 
 \emph on
 function
@@ -1363,14 +1364,18 @@ where
 \end_inset
 
  is the measurement, 
-\begin_inset Formula $q$
+\begin_inset Formula $q\in SE(2)$
 \end_inset
 
  is the unknown variable, 
 \begin_inset Formula $h(q)$
 \end_inset
 
- is a (possibly nonlinear) measurement function, and 
+ is a 
+\series bold
+measurement function
+\series default
+, and 
 \begin_inset Formula $\Sigma$
 \end_inset
 
@@ -1546,7 +1551,7 @@ E(q)\define h(q)-m
 
 \end_inset
 
-which is done on line 12.
+which is done on line 14.
  Importantly, because we want to use this factor for nonlinear optimization
  (see e.g., 
 \begin_inset CommandInset citation
@@ -1599,11 +1604,11 @@ q_{y}
 \begin_inset Formula $q=\left(q_{x},q_{y},q_{\theta}\right)$
 \end_inset
 
-, yields the following simple 
+, yields the following 
 \begin_inset Formula $2\times3$
 \end_inset
 
- matrix in tangent space which is the same the as the rotation matrix:
+ matrix:
 \end_layout
 
 \begin_layout Standard
@@ -1618,6 +1623,171 @@ H=\left[\begin{array}{ccc}
 \end_inset
 
 
+\end_layout
+
+\begin_layout Paragraph*
+Important Note
+\end_layout
+
+\begin_layout Standard
+Many of our users, when attempting to create a custom factor, are initially
+ surprised at the Jacobian matrix not agreeing with their intuition.
+ For example, above you might simply expect a 
+\begin_inset Formula $2\times3$
+\end_inset
+
+ identity matrix.
+ This 
+\emph on
+would
+\emph default
+ be true for variables belonging to a vector space.
+ However, in GTSAM we define the Jacobian more generally to be the matrix
+ 
+\begin_inset Formula $H$
+\end_inset
+
+ such that
+\begin_inset Formula 
+\[
+h(q\exp\hat{\xi})\approx h(q)+H\xi
+\]
+
+\end_inset
+
+where 
+\begin_inset Formula $\xi=(\delta x,\delta y,\delta\theta)$
+\end_inset
+
+ is an incremental update and 
+\begin_inset Formula $\exp\hat{\xi}$
+\end_inset
+
+ is the 
+\series bold
+exponential map
+\series default
+ for the variable we want to update.
+ In this case 
+\begin_inset Formula $q\in SE(2)$
+\end_inset
+
+, where 
+\begin_inset Formula $SE(2)$
+\end_inset
+
+ is the group of 2D rigid transforms, implemented by 
+\series bold
+\emph on
+Pose2
+\emph default
+.
+ 
+\series default
+The exponential map for 
+\begin_inset Formula $SE(2)$
+\end_inset
+
+ can be approximated to first order as
+\begin_inset Formula 
+\[
+\exp\hat{\xi}\approx\left[\begin{array}{ccc}
+1 & -\delta\theta & \delta x\\
+\delta\theta & 1 & \delta y\\
+0 & 0 & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+when using the 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ matrix representation for 2D poses, and hence 
+\begin_inset Formula 
+\[
+h(qe^{\hat{\xi}})\approx h\left(\left[\begin{array}{ccc}
+\cos(q_{\theta}) & -\sin(q_{\theta}) & q_{x}\\
+\sin(q_{\theta}) & \cos(q_{\theta}) & q_{y}\\
+0 & 0 & 1
+\end{array}\right]\left[\begin{array}{ccc}
+1 & -\delta\theta & \delta x\\
+\delta\theta & 1 & \delta y\\
+0 & 0 & 1
+\end{array}\right]\right)=\left[\begin{array}{c}
+q_{x}+\cos(q_{\theta})\delta x-\sin(q_{\theta})\delta y\\
+q_{y}+\sin(q_{\theta})\delta x+\cos(q_{\theta})\delta y
+\end{array}\right]
+\]
+
+\end_inset
+
+which then explains the Jacobian 
+\begin_inset Formula $H$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Standard
+Lie groups are very relevant in the robotics context, and you can read more
+ here:
+\end_layout
+
+\begin_layout Itemize
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://github.com/borglab/gtsam/blob/develop/doc/LieGroups.pdf
+\end_layout
+
+\end_inset
+
+ 
+\end_layout
+
+\begin_layout Itemize
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://github.com/borglab/gtsam/blob/develop/doc/math.pdf
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+In some cases you want to go even beyond Lie groups to a looser concept,
+ 
+\series bold
+manifolds
+\series default
+, because not all unknown variables behave like a group, e.g., the space of
+ 3D planes, 2D lines, directions in space, etc.
+ For manifolds we do not always have an exponential map, but we have a retractio
+n that plays the same role.
+ Some of this is explained here:
+\end_layout
+
+\begin_layout Itemize
+\begin_inset Flex URL
+status open
+
+\begin_layout Plain Layout
+
+https://gtsam.org/notes/GTSAM-Concepts.html 
+\end_layout
+
+\end_inset
+
+
 \end_layout
 
 \begin_layout Subsection
@@ -1680,13 +1850,13 @@ UnaryFactor
 \series default
 \emph default
  instances, and add them to graph.
- GTSAM uses shared pointers to refer to factors in factor graphs, and 
+ GTSAM uses shared pointers to refer to factors, and 
 \series bold
 \emph on
-boost::make_shared
+emplace_shared
 \series default
 \emph default
- is a convenience function to simultaneously construct a class and create
+ is a convenience method to simultaneously construct a class and create
  a 
 \series bold
 \emph on
@@ -1694,22 +1864,6 @@ shared_ptr
 \series default
 \emph default
  to it.
- 
-\begin_inset Note Note
-status collapsed
-
-\begin_layout Plain Layout
-and on lines 4-6 we add three newly created 
-\series bold
-\emph on
-UnaryFactor
-\series default
-\emph default
- instances to the graph.
-\end_layout
-
-\end_inset
-
  We obtain the factor graph from Figure 
 \begin_inset CommandInset ref
 LatexCommand vref

examples/Data/randomGrid3D.xml

@@ -7,7 +7,7 @@
 			<count>32</count>
 			<item_version>1</item_version>
 			<item class_id="3" tracking_level="0" version="1">
-				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
 					<Base class_id="5" tracking_level="0" version="0">
 						<Base class_id="6" tracking_level="0" version="0">
 							<keys_>

examples/Data/toy3D.xml

@@ -7,7 +7,7 @@
 			<count>2</count>
 			<item_version>1</item_version>
 			<item class_id="3" tracking_level="0" version="1">
-				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
 					<Base class_id="5" tracking_level="0" version="0">
 						<Base class_id="6" tracking_level="0" version="0">
 							<keys_>

examples/RangeISAMExample_plaza2.cpp

@@ -10,62 +10,81 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file RangeISAMExample_plaza1.cpp
+ * @file RangeISAMExample_plaza2.cpp
  * @brief A 2D Range SLAM example
  * @date June 20, 2013
- * @author FRank Dellaert
+ * @author Frank Dellaert
  */
 
-// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
+// Both relative poses and recovered trajectory poses will be stored as Pose2.
 #include <gtsam/geometry/Pose2.h>
+using gtsam::Pose2;
 
-// Each variable in the system (poses and landmarks) must be identified with a unique key.
+// gtsam::Vectors are dynamic Eigen vectors, Vector3 is statically sized.
-// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+#include <gtsam/base/Vector.h>
-// Here we will use Symbols
+using gtsam::Vector;
+using gtsam::Vector3;
+
+// Unknown landmarks are of type Point2 (which is just a 2D Eigen vector).
+#include <gtsam/geometry/Point2.h>
+using gtsam::Point2;
+
+// Each variable in the system (poses and landmarks) must be identified with a
+// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
+// (X1, X2, L1). Here we will use Symbols.
 #include <gtsam/inference/Symbol.h>
+using gtsam::Symbol;
 
-// We want to use iSAM2 to solve the range-SLAM problem incrementally
+// We want to use iSAM2 to solve the range-SLAM problem incrementally.
 #include <gtsam/nonlinear/ISAM2.h>
 
-// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
+// iSAM2 requires as input a set set of new factors to be added stored in a
-// and initial guesses for any new variables used in the added factors
+// factor graph, and initial guesses for any new variables in the added factors.
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 
-// We will use a non-liear solver to batch-inituialize from the first 150 frames
+// We will use a non-linear solver to batch-initialize from the first 150 frames
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
-// In GTSAM, measurement functions are represented as 'factors'. Several common factors
+// Measurement functions are represented as 'factors'. Several common factors
-// have been provided with the library for solving robotics SLAM problems.
+// have been provided with the library for solving robotics SLAM problems:
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/sam/RangeFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
 
-// Standard headers, added last, so we know headers above work on their own
+// Timing, with functions below, provides nice facilities to benchmark.
+#include <gtsam/base/timing.h>
+using gtsam::tictoc_print_;
+
+// Standard headers, added last, so we know headers above work on their own.
 #include <fstream>
 #include <iostream>
+#include <random>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
 
-using namespace std;
-using namespace gtsam;
 namespace NM = gtsam::noiseModel;
 
-// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
+// Data is second UWB ranging dataset, B2 or "plaza 2", from
-// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
+// "Navigating with Ranging Radios: Five Data Sets with Ground Truth"
+// by Joseph Djugash, Bradley Hamner, and Stephan Roth
+// https://www.ri.cmu.edu/pub_files/2009/9/Final_5datasetsRangingRadios.pdf
 
 // load the odometry
 // DR: Odometry Input (delta distance traveled and delta heading change)
-//    Time (sec)  Delta Dist. Trav. (m) Delta Heading (rad)
+//    Time (sec)  Delta Distance Traveled (m) Delta Heading (rad)
-typedef pair<double, Pose2> TimedOdometry;
+using TimedOdometry = std::pair<double, Pose2>;
-list<TimedOdometry> readOdometry() {
+std::list<TimedOdometry> readOdometry() {
-  list<TimedOdometry> odometryList;
+  std::list<TimedOdometry> odometryList;
-  string data_file = findExampleDataFile("Plaza2_DR.txt");
+  std::string data_file = gtsam::findExampleDataFile("Plaza2_DR.txt");
-  ifstream is(data_file.c_str());
+  std::ifstream is(data_file.c_str());
 
   while (is) {
     double t, distance_traveled, delta_heading;
     is >> t >> distance_traveled >> delta_heading;
-    odometryList.push_back(
+    odometryList.emplace_back(t, Pose2(distance_traveled, 0, delta_heading));
-        TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
   }
   is.clear(); /* clears the end-of-file and error flags */
   return odometryList;
@@ -73,90 +92,85 @@ list<TimedOdometry> readOdometry() {
 
 // load the ranges from TD
 //    Time (sec)  Sender / Antenna ID Receiver Node ID  Range (m)
-typedef boost::tuple<double, size_t, double> RangeTriple;
+using RangeTriple = boost::tuple<double, size_t, double>;
-vector<RangeTriple> readTriples() {
+std::vector<RangeTriple> readTriples() {
-  vector<RangeTriple> triples;
+  std::vector<RangeTriple> triples;
-  string data_file = findExampleDataFile("Plaza2_TD.txt");
+  std::string data_file = gtsam::findExampleDataFile("Plaza2_TD.txt");
-  ifstream is(data_file.c_str());
+  std::ifstream is(data_file.c_str());
 
   while (is) {
-    double t, sender, range;
+    double t, range, sender, receiver;
-    size_t receiver;
     is >> t >> sender >> receiver >> range;
-    triples.push_back(RangeTriple(t, receiver, range));
+    triples.emplace_back(t, receiver, range);
   }
   is.clear(); /* clears the end-of-file and error flags */
   return triples;
 }
 
 // main
-int main (int argc, char** argv) {
+int main(int argc, char** argv) {
-
   // load Plaza2 data
-  list<TimedOdometry> odometry = readOdometry();
+  std::list<TimedOdometry> odometry = readOdometry();
-//  size_t M = odometry.size();
+  size_t M = odometry.size();
+  std::cout << "Read " << M << " odometry entries." << std::endl;
 
-  vector<RangeTriple> triples = readTriples();
+  std::vector<RangeTriple> triples = readTriples();
   size_t K = triples.size();
+  std::cout << "Read " << K << " range triples." << std::endl;
 
   // parameters
-  size_t minK = 150; // minimum number of range measurements to process initially
+  size_t minK =
-  size_t incK = 25; // minimum number of range measurements to process after
+      150;  // minimum number of range measurements to process initially
-  bool groundTruth = false;
+  size_t incK = 25;  // minimum number of range measurements to process after
   bool robust = true;
 
   // Set Noise parameters
-  Vector priorSigmas = Vector3(1,1,M_PI);
+  Vector priorSigmas = Vector3(1, 1, M_PI);
   Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
-  double sigmaR = 100; // range standard deviation
+  double sigmaR = 100;        // range standard deviation
-  const NM::Base::shared_ptr // all same type
+  const NM::Base::shared_ptr  // all same type
-  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
+      priorNoise = NM::Diagonal::Sigmas(priorSigmas),  // prior
-  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
+      looseNoise = NM::Isotropic::Sigma(2, 1000),      // loose LM prior
-  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
+      odoNoise = NM::Diagonal::Sigmas(odoSigmas),      // odometry
-  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
+      gaussian = NM::Isotropic::Sigma(1, sigmaR),      // non-robust
-  rangeNoise = robust ? tukey : gaussian;
+      tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15),
+                                 gaussian),  // robust
+      rangeNoise = robust ? tukey : gaussian;
 
   // Initialize iSAM
-  ISAM2 isam;
+  gtsam::ISAM2 isam;
 
   // Add prior on first pose
-  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
+  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.021089);
-      M_PI - 2.02108900000000);
+  gtsam::NonlinearFactorGraph newFactors;
-  NonlinearFactorGraph newFactors;
   newFactors.addPrior(0, pose0, priorNoise);
-  Values initial;
+  gtsam::Values initial;
   initial.insert(0, pose0);
 
-  //  initialize points
+  // We will initialize landmarks randomly, and keep track of which landmarks we
-  if (groundTruth) { // from TL file
+  // already added with a set.
-    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
+  std::mt19937_64 rng;
-    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
+  std::normal_distribution<double> normal(0.0, 100.0);
-    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
+  std::set<Symbol> initializedLandmarks;
-    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
-  } else { // drawn from sigma=1 Gaussian in matlab version
-    initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
-    initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
-    initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
-    initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
-  }
 
   // set some loop variables
-  size_t i = 1; // step counter
+  size_t i = 1;  // step counter
-  size_t k = 0; // range measurement counter
+  size_t k = 0;  // range measurement counter
   bool initialized = false;
   Pose2 lastPose = pose0;
   size_t countK = 0;
 
   // Loop over odometry
   gttic_(iSAM);
-  for(const TimedOdometry& timedOdometry: odometry) {
+  for (const TimedOdometry& timedOdometry : odometry) {
-    //--------------------------------- odometry loop -----------------------------------------
+    //--------------------------------- odometry loop --------------------------
     double t;
     Pose2 odometry;
     boost::tie(t, odometry) = timedOdometry;
 
     // add odometry factor
-    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry, odoNoise));
+    newFactors.emplace_shared<gtsam::BetweenFactor<Pose2>>(i - 1, i, odometry,
+                                                           odoNoise);
 
     // predict pose and add as initial estimate
     Pose2 predictedPose = lastPose.compose(odometry);
@@ -166,17 +180,30 @@ int main (int argc, char** argv) {
     // Check if there are range factors to be added
     while (k < K && t >= boost::get<0>(triples[k])) {
       size_t j = boost::get<1>(triples[k]);
+      Symbol landmark_key('L', j);
       double range = boost::get<2>(triples[k]);
-      newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
+      newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>(
+          i, landmark_key, range, rangeNoise);
+      if (initializedLandmarks.count(landmark_key) == 0) {
+        std::cout << "adding landmark " << j << std::endl;
+        double x = normal(rng), y = normal(rng);
+        initial.insert(landmark_key, Point2(x, y));
+        initializedLandmarks.insert(landmark_key);
+        // We also add a very loose prior on the landmark in case there is only
+        // one sighting, which cannot fully determine the landmark.
+        newFactors.emplace_shared<gtsam::PriorFactor<Point2>>(
+            landmark_key, Point2(0, 0), looseNoise);
+      }
       k = k + 1;
       countK = countK + 1;
     }
 
     // Check whether to update iSAM 2
     if ((k > minK) && (countK > incK)) {
-      if (!initialized) { // Do a full optimize for first minK ranges
+      if (!initialized) {  // Do a full optimize for first minK ranges
+        std::cout << "Initializing at time " << k << std::endl;
         gttic_(batchInitialization);
-        LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
+        gtsam::LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
         initial = batchOptimizer.optimize();
         gttoc_(batchInitialization);
         initialized = true;
@@ -185,21 +212,27 @@ int main (int argc, char** argv) {
       isam.update(newFactors, initial);
       gttoc_(update);
       gttic_(calculateEstimate);
-      Values result = isam.calculateEstimate();
+      gtsam::Values result = isam.calculateEstimate();
       gttoc_(calculateEstimate);
       lastPose = result.at<Pose2>(i);
-      newFactors = NonlinearFactorGraph();
+      newFactors = gtsam::NonlinearFactorGraph();
-      initial = Values();
+      initial = gtsam::Values();
       countK = 0;
     }
     i += 1;
-    //--------------------------------- odometry loop -----------------------------------------
+    //--------------------------------- odometry loop --------------------------
-  } // end for
+  }  // end for
   gttoc_(iSAM);
 
   // Print timings
   tictoc_print_();
 
+  // Print optimized landmarks:
+  gtsam::Values finalResult = isam.calculateEstimate();
+  for (auto&& landmark_key : initializedLandmarks) {
+    Point2 p = finalResult.at<Point2>(landmark_key);
+    std::cout << landmark_key << ":" << p.transpose() << "\n";
+  }
+
   exit(0);
 }
-

examples/SFMExampleExpressions_bal.cpp

@@ -26,10 +26,12 @@
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
 // Header order is close to far
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/sfm/SfmData.h>  // for loading BAL datasets !
 #include <gtsam/slam/dataset.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <boost/format.hpp>
 #include <vector>
 
 using namespace std;

examples/SFMExample_bal.cpp

@@ -16,12 +16,14 @@
  */
 
 // For an explanation of headers, see SFMExample.cpp
-#include <gtsam/inference/Symbol.h>
+#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/inference/Symbol.h>
-#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
+
-#include <gtsam/slam/dataset.h>
+#include <boost/format.hpp>
 #include <vector>
 
 using namespace std;

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -17,16 +17,16 @@
  */
 
 // For an explanation of headers, see SFMExample.cpp
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/inference/Ordering.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/sfm/SfmData.h>  // for loading BAL datasets !
 #include <gtsam/slam/dataset.h>
-
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/inference/Ordering.h>
 #include <gtsam/base/timing.h>
 
+#include <boost/format.hpp>
 #include <vector>
 
 using namespace std;

examples/SFMdata.h

@@ -22,6 +22,8 @@
  * Passing function argument allows to specificy an initial position, a pose increment and step count.
  */
 
+#pragma once
+
 // As this is a full 3D problem, we will use Pose3 variables to represent the camera
 // positions and Point3 variables (x, y, z) to represent the landmark coordinates.
 // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).

gtsam/base/FastSet.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <boost/version.hpp>
 #if BOOST_VERSION >= 107400
 #include <boost/serialization/library_version_type.hpp>
 #endif

gtsam/base/Matrix.cpp

@@ -25,6 +25,7 @@
 
 #include <boost/tuple/tuple.hpp>
 #include <boost/tokenizer.hpp>
+#include <boost/format.hpp>
 
 #include <cstdarg>
 #include <cstring>

gtsam/base/Matrix.h

@@ -26,12 +26,9 @@
 
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Vector.h>
-#include <gtsam/config.h>
-
-#include <boost/format.hpp>
-#include <functional>
 #include <boost/tuple/tuple.hpp>
-#include <boost/math/special_functions/fpclassify.hpp>
+
+#include <vector>
 
 /**
  * Matrix is a typedef in the gtsam namespace
@@ -523,82 +520,4 @@ GTSAM_EXPORT Matrix LLt(const Matrix& A);
 GTSAM_EXPORT Matrix RtR(const Matrix& A);
 
 GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
-} // namespace gtsam
+}  // namespace gtsam
-
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/array.hpp>
-#include <boost/serialization/split_free.hpp>
-
-namespace boost {
-  namespace serialization {
-
-    /**
-     * Ref. https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063
-     * 
-     * Eigen supports calling resize() on both static and dynamic matrices.
-     * This allows for a uniform API, with resize having no effect if the static matrix
-     * is already the correct size.
-     * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing
-     * 
-     * We use all the Matrix template parameters to ensure wide compatibility.
-     * 
-     * eigen_typekit in ROS uses the same code
-     * http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html
-     */
-
-    // split version - sends sizes ahead
-    template<class Archive,
-             typename Scalar_,
-             int Rows_,
-             int Cols_,
-             int Ops_,
-             int MaxRows_,
-             int MaxCols_>
-    void save(Archive & ar,
-              const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
-              const unsigned int /*version*/) {
-      const size_t rows = m.rows(), cols = m.cols();
-      ar << BOOST_SERIALIZATION_NVP(rows);
-      ar << BOOST_SERIALIZATION_NVP(cols);
-      ar << make_nvp("data", make_array(m.data(), m.size()));
-    }
-
-    template<class Archive,
-             typename Scalar_,
-             int Rows_,
-             int Cols_,
-             int Ops_,
-             int MaxRows_,
-             int MaxCols_>
-    void load(Archive & ar,
-              Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
-              const unsigned int /*version*/) {
-      size_t rows, cols;
-      ar >> BOOST_SERIALIZATION_NVP(rows);
-      ar >> BOOST_SERIALIZATION_NVP(cols);
-      m.resize(rows, cols);
-      ar >> make_nvp("data", make_array(m.data(), m.size()));
-    }
-
-    // templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
-    template<class Archive,
-             typename Scalar_,
-             int Rows_,
-             int Cols_,
-             int Ops_,
-             int MaxRows_,
-             int MaxCols_>
-    void serialize(Archive & ar,
-              Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
-              const unsigned int version) {
-      split_free(ar, m, version);
-    }
-
-    // specialized to Matrix for MATLAB wrapper
-    template <class Archive>
-    void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
-      split_free(ar, m, version);
-    }
-
-  } // namespace serialization
-} // namespace boost

gtsam/base/MatrixSerialization.h

@@ -0,0 +1,89 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    MatrixSerialization.h
+ * @brief   Serialization for matrices
+ * @author  Frank Dellaert
+ * @date    February 2022
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+
+#include <boost/serialization/array.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/split_free.hpp>
+
+namespace boost {
+namespace serialization {
+
+/**
+ * Ref.
+ * https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063
+ *
+ * Eigen supports calling resize() on both static and dynamic matrices.
+ * This allows for a uniform API, with resize having no effect if the static
+ * matrix is already the correct size.
+ * https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing
+ *
+ * We use all the Matrix template parameters to ensure wide compatibility.
+ *
+ * eigen_typekit in ROS uses the same code
+ * http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html
+ */
+
+// split version - sends sizes ahead
+template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
+          int MaxRows_, int MaxCols_>
+void save(
+    Archive& ar,
+    const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
+    const unsigned int /*version*/) {
+  const size_t rows = m.rows(), cols = m.cols();
+  ar << BOOST_SERIALIZATION_NVP(rows);
+  ar << BOOST_SERIALIZATION_NVP(cols);
+  ar << make_nvp("data", make_array(m.data(), m.size()));
+}
+
+template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
+          int MaxRows_, int MaxCols_>
+void load(Archive& ar,
+          Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
+          const unsigned int /*version*/) {
+  size_t rows, cols;
+  ar >> BOOST_SERIALIZATION_NVP(rows);
+  ar >> BOOST_SERIALIZATION_NVP(cols);
+  m.resize(rows, cols);
+  ar >> make_nvp("data", make_array(m.data(), m.size()));
+}
+
+// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
+template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
+          int MaxRows_, int MaxCols_>
+void serialize(
+    Archive& ar,
+    Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
+    const unsigned int version) {
+  split_free(ar, m, version);
+}
+
+// specialized to Matrix for MATLAB wrapper
+template <class Archive>
+void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
+  split_free(ar, m, version);
+}
+
+}  // namespace serialization
+}  // namespace boost

gtsam/base/Value.h

@@ -21,6 +21,7 @@
 #include <gtsam/config.h>      // Configuration from CMake
 
 #include <gtsam/base/Vector.h>
+#include <boost/serialization/nvp.hpp>
 #include <boost/serialization/assume_abstract.hpp>
 #include <memory>
 

gtsam/base/Vector.h

@@ -264,46 +264,4 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
  * concatenate Vectors
  */
 GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
-} // namespace gtsam
+}  // namespace gtsam
-
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/array.hpp>
-#include <boost/serialization/split_free.hpp>
-
-namespace boost {
-  namespace serialization {
-
-    // split version - copies into an STL vector for serialization
-    template<class Archive>
-    void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) {
-      const size_t size = v.size();
-      ar << BOOST_SERIALIZATION_NVP(size);
-      ar << make_nvp("data", make_array(v.data(), v.size()));
-    }
-
-    template<class Archive>
-    void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) {
-      size_t size;
-      ar >> BOOST_SERIALIZATION_NVP(size);
-      v.resize(size);
-      ar >> make_nvp("data", make_array(v.data(), v.size()));
-    }
-
-    // split version - copies into an STL vector for serialization
-    template<class Archive, int D>
-    void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
-      ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
-    }
-
-    template<class Archive, int D>
-    void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
-      ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
-    }
-
-  } // namespace serialization
-} // namespace boost
-
-BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
-BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
-BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
-BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)

gtsam/base/VectorSerialization.h

@@ -0,0 +1,65 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    VectorSerialization.h
+ * @brief   serialization for Vectors
+ * @author  Frank Dellaert
+ * @date    February 2022
+ */
+
+#pragma once
+
+#include <gtsam/base/Vector.h>
+
+#include <boost/serialization/array.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/split_free.hpp>
+
+namespace boost {
+namespace serialization {
+
+// split version - copies into an STL vector for serialization
+template <class Archive>
+void save(Archive& ar, const gtsam::Vector& v, unsigned int /*version*/) {
+  const size_t size = v.size();
+  ar << BOOST_SERIALIZATION_NVP(size);
+  ar << make_nvp("data", make_array(v.data(), v.size()));
+}
+
+template <class Archive>
+void load(Archive& ar, gtsam::Vector& v, unsigned int /*version*/) {
+  size_t size;
+  ar >> BOOST_SERIALIZATION_NVP(size);
+  v.resize(size);
+  ar >> make_nvp("data", make_array(v.data(), v.size()));
+}
+
+// split version - copies into an STL vector for serialization
+template <class Archive, int D>
+void save(Archive& ar, const Eigen::Matrix<double, D, 1>& v,
+          unsigned int /*version*/) {
+  ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
+}
+
+template <class Archive, int D>
+void load(Archive& ar, Eigen::Matrix<double, D, 1>& v,
+          unsigned int /*version*/) {
+  ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
+BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)

gtsam/base/VerticalBlockMatrix.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/FastVector.h>
 
 namespace gtsam {

gtsam/base/base.i

@@ -82,6 +82,7 @@ class IndexPairSetMap {
 };
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/MatrixSerialization.h>
 bool linear_independent(Matrix A, Matrix B, double tol);
 
 #include <gtsam/base/Value.h>

gtsam/base/cholesky.h

@@ -18,7 +18,6 @@
 #pragma once
 
 #include <gtsam/base/Matrix.h>
-#include <boost/shared_ptr.hpp>
 
 namespace gtsam {
 

gtsam/base/serializationTestHelpers.h

@@ -42,7 +42,7 @@ T create() {
 }
 
 // Creates or empties a folder in the build folder and returns the relative path
-boost::filesystem::path resetFilesystem(
+inline boost::filesystem::path resetFilesystem(
     boost::filesystem::path folder = "actual") {
   boost::filesystem::remove_all(folder);
   boost::filesystem::create_directory(folder);

gtsam/base/tests/testSerializationBase.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/FastList.h>
 #include <gtsam/base/FastMap.h>

gtsam/basis/Basis.h

@@ -92,7 +92,7 @@ Matrix kroneckerProductIdentity(const Weights& w) {
 
 /// CRTP Base class for function bases
 template <typename DERIVED>
-class GTSAM_EXPORT Basis {
+class Basis {
  public:
   /**
    * Calculate weights for all x in vector X.
@@ -497,11 +497,6 @@ class GTSAM_EXPORT Basis {
     }
   };
 
-  // Vector version for MATLAB :-(
-  static double Derivative(double x, const Vector& p,  //
-                           OptionalJacobian</*1xN*/ -1, -1> H = boost::none) {
-    return DerivativeFunctor(x)(p.transpose(), H);
-  }
 };
 
 }  // namespace gtsam

gtsam/basis/BasisFactors.h

@@ -29,9 +29,12 @@ namespace gtsam {
  * pseudo-spectral parameterization.
  *
  * @tparam BASIS The basis class to use e.g. Chebyshev2
+ *
+ * Example, degree 8 Chebyshev polynomial measured at x=0.5:
+ *  EvaluationFactor<Chebyshev2> factor(key, measured, model, 8, 0.5);
  */
 template <class BASIS>
-class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
+class EvaluationFactor : public FunctorizedFactor<double, Vector> {
  private:
   using Base = FunctorizedFactor<double, Vector>;
 
@@ -47,7 +50,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
    * @param N The degree of the polynomial.
    * @param x The point at which to evaluate the polynomial.
    */
-  EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model,
+  EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
                    const size_t N, double x)
       : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {}
 
@@ -62,7 +65,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
    * @param a Lower bound for the polynomial.
    * @param b Upper bound for the polynomial.
    */
-  EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model,
+  EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
                    const size_t N, double x, double a, double b)
       : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {}
 
@@ -85,7 +88,7 @@ class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor<double, Vector> {
  * @param M: Size of the evaluated state vector.
  */
 template <class BASIS, int M>
-class GTSAM_EXPORT VectorEvaluationFactor
+class VectorEvaluationFactor
     : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
  private:
   using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
@@ -148,7 +151,7 @@ class GTSAM_EXPORT VectorEvaluationFactor
  *  where N is the degree and i is the component index.
  */
 template <class BASIS, size_t P>
-class GTSAM_EXPORT VectorComponentFactor
+class VectorComponentFactor
     : public FunctorizedFactor<double, ParameterMatrix<P>> {
  private:
   using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
@@ -217,7 +220,7 @@ class GTSAM_EXPORT VectorComponentFactor
  * where `x` is the value (e.g. timestep) at which the rotation was evaluated.
  */
 template <class BASIS, typename T>
-class GTSAM_EXPORT ManifoldEvaluationFactor
+class ManifoldEvaluationFactor
     : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
  private:
   using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
@@ -269,7 +272,7 @@ class GTSAM_EXPORT ManifoldEvaluationFactor
  * @param BASIS: The basis class to use e.g. Chebyshev2
  */
 template <class BASIS>
-class GTSAM_EXPORT DerivativeFactor
+class DerivativeFactor
     : public FunctorizedFactor<double, typename BASIS::Parameters> {
  private:
   using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
@@ -318,7 +321,7 @@ class GTSAM_EXPORT DerivativeFactor
  * @param M: Size of the evaluated state vector derivative.
  */
 template <class BASIS, int M>
-class GTSAM_EXPORT VectorDerivativeFactor
+class VectorDerivativeFactor
     : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
  private:
   using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
@@ -371,7 +374,7 @@ class GTSAM_EXPORT VectorDerivativeFactor
  * @param P: Size of the control component derivative.
  */
 template <class BASIS, int P>
-class GTSAM_EXPORT ComponentDerivativeFactor
+class ComponentDerivativeFactor
     : public FunctorizedFactor<double, ParameterMatrix<P>> {
  private:
   using Base = FunctorizedFactor<double, ParameterMatrix<P>>;

gtsam/basis/Chebyshev.h

@@ -21,8 +21,6 @@
 #include <gtsam/base/Manifold.h>
 #include <gtsam/basis/Basis.h>
 
-#include <unsupported/Eigen/KroneckerProduct>
-
 namespace gtsam {
 
 /**
@@ -31,7 +29,7 @@ namespace gtsam {
  * These are typically denoted with the symbol T_n, where n is the degree.
  * The parameter N is the number of coefficients, i.e., N = n+1.
  */
-struct Chebyshev1Basis : Basis<Chebyshev1Basis> {
+struct GTSAM_EXPORT Chebyshev1Basis : Basis<Chebyshev1Basis> {
   using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
 
   Parameters parameters_;
@@ -79,7 +77,7 @@ struct Chebyshev1Basis : Basis<Chebyshev1Basis> {
  * functions. In this sense, they are like the sines and cosines of the Fourier
  * basis.
  */
-struct Chebyshev2Basis : Basis<Chebyshev2Basis> {
+struct GTSAM_EXPORT Chebyshev2Basis : Basis<Chebyshev2Basis> {
   using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
 
   /**

gtsam/basis/Chebyshev2.h

@@ -22,8 +22,7 @@
  *
  * This is different from Chebyshev.h since it leverage ideas from
  * pseudo-spectral optimization, i.e. we don't decompose into basis functions,
- * rather estimate function parameters that enforce function nodes at Chebyshev
+ * rather estimate function values at the Chebyshev points.
- * points.
  *
  * Please refer to Agrawal21icra for more details.
  *

gtsam/basis/Fourier.h

@@ -24,7 +24,7 @@
 namespace gtsam {
 
 /// Fourier basis
-class GTSAM_EXPORT FourierBasis : public Basis<FourierBasis> {
+class FourierBasis : public Basis<FourierBasis> {
  public:
   using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
   using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;

gtsam/basis/basis.i

@@ -44,9 +44,6 @@ class Chebyshev2 {
   static Matrix DerivativeWeights(size_t N, double x, double a, double b);
   static Matrix IntegrationWeights(size_t N, double a, double b);
   static Matrix DifferentiationMatrix(size_t N, double a, double b);
-
-  // TODO Needs OptionalJacobian
-  // static double Derivative(double x, Vector f);
 };
 
 #include <gtsam/basis/ParameterMatrix.h>

gtsam/basis/tests/testBasisFactors.cpp

@@ -0,0 +1,230 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------1-------------------------------------------
+ */
+
+/**
+ * @file testBasisFactors.cpp
+ * @date May 31, 2020
+ * @author Varun Agrawal
+ * @brief unit tests for factors in BasisFactors.h
+ */
+
+#include <gtsam/basis/Basis.h>
+#include <gtsam/basis/BasisFactors.h>
+#include <gtsam/basis/Chebyshev2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/FunctorizedFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/Vector.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using gtsam::noiseModel::Isotropic;
+using gtsam::Pose2;
+using gtsam::Vector;
+using gtsam::Values;
+using gtsam::Chebyshev2;
+using gtsam::ParameterMatrix;
+using gtsam::LevenbergMarquardtParams;
+using gtsam::LevenbergMarquardtOptimizer;
+using gtsam::NonlinearFactorGraph;
+using gtsam::NonlinearOptimizerParams;
+
+constexpr size_t N = 2;
+
+// Key used in all tests
+const gtsam::Symbol key('X', 0);
+
+//******************************************************************************
+TEST(BasisFactors, EvaluationFactor) {
+  using gtsam::EvaluationFactor;
+
+  double measured = 0;
+
+  auto model = Isotropic::Sigma(1, 1.0);
+  EvaluationFactor<Chebyshev2> factor(key, measured, model, N, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(factor);
+
+  Vector functionValues(N);
+  functionValues.setZero();
+
+  Values initial;
+  initial.insert<Vector>(key, functionValues);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(BasisFactors, VectorEvaluationFactor) {
+  using gtsam::VectorEvaluationFactor;
+  const size_t M = 4;
+
+  const Vector measured = Vector::Zero(M);
+
+  auto model = Isotropic::Sigma(M, 1.0);
+  VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(factor);
+
+  ParameterMatrix<M> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(BasisFactors, Print) {
+  using gtsam::VectorEvaluationFactor;
+  const size_t M = 1;
+
+  const Vector measured = Vector::Ones(M) * 42;
+
+  auto model = Isotropic::Sigma(M, 1.0);
+  VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0);
+
+  std::string expected =
+      "  keys = { X0 }\n"
+      "  noise model: unit (1) \n"
+      "FunctorizedFactor(X0)\n"
+      "  measurement: [\n"
+      "	42\n"
+      "]\n"
+      "  noise model sigmas: 1\n";
+
+  EXPECT(assert_print_equal(expected, factor));
+}
+
+//******************************************************************************
+TEST(BasisFactors, VectorComponentFactor) {
+  using gtsam::VectorComponentFactor;
+  const int P = 4;
+  const size_t i = 2;
+  const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0;
+  auto model = Isotropic::Sigma(1, 1.0);
+  VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i,
+                                                    t, a, b);
+
+  NonlinearFactorGraph graph;
+  graph.add(factor);
+
+  ParameterMatrix<P> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<P>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(BasisFactors, ManifoldEvaluationFactor) {
+  using gtsam::ManifoldEvaluationFactor;
+  const Pose2 measured;
+  const double t = 3.0, a = 2.0, b = 4.0;
+  auto model = Isotropic::Sigma(3, 1.0);
+  ManifoldEvaluationFactor<Chebyshev2, Pose2> factor(key, measured, model, N,
+                                                     t, a, b);
+
+  NonlinearFactorGraph graph;
+  graph.add(factor);
+
+  ParameterMatrix<3> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<3>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(BasisFactors, VecDerivativePrior) {
+  using gtsam::VectorDerivativeFactor;
+  const size_t M = 4;
+
+  const Vector measured = Vector::Zero(M);
+  auto model = Isotropic::Sigma(M, 1.0);
+  VectorDerivativeFactor<Chebyshev2, M> vecDPrior(key, measured, model, N, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(vecDPrior);
+
+  ParameterMatrix<M> stateMatrix(N);
+
+  Values initial;
+  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+//******************************************************************************
+TEST(BasisFactors, ComponentDerivativeFactor) {
+  using gtsam::ComponentDerivativeFactor;
+  const size_t M = 4;
+
+  double measured = 0;
+  auto model = Isotropic::Sigma(1, 1.0);
+  ComponentDerivativeFactor<Chebyshev2, M> controlDPrior(key, measured, model,
+                                                         N, 0, 0);
+
+  NonlinearFactorGraph graph;
+  graph.add(controlDPrior);
+
+  Values initial;
+  ParameterMatrix<M> stateMatrix(N);
+  initial.insert<ParameterMatrix<M>>(key, stateMatrix);
+
+  LevenbergMarquardtParams parameters;
+  parameters.setMaxIterations(20);
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
+
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/basis/tests/testChebyshev.cpp

@@ -25,9 +25,10 @@
 using namespace std;
 using namespace gtsam;
 
+namespace {
 auto model = noiseModel::Unit::Create(1);
-
 const size_t N = 3;
+}  // namespace
 
 //******************************************************************************
 TEST(Chebyshev, Chebyshev1) {

gtsam/basis/tests/testChebyshev2.cpp

@@ -10,26 +10,30 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file testChebyshev.cpp
+ * @file testChebyshev2.cpp
  * @date July 4, 2020
  * @author Varun Agrawal
  * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral
  *        methods
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
 #include <gtsam/basis/Chebyshev2.h>
 #include <gtsam/basis/FitBasis.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/base/Testable.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
 using namespace boost::placeholders;
 
+namespace {
 noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
 
 const size_t N = 32;
+}  // namespace
 
 //******************************************************************************
 TEST(Chebyshev2, Point) {
@@ -121,12 +125,30 @@ TEST(Chebyshev2, InterpolateVector) {
   EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
+//******************************************************************************
+// Interpolating poses using the exponential map
+TEST(Chebyshev2, InterpolatePose2) {
+  double t = 30, a = 0, b = 100;
+
+  ParameterMatrix<3> X(N);
+  X.row(0) = Chebyshev2::Points(N, a, b);  // slope 1 ramp
+  X.row(1) = Vector::Zero(N);
+  X.row(2) = 0.1 * Vector::Ones(N);
+
+  Vector xi(3);
+  xi << t, 0, 0.1;
+  Chebyshev2::ManifoldEvaluationFunctor<Pose2> fx(N, t, a, b);
+  // We use xi as canonical coordinates via exponential map
+  Pose2 expected = Pose2::ChartAtOrigin::Retract(xi);
+  EXPECT(assert_equal(expected, fx(X)));
+}
+
 //******************************************************************************
 TEST(Chebyshev2, Decomposition) {
   // Create example sequence
   Sequence sequence;
   for (size_t i = 0; i < 16; i++) {
-    double x = (double)i / 16. - 0.99, y = x;
+    double x = (1.0/ 16)*i - 0.99, y = x;
     sequence[x] = y;
   }
 
@@ -144,11 +166,11 @@ TEST(Chebyshev2, DifferentiationMatrix3) {
   // Trefethen00book, p.55
   const size_t N = 3;
   Matrix expected(N, N);
-  // Differentiation matrix computed from Chebfun
+  // Differentiation matrix computed from chebfun
   expected << 1.5000, -2.0000, 0.5000,  //
       0.5000, -0.0000, -0.5000,         //
       -0.5000, 2.0000, -1.5000;
-  // multiply by -1 since the cheb points have a phase shift wrt Trefethen
+  // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen
   // This was verified with chebfun
   expected = -expected;
 
@@ -167,7 +189,7 @@ TEST(Chebyshev2, DerivativeMatrix6) {
       0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236,            //
       -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180,           //
       0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000;
-  // multiply by -1 since the cheb points have a phase shift wrt Trefethen
+  // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen
   // This was verified with chebfun
   expected = -expected;
 
@@ -252,7 +274,7 @@ TEST(Chebyshev2, DerivativeWeights2) {
   Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b);
   EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8);
 
-  // test if derivative calculation and cheb point is correct
+  // test if derivative calculation and Chebyshev point is correct
   double x3 = Chebyshev2::Point(N, 3, a, b);
   Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b);
   EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8);

gtsam/discrete/AlgebraicDecisionTree.cpp

@@ -0,0 +1,28 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file AlgebraicDecisionTree.cpp
+ *  @date Feb 20, 2022
+ *  @author Mike Sheffler
+ *  @author Duy-Nguyen Ta
+ *  @author Frank Dellaert
+ */
+
+#include "AlgebraicDecisionTree.h"
+
+#include <gtsam/base/types.h>
+
+namespace gtsam {
+
+  template class AlgebraicDecisionTree<Key>;
+
+}  // namespace gtsam

gtsam/discrete/AlgebraicDecisionTree.h

@@ -127,7 +127,7 @@ namespace gtsam {
         return map.at(label);
       };
       std::function<double(const double&)> op = Ring::id;
-      this->root_ = this->template convertFrom(other.root_, L_of_M, op);
+      this->root_ = DecisionTree<L, double>::convertFrom(other.root_, L_of_M, op);
     }
 
     /** sum */

gtsam/discrete/Assignment.h

@@ -33,6 +33,8 @@ namespace gtsam {
 template <class L>
 class Assignment : public std::map<L, size_t> {
  public:
+  using std::map<L, size_t>::operator=;
+
   void print(const std::string& s = "Assignment: ") const {
     std::cout << s << ": ";
     for (const typename Assignment::value_type& keyValue : *this)

gtsam/discrete/DecisionTree-inl.h

@@ -112,6 +112,13 @@ namespace gtsam {
       return f;
     }
 
+    /// Apply unary operator with assignment
+    NodePtr apply(const UnaryAssignment& op,
+                  const Assignment<L>& choices) const override {
+      NodePtr f(new Leaf(op(choices, constant_)));
+      return f;
+    }
+
     // Apply binary operator "h = f op g" on Leaf node
     // Note op is not assumed commutative so we need to keep track of order
     // Simply calls apply on argument to call correct virtual method:
@@ -322,12 +329,48 @@ namespace gtsam {
       for (const NodePtr& branch : f.branches_) push_back(branch->apply(op));
     }
 
+    /**
+     * @brief Constructor which accepts a UnaryAssignment op and the
+     * corresponding assignment.
+     *
+     * @param label The label for this node.
+     * @param f The original choice node to apply the op on.
+     * @param op Function to apply on the choice node. Takes Assignment and
+     * value as arguments.
+     * @param choices The Assignment that will go to op.
+     */
+    Choice(const L& label, const Choice& f, const UnaryAssignment& op,
+           const Assignment<L>& choices)
+        : label_(label), allSame_(true) {
+      branches_.reserve(f.branches_.size());  // reserve space
+
+      Assignment<L> choices_ = choices;
+
+      for (size_t i = 0; i < f.branches_.size(); i++) {
+        choices_[label_] = i;  // Set assignment for label to i
+
+        const NodePtr branch = f.branches_[i];
+        push_back(branch->apply(op, choices_));
+
+        // Remove the choice so we are backtracking
+        auto choice_it = choices_.find(label_);
+        choices_.erase(choice_it);
+      }
+    }
+
     /** apply unary operator */
     NodePtr apply(const Unary& op) const override {
       auto r = boost::make_shared<Choice>(label_, *this, op);
       return Unique(r);
     }
 
+    /// Apply unary operator with assignment
+    NodePtr apply(const UnaryAssignment& op,
+                  const Assignment<L>& choices) const override {
+      auto r = boost::make_shared<Choice>(label_, *this, op, choices);
+      return Unique(r);
+    }
+
     // Apply binary operator "h = f op g" on Choice node
     // Note op is not assumed commutative so we need to keep track of order
     // Simply calls apply on argument to call correct virtual method:
@@ -592,11 +635,13 @@ namespace gtsam {
       std::function<Y(const X&)> Y_of_X) const {
     using LY = DecisionTree<L, Y>;
 
-    // ugliness below because apparently we can't have templated virtual
+    // Ugliness below because apparently we can't have templated virtual
-    // functions If leaf, apply unary conversion "op" and create a unique leaf
+    // functions.
+    // If leaf, apply unary conversion "op" and create a unique leaf.
     using MXLeaf = typename DecisionTree<M, X>::Leaf;
-    if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f))
+    if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f)) {
       return NodePtr(new Leaf(Y_of_X(leaf->constant())));
+    }
 
     // Check if Choice
     using MXChoice = typename DecisionTree<M, X>::Choice;
@@ -666,8 +711,13 @@ namespace gtsam {
       if (!choice)
         throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
       for (size_t i = 0; i < choice->nrChoices(); i++) {
-        choices[choice->label()] = i;    // Set assignment for label to i
+        choices[choice->label()] = i;  // Set assignment for label to i
+
         (*this)(choice->branches()[i]);  // recurse!
+
+        // Remove the choice so we are backtracking
+        auto choice_it = choices.find(choice->label());
+        choices.erase(choice_it);
       }
     }
   };
@@ -679,6 +729,14 @@ namespace gtsam {
     visit(root_);
   }
 
+  /****************************************************************************/
+  template <typename L, typename Y>
+  size_t DecisionTree<L, Y>::nrLeaves() const {
+    size_t total = 0;
+    visit([&total](const Y& node) { total += 1; });
+    return total;
+  }
+
   /****************************************************************************/
   // fold is just done with a visit
   template <typename L, typename Y>
@@ -734,6 +792,19 @@ namespace gtsam {
     return DecisionTree(root_->apply(op));
   }
 
+  /// Apply unary operator with assignment
+  template <typename L, typename Y>
+  DecisionTree<L, Y> DecisionTree<L, Y>::apply(
+      const UnaryAssignment& op) const {
+    // It is unclear what should happen if tree is empty:
+    if (empty()) {
+      throw std::runtime_error(
+          "DecisionTree::apply(unary op) undefined for empty tree.");
+    }
+    Assignment<L> choices;
+    return DecisionTree(root_->apply(op, choices));
+  }
+
   /****************************************************************************/
   template<typename L, typename Y>
   DecisionTree<L, Y> DecisionTree<L, Y>::apply(const DecisionTree& g,

gtsam/discrete/DecisionTree.h

@@ -54,6 +54,7 @@ namespace gtsam {
 
     /** Handy typedefs for unary and binary function types */
     using Unary = std::function<Y(const Y&)>;
+    using UnaryAssignment = std::function<Y(const Assignment<L>&, const Y&)>;
     using Binary = std::function<Y(const Y&, const Y&)>;
 
     /** A label annotated with cardinality */
@@ -103,6 +104,8 @@ namespace gtsam {
                                                  &DefaultCompare) const = 0;
       virtual const Y& operator()(const Assignment<L>& x) const = 0;
       virtual Ptr apply(const Unary& op) const = 0;
+      virtual Ptr apply(const UnaryAssignment& op,
+                        const Assignment<L>& choices) const = 0;
       virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0;
       virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0;
       virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0;
@@ -259,6 +262,9 @@ namespace gtsam {
     template <typename Func>
     void visitWith(Func f) const;
 
+    /// Return the number of leaves in the tree.
+    size_t nrLeaves() const;
+
     /**
      * @brief Fold a binary function over the tree, returning accumulator.
      *
@@ -283,6 +289,16 @@ namespace gtsam {
     /** apply Unary operation "op" to f */
     DecisionTree apply(const Unary& op) const;
 
+    /**
+     * @brief Apply Unary operation "op" to f while also providing the
+     * corresponding assignment.
+     *
+     * @param op Function which takes Assignment<L> and Y as input and returns
+     * object of type Y.
+     * @return DecisionTree
+     */
+    DecisionTree apply(const UnaryAssignment& op) const;
+
     /** apply binary operation "op" to f and g */
     DecisionTree apply(const DecisionTree& g, const Binary& op) const;
 
@@ -337,6 +353,13 @@ namespace gtsam {
     return f.apply(op);
   }
 
+  /// Apply unary operator `op` with Assignment to DecisionTree `f`.
+  template<typename L, typename Y>
+  DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
+      const typename DecisionTree<L, Y>::UnaryAssignment& op) {
+    return f.apply(op);
+  }
+
   /// Apply binary operator `op` to DecisionTree `f`.
   template<typename L, typename Y>
   DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,

gtsam/discrete/DecisionTreeFactor.cpp

@@ -156,10 +156,7 @@ namespace gtsam {
   std::vector<std::pair<DiscreteValues, double>> DecisionTreeFactor::enumerate()
       const {
     // Get all possible assignments
-    std::vector<std::pair<Key, size_t>> pairs;
+    std::vector<std::pair<Key, size_t>> pairs = discreteKeys();
-    for (auto& key : keys()) {
-      pairs.emplace_back(key, cardinalities_.at(key));
-    }
     // Reverse to make cartesian product output a more natural ordering.
     std::vector<std::pair<Key, size_t>> rpairs(pairs.rbegin(), pairs.rend());
     const auto assignments = DiscreteValues::CartesianProduct(rpairs);

gtsam/discrete/DiscreteJunctionTree.h

@@ -16,6 +16,8 @@
  * @author Richard Roberts
  */
 
+#pragma once
+
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteBayesTree.h>
 #include <gtsam/inference/JunctionTree.h>

gtsam/discrete/DiscreteKey.h

@@ -72,5 +72,5 @@ namespace gtsam {
   }; // DiscreteKeys
 
   /// Create a list from two keys
-  DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
+  GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
 }

gtsam/discrete/DiscreteLookupDAG.h

@@ -36,7 +36,7 @@ class DiscreteBayesNet;
  * Inherits from discrete conditional for convenience, but is not normalized.
  * Is used in the max-product algorithm.
  */
-class DiscreteLookupTable : public DiscreteConditional {
+class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
  public:
   using This = DiscreteLookupTable;
   using shared_ptr = boost::shared_ptr<This>;
@@ -46,7 +46,7 @@ class DiscreteLookupTable : public DiscreteConditional {
    * @brief Construct a new Discrete Lookup Table object
    *
    * @param nFrontals number of frontal variables
-   * @param keys a orted list of gtsam::Keys
+   * @param keys a sorted list of gtsam::Keys
    * @param potentials the algebraic decision tree with lookup values
    */
   DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,

gtsam/discrete/DiscreteMarginals.h

@@ -29,7 +29,7 @@ namespace gtsam {
   /**
    * A class for computing marginals of variables in a DiscreteFactorGraph
    */
-class GTSAM_EXPORT DiscreteMarginals {
+class DiscreteMarginals {
 
   protected:
 

gtsam/discrete/DiscreteValues.h

@@ -37,7 +37,7 @@ namespace gtsam {
  * stores cardinality of a Discrete variable. It should be handled naturally in
  * the new class DiscreteValue, as the variable's type (domain)
  */
-class DiscreteValues : public Assignment<Key> {
+class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
  public:
   using Base = Assignment<Key>;  // base class
 

gtsam/discrete/tests/testAlgebraicDecisionTree.cpp

@@ -318,7 +318,7 @@ TEST(ADT, factor_graph) {
   dot(fg, "Marginalized-3E");
   fg = fg.combine(L, &add_);
   dot(fg, "Marginalized-2L");
-  EXPECT(adds = 54);
+  LONGS_EQUAL(49, adds);
   gttoc_(marg);
   tictoc_getNode(margNode, marg);
   elapsed = margNode->secs() + margNode->wall();

gtsam/discrete/tests/testDecisionTree.cpp

@@ -17,17 +17,19 @@
  * @date    Jan 30, 2012
  */
 
-#include <boost/assign/std/vector.hpp>
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/discrete/Signature.h>
-
 // #define DT_DEBUG_MEMORY
 // #define DT_NO_PRUNING
 #define DISABLE_DOT
 #include <gtsam/discrete/DecisionTree-inl.h>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/discrete/Signature.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/vector.hpp>
+using namespace boost::assign;
+
 using namespace std;
 using namespace gtsam;
 
@@ -88,6 +90,7 @@ struct DT : public DecisionTree<string, int> {
     auto valueFormatter = [](const int& v) {
       return (boost::format("%d") % v).str();
     };
+    std::cout << s;
     Base::print("", keyFormatter, valueFormatter);
   }
   /// Equality method customized to int node type
@@ -148,9 +151,9 @@ TEST(DecisionTree, example) {
   DOT(notb);
 
   // Check supplying empty trees yields an exception
-  CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error);
+  CHECK_EXCEPTION(gtsam::apply(empty, &Ring::id), std::runtime_error);
-  CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error);
+  CHECK_EXCEPTION(gtsam::apply(empty, a, &Ring::mul), std::runtime_error);
-  CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error);
+  CHECK_EXCEPTION(gtsam::apply(a, empty, &Ring::mul), std::runtime_error);
 
   // apply, two nodes, in natural order
   DT anotb = apply(a, notb, &Ring::mul);
@@ -344,6 +347,44 @@ TEST(DecisionTree, visitWith) {
   EXPECT_DOUBLES_EQUAL(6.0, sum, 1e-9);
 }
 
+/* ************************************************************************** */
+// Test visit, with Choices argument.
+TEST(DecisionTree, VisitWithPruned) {
+  // Create pruned tree
+  std::pair<string, size_t> A("A", 2), B("B", 2), C("C", 2);
+  std::vector<std::pair<string, size_t>> labels = {C, B, A};
+  std::vector<int> nodes = {0, 0, 2, 3, 4, 4, 6, 7};
+  DT tree(labels, nodes);
+
+  std::vector<Assignment<string>> choices;
+  auto func = [&](const Assignment<string>& choice, const int& d) {
+    choices.push_back(choice);
+  };
+  tree.visitWith(func);
+
+  EXPECT_LONGS_EQUAL(6, choices.size());
+
+  Assignment<string> expectedAssignment;
+
+  expectedAssignment = {{"B", 0}, {"C", 0}};
+  EXPECT(expectedAssignment == choices.at(0));
+
+  expectedAssignment = {{"A", 0}, {"B", 1}, {"C", 0}};
+  EXPECT(expectedAssignment == choices.at(1));
+
+  expectedAssignment = {{"A", 1}, {"B", 1}, {"C", 0}};
+  EXPECT(expectedAssignment == choices.at(2));
+
+  expectedAssignment = {{"B", 0}, {"C", 1}};
+  EXPECT(expectedAssignment == choices.at(3));
+
+  expectedAssignment = {{"A", 0}, {"B", 1}, {"C", 1}};
+  EXPECT(expectedAssignment == choices.at(4));
+
+  expectedAssignment = {{"A", 1}, {"B", 1}, {"C", 1}};
+  EXPECT(expectedAssignment == choices.at(5));
+}
+
 /* ************************************************************************** */
 // Test fold.
 TEST(DecisionTree, fold) {
@@ -411,6 +452,43 @@ TEST(DecisionTree, threshold) {
   EXPECT_LONGS_EQUAL(2, thresholded.fold(count, 0));
 }
 
+/* ************************************************************************** */
+// Test apply with assignment.
+TEST(DecisionTree, ApplyWithAssignment) {
+  // Create three level tree
+  vector<DT::LabelC> keys;
+  keys += DT::LabelC("C", 2), DT::LabelC("B", 2), DT::LabelC("A", 2);
+  DT tree(keys, "1 2 3 4 5 6 7 8");
+
+  DecisionTree<string, double> probTree(
+      keys, "0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08");
+  double threshold = 0.045;
+
+  // We test pruning one tree by indexing into another.
+  auto pruner = [&](const Assignment<string>& choices, const int& x) {
+    // Prune out all the leaves with even numbers
+    if (probTree(choices) < threshold) {
+      return 0;
+    } else {
+      return x;
+    }
+  };
+  DT prunedTree = tree.apply(pruner);
+
+  DT expectedTree(keys, "0 0 0 0 5 6 7 8");
+  EXPECT(assert_equal(expectedTree, prunedTree));
+
+  size_t count = 0;
+  auto counter = [&](const Assignment<string>& choices, const int& x) {
+    count += 1;
+    return x;
+  };
+  DT prunedTree2 = prunedTree.apply(counter);
+
+  // Check if apply doesn't enumerate all leaves.
+  EXPECT_LONGS_EQUAL(5, count);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/discrete/tests/testDecisionTreeFactor.cpp

@@ -107,7 +107,7 @@ TEST(DecisionTreeFactor, enumerate) {
 }
 
 /* ************************************************************************* */
-TEST(DiscreteFactorGraph, DotWithNames) {
+TEST(DecisionTreeFactor, DotWithNames) {
   DiscreteKey A(12, 3), B(5, 2);
   DecisionTreeFactor f(A & B, "1 2  3 4  5 6");
   auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };

gtsam/geometry/BearingRange.h

@@ -22,6 +22,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/OptionalJacobian.h>
 #include <boost/concept/assert.hpp>
+#include <boost/serialization/nvp.hpp>
 #include <iostream>
 
 namespace gtsam {

gtsam/geometry/Cal3DS2.h

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <gtsam/geometry/Cal3DS2_Base.h>
+#include <boost/shared_ptr.hpp>
 
 namespace gtsam {
 

gtsam/geometry/Cal3DS2_Base.h

@@ -21,6 +21,7 @@
 
 #include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
+#include <boost/shared_ptr.hpp>
 
 namespace gtsam {
 

gtsam/geometry/Cal3Fisheye.h

@@ -22,6 +22,8 @@
 #include <gtsam/geometry/Cal3.h>
 #include <gtsam/geometry/Point2.h>
 
+#include <boost/shared_ptr.hpp>
+
 #include <string>
 
 namespace gtsam {

gtsam/geometry/Cyclic.h

@@ -15,6 +15,8 @@
  * @author Frank Dellaert
  **/
 
+#pragma once
+
 #include <gtsam/base/Group.h>
 #include <gtsam/base/Testable.h>
 

gtsam/geometry/Line3.cpp

@@ -117,4 +117,4 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
   return Line3(cRl, c_ab[0], c_ab[1]);
 }
 
-}
+}  // namespace gtsam

gtsam/geometry/Line3.h

@@ -21,12 +21,27 @@
 
 namespace gtsam {
 
+class Line3;
+
+/**
+ * Transform a line from world to camera frame
+ * @param wTc - Pose3 of camera in world frame
+ * @param wL - Line3 in world frame
+ * @param Dpose - OptionalJacobian of transformed line with respect to p
+ * @param Dline -  OptionalJacobian of transformed line with respect to l
+ * @return Transformed line in camera frame
+ */
+GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
+                  OptionalJacobian<4, 6> Dpose = boost::none,
+                  OptionalJacobian<4, 4> Dline = boost::none);
+
+
 /**
  * A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
  * @addtogroup geometry
  * \nosubgrouping
  */
-class Line3 {
+class GTSAM_EXPORT Line3 {
  private:
   Rot3 R_;    // Rotation of line about x and y in world frame
   double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
@@ -136,18 +151,6 @@ class Line3 {
                            OptionalJacobian<4, 4> Dline);
 };
 
-/**
- * Transform a line from world to camera frame
- * @param wTc - Pose3 of camera in world frame
- * @param wL - Line3 in world frame
- * @param Dpose - OptionalJacobian of transformed line with respect to p
- * @param Dline -  OptionalJacobian of transformed line with respect to l
- * @return Transformed line in camera frame
- */
-Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
-                  OptionalJacobian<4, 6> Dpose = boost::none,
-                  OptionalJacobian<4, 4> Dline = boost::none);
-
 template<>
 struct traits<Line3> : public internal::Manifold<Line3> {};
 

gtsam/geometry/PinholeCamera.h

@@ -30,7 +30,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename Calibration>
-class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
+class PinholeCamera: public PinholeBaseK<Calibration> {
 
 public:
 
@@ -230,13 +230,15 @@ public:
   Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
       OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
     // We just call 3-derivative version in Base
-    Matrix26 Dpose;
+    if (Dcamera){
-    Eigen::Matrix<double, 2, DimK> Dcal;
+      Matrix26 Dpose;
-    Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
+      Eigen::Matrix<double, 2, DimK> Dcal;
-        Dcamera ? &Dcal : 0);
+      const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
-    if (Dcamera)
       *Dcamera << Dpose, Dcal;
-    return pi;
+      return pi;
+    } else {
+      return Base::project(pw, boost::none, Dpoint, boost::none);
+    }
   }
 
   /// project a 3D point from world coordinates into the image

gtsam/geometry/PinholePose.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename CALIBRATION>
-class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
+class PinholeBaseK: public PinholeBase {
 
 private:
 

gtsam/geometry/Point3.cpp

@@ -17,6 +17,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <cmath>
 #include <iostream>
+#include <vector>
 
 using namespace std;
 

gtsam/geometry/Pose2.cpp

@@ -213,6 +213,14 @@ Point2 Pose2::transformTo(const Point2& point,
   return q;
 }
 
+Matrix Pose2::transformTo(const Matrix& points) const {
+  if (points.rows() != 2) {
+    throw std::invalid_argument("Pose2:transformTo expects 2*N matrix.");
+  }
+  const Matrix2 Rt = rotation().transpose();
+  return Rt * (points.colwise() - t_);  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transformFrom(const Point2& point,
@@ -224,6 +232,15 @@ Point2 Pose2::transformFrom(const Point2& point,
   return q + t_;
 }
 
+
+Matrix Pose2::transformFrom(const Matrix& points) const {
+  if (points.rows() != 2) {
+    throw std::invalid_argument("Pose2:transformFrom expects 2*N matrix.");
+  }
+  const Matrix2 R = rotation().matrix();
+  return (R * points).colwise() + t_;  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
     OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const {

gtsam/geometry/Pose2.h

@@ -199,13 +199,29 @@ public:
       OptionalJacobian<2, 3> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in world coordinates and transform to Pose.
+   * @param points 2*N matrix in world coordinates
+   * @return points in Pose coordinates, as 2*N Matrix
+   */
+  Matrix transformTo(const Matrix& points) const;
+
   /** Return point coordinates in global frame */
   GTSAM_EXPORT Point2 transformFrom(const Point2& point,
       OptionalJacobian<2, 3> Dpose = boost::none,
       OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in Pose coordinates and transform to world.
+   * @param points 2*N matrix in Pose coordinates
+   * @return points in world coordinates, as 2*N Matrix
+   */
+  Matrix transformFrom(const Matrix& points) const;
+
   /** syntactic sugar for transformFrom */
-  inline Point2 operator*(const Point2& point) const { return transformFrom(point);}
+  inline Point2 operator*(const Point2& point) const { 
+    return transformFrom(point);
+  }
 
   /// @}
   /// @name Standard Interface

gtsam/geometry/Pose3.cpp

@@ -59,7 +59,7 @@ Matrix6 Pose3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
   Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
   Matrix6 adj;
-  adj << R, Z_3x3, A, R;
+  adj << R, Z_3x3, A, R;  // Gives [R 0; A R]
   return adj;
 }
 
@@ -354,6 +354,14 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
   return R_ * point + t_;
 }
 
+Matrix Pose3::transformFrom(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
+  }
+  const Matrix3 R = R_.matrix();
+  return (R * points).colwise() + t_;  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
     OptionalJacobian<3, 3> Hpoint) const {
@@ -374,6 +382,14 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
   return q;
 }
 
+Matrix Pose3::transformTo(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
+  }
+  const Matrix3 Rt = R_.transpose();
+  return Rt * (points.colwise() - t_);  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
                     OptionalJacobian<1, 3> Hpoint) const {
@@ -431,7 +447,7 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
 boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   const size_t n = abPointPairs.size();
   if (n < 3) {
-    return boost::none; // we need at least three pairs
+    return boost::none;  // we need at least three pairs
   }
 
   // calculate centroids
@@ -451,6 +467,18 @@ boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   return Pose3(aRb, aTb);
 }
 
+boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
+  if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
+    throw std::invalid_argument(
+      "Pose3:Align expects 3*N matrices of equal shape.");
+  }
+  Point3Pairs abPointPairs;
+  for (size_t j=0; j < a.cols(); j++) {
+    abPointPairs.emplace_back(a.col(j), b.col(j));
+  }
+  return Pose3::Align(abPointPairs);
+}
+
 boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
   Point3Pairs abPointPairs;
   for (const Point3Pair &baPair : baPointPairs) {

gtsam/geometry/Pose3.h

@@ -85,6 +85,9 @@ public:
    */
   static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
 
+  // Version of Pose3::Align that takes 2 matrices.
+  static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
+
   /// @}
   /// @name Testable
   /// @{
@@ -249,6 +252,13 @@ public:
   Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
       boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in Pose coordinates and transform to world.
+   * @param points 3*N matrix in Pose coordinates
+   * @return points in world coordinates, as 3*N Matrix
+   */
+  Matrix transformFrom(const Matrix& points) const;
+
   /** syntactic sugar for transformFrom */
   inline Point3 operator*(const Point3& point) const {
     return transformFrom(point);
@@ -264,6 +274,13 @@ public:
   Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
       boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in world coordinates and transform to Pose.
+   * @param points 3*N matrix in world coordinates
+   * @return points in Pose coordinates, as 3*N Matrix
+   */
+  Matrix transformTo(const Matrix& points) const;
+
   /// @}
   /// @name Standard Interface
   /// @{

gtsam/geometry/SOn.cpp

@@ -22,7 +22,7 @@
 namespace gtsam {
 
 template <>
-GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
+void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
   size_t n = AmbientDim(xi.size());
   if (n < 2)
     throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
@@ -48,7 +48,7 @@ GTSAM_EXPORT void SOn::Hat(const Vector &xi, Eigen::Ref<Matrix> X) {
   }
 }
 
-template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) {
+template <> Matrix SOn::Hat(const Vector &xi) {
   size_t n = AmbientDim(xi.size());
   Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
   SOn::Hat(xi, X);
@@ -56,7 +56,6 @@ template <> GTSAM_EXPORT Matrix SOn::Hat(const Vector &xi) {
 }
 
 template <>
-GTSAM_EXPORT
 Vector SOn::Vee(const Matrix& X) {
   const size_t n = X.rows();
   if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
@@ -104,7 +103,9 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
 }
 
 // Dynamic version of vec
-template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const {
+template <>
+typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const
+{
   const size_t n = rows(), n2 = n * n;
 
   // Vectorize

gtsam/geometry/SOn.h

@@ -24,6 +24,8 @@
 #include <gtsam/dllexport.h>
 #include <Eigen/Core>
 
+#include <boost/serialization/nvp.hpp>
+
 #include <iostream> // TODO(frank): how to avoid?
 #include <string>
 #include <type_traits>
@@ -356,17 +358,21 @@ Vector SOn::Vee(const Matrix& X);
 using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
 
 template <>
+GTSAM_EXPORT
 SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
                                            DynamicJacobian H2) const;
 
 template <>
+GTSAM_EXPORT
 SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
                                            DynamicJacobian H2) const;
 
 /*
  * Specialize dynamic vec.
  */
-template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
+template <> 
+GTSAM_EXPORT
+typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
 
 /** Serialization function */
 template<class Archive>

gtsam/geometry/Unit3.h

@@ -23,11 +23,12 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/VectorSerialization.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
 #include <boost/optional.hpp>
-#include <boost/serialization/nvp.hpp>
 
 #include <random>
 #include <string>
@@ -39,7 +40,7 @@
 namespace gtsam {
 
 /// Represents a 3D point on a unit sphere.
-class Unit3 {
+class GTSAM_EXPORT Unit3 {
 
 private:
 
@@ -96,7 +97,7 @@ public:
   }
 
   /// Named constructor from Point3 with optional Jacobian
-  GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, //
+  static Unit3 FromPoint3(const Point3& point, //
       OptionalJacobian<2, 3> H = boost::none);
 
   /**
@@ -105,7 +106,7 @@ public:
    *   std::mt19937 engine(42);
    *   Unit3 unit = Unit3::Random(engine);
    */
-  GTSAM_EXPORT static Unit3 Random(std::mt19937 & rng);
+  static Unit3 Random(std::mt19937 & rng);
 
   /// @}
 
@@ -115,7 +116,7 @@ public:
   friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
 
   /// The print fuction
-  GTSAM_EXPORT void print(const std::string& s = std::string()) const;
+  void print(const std::string& s = std::string()) const;
 
   /// The equals function with tolerance
   bool equals(const Unit3& s, double tol = 1e-9) const {
@@ -132,16 +133,16 @@ public:
    * tangent to the sphere at the current direction.
    * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
    */
-  GTSAM_EXPORT const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
+  const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
-  GTSAM_EXPORT Matrix3 skew() const;
+  Matrix3 skew() const;
 
   /// Return unit-norm Point3
-  GTSAM_EXPORT Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
+  Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
 
   /// Return unit-norm Vector
-  GTSAM_EXPORT Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
+  Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
 
   /// Return scaled direction as Point3
   friend Point3 operator*(double s, const Unit3& d) {
@@ -149,20 +150,20 @@ public:
   }
 
   /// Return dot product with q
-  GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
+  double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
                              OptionalJacobian<1,2> H2 = boost::none) const;
 
   /// Signed, vector-valued error between two directions
   /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
-  GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
+  Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
 
   /// Signed, vector-valued error between two directions
   /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
-  GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
+  Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
                       OptionalJacobian<2, 2> H_q = boost::none) const;
 
   /// Distance between two directions
-  GTSAM_EXPORT double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
+  double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
 
   /// Cross-product between two Unit3s
   Unit3 cross(const Unit3& q) const {
@@ -195,10 +196,10 @@ public:
   };
 
   /// The retract function
-  GTSAM_EXPORT Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
+  Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
 
   /// The local coordinates function
-  GTSAM_EXPORT Vector2 localCoordinates(const Unit3& s) const;
+  Vector2 localCoordinates(const Unit3& s) const;
 
   /// @}
 

gtsam/geometry/geometry.i

@@ -406,6 +406,10 @@ class Pose2 {
   gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
   gtsam::Point2 transformTo(const gtsam::Point2& p) const;
 
+  // Matrix versions
+  Matrix transformFrom(const Matrix& points) const;
+  Matrix transformTo(const Matrix& points) const;
+
   // Standard Interface
   double x() const;
   double y() const;
@@ -431,6 +435,9 @@ class Pose3 {
   Pose3(const gtsam::Pose2& pose2);
   Pose3(Matrix mat);
 
+  static boost::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
+  static boost::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::Pose3& pose, double tol) const;
@@ -470,6 +477,10 @@ class Pose3 {
   gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
   gtsam::Point3 transformTo(const gtsam::Point3& point) const;
 
+  // Matrix versions
+  Matrix transformFrom(const Matrix& points) const;
+  Matrix transformTo(const Matrix& points) const;
+
   // Standard Interface
   gtsam::Rot3 rotation() const;
   gtsam::Point3 translation() const;

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -160,7 +160,7 @@ TEST(Cal3Bundler, retract) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, Print) {
+TEST(Cal3Bundler, Print) {
   Cal3Bundler cal(1, 2, 3, 4, 5);
   std::stringstream os;
   os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()

gtsam/geometry/tests/testPose2.cpp

@@ -796,44 +796,39 @@ TEST(Pose2, align_4) {
 }
 
 //******************************************************************************
+namespace {
+Pose2 id;
 Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
 Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
+}  // namespace
 
 //******************************************************************************
-TEST(Pose2 , Invariants) {
+TEST(Pose2, Invariants) {
-  Pose2 id;
+  EXPECT(check_group_invariants(id, id));
-
+  EXPECT(check_group_invariants(id, T1));
-  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(T2, id));
-  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2, T1));
-  EXPECT(check_group_invariants(T2,id));
-  EXPECT(check_group_invariants(T2,T1));
-
-  EXPECT(check_manifold_invariants(id,id));
-  EXPECT(check_manifold_invariants(id,T1));
-  EXPECT(check_manifold_invariants(T2,id));
-  EXPECT(check_manifold_invariants(T2,T1));
 
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, T1));
+  EXPECT(check_manifold_invariants(T2, id));
+  EXPECT(check_manifold_invariants(T2, T1));
 }
 
 //******************************************************************************
-TEST(Pose2 , LieGroupDerivatives) {
+TEST(Pose2, LieGroupDerivatives) {
-  Pose2 id;
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
-
 }
 
 //******************************************************************************
-TEST(Pose2 , ChartDerivatives) {
+TEST(Pose2, ChartDerivatives) {
-  Pose2 id;
+  CHECK_CHART_DERIVATIVES(id, id);
-
+  CHECK_CHART_DERIVATIVES(id, T2);
-  CHECK_CHART_DERIVATIVES(id,id);
+  CHECK_CHART_DERIVATIVES(T2, id);
-  CHECK_CHART_DERIVATIVES(id,T2);
+  CHECK_CHART_DERIVATIVES(T2, T1);
-  CHECK_CHART_DERIVATIVES(T2,id);
-  CHECK_CHART_DERIVATIVES(T2,T1);
 }
 
 //******************************************************************************

gtsam/geometry/tests/testQuaternion.cpp

@@ -80,12 +80,6 @@ TEST(Quaternion , Compose) {
   EXPECT(traits<Q>::Equals(expected, actual));
 }
 
-//******************************************************************************
-Vector3 Q_z_axis(0, 0, 1);
-Q id(Eigen::AngleAxisd(0, Q_z_axis));
-Q R1(Eigen::AngleAxisd(1, Q_z_axis));
-Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
-
 //******************************************************************************
 TEST(Quaternion , Between) {
   Vector3 z_axis(0, 0, 1);
@@ -108,7 +102,15 @@ TEST(Quaternion , Inverse) {
 }
 
 //******************************************************************************
-TEST(Quaternion , Invariants) {
+namespace {
+Vector3 Q_z_axis(0, 0, 1);
+Q id(Eigen::AngleAxisd(0, Q_z_axis));
+Q R1(Eigen::AngleAxisd(1, Q_z_axis));
+Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
+}  // namespace
+
+//******************************************************************************
+TEST(Quaternion, Invariants) {
   EXPECT(check_group_invariants(id, id));
   EXPECT(check_group_invariants(id, R1));
   EXPECT(check_group_invariants(R2, id));
@@ -121,7 +123,7 @@ TEST(Quaternion , Invariants) {
 }
 
 //******************************************************************************
-TEST(Quaternion , LieGroupDerivatives) {
+TEST(Quaternion, LieGroupDerivatives) {
   CHECK_LIE_GROUP_DERIVATIVES(id, id);
   CHECK_LIE_GROUP_DERIVATIVES(id, R2);
   CHECK_LIE_GROUP_DERIVATIVES(R2, id);
@@ -129,7 +131,7 @@ TEST(Quaternion , LieGroupDerivatives) {
 }
 
 //******************************************************************************
-TEST(Quaternion , ChartDerivatives) {
+TEST(Quaternion, ChartDerivatives) {
   CHECK_CHART_DERIVATIVES(id, id);
   CHECK_CHART_DERIVATIVES(id, R2);
   CHECK_CHART_DERIVATIVES(R2, id);

gtsam/geometry/tests/testRot2.cpp

@@ -156,44 +156,39 @@ TEST( Rot2, relativeBearing )
 }
 
 //******************************************************************************
+namespace {
+Rot2 id;
 Rot2 T1(0.1);
 Rot2 T2(0.2);
+}  // namespace
 
 //******************************************************************************
-TEST(Rot2 , Invariants) {
+TEST(Rot2, Invariants) {
-  Rot2 id;
+  EXPECT(check_group_invariants(id, id));
-
+  EXPECT(check_group_invariants(id, T1));
-  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(T2, id));
-  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2, T1));
-  EXPECT(check_group_invariants(T2,id));
-  EXPECT(check_group_invariants(T2,T1));
-
-  EXPECT(check_manifold_invariants(id,id));
-  EXPECT(check_manifold_invariants(id,T1));
-  EXPECT(check_manifold_invariants(T2,id));
-  EXPECT(check_manifold_invariants(T2,T1));
 
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, T1));
+  EXPECT(check_manifold_invariants(T2, id));
+  EXPECT(check_manifold_invariants(T2, T1));
 }
 
 //******************************************************************************
-TEST(Rot2 , LieGroupDerivatives) {
+TEST(Rot2, LieGroupDerivatives) {
-  Rot2 id;
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
-
 }
 
 //******************************************************************************
-TEST(Rot2 , ChartDerivatives) {
+TEST(Rot2, ChartDerivatives) {
-  Rot2 id;
+  CHECK_CHART_DERIVATIVES(id, id);
-
+  CHECK_CHART_DERIVATIVES(id, T2);
-  CHECK_CHART_DERIVATIVES(id,id);
+  CHECK_CHART_DERIVATIVES(T2, id);
-  CHECK_CHART_DERIVATIVES(id,T2);
+  CHECK_CHART_DERIVATIVES(T2, T1);
-  CHECK_CHART_DERIVATIVES(T2,id);
-  CHECK_CHART_DERIVATIVES(T2,T1);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -640,46 +640,44 @@ TEST( Rot3, slerp)
 }
 
 //******************************************************************************
+namespace {
+Rot3 id;
 Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
 Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
+}  // namespace
 
 //******************************************************************************
-TEST(Rot3 , Invariants) {
+TEST(Rot3, Invariants) {
-  Rot3 id;
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, T1));
+  EXPECT(check_group_invariants(T2, id));
+  EXPECT(check_group_invariants(T2, T1));
+  EXPECT(check_group_invariants(T1, T2));
 
-  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_manifold_invariants(id, id));
-  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_manifold_invariants(id, T1));
-  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2, id));
-  EXPECT(check_group_invariants(T2,T1));
+  EXPECT(check_manifold_invariants(T2, T1));
-  EXPECT(check_group_invariants(T1,T2));
+  EXPECT(check_manifold_invariants(T1, T2));
-
-  EXPECT(check_manifold_invariants(id,id));
-  EXPECT(check_manifold_invariants(id,T1));
-  EXPECT(check_manifold_invariants(T2,id));
-  EXPECT(check_manifold_invariants(T2,T1));
-  EXPECT(check_manifold_invariants(T1,T2));
 }
 
 //******************************************************************************
-TEST(Rot3 , LieGroupDerivatives) {
+TEST(Rot3, LieGroupDerivatives) {
-  Rot3 id;
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T1, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
-  CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
 }
 
 //******************************************************************************
-TEST(Rot3 , ChartDerivatives) {
+TEST(Rot3, ChartDerivatives) {
-  Rot3 id;
   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
-    CHECK_CHART_DERIVATIVES(id,id);
+    CHECK_CHART_DERIVATIVES(id, id);
-    CHECK_CHART_DERIVATIVES(id,T2);
+    CHECK_CHART_DERIVATIVES(id, T2);
-    CHECK_CHART_DERIVATIVES(T2,id);
+    CHECK_CHART_DERIVATIVES(T2, id);
-    CHECK_CHART_DERIVATIVES(T1,T2);
+    CHECK_CHART_DERIVATIVES(T1, T2);
-    CHECK_CHART_DERIVATIVES(T2,T1);
+    CHECK_CHART_DERIVATIVES(T2, T1);
   }
 }
 

gtsam/geometry/tests/testSO3.cpp

@@ -67,10 +67,12 @@ TEST(SO3, ClosestTo) {
 }
 
 //******************************************************************************
+namespace {
 SO3 id;
 Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
 SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
 SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
+}  // namespace
 
 /* ************************************************************************* */
 TEST(SO3, ChordalMean) {
@@ -79,16 +81,16 @@ TEST(SO3, ChordalMean) {
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO3, Hat) {
-  // Check that Hat specialization is equal to dynamic version
   EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
   EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
   EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO3, Vee) {
-  // Check that Hat specialization is equal to dynamic version
   auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
   EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
   EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));

gtsam/geometry/tests/testSO4.cpp

@@ -48,6 +48,14 @@ TEST(SO4, Concept) {
 }
 
 //******************************************************************************
+TEST(SO4, Random) {
+  std::mt19937 rng(42);
+  auto Q = SO4::Random(rng);
+  EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
+}
+
+//******************************************************************************
+namespace {
 SO4 id;
 Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
 SO4 Q1 = SO4::Expmap(v1);
@@ -55,13 +63,8 @@ Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
 SO4 Q2 = SO4::Expmap(v2);
 Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
 SO4 Q3 = SO4::Expmap(v3);
+}  // namespace
 
-//******************************************************************************
-TEST(SO4, Random) {
-  std::mt19937 rng(42);
-  auto Q = SO4::Random(rng);
-  EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
-}
 //******************************************************************************
 TEST(SO4, Expmap) {
   // If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
@@ -84,16 +87,16 @@ TEST(SO4, Expmap) {
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO4, Hat) {
-  // Check that Hat specialization is equal to dynamic version
   EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
   EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
   EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO4, Vee) {
-  // Check that Hat specialization is equal to dynamic version
   auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
   EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
   EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
@@ -116,8 +119,8 @@ TEST(SO4, Retract) {
 }
 
 //******************************************************************************
+// Check that Cayley is identical to dynamic version
 TEST(SO4, Local) {
-  // Check that Cayley is identical to dynamic version
   EXPECT(
       assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
   EXPECT(
@@ -166,9 +169,7 @@ TEST(SO4, vec) {
   Matrix actualH;
   const Vector16 actual = Q2.vec(actualH);
   EXPECT(assert_equal(expected, actual));
-  std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
+  std::function<Vector16(const SO4&)> f = [](const SO4& Q) { return Q.vec(); };
-    return Q.vec();
-  };
   const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
   EXPECT(assert_equal(numericalH, actualH));
 }

gtsam/geometry/tests/testTriangulation.cpp

@@ -19,6 +19,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -38,7 +39,7 @@ using namespace boost::assign;
 // Some common constants
 
 static const boost::shared_ptr<Cal3_S2> sharedCal =  //
-    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
+    boost::make_shared<Cal3_S2>(1500, 1200, 0.1, 640, 480);
 
 // Looking along X-axis, 1 meter above ground plane (x-y)
 static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
@@ -95,11 +96,123 @@ TEST(triangulation, twoPoses) {
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
+//******************************************************************************
+// Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
+TEST(triangulation, twoPosesCal3DS2) {
+  static const boost::shared_ptr<Cal3DS2> sharedDistortedCal =  //
+      boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
+                                  -0.0003);
+
+  PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal);
+
+  PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal);
+
+  // 0. Project two landmarks into two cameras and triangulate
+  Point2 z1Distorted = camera1Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(landmark);
+
+  vector<Pose3> poses;
+  Point2Vector measurements;
+
+  poses += pose1, pose2;
+  measurements += z1Distorted, z2Distorted;
+
+  double rank_tol = 1e-9;
+
+  // 1. Test simple DLT, perfect in no noise situation
+  bool optimize = false;
+  boost::optional<Point3> actual1 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+
+  // 2. test with optimization on, same answer
+  optimize = true;
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+
+  // 3. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
+  measurements.at(0) += Point2(0.1, 0.5);
+  measurements.at(1) += Point2(-0.2, 0.3);
+  optimize = false;
+  boost::optional<Point3> actual3 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
+
+  // 4. Now with optimization on
+  optimize = true;
+  boost::optional<Point3> actual4 =  //
+      triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
+                                 rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
+}
+
+//******************************************************************************
+// Simple test with a well-behaved two camera situation with Fisheye
+// calibration.
+TEST(triangulation, twoPosesFisheye) {
+  using Calibration = Cal3Fisheye;
+  static const boost::shared_ptr<Calibration> sharedDistortedCal =  //
+      boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
+                                      0.0001, -0.0003);
+
+  PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal);
+
+  PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal);
+
+  // 0. Project two landmarks into two cameras and triangulate
+  Point2 z1Distorted = camera1Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(landmark);
+
+  vector<Pose3> poses;
+  Point2Vector measurements;
+
+  poses += pose1, pose2;
+  measurements += z1Distorted, z2Distorted;
+
+  double rank_tol = 1e-9;
+
+  // 1. Test simple DLT, perfect in no noise situation
+  bool optimize = false;
+  boost::optional<Point3> actual1 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+
+  // 2. test with optimization on, same answer
+  optimize = true;
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+
+  // 3. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
+  measurements.at(0) += Point2(0.1, 0.5);
+  measurements.at(1) += Point2(-0.2, 0.3);
+  optimize = false;
+  boost::optional<Point3> actual3 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
+
+  // 4. Now with optimization on
+  optimize = true;
+  boost::optional<Point3> actual4 =  //
+      triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
+                                     rank_tol, optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
+}
+
 //******************************************************************************
 // Similar, but now with Bundler calibration
 TEST(triangulation, twoPosesBundler) {
   boost::shared_ptr<Cal3Bundler> bundlerCal =  //
-      boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
+      boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
   PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
   PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
 
@@ -117,7 +230,8 @@ TEST(triangulation, twoPosesBundler) {
   double rank_tol = 1e-9;
 
   boost::optional<Point3> actual =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
+                                     optimize);
   EXPECT(assert_equal(landmark, *actual, 1e-7));
 
   // Add some noise and try again
@@ -125,8 +239,9 @@ TEST(triangulation, twoPosesBundler) {
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
-  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
+                                     optimize);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
 }
 
 //******************************************************************************
@@ -336,6 +451,31 @@ TEST(triangulation, fourPoses_distinct_Ks) {
 #endif
 }
 
+//******************************************************************************
+TEST(triangulation, fourPoses_distinct_Ks_distortion) {
+  Cal3DS2 K1(1500, 1200, 0, 640, 480,  -.3, 0.1, 0.0001, -0.0003);
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  PinholeCamera<Cal3DS2> camera1(pose1, K1);
+
+  // create second camera 1 meter to the right of first camera
+  Cal3DS2 K2(1600, 1300, 0, 650, 440,  -.2, 0.05, 0.0002, -0.0001);
+  PinholeCamera<Cal3DS2> camera2(pose2, K2);
+
+  // 1. Project two landmarks into two cameras and triangulate
+  Point2 z1 = camera1.project(landmark);
+  Point2 z2 = camera2.project(landmark);
+
+  CameraSet<PinholeCamera<Cal3DS2>> cameras;
+  Point2Vector measurements;
+
+  cameras += camera1, camera2;
+  measurements += z1, z2;
+
+  boost::optional<Point3> actual =  //
+          triangulatePoint3<Cal3DS2>(cameras, measurements);
+  EXPECT(assert_equal(landmark, *actual, 1e-2));
+}
+
 //******************************************************************************
 TEST(triangulation, outliersAndFarLandmarks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);

gtsam/geometry/triangulation.h

@@ -227,6 +227,109 @@ std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFrom
   return projection_matrices;
 }
 
+/** Create a pinhole calibration from a different Cal3 object, removing
+ * distortion.
+ *
+ * @tparam CALIBRATION Original calibration object.
+ * @param cal Input calibration object.
+ * @return Cal3_S2 with only the pinhole elements of cal.
+ */
+template <class CALIBRATION>
+Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
+  const auto& K = cal.K();
+  return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
+}
+
+/** Internal undistortMeasurement to be used by undistortMeasurement and
+ * undistortMeasurements */
+template <class CALIBRATION, class MEASUREMENT>
+MEASUREMENT undistortMeasurementInternal(
+    const CALIBRATION& cal, const MEASUREMENT& measurement,
+    boost::optional<Cal3_S2> pinholeCal = boost::none) {
+  if (!pinholeCal) {
+    pinholeCal = createPinholeCalibration(cal);
+  }
+  return pinholeCal->uncalibrate(cal.calibrate(measurement));
+}
+
+/** Remove distortion for measurements so as if the measurements came from a
+ * pinhole camera.
+ *
+ * Removes distortion but maintains the K matrix of the initial cal. Operates by
+ * calibrating using full calibration and uncalibrating with only the pinhole
+ * component of the calibration.
+ * @tparam CALIBRATION Calibration type to use.
+ * @param cal Calibration with which measurements were taken.
+ * @param measurements Vector of measurements to undistort.
+ * @return measurements with the effect of the distortion of sharedCal removed.
+ */
+template <class CALIBRATION>
+Point2Vector undistortMeasurements(const CALIBRATION& cal,
+                                   const Point2Vector& measurements) {
+  Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
+  Point2Vector undistortedMeasurements;
+  // Calibrate with cal and uncalibrate with pinhole version of cal so that
+  // measurements are undistorted.
+  std::transform(measurements.begin(), measurements.end(),
+                 std::back_inserter(undistortedMeasurements),
+                 [&cal, &pinholeCalibration](const Point2& measurement) {
+                   return undistortMeasurementInternal<CALIBRATION>(
+                       cal, measurement, pinholeCalibration);
+                 });
+  return undistortedMeasurements;
+}
+
+/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
+template <>
+inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
+                                          const Point2Vector& measurements) {
+  return measurements;
+}
+
+/** Remove distortion for measurements so as if the measurements came from a
+ * pinhole camera.
+ *
+ * Removes distortion but maintains the K matrix of the initial calibrations.
+ * Operates by calibrating using full calibration and uncalibrating with only
+ * the pinhole component of the calibration.
+ * @tparam CAMERA Camera type to use.
+ * @param cameras Cameras corresponding to each measurement.
+ * @param measurements Vector of measurements to undistort.
+ * @return measurements with the effect of the distortion of the camera removed.
+ */
+template <class CAMERA>
+typename CAMERA::MeasurementVector undistortMeasurements(
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements) {
+  const size_t num_meas = cameras.size();
+  assert(num_meas == measurements.size());
+  typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
+  for (size_t ii = 0; ii < num_meas; ++ii) {
+    // Calibrate with cal and uncalibrate with pinhole version of cal so that
+    // measurements are undistorted.
+    undistortedMeasurements[ii] =
+        undistortMeasurementInternal<typename CAMERA::CalibrationType>(
+            cameras[ii].calibration(), measurements[ii]);
+  }
+  return undistortedMeasurements;
+}
+
+/** Specialize for Cal3_S2 to do nothing. */
+template <class CAMERA = PinholeCamera<Cal3_S2>>
+inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
+    const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
+    const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
+  return measurements;
+}
+
+/** Specialize for SphericalCamera to do nothing. */
+template <class CAMERA = SphericalCamera>
+inline SphericalCamera::MeasurementVector undistortMeasurements(
+    const CameraSet<SphericalCamera>& cameras,
+    const SphericalCamera::MeasurementVector& measurements) {
+  return measurements;
+}
+
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
  * of poses (at least 2) using the DLT. The function checks that the
@@ -253,8 +356,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   // construct projection matrices from poses & calibration
   auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
 
+  // Undistort the measurements, leaving only the pinhole elements in effect.
+  auto undistortedMeasurements =
+      undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
+
   // Triangulate linearly
-  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
+  Point3 point =
+      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
 
   // Then refine using non-linear optimization
   if (optimize)
@@ -300,7 +408,13 @@ Point3 triangulatePoint3(
 
   // construct projection matrices from poses & calibration
   auto projection_matrices = projectionMatricesFromCameras(cameras);
-  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
+
+  // Undistort the measurements, leaving only the pinhole elements in effect.
+  auto undistortedMeasurements =
+      undistortMeasurements<CAMERA>(cameras, measurements);
+
+  Point3 point =
+      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
 
   // The n refine using non-linear optimization
   if (optimize)

gtsam/inference/BayesTree.h

@@ -139,7 +139,7 @@ namespace gtsam {
       return nodes_.empty();
     }
 
-    /** return nodes */
+    /** Return nodes. Each node is a clique of variables obtained after elimination. */
     const Nodes& nodes() const { return nodes_; }
 
     /** Access node by variable */

gtsam/inference/Ordering.cpp

@@ -25,11 +25,7 @@
 #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
 
 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
-#ifdef GTSAM_USE_SYSTEM_METIS
 #include <metis.h>
-#else
-#include <gtsam/3rdparty/metis/include/metis.h>
-#endif
 #endif
 
 using namespace std;

gtsam/inference/tests/testOrdering.cpp

@@ -270,17 +270,7 @@ TEST(Ordering, MetisLoop) {
   symbolicGraph.push_factor(0, 5);
 
   // METIS
-#if !defined(__APPLE__)
+#if defined(__APPLE__)
-  {
-    Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
-    //  - P( 0 4 1)
-    //  | - P( 2 | 4 1)
-    //  | | - P( 3 | 4 2)
-    //  | - P( 5 | 0 1)
-    Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1));
-    EXPECT(assert_equal(expected, actual));
-  }
-#else
   {
     Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
     //  - P( 1 0 3)
@@ -290,6 +280,26 @@ TEST(Ordering, MetisLoop) {
     Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3));
     EXPECT(assert_equal(expected, actual));
   }
+#elif defined(_WIN32)
+  {
+    Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
+    //  - P( 0 5 2)
+    //  | - P( 3 | 5 2)
+    //  | | - P( 4 | 5 3)
+    //  | - P( 1 | 0 2)
+    Ordering expected = Ordering(list_of(4)(3)(1)(0)(5)(2));
+    EXPECT(assert_equal(expected, actual));
+  }
+#else
+  {
+    Ordering actual = Ordering::Create(Ordering::METIS, symbolicGraph);
+    //  - P( 0 4 1)
+    //  | - P( 2 | 4 1)
+    //  | | - P( 3 | 4 2)
+    //  | - P( 5 | 0 1)
+    Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1));
+    EXPECT(assert_equal(expected, actual));
+  }
 #endif
 }
 #endif

gtsam/linear/GaussianConditional.cpp

@@ -89,11 +89,15 @@ namespace gtsam {
 
   /* ************************************************************************ */
   void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
-    cout << s << "  Conditional density ";
+    cout << s << " p(";
     for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
-      cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
+      cout << (boost::format("%1%")%(formatter(*it))).str() << " ";
     }
-    cout << endl;
+    cout << "|";
+    for (const_iterator it = beginParents(); it != endParents(); ++it) {
+      cout << " " << (boost::format("%1%")%(formatter(*it))).str();
+    }
+    cout << ")" << endl;
     cout << formatMatrixIndented("  R = ", R()) << endl;
     for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
       cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))

gtsam/linear/GaussianDensity.cpp

@@ -17,42 +17,41 @@
  */
 
 #include <gtsam/linear/GaussianDensity.h>
+#include <boost/format.hpp>
+#include <string>
 
-using namespace std;
+using std::cout;
+using std::endl;
+using std::string;
 
 namespace gtsam {
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  GaussianDensity GaussianDensity::FromMeanAndStddev(Key key,
+GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean,
-                                                     const Vector& mean,
+                                                   double sigma) {
-                                                     double sigma) {
+  return GaussianDensity(key, mean, Matrix::Identity(mean.size(), mean.size()),
-    return GaussianDensity(key, mean,
+                         noiseModel::Isotropic::Sigma(mean.size(), sigma));
-                           Matrix::Identity(mean.size(), mean.size()),
+}
-                           noiseModel::Isotropic::Sigma(mean.size(), sigma));
-  }
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  void GaussianDensity::print(const string &s, const KeyFormatter& formatter) const
+void GaussianDensity::print(const string& s,
-  {
+                            const KeyFormatter& formatter) const {
-    cout << s << ": density on ";
+  cout << s << ": density on ";
-    for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
+  for (const_iterator it = beginFrontals(); it != endFrontals(); ++it)
-      cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
+    cout << (boost::format("[%1%]") % (formatter(*it))).str() << " ";
-    cout << endl;
+  cout << endl;
-    gtsam::print(mean(), "mean: ");
+  gtsam::print(mean(), "mean: ");
-    gtsam::print(covariance(), "covariance: ");
+  gtsam::print(covariance(), "covariance: ");
-    if(model_)
+  if (model_) model_->print("Noise model: ");
-      model_->print("Noise model: ");
+}
-  }
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Vector GaussianDensity::mean() const {
+Vector GaussianDensity::mean() const {
-    VectorValues soln = this->solve(VectorValues());
+  VectorValues soln = this->solve(VectorValues());
-    return soln[firstFrontalKey()];
+  return soln[firstFrontalKey()];
-  }
+}
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
-  Matrix GaussianDensity::covariance() const {
+Matrix GaussianDensity::covariance() const { return information().inverse(); }
-    return information().inverse();
-  }
 
-} // gtsam
+}  // namespace gtsam

gtsam/linear/GaussianJunctionTree.h

@@ -16,6 +16,8 @@
  * @author Richard Roberts
  */
 
+#pragma once
+
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/inference/JunctionTree.h>

gtsam/linear/LossFunctions.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/linear/LossFunctions.h>
 
 #include <iostream>
+#include <vector>
 
 using namespace std;
 

gtsam/linear/NoiseModel.cpp

@@ -134,7 +134,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
 
 /* ************************************************************************* */
 void Gaussian::print(const string& name) const {
-  gtsam::print(thisR(), name + "Gaussian");
+  gtsam::print(thisR(), name + "Gaussian ");
 }
 
 /* ************************************************************************* */
@@ -285,7 +285,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
 
 /* ************************************************************************* */
 void Diagonal::print(const string& name) const {
-  gtsam::print(sigmas_, name + "diagonal sigmas");
+  gtsam::print(sigmas_, name + "diagonal sigmas ");
 }
 
 /* ************************************************************************* */
@@ -355,8 +355,8 @@ bool Constrained::constrained(size_t i) const {
 
 /* ************************************************************************* */
 void Constrained::print(const std::string& name) const {
-  gtsam::print(sigmas_, name + "constrained sigmas");
+  gtsam::print(sigmas_, name + "constrained sigmas ");
-  gtsam::print(mu_, name + "constrained mu");
+  gtsam::print(mu_, name + "constrained mu ");
 }
 
 /* ************************************************************************* */

gtsam/linear/linearAlgorithms-inst.h

@@ -15,6 +15,8 @@
  * @author  Richard Roberts
  */
 
+#pragma once
+
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/base/treeTraversal-inst.h>

gtsam/linear/tests/powerMethodExample.h

@@ -19,6 +19,8 @@
  * PowerMethod/AcceleratedPowerMethod
  */
 
+#pragma once
+
 #include <gtsam/inference/Symbol.h>
 
 #include <iostream>

gtsam/linear/tests/testGaussianConditional.cpp

@@ -40,6 +40,7 @@ using namespace gtsam;
 using namespace std;
 using namespace boost::assign;
 using symbol_shorthand::X;
+using symbol_shorthand::Y;
 
 static const double tol = 1e-5;
 
@@ -396,6 +397,45 @@ TEST(GaussianConditional, sample) {
   // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(GaussianConditional, Print) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
+  const Vector2 b(20, 40);
+  const double sigma = 3;
+
+  std::string s = "GaussianConditional";
+
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+
+  // Test printing for single parent.
+  std::string expected =
+    "GaussianConditional p(x0 | x1)\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  S[x1] = [ -1 -2 ]\n"
+    "          [ -3 -4 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected, conditional, s));
+
+  // Test printing for multiple parents.
+  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, Y(0), A2,
+                                                             Y(1), b, sigma);
+  std::string expected2 =
+    "GaussianConditional p(x0 | y0 y1)\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  S[y0] = [ -1 -2 ]\n"
+    "          [ -3 -4 ]\n"
+    "  S[y1] = [ -5 -6 ]\n"
+    "          [ -7 -8 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected2, conditional2, s));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/navigation/CombinedImuFactor.cpp

@@ -256,6 +256,3 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
 }
  /// namespace gtsam
 
-/// Boost serialization export definition for derived class
-BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams)
-

gtsam/navigation/CombinedImuFactor.h

@@ -351,6 +351,3 @@ template <>
 struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
 
 }  // namespace gtsam
-
-/// Add Boost serialization export key (declaration) for derived class
-BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)

gtsam/navigation/ConstantVelocityFactor.h

@@ -15,6 +15,8 @@
  * @author  Asa Hammond
  */
 
+#pragma once
+
 #include <gtsam/navigation/NavState.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 

gtsam/navigation/MagFactor.h

@@ -16,6 +16,8 @@
  * @date   January 29, 2014
  */
 
+#pragma once
+
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>