diff --git a/CMakeLists.txt b/CMakeLists.txt
index e3b462eec..90162fe29 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,14 +71,7 @@ include(GtsamPrinting)
 ############### Decide on BOOST ######################################
 # Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION
 option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON)
-if(GTSAM_ENABLE_BOOST_SERIALIZATION)
-  add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION)
-endif()
-
 option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON)
-if(GTSAM_USE_BOOST_FEATURES)
-  add_definitions(-DGTSAM_USE_BOOST_FEATURES)
-endif()
 
 if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES)
 include(cmake/HandleBoost.cmake)
diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md
index 9911b3764..733672fbe 100644
--- a/GTSAM-Concepts.md
+++ b/GTSAM-Concepts.md
@@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr
 In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
 
 * values:
-   * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
+   * `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
 * types:
     * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
     * `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
diff --git a/INSTALL.md b/INSTALL.md
index 10bee196c..51f00fd3b 100644
--- a/INSTALL.md
+++ b/INSTALL.md
@@ -27,7 +27,7 @@ $ make install
        downloaded from https://www.threadingbuildingblocks.org/
      - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
        `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
-       achieved with MKL disabled. We therefore advise you to benchmark your problem 
+       achieved with MKL disabled. We therefore advise you to benchmark your problem
        before using MKL.
 
     Tested compilers:
@@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive)
 
 #### CMAKE_INSTALL_PREFIX
 
-The install folder. The default is typically `/usr/local/`. 
+The install folder. The default is typically `/usr/local/`.
 To configure to install to your home directory, you could execute:
 
 ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..```
 
-#### GTSAM_TOOLBOX_INSTALL_PATH 
+#### GTSAM_TOOLBOX_INSTALL_PATH
 
 The Matlab toolbox will be installed in a subdirectory
 of this folder, called 'gtsam'.
@@ -142,7 +142,7 @@ of this folder, called 'gtsam'.
 
 #### GTSAM_BUILD_CONVENIENCE_LIBRARIES
 
-This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. 
+This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam.
 Set with the command line as follows:
 
 ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..```
@@ -159,6 +159,16 @@ Set with the command line as follows:
   ON:             When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam.  In addition, if tests are enabled, the unit tests will be built as well.  The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`.
   OFF (Default)  If disabled, no `gtsam_unstable` code will be included in build or install.
 
+## Convenience Options:
+
+#### GTSAM_BUILD_EXAMPLES_ALWAYS
+
+Whether or not to force building examples, can be true or false.
+
+#### GTSAM_BUILD_TESTS
+
+Whether or not to build tests, can be true or false.
+
 ## Check
 
 `make check` will build and run all of the tests. Note that the tests will only be
@@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM.
 
 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode.
 2. Enable TBB. On modern processors with multiple cores, this can easily speed up
-    optimization by 30-50%. Please note that this may not be true for very small 
+    optimization by 30-50%. Please note that this may not be true for very small
     problems where the overhead of dispatching work to multiple threads outweighs
     the benefit. We recommend that you benchmark your problem with/without TBB.
 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of
diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp
index 6ce4f76fa..436b6e283 100644
--- a/examples/CombinedImuFactorsExample.cpp
+++ b/examples/CombinedImuFactorsExample.cpp
@@ -48,6 +48,7 @@
 #include <cstring>
 #include <fstream>
 #include <iostream>
+#include <cassert>
 
 using namespace gtsam;
 using namespace std;
diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index 342f1f220..7cbc8989f 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -49,6 +49,7 @@
 #include <gtsam/slam/dataset.h>
 
 #include <cstring>
+#include <cassert>
 #include <fstream>
 #include <iostream>
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index a2ad8cf0b..bc4e6e340 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -29,6 +29,8 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Point2.h>
 
+#include <cassert>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt
index 190b73db9..aeac7b403 100644
--- a/gtsam/3rdparty/cephes/CMakeLists.txt
+++ b/gtsam/3rdparty/cephes/CMakeLists.txt
@@ -89,7 +89,7 @@ set(CEPHES_SOURCES
     cephes/zetac.c)
 
 # Add library source files
-add_library(cephes-gtsam SHARED ${CEPHES_SOURCES})
+add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES})
 
 # Add include directory (aka headers)
 target_include_directories(
diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h
index 372f02d06..03687f562 100644
--- a/gtsam/base/ConcurrentMap.h
+++ b/gtsam/base/ConcurrentMap.h
@@ -48,7 +48,7 @@ using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
 
 #endif
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/split_member.hpp>
 #endif
@@ -101,7 +101,7 @@ public:
 #endif
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h
index 414c1e83f..1a3b9ed3f 100644
--- a/gtsam/base/FastList.h
+++ b/gtsam/base/FastList.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/base/FastDefaultAllocator.h>
 #include <list>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/version.hpp>
 #if BOOST_VERSION >= 107400
@@ -79,7 +79,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h
index e8ef3fc4f..75998fd2f 100644
--- a/gtsam/base/FastMap.h
+++ b/gtsam/base/FastMap.h
@@ -19,7 +19,7 @@
 #pragma once
 
 #include <gtsam/base/FastDefaultAllocator.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/map.hpp>
 #endif
@@ -69,7 +69,7 @@ public:
   bool exists(const KEY& e) const { return this->find(e) != this->end(); }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index a1be08d80..9a2f62497 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -18,7 +18,9 @@
 
 #pragma once
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <gtsam/config.h>
+
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/version.hpp>
 #if BOOST_VERSION >= 107400
 #include <boost/serialization/library_version_type.hpp>
@@ -123,7 +125,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index bb92b6b2e..62ea8ec39 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -173,7 +173,7 @@ public:
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -184,9 +184,8 @@ public:
 	}
 #endif
 
-
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  constexpr static const bool NeedsToAlign = (sizeof(T) % 16) == 0;
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index ce021e10e..862ae6f4d 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -36,7 +36,7 @@ namespace gtsam {
 template <class Class, int N>
 struct LieGroup {
 
-  enum { dimension = N };
+  inline constexpr static auto dimension = N;
   typedef OptionalJacobian<N, N> ChartJacobian;
   typedef Eigen::Matrix<double, N, N> Jacobian;
   typedef Eigen::Matrix<double, N, 1> TangentVector;
@@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
   /// @name Manifold
   /// @{
   typedef Class ManifoldType;
-  enum { dimension = Class::dimension };
+  inline constexpr static auto dimension = Class::dimension;
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
 
diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index cb30fa9c0..922b8dd2f 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -55,7 +55,7 @@ namespace internal {
 template<class Class>
 struct HasManifoldPrereqs {
 
-  enum { dim = Class::dimension };
+  inline constexpr static auto dim = Class::dimension;
 
   Class p, q;
   Eigen::Matrix<double, dim, 1> v;
@@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
   GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
 
   // Dimension of the manifold
-  enum { dimension = Class::dimension };
+  inline constexpr static auto dimension = Class::dimension;
 
   // Typedefs required by all manifold types.
   typedef Class ManifoldType;
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 247c53cce..1e04bc58a 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -24,6 +24,7 @@
 #include <Eigen/LU>
 
 #include <cstdarg>
+#include <cassert>
 #include <cstring>
 #include <iomanip>
 #include <list>
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 6dba9cb05..c8dc46ed5 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -479,7 +479,7 @@ struct MultiplyWithInverse {
  */
 template <typename T, int N>
 struct MultiplyWithInverseFunction {
-  enum { M = traits<T>::dimension };
+  inline constexpr static auto M = traits<T>::dimension;
   typedef Eigen::Matrix<double, N, 1> VectorN;
   typedef Eigen::Matrix<double, N, N> MatrixN;
 
diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h
index 9cf730230..11b6a417a 100644
--- a/gtsam/base/MatrixSerialization.h
+++ b/gtsam/base/MatrixSerialization.h
@@ -19,7 +19,7 @@
 // \callgraph
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
 #include <gtsam/base/Matrix.h>
diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h
index beb376c19..bb7ba72b8 100644
--- a/gtsam/base/ProductLieGroup.h
+++ b/gtsam/base/ProductLieGroup.h
@@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair<G, H> {
   typedef std::pair<G, H> Base;
 
 protected:
-  enum {dimension1 = traits<G>::dimension};
-  enum {dimension2 = traits<H>::dimension};
+  constexpr static const size_t dimension1 = traits<G>::dimension;
+  constexpr static const size_t dimension2 = traits<H>::dimension;
 
 public:
   /// Default constructor yields identity
@@ -67,9 +67,9 @@ public:
 
   /// @name Manifold
   /// @{
-  enum {dimension = dimension1 + dimension2};
-  inline static size_t Dim() {return dimension;}
-  inline size_t dim() const {return dimension;}
+  inline constexpr static auto dimension = dimension1 + dimension2;
+  inline static size_t Dim() { return dimension; }
+  inline size_t dim() const { return dimension; }
 
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h
index 1783857cb..378e91144 100644
--- a/gtsam/base/SymmetricBlockMatrix.h
+++ b/gtsam/base/SymmetricBlockMatrix.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/types.h>
 #include <gtsam/dllexport.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <cassert>
@@ -386,7 +386,7 @@ namespace gtsam {
     template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index c36de42ab..09a3f4878 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -21,7 +21,7 @@
 #include <gtsam/config.h>      // Configuration from CMake
 
 #include <gtsam/base/Vector.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/assume_abstract.hpp>
 #endif
@@ -121,7 +121,7 @@ namespace gtsam {
      *       The last two links explain why these export lines have to be in the same source module that includes
      *       any of the archive class headers.
      * */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) {
@@ -132,6 +132,6 @@ namespace gtsam {
 
 } /* namespace gtsam */
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
 #endif
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index ce7aeba7b..dbd48dfc8 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -24,6 +24,7 @@
 #include <iostream>
 #include <fstream>
 #include <sstream>
+#include <cassert>
 #include <iomanip>
 #include <cmath>
 #include <cstdio>
diff --git a/gtsam/base/VectorSerialization.h b/gtsam/base/VectorSerialization.h
index 7acf108f4..39d7e0299 100644
--- a/gtsam/base/VectorSerialization.h
+++ b/gtsam/base/VectorSerialization.h
@@ -17,7 +17,7 @@
  */
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
 #include <gtsam/base/Vector.h>
diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h
index a9e9ca106..c5c4ad622 100644
--- a/gtsam/base/VectorSpace.h
+++ b/gtsam/base/VectorSpace.h
@@ -163,7 +163,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
 template<class Class>
 struct HasVectorSpacePrereqs {
 
-  enum { dim = Class::dimension };
+  inline constexpr static auto dim = Class::dimension;
 
   Class p, q;
   Vector v;
@@ -197,7 +197,7 @@ GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
 
   /// @name Manifold
   /// @{
-  enum { dimension = Class::dimension};
+  inline constexpr static auto dimension = Class::dimension;
   typedef Class ManifoldType;
   /// @}
 };
@@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
   /// @name Manifold
   /// @{
   typedef Scalar ManifoldType;
-  enum { dimension = 1 };
+  inline constexpr static auto dimension = 1;
   typedef Eigen::Matrix<double, 1, 1> TangentVector;
   typedef OptionalJacobian<1, 1> ChartJacobian;
 
@@ -305,7 +305,7 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
 
   /// @name Manifold
   /// @{
-  enum { dimension = M*N};
+  inline constexpr static auto dimension = M * N;
   typedef Fixed ManifoldType;
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
@@ -377,7 +377,7 @@ struct DynamicTraits {
 
   /// @name Manifold
   /// @{
-  enum { dimension = Eigen::Dynamic };
+  inline constexpr static auto dimension = Eigen::Dynamic;
   typedef Eigen::VectorXd TangentVector;
   typedef Eigen::MatrixXd Jacobian;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index 777441300..0ff8a8ae2 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -21,6 +21,8 @@
 #include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   // Forward declarations
@@ -219,7 +221,7 @@ namespace gtsam {
     friend class SymmetricBlockMatrix;
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp
index 71cef2524..9fee627fe 100644
--- a/gtsam/base/cholesky.cpp
+++ b/gtsam/base/cholesky.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h
index 18612bc22..ef7ed497f 100644
--- a/gtsam/base/serialization.h
+++ b/gtsam/base/serialization.h
@@ -17,9 +17,12 @@
  * @date Feb 7, 2012
  */
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
+#include <gtsam/config.h>
+
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+
 #include <Eigen/Core>
 #include <fstream>
 #include <sstream>
diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h
index 09b36cdc8..cc6dadafd 100644
--- a/gtsam/base/serializationTestHelpers.h
+++ b/gtsam/base/serializationTestHelpers.h
@@ -17,9 +17,12 @@
  * @date Feb 7, 2012
  */
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #pragma once
 
+#include <gtsam/config.h>
+
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+
 #include <iostream>
 #include <sstream>
 #include <string>
diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h
index 7e0f2e844..93a5c8dba 100644
--- a/gtsam/base/std_optional_serialization.h
+++ b/gtsam/base/std_optional_serialization.h
@@ -11,7 +11,7 @@
 #pragma once
 
 // Defined only if boost serialization is enabled
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <optional>
 #include <boost/config.hpp>
 
diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
index e8838a476..b14f699a3 100644
--- a/gtsam/base/tests/testNumericalDerivative.cpp
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -18,6 +18,8 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h
index 1b1c44acc..960440daf 100644
--- a/gtsam/basis/Basis.h
+++ b/gtsam/basis/Basis.h
@@ -291,7 +291,7 @@ class Basis {
    */
   template <class T>
   class ManifoldEvaluationFunctor : public VectorEvaluationFunctor {
-    enum { M = traits<T>::dimension };
+    inline constexpr static auto M = traits<T>::dimension;
     using Base = VectorEvaluationFunctor;
 
    public:
diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp
index 63fca64cc..71c3db7f0 100644
--- a/gtsam/basis/Chebyshev2.cpp
+++ b/gtsam/basis/Chebyshev2.cpp
@@ -20,6 +20,14 @@
 
 namespace gtsam {
 
+double Chebyshev2::Point(size_t N, int j, double a, double b) {
+  assert(j >= 0 && size_t(j) < N);
+  const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
+  // We add -PI so that we get values ordered from -1 to +1
+  // sin(-M_PI_2 + dtheta*j); also works
+  return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
+}
+
 Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
   // Allocate space for weights
   Weights weights(N);
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index 849a51104..207f4b617 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -61,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
    * @param b Upper bound of interval (default: 1)
    * @return double
    */
-  static double Point(size_t N, int j, double a = -1, double b = 1) {
-    assert(j >= 0 && size_t(j) < N);
-    const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
-    // We add -PI so that we get values ordered from -1 to +1
-    // sin(-M_PI_2 + dtheta*j); also works
-    return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
-  }
+  static double Point(size_t N, int j, double a = -1, double b = 1);
 
   /// All Chebyshev points
   static Vector Points(size_t N) {
diff --git a/gtsam/config.h.in b/gtsam/config.h.in
index 719cbc6cb..8b4903d3a 100644
--- a/gtsam/config.h.in
+++ b/gtsam/config.h.in
@@ -88,3 +88,7 @@
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
 
 #cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP
+
+// Boost flags
+#cmakedefine01 GTSAM_ENABLE_BOOST_SERIALIZATION
+#cmakedefine01 GTSAM_USE_BOOST_FEATURES
diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index bda44bb9d..efc19d9ee 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -144,7 +144,7 @@ namespace gtsam {
    private:
     using Base = DecisionTree<L, Y>::Node;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
@@ -471,7 +471,7 @@ namespace gtsam {
    private:
     using Base = DecisionTree<L, Y>::Node;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 486f798e9..7c8d2742d 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -23,7 +23,7 @@
 #include <gtsam/base/types.h>
 #include <gtsam/discrete/Assignment.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <memory>
@@ -132,7 +132,7 @@ namespace gtsam {
       virtual bool isLeaf() const = 0;
 
      private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template <class ARCHIVE>
@@ -440,7 +440,7 @@ namespace gtsam {
     /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index f68d2ae00..1ac782b88 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -48,7 +48,7 @@ namespace gtsam {
       return false;
     } else {
       const auto& f(static_cast<const DecisionTreeFactor&>(other));
-      return ADT::equals(f, tol);
+      return Base::equals(other, tol) && ADT::equals(f, tol);
     }
   }
 
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index f55ab11b3..8b6d91be7 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -321,7 +321,7 @@ namespace gtsam {
   /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h
index a5a4621aa..738b91aa5 100644
--- a/gtsam/discrete/DiscreteBayesNet.h
+++ b/gtsam/discrete/DiscreteBayesNet.h
@@ -147,7 +147,7 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
     /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index 399126d51..be0f42bea 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -30,6 +30,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <cassert>
 
 using namespace std;
 using std::pair;
diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h
index 8cba6dbe7..3ec9ae590 100644
--- a/gtsam/discrete/DiscreteConditional.h
+++ b/gtsam/discrete/DiscreteConditional.h
@@ -275,7 +275,7 @@ class GTSAM_EXPORT DiscreteConditional
                                   bool forceComplete) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp
index 2b11046f4..68cc1df7d 100644
--- a/gtsam/discrete/DiscreteFactor.cpp
+++ b/gtsam/discrete/DiscreteFactor.cpp
@@ -28,6 +28,11 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const {
+  return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_;
+}
+
 /* ************************************************************************ */
 DiscreteKeys DiscreteFactor::discreteKeys() const {
   DiscreteKeys result;
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index fa4ed2613..63cd7844c 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -78,7 +78,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// @{
 
   /// equals
-  virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
+  virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const;
 
   /// print
   void print(
@@ -182,7 +182,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h
index 44cc192ef..d200792c8 100644
--- a/gtsam/discrete/DiscreteKey.h
+++ b/gtsam/discrete/DiscreteKey.h
@@ -21,7 +21,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/inference/Key.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/vector.hpp>
 #endif
 #include <map>
@@ -87,7 +87,7 @@ namespace gtsam {
     /// Check equality to another DiscreteKeys object.
     bool equals(const DiscreteKeys& other, double tol = 0) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp
index ee381fe44..c1c301525 100644
--- a/gtsam/discrete/DiscreteLookupDAG.cpp
+++ b/gtsam/discrete/DiscreteLookupDAG.cpp
@@ -23,6 +23,7 @@
 #include <iterator>
 #include <string>
 #include <utility>
+#include <cassert>
 
 using std::pair;
 using std::vector;
diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h
index 7f31a3f48..1c0be2f06 100644
--- a/gtsam/discrete/DiscreteLookupDAG.h
+++ b/gtsam/discrete/DiscreteLookupDAG.h
@@ -136,7 +136,7 @@ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet<DiscreteLookupTable> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp
index ea51a996c..22548da07 100644
--- a/gtsam/discrete/TableFactor.cpp
+++ b/gtsam/discrete/TableFactor.cpp
@@ -62,35 +62,98 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
     : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
 
 /**
- * @brief Compute the correct ordering of the leaves in the decision tree.
+ * @brief Compute the indexing of the leaves in the decision tree based on the
+ * assignment and add the (index, leaf) pair to a SparseVector.
  *
- * This is done by first taking all the values which have modulo 0 value with
- * the cardinality of the innermost key `n`, and we go up to modulo n.
+ * We visit each leaf in the tree, and using the cardinalities of the keys,
+ * compute the correct index to add the leaf to a SparseVector which
+ *  is then used to create the TableFactor.
  *
  * @param dt The DecisionTree
- * @return std::vector<double>
+ * @return Eigen::SparseVector<double>
  */
-std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
-                                        const DecisionTreeFactor& dt) {
-  std::vector<double> probs = dt.probabilities();
-  std::vector<double> ordered;
+static Eigen::SparseVector<double> ComputeSparseTable(
+    const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) {
+  // SparseVector needs to know the maximum possible index,
+  // so we compute the product of cardinalities.
+  size_t cardinalityProduct = 1;
+  for (auto&& [_, c] : dt.cardinalities()) {
+    cardinalityProduct *= c;
+  }
+  Eigen::SparseVector<double> sparseTable(cardinalityProduct);
+  size_t nrValues = 0;
+  dt.visit([&nrValues](double x) {
+    if (x > 0) nrValues += 1;
+  });
+  sparseTable.reserve(nrValues);
 
-  size_t n = dkeys[0].second;
+  std::set<Key> allKeys(dt.keys().begin(), dt.keys().end());
 
-  for (size_t k = 0; k < n; ++k) {
-    for (size_t idx = 0; idx < probs.size(); ++idx) {
-      if (idx % n == k) {
-        ordered.push_back(probs[idx]);
+  /**
+   * @brief Functor which is called by the DecisionTree for each leaf.
+   * For each leaf value, we use the corresponding assignment to compute a
+   * corresponding index into a SparseVector. We then populate sparseTable with
+   * the value at the computed index.
+   *
+   * Takes advantage of the sparsity of the DecisionTree to be efficient. When
+   * merged branches are encountered, we enumerate over the missing keys.
+   *
+   */
+  auto op = [&](const Assignment<Key>& assignment, double p) {
+    if (p > 0) {
+      // Get all the keys involved in this assignment
+      std::set<Key> assignmentKeys;
+      for (auto&& [k, _] : assignment) {
+        assignmentKeys.insert(k);
+      }
+
+      // Find the keys missing in the assignment
+      std::vector<Key> diff;
+      std::set_difference(allKeys.begin(), allKeys.end(),
+                          assignmentKeys.begin(), assignmentKeys.end(),
+                          std::back_inserter(diff));
+
+      // Generate all assignments using the missing keys
+      DiscreteKeys extras;
+      for (auto&& key : diff) {
+        extras.push_back({key, dt.cardinality(key)});
+      }
+      auto&& extraAssignments = DiscreteValues::CartesianProduct(extras);
+
+      for (auto&& extra : extraAssignments) {
+        // Create new assignment using the extra assignment
+        DiscreteValues updatedAssignment(assignment);
+        updatedAssignment.insert(extra);
+
+        // Generate index and add to the sparse vector.
+        Eigen::Index idx = 0;
+        size_t previousCardinality = 1;
+        // We go in reverse since a DecisionTree has the highest label first
+        for (auto&& it = updatedAssignment.rbegin();
+             it != updatedAssignment.rend(); it++) {
+          idx += previousCardinality * it->second;
+          previousCardinality *= dt.cardinality(it->first);
+        }
+        sparseTable.coeffRef(idx) = p;
       }
     }
-  }
-  return ordered;
+  };
+
+  // Visit each leaf in `dt` to get the Assignment and leaf value
+  // to populate the sparseTable.
+  dt.visitWith(op);
+
+  return sparseTable;
 }
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteKeys& dkeys,
                          const DecisionTreeFactor& dtf)
-    : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
+    : TableFactor(dkeys, ComputeSparseTable(dkeys, dtf)) {}
+
+/* ************************************************************************ */
+TableFactor::TableFactor(const DecisionTreeFactor& dtf)
+    : TableFactor(dtf.discreteKeys(), dtf) {}
 
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteConditional& c)
@@ -98,7 +161,17 @@ TableFactor::TableFactor(const DiscreteConditional& c)
 
 /* ************************************************************************ */
 Eigen::SparseVector<double> TableFactor::Convert(
-    const std::vector<double>& table) {
+    const DiscreteKeys& keys, const std::vector<double>& table) {
+  size_t max_size = 1;
+  for (auto&& [_, cardinality] : keys.cardinalities()) {
+    max_size *= cardinality;
+  }
+  if (table.size() != max_size) {
+    throw std::runtime_error(
+        "The cardinalities of the keys don't match the number of values in the "
+        "input.");
+  }
+
   Eigen::SparseVector<double> sparse_table(table.size());
   // Count number of nonzero elements in table and reserve the space.
   const uint64_t nnz = std::count_if(table.begin(), table.end(),
@@ -113,13 +186,14 @@ Eigen::SparseVector<double> TableFactor::Convert(
 }
 
 /* ************************************************************************ */
-Eigen::SparseVector<double> TableFactor::Convert(const std::string& table) {
+Eigen::SparseVector<double> TableFactor::Convert(const DiscreteKeys& keys,
+                                                 const std::string& table) {
   // Convert string to doubles.
   std::vector<double> ys;
   std::istringstream iss(table);
   std::copy(std::istream_iterator<double>(iss), std::istream_iterator<double>(),
             std::back_inserter(ys));
-  return Convert(ys);
+  return Convert(keys, ys);
 }
 
 /* ************************************************************************ */
@@ -128,7 +202,8 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const {
     return false;
   } else {
     const auto& f(static_cast<const TableFactor&>(other));
-    return sparse_table_.isApprox(f.sparse_table_, tol);
+    return Base::equals(other, tol) &&
+           sparse_table_.isApprox(f.sparse_table_, tol);
   }
 }
 
@@ -176,12 +251,43 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const {
 /* ************************************************************************ */
 DecisionTreeFactor TableFactor::toDecisionTreeFactor() const {
   DiscreteKeys dkeys = discreteKeys();
-  std::vector<double> table;
+
+  // Record key assignment and value pairs in pair_table.
+  // The assignments are stored in descending order of keys so that the order of
+  // the values matches what is expected by a DecisionTree.
+  // This is why we reverse the keys and then
+  // query for the key value/assignment.
+  DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend());
+  std::vector<std::pair<uint64_t, double>> pair_table;
   for (auto i = 0; i < sparse_table_.size(); i++) {
-    table.push_back(sparse_table_.coeff(i));
+    std::stringstream ss;
+    for (auto&& [key, _] : rdkeys) {
+      ss << keyValueForIndex(key, i);
+    }
+    // k will be in reverse key order already
+    uint64_t k;
+    ss >> k;
+    pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
   }
-  // NOTE(Varun): This constructor is really expensive!!
-  DecisionTreeFactor f(dkeys, table);
+
+  // Sort the pair_table (of assignment-value pairs) based on assignment so we
+  // get values in reverse key order.
+  std::sort(
+      pair_table.begin(), pair_table.end(),
+      [](const std::pair<uint64_t, double>& a,
+         const std::pair<uint64_t, double>& b) { return a.first < b.first; });
+
+  // Create the table vector by extracting the values from pair_table.
+  // The pair_table has already been sorted in the desired order,
+  // so the values will be in descending key order.
+  std::vector<double> table;
+  std::for_each(pair_table.begin(), pair_table.end(),
+                [&table](const std::pair<uint64_t, double>& pair) {
+                  table.push_back(pair.second);
+                });
+
+  AlgebraicDecisionTree<Key> tree(rdkeys, table);
+  DecisionTreeFactor f(dkeys, tree);
   return f;
 }
 
@@ -250,7 +356,8 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const {
     for (auto&& kv : assignment) {
       cout << "(" << formatter(kv.first) << ", " << kv.second << ")";
     }
-    cout << " | " << it.value() << " | " << it.index() << endl;
+    cout << " | " << std::setw(10) << std::left << it.value() << " | "
+         << it.index() << endl;
   }
   cout << "number of nnzs: " << sparse_table_.nonZeros() << endl;
 }
diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h
index d4a12f2df..dbe4671f9 100644
--- a/gtsam/discrete/TableFactor.h
+++ b/gtsam/discrete/TableFactor.h
@@ -81,12 +81,16 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return DiscreteKey(keys_[i], cardinalities_.at(keys_[i]));
   }
 
-  /// Convert probability table given as doubles to SparseVector.
-  /// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
-  static Eigen::SparseVector<double> Convert(const std::vector<double>& table);
+  /**
+   * Convert probability table given as doubles to SparseVector.
+   * Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5}
+   */
+  static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
+                                             const std::vector<double>& table);
 
   /// Convert probability table given as string to SparseVector.
-  static Eigen::SparseVector<double> Convert(const std::string& table);
+  static Eigen::SparseVector<double> Convert(const DiscreteKeys& keys,
+                                             const std::string& table);
 
  public:
   // typedefs needed to play nice with gtsam
@@ -111,11 +115,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
 
   /** Constructor from doubles */
   TableFactor(const DiscreteKeys& keys, const std::vector<double>& table)
-      : TableFactor(keys, Convert(table)) {}
+      : TableFactor(keys, Convert(keys, table)) {}
 
   /** Constructor from string */
   TableFactor(const DiscreteKeys& keys, const std::string& table)
-      : TableFactor(keys, Convert(table)) {}
+      : TableFactor(keys, Convert(keys, table)) {}
 
   /// Single-key specialization
   template <class SOURCE>
@@ -128,6 +132,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
 
   /// Constructor from DecisionTreeFactor
   TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf);
+  TableFactor(const DecisionTreeFactor& dtf);
 
   /// Constructor from DecisionTree<Key, double>/AlgebraicDecisionTree
   TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& dtree);
@@ -151,6 +156,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   // /// @name Standard Interface
   // /// @{
 
+  /// Getter for the underlying sparse vector
+  Eigen::SparseVector<double> sparseTable() const { return sparse_table_; }
+
   /// Evaluate probability distribution, is just look up in TableFactor.
   double evaluate(const Assignment<Key>& values) const override;
 
diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp
index bd3dac514..066f4aae3 100644
--- a/gtsam/discrete/tests/testTableFactor.cpp
+++ b/gtsam/discrete/tests/testTableFactor.cpp
@@ -134,14 +134,45 @@ TEST(TableFactor, constructors) {
   EXPECT(assert_equal(expected, f4));
 
   // Test for 9=3x3 values.
-  DiscreteKey V(0, 3), W(1, 3);
+  DiscreteKey V(0, 3), W(1, 3), O(100, 3);
   DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
   TableFactor f5(conditional5);
-  // GTSAM_PRINT(f5);
-  TableFactor expected_f5(
-      X & Y,
-      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667");
+
+  std::string expected_values =
+      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667";
+  TableFactor expected_f5(V & W, expected_values);
   EXPECT(assert_equal(expected_f5, f5, 1e-6));
+
+  TableFactor f5_with_wrong_keys(V & O, expected_values);
+  EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9));
+}
+
+/* ************************************************************************* */
+// Check conversion from DecisionTreeFactor.
+TEST(TableFactor, Conversion) {
+  /* This is the DecisionTree we are using
+  Choice(m2)
+  0 Choice(m1)
+  0 0 Leaf    0
+  0 1 Choice(m0)
+  0 1 0 Leaf    0
+  0 1 1 Leaf 0.14649446  // 3
+  1 Choice(m1)
+  1 0 Choice(m0)
+  1 0 0 Leaf    0
+  1 0 1 Leaf 0.14648756  // 5
+  1 1 Choice(m0)
+  1 1 0 Leaf 0.14649446  // 6
+  1 1 1 Leaf 0.23918345  // 7
+  */
+  DiscreteKeys dkeys = {{0, 2}, {1, 2}, {2, 2}};
+  DecisionTreeFactor dtf(
+      dkeys, std::vector<double>{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446,
+                                 0.23918345});
+
+  TableFactor tf(dtf.discreteKeys(), dtf);
+
+  EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor()));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h
index 19194bb7a..9c4e42edf 100644
--- a/gtsam/geometry/BearingRange.h
+++ b/gtsam/geometry/BearingRange.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Manifold.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/OptionalJacobian.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <iostream>
@@ -55,9 +55,9 @@ private:
   R range_;
 
 public:
-  enum { dimB = traits<B>::dimension };
-  enum { dimR = traits<R>::dimension };
-  enum { dimension = dimB + dimR };
+  constexpr static const size_t dimB = traits<B>::dimension;
+  constexpr static const size_t dimR = traits<R>::dimension;
+  constexpr static const size_t dimension = dimB + dimR;
 
   /// @name Standard Constructors
   /// @{
@@ -148,7 +148,7 @@ public:
   /// @{
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
@@ -162,9 +162,7 @@ private:
   /// @}
 
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-  enum {
-    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
-  };
+  inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0;
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h
index 1b66eb336..63efb0d49 100644
--- a/gtsam/geometry/Cal3.h
+++ b/gtsam/geometry/Cal3.h
@@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 {
   double u0_ = 0.0f, v0_ = 0.0f;  ///< principal point
 
  public:
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
   ///< shared pointer to calibration object
   using shared_ptr = std::shared_ptr<Cal3>;
 
@@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3 {
   /// @{
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 081688d48..71b5fba1d 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f {
   // Note: u0 and v0 are constants and not optimized.
 
  public:
-  enum { dimension = 3 };
+  inline constexpr static auto dimension = 3;
   using shared_ptr = std::shared_ptr<Cal3Bundler>;
 
   /// @name Constructors
@@ -145,7 +145,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index a7ca08afc..8c1bbe302 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
   using Base = Cal3DS2_Base;
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3DS2>;
@@ -104,7 +104,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h
index b0ef0368b..986c9842b 100644
--- a/gtsam/geometry/Cal3DS2_Base.h
+++ b/gtsam/geometry/Cal3DS2_Base.h
@@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3DS2_Base>;
@@ -156,7 +156,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h
index 3f1cac148..f10a31b65 100644
--- a/gtsam/geometry/Cal3Fisheye.h
+++ b/gtsam/geometry/Cal3Fisheye.h
@@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   double tol_ = 1e-5;             ///< tolerance value when calibrating
 
  public:
-  enum { dimension = 9 };
+  inline constexpr static auto dimension = 9;
   ///< shared pointer to fisheye calibration object
   using shared_ptr = std::shared_ptr<Cal3Fisheye>;
 
@@ -184,7 +184,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 309b6dca1..cef2b8f9b 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   double xi_ = 0.0f;  ///< mirror parameter
 
  public:
-  enum { dimension = 10 };
+  inline constexpr static auto dimension = 10;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3Unified>;
@@ -138,7 +138,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index 885be614f..8a587af9b 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -33,7 +33,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3_S2 : public Cal3 {
  public:
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
 
   ///< shared pointer to calibration object
   using shared_ptr = std::shared_ptr<Cal3_S2>;
@@ -132,7 +132,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
   /// @{
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h
index 4fee1a022..58c7dd37a 100644
--- a/gtsam/geometry/Cal3_S2Stereo.h
+++ b/gtsam/geometry/Cal3_S2Stereo.h
@@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   double b_ = 1.0f;  ///< Stereo baseline.
 
  public:
-  enum { dimension = 6 };
+  inline constexpr static auto dimension = 6;
 
   ///< shared pointer to stereo calibration object
   using shared_ptr = std::shared_ptr<Cal3_S2Stereo>;
@@ -143,7 +143,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
   /// @{
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp
index 319155dc9..b8e365eed 100644
--- a/gtsam/geometry/Cal3f.cpp
+++ b/gtsam/geometry/Cal3f.cpp
@@ -28,6 +28,7 @@
 #include <gtsam/geometry/Cal3f.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h
index f053f3d11..b6cf4435b 100644
--- a/gtsam/geometry/Cal3f.h
+++ b/gtsam/geometry/Cal3f.h
@@ -34,7 +34,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT Cal3f : public Cal3 {
  public:
-  enum { dimension = 1 };
+  inline constexpr static auto dimension = 1;
   using shared_ptr = std::shared_ptr<Cal3f>;
 
   /// @name Constructors
@@ -118,7 +118,7 @@ class GTSAM_EXPORT Cal3f : public Cal3 {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index dca15feb2..d364318c9 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -25,7 +25,7 @@
 #include <gtsam/base/Manifold.h>
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/dllexport.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -230,7 +230,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -252,9 +252,7 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
 
 public:
 
-  enum {
-    dimension = 6
-  };
+  inline constexpr static auto dimension = 6;
 
   /// @name Standard Constructors
   /// @{
@@ -408,7 +406,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 26d4952c8..36c1e1f54 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -26,6 +26,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
@@ -471,7 +472,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index d514c4f19..74b736685 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -82,7 +82,7 @@ class EssentialMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 5 };
+  inline constexpr static auto dimension = 5;
   inline static size_t Dim() { return dimension;}
   inline size_t dim() const { return dimension;}
 
@@ -180,7 +180,7 @@ class EssentialMatrix {
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h
index df2c2881a..0752ced7d 100644
--- a/gtsam/geometry/FundamentalMatrix.h
+++ b/gtsam/geometry/FundamentalMatrix.h
@@ -102,7 +102,7 @@ class GTSAM_EXPORT FundamentalMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 7 };  // 3 for U, 1 for s, 3 for V
+  inline constexpr static auto dimension = 7;  // 3 for U, 1 for s, 3 for V
   inline static size_t Dim() { return dimension; }
   inline size_t dim() const { return dimension; }
 
@@ -179,7 +179,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
 
   /// @name Manifold
   /// @{
-  enum { dimension = 7 };  // 5 for E, 1 for fa, 1 for fb
+  inline constexpr static auto dimension = 7;  // 5 for E, 1 for fa, 1 for fb
   inline static size_t Dim() { return dimension; }
   inline size_t dim() const { return dimension; }
 
diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h
index def49d268..5ae50e138 100644
--- a/gtsam/geometry/Line3.h
+++ b/gtsam/geometry/Line3.h
@@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 {
   double a_, b_;  // Intersection of line with the world x-y plane rotated by R_
   // Also the closest point on line to origin
  public:
-  enum { dimension = 4 };
+  inline constexpr static auto dimension = 4;
 
   /** Default constructor is the Z axis **/
   Line3() :
diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h
index 07c8445fe..3d84086ac 100644
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@@ -39,9 +39,7 @@ private:
   double d_;    ///< The perpendicular distance to this plane
 
 public:
-  enum {
-    dimension = 3
-  };
+  inline constexpr static auto dimension = 3;
 
   /// @name Constructors
   /// @{
diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h
index 8329664fd..f8599572d 100644
--- a/gtsam/geometry/PinholeCamera.h
+++ b/gtsam/geometry/PinholeCamera.h
@@ -51,9 +51,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 6 + DimK
-  }; ///< Dimension depends on calibration
+  inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration
 
   /// @name Standard Constructors
   /// @{
@@ -326,7 +324,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h
index df6ec5c08..252d8bc6f 100644
--- a/gtsam/geometry/PinholePose.h
+++ b/gtsam/geometry/PinholePose.h
@@ -212,7 +212,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -245,9 +245,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 6
-  }; ///< There are 6 DOF to optimize for
+  inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for
 
   /// @name Standard Constructors
   /// @{
@@ -427,7 +425,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h
index 55269f9d6..45cdb11e4 100644
--- a/gtsam/geometry/PinholeSet.h
+++ b/gtsam/geometry/PinholeSet.h
@@ -64,7 +64,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 76beea11a..66d57f785 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -19,7 +19,7 @@
 
 #include <gtsam/base/VectorSpace.h>
 #include <gtsam/base/std_optional_serialization.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index ef91108eb..79fd6b9bf 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
                 p.x() * q.y() - p.y() * q.x());
 }
 
+Point3 doubleCross(const Point3 &p, const Point3 &q,  //
+                   OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) {
+  if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose();
+  if (H2) {
+    const Matrix3 W = skewSymmetric(p);
+    *H2 = W * W;
+  }
+  return gtsam::cross(p, gtsam::cross(p, q));
+}
+
 double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
            OptionalJacobian<1, 3> H2) {
   if (H1) *H1 << q.x(), q.y(), q.z();
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 8da83817d..9bc943a43 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -26,7 +26,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 #include <gtsam/base/VectorSerialization.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <numeric>
@@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});
 /// normalize, with optional Jacobian
 GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});
 
-/// cross product @return this x q
+/// cross product @return p x q
 GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                           OptionalJacobian<3, 3> H_p = {},
                           OptionalJacobian<3, 3> H_q = {});
 
+/// double cross product @return p x (p x q)
+GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
+                                OptionalJacobian<3, 3> H1 = {},
+                                OptionalJacobian<3, 3> H2 = {});
+
 /// dot product
 GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                         OptionalJacobian<1, 3> H_p = {},
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 05678dc27..640481406 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/concepts.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <iomanip>
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index aad81493f..3f85a7f42 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -81,9 +81,13 @@ public:
   Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
 
   /** Constructor from 3*3 matrix */
-  Pose2(const Matrix &T) :
-    r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
-    assert(T.rows() == 3 && T.cols() == 3);
+  Pose2(const Matrix &T)
+      : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
+#ifndef NDEBUG
+    if (T.rows() != 3 || T.cols() != 3) {
+      throw;
+    }
+#endif
   }
 
   /// @}
@@ -335,7 +339,7 @@ public:
 
  private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
   // Serialization function
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 894314491..6d9256f93 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -11,23 +11,21 @@
 
 /**
  * @file  Pose3.cpp
- * @brief 3D Pose
+ * @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
  */
 
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/concepts.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/concepts.h>
 
 #include <cmath>
 #include <iostream>
-#include <limits>
 #include <string>
 
 namespace gtsam {
 
-using std::vector;
-
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3)
 
@@ -45,6 +43,20 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
   return Pose3(R, t);
 }
 
+// Pose2 constructor Jacobian is always the same.
+static const Matrix63 Hpose2 = (Matrix63() << //
+    0., 0., 0., //
+    0., 0., 0.,//
+    0., 0., 1.,//
+    1., 0., 0.,//
+    0., 1., 0.,//
+    0., 0., 0.).finished();
+
+Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
+  if (H) *H << Hpose2;
+  return Pose3(p);
+}
+
 /* ************************************************************************* */
 Pose3 Pose3::inverse() const {
   Rot3 Rt = R_.inverse();
@@ -114,7 +126,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
 
 /* ************************************************************************* */
 Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
-    OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
+                       OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
   if (Hxi) {
     Hxi->setZero();
     for (int i = 0; i < 6; ++i) {
@@ -161,27 +173,31 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
 /* ************************************************************************* */
 Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
   return Pose3(interpolate<Rot3>(R_, T.R_, t),
-                interpolate<Point3>(t_, T.t_, t));
+               interpolate<Point3>(t_, T.t_, t));
 }
 
 /* ************************************************************************* */
-/** Modified from Murray94book version (which assumes w and v normalized?) */
+// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's
+// elegant Lie group document, at https://www.ethaneade.org/lie.pdf.
 Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
-  if (Hxi) *Hxi = ExpmapDerivative(xi);
+  // Get angular velocity omega and translational velocity v from twist xi
+  const Vector3 w = xi.head<3>(), v = xi.tail<3>();
 
-  // get angular velocity omega and translational velocity v from twist xi
-  Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
+  // Compute rotation using Expmap
+  Matrix3 Jr;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
 
-  Rot3 R = Rot3::Expmap(omega);
-  double theta2 = omega.dot(omega);
-  if (theta2 > std::numeric_limits<double>::epsilon()) {
-    Vector3 t_parallel = omega * omega.dot(v);  // translation parallel to axis
-    Vector3 omega_cross_v = omega.cross(v);     // points towards axis
-    Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
-    return Pose3(R, t);
-  } else {
-    return Pose3(R, v);
+  // Compute translation and optionally its Jacobian Q in w
+  // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
+  Matrix3 Q;
+  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
+
+  if (Hxi) {
+    *Hxi << Jr, Z_3x3,  //
+        Q, Jr;
   }
+
+  return Pose3(R, t);
 }
 
 /* ************************************************************************* */
@@ -240,55 +256,55 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 }
 
 /* ************************************************************************* */
-Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
+Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
+                                           double nearZeroThreshold) {
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
-  const Matrix3 V = skewSymmetric(v);
-  const Matrix3 W = skewSymmetric(w);
-
   Matrix3 Q;
-
-#ifdef NUMERICAL_EXPMAP_DERIV
-  Matrix3 Qj = Z_3x3;
-  double invFac = 1.0;
-  Q = Z_3x3;
-  Matrix3 Wj = I_3x3;
-  for (size_t j=1; j<10; ++j) {
-    Qj = Qj*W + Wj*V;
-    invFac = -invFac/(j+1);
-    Q = Q + invFac*Qj;
-    Wj = Wj*W;
-  }
-#else
-  // The closed-form formula in Barfoot14tro eq. (102)
-  double phi = w.norm();
-  const Matrix3 WVW = W * V * W;
-  if (std::abs(phi) > nearZeroThreshold) {
-    const double s = sin(phi), c = cos(phi);
-    const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi,
-                 phi5 = phi4 * phi;
-    // Invert the sign of odd-order terms to have the right Jacobian
-    Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) +
-        (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) -
-        0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) *
-            (WVW * W + W * WVW);
-  } else {
-    Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) -
-        1. / 24. * (W * W * V + V * W * W - 3 * WVW) +
-        1. / 120. * (WVW * W + W * WVW);
-  }
-#endif
-
+  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
   return Q;
 }
 
+/* ************************************************************************* */
+// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
+//   t_parallel = w * w.dot(v);  // translation parallel to axis
+//   w_cross_v = w.cross(v);     // translation orthogonal to axis
+//   t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
+// but functor does not need R, deals automatically with the case where theta2
+// is near zero, and also gives us the machinery for the Jacobians.
+Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
+                                 OptionalJacobian<3, 3> Q,
+                                 OptionalJacobian<3, 3> J,
+                                 double nearZeroThreshold) {
+  const double theta2 = w.dot(w);
+  bool nearZero = (theta2 <= nearZeroThreshold);
+
+  // Instantiate functor for Dexp-related operations:
+  so3::DexpFunctor local(w, nearZero);
+
+  // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
+  // don't need Jacobians, and returns Jacobian of t with respect to w if asked.
+  Matrix3 H;
+  Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
+
+  // We return Jacobians for use in Expmap, so we multiply with X, that
+  // translates from left to right for our right expmap convention:
+  if (Q) {
+    Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
+    *Q = X * H;
+  }
+
+  if (J) {
+    *J = local.rightJacobian();  // = X * local.leftJacobian();
+  }
+
+  return t;
+}
+
 /* ************************************************************************* */
 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
-  const Vector3 w = xi.head<3>();
-  const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   Matrix6 J;
-  J << Jw, Z_3x3, Q, Jw;
+  Expmap(xi, J);
   return J;
 }
 
@@ -311,7 +327,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
 }
 
 /* ************************************************************************* */
-
 const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
   if (Hself) {
     *Hself << I_3x3, Z_3x3;
@@ -329,14 +344,14 @@ Matrix4 Pose3::matrix() const {
 
 /* ************************************************************************* */
 Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
-                                                 OptionalJacobian<6, 6> HaTb) const {
+                               OptionalJacobian<6, 6> HaTb) const {
   const Pose3& wTa = *this;
   return wTa.compose(aTb, Hself, HaTb);
 }
 
 /* ************************************************************************* */
 Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
-                                               OptionalJacobian<6, 6> HwTb) const {
+                             OptionalJacobian<6, 6> HwTb) const {
   if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
   if (HwTb) *HwTb = I_6x6;
   const Pose3& wTa = *this;
@@ -345,7 +360,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
 
 /* ************************************************************************* */
 Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
-    OptionalJacobian<3, 3> Hpoint) const {
+                            OptionalJacobian<3, 3> Hpoint) const {
   // Only get matrix once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 R = R_.matrix();
@@ -369,7 +384,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const {
 
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
-    OptionalJacobian<3, 3> Hpoint) const {
+                          OptionalJacobian<3, 3> Hpoint) const {
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 Rt = R_.transpose();
@@ -475,7 +490,7 @@ std::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
 std::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.");
+        "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
   for (Eigen::Index j = 0; j < a.cols(); j++) {
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 8a807cc23..5805ce41b 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -11,7 +11,7 @@
 
 /**
  *@file  Pose3.h
- *@brief 3D Pose
+ * @brief 3D Pose manifold SO(3) x R^3 and group SE(3)
  */
 
 // \callgraph
@@ -78,6 +78,9 @@ public:
                       OptionalJacobian<6, 3> HR = {},
                       OptionalJacobian<6, 3> Ht = {});
 
+  /** Construct from Pose2 in the xy plane, with derivative. */
+  static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});
+
   /**
    *  Create Pose3 by aligning two point pairs
    *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
@@ -217,10 +220,27 @@ public:
   *  (see Chirikjian11book2, pg 44, eq 10.95.
   *  The closed-form formula is identical to formula 102 in Barfoot14tro where
   *  Q_l of the SE3 Expmap left derivative matrix is given.
+  *  This is the Jacobian of ExpmapTranslation and computed there.
   */
   static Matrix3 ComputeQforExpmapDerivative(
       const Vector6& xi, double nearZeroThreshold = 1e-5);
 
+  /**
+   * Compute the translation part of the exponential map, with Jacobians.
+   * @param w 3D angular velocity
+   * @param v 3D velocity
+   * @param Q Optionally, compute 3x3 Jacobian wrpt w
+   * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
+   * @param nearZeroThreshold threshold for small values
+   * @note This function returns Jacobians Q and J corresponding to the bottom
+   * blocks of the SE(3) exponential, and translated from left to right from the
+   * applyLeftJacobian Jacobians.
+   */
+  static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
+                                   OptionalJacobian<3, 3> Q = {},
+                                   OptionalJacobian<3, 3> J = {},
+                                   double nearZeroThreshold = 1e-5);
+
   using LieGroup<Pose3, 6>::inverse; // version with derivative
 
   /**
@@ -389,7 +409,7 @@ public:
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h
index f9ed2a383..36f891505 100644
--- a/gtsam/geometry/Quaternion.h
+++ b/gtsam/geometry/Quaternion.h
@@ -45,9 +45,7 @@ struct traits<QUATERNION_TYPE> {
   /// @}
   /// @name Basic manifold traits
   /// @{
-  enum {
-    dimension = 3
-  };
+  inline constexpr static auto dimension = 3;
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
 
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 6bb97ff6c..07f328b96 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -213,7 +213,7 @@ namespace gtsam {
     static Rot2 ClosestTo(const Matrix2& M);
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index a936bd02a..10ba5894e 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -23,6 +23,7 @@
 #include <gtsam/geometry/SO3.h>
 
 #include <cmath>
+#include <cassert>
 #include <random>
 
 using namespace std;
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 7e05ee4da..71f4cbacd 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -159,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
     inline static Rot3 RzRyRx(const Vector& xyz,
                               OptionalJacobian<3, 3> H = {}) {
-      assert(xyz.size() == 3);
+#ifndef NDEBUG
+      if (xyz.size() != 3) {
+        throw;
+      }
+#endif
       Rot3 out;
       if (H) {
         Vector3 Hx, Hy, Hz;
@@ -528,7 +532,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
index 2585c37be..41f7ce810 100644
--- a/gtsam/geometry/SO3.cpp
+++ b/gtsam/geometry/SO3.cpp
@@ -18,13 +18,14 @@
  * @date    December 2014
  */
 
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/SO3.h>
 
 #include <Eigen/SVD>
-
 #include <cmath>
-#include <iostream>
 #include <limits>
 
 namespace gtsam {
@@ -32,6 +33,15 @@ namespace gtsam {
 //******************************************************************************
 namespace so3 {
 
+static constexpr double one_6th = 1.0 / 6.0;
+static constexpr double one_12th = 1.0 / 12.0;
+static constexpr double one_24th = 1.0 / 24.0;
+static constexpr double one_60th = 1.0 / 60.0;
+static constexpr double one_120th = 1.0 / 120.0;
+static constexpr double one_180th = 1.0 / 180.0;
+static constexpr double one_720th = 1.0 / 720.0;
+static constexpr double one_1260th = 1.0 / 1260.0;
+
 GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
   Matrix99 H;
   auto R = Q.matrix();
@@ -41,7 +51,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
   return H;
 }
 
-GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
+GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
+                             OptionalJacobian<9, 9> H) {
   Matrix3 MR = M * R.matrix();
   if (H) *H = Dcompose(R);
   return MR;
@@ -51,82 +62,128 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
   nearZero =
       nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
   if (!nearZero) {
-    sin_theta = std::sin(theta);
+    const double sin_theta = std::sin(theta);
+    A = sin_theta / theta;
     const double s2 = std::sin(theta / 2.0);
-    one_minus_cos = 2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    const double one_minus_cos =
+        2.0 * s2 * s2;  // numerically better than [1 - cos(theta)]
+    B = one_minus_cos / theta2;
+  } else {
+    // Taylor expansion at 0
+    A = 1.0 - theta2 * one_6th;
+    B = 0.5 - theta2 * one_24th;
   }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
-    : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
-  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
-  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+    : theta2(omega.dot(omega)),
+      theta(std::sqrt(theta2)),
+      W(skewSymmetric(omega)),
+      WW(W * W) {
   init(nearZeroApprox);
-  if (!nearZero) {
-    K = W / theta;
-    KK = K * K;
-  }
 }
 
 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
                              bool nearZeroApprox)
-    : theta2(angle * angle), theta(angle) {
-  const double ax = axis.x(), ay = axis.y(), az = axis.z();
-  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
-  W = K * angle;
+    : theta2(angle * angle),
+      theta(angle),
+      W(skewSymmetric(axis * angle)),
+      WW(W * W) {
   init(nearZeroApprox);
-  if (!nearZero) {
-    KK = K * K;
-  }
 }
 
-SO3 ExpmapFunctor::expmap() const {
-  if (nearZero)
-    return SO3(I_3x3 + W);
-  else
-    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
-}
+SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  if (nearZero) {
-    dexp_ = I_3x3 - 0.5 * W;
+  if (!nearZero) {
+    C = (1 - A) / theta2;
+    D = (1.0 - A / (2.0 * B)) / theta2;
+    E = (2.0 * B - A) / theta2;
+    F = (3.0 * C - B) / theta2;
   } else {
-    a = one_minus_cos / theta;
-    b = 1.0 - sin_theta / theta;
-    dexp_ = I_3x3 - a * K + b * KK;
+    // Taylor expansion at 0
+    // TODO(Frank): flipping signs here does not trigger any tests: harden!
+    C = one_6th - theta2 * one_120th;
+    D = one_12th + theta2 * one_720th;
+    E = one_12th - theta2 * one_180th;
+    F = one_60th - theta2 * one_1260th;
   }
 }
 
+Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
+  // Wv = omega x v
+  const Vector3 Wv = gtsam::cross(omega, v);
+  if (H) {
+    // Apply product rule to (B Wv)
+    // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
+    // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
+    *H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
+  }
+  return B * Wv;
+}
+
+Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
+                                  OptionalJacobian<3, 3> H) const {
+  // WWv = omega x (omega x * v)
+  Matrix3 doubleCrossJacobian;
+  const Vector3 WWv =
+      gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
+  if (H) {
+    // Apply product rule to (C WWv)
+    // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
+    // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
+    *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
+  }
+  return C * WWv;
+}
+
+// Multiplies v with left Jacobian through vector operations only.
 Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                OptionalJacobian<3, 3> H2) const {
-  if (H1) {
-    if (nearZero) {
-      *H1 = 0.5 * skewSymmetric(v);
-    } else {
-      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
-      const Vector3 Kv = K * v;
-      const double Da = (sin_theta - 2.0 * a) / theta2;
-      const double Db = (one_minus_cos - 3.0 * b) / theta2;
-      *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
-            skewSymmetric(Kv * b / theta) +
-            (a * I_3x3 - b * K) * skewSymmetric(v / theta);
-    }
-  }
-  if (H2) *H2 = dexp_;
-  return dexp_ * v;
+  Matrix3 D_BWv_w, D_CWWv_w;
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
+  if (H1) *H1 = -D_BWv_w + D_CWWv_w;
+  if (H2) *H2 = rightJacobian();
+  return v - BWv + CWWv;
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = dexp_.inverse();
-  const Vector3 c = invDexp * v;
+  const Matrix3 invJr = rightJacobianInverse();
+  const Vector3 c = invJr * v;
   if (H1) {
-    Matrix3 D_dexpv_omega;
-    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_omega;
+    Matrix3 H;
+    applyDexp(c, H);  // get derivative H of forward mapping
+    *H1 = -invJr * H;
   }
-  if (H2) *H2 = invDexp;
+  if (H2) *H2 = invJr;
+  return c;
+}
+
+Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
+                                       OptionalJacobian<3, 3> H1,
+                                       OptionalJacobian<3, 3> H2) const {
+  Matrix3 D_BWv_w, D_CWWv_w;
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
+  if (H1) *H1 = D_BWv_w + D_CWWv_w;
+  if (H2) *H2 = leftJacobian();
+  return v + BWv + CWWv;
+}
+
+Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
+                                              OptionalJacobian<3, 3> H1,
+                                              OptionalJacobian<3, 3> H2) const {
+  const Matrix3 invJl = leftJacobianInverse();
+  const Vector3 c = invJl * v;
+  if (H1) {
+    Matrix3 H;
+    applyLeftJacobian(c, H);  // get derivative H of forward mapping
+    *H1 = -invJl * H;
+  }
+  if (H2) *H2 = invJl;
   return c;
 }
 
@@ -168,12 +225,7 @@ SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
 template <>
 GTSAM_EXPORT
 Matrix3 SO3::Hat(const Vector3& xi) {
-  // skew symmetric matrix X = xi^
-  Matrix3 Y = Z_3x3;
-  Y(0, 1) = -xi(2);
-  Y(0, 2) = +xi(1);
-  Y(1, 2) = -xi(0);
-  return Y - Y.transpose();
+  return skewSymmetric(xi);
 }
 
 //******************************************************************************
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
index cd67bfb8c..36329a19d 100644
--- a/gtsam/geometry/SO3.h
+++ b/gtsam/geometry/SO3.h
@@ -26,7 +26,6 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/dllexport.h>
 
-#include <cmath>
 #include <vector>
 
 namespace gtsam {
@@ -99,7 +98,7 @@ template <>
 GTSAM_EXPORT
 Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 template <class Archive>
 /** Serialization function */
 void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
@@ -133,16 +132,17 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
 // functor also implements dedicated methods to apply dexp and/or inv(dexp).
 
 /// Functor implementing Exponential map
-class GTSAM_EXPORT ExpmapFunctor {
- protected:
-  const double theta2;
-  Matrix3 W, K, KK;
+/// Math is based on Ethan Eade's elegant Lie group document, at
+/// https://www.ethaneade.org/lie.pdf.
+struct GTSAM_EXPORT ExpmapFunctor {
+  const double theta2, theta;
+  const Matrix3 W, WW;
   bool nearZero;
-  double theta, sin_theta, one_minus_cos;  // only defined if !nearZero
 
-  void init(bool nearZeroApprox = false);
+  // Ethan Eade's constants:
+  double A;  // A = sin(theta) / theta
+  double B;  // B = (1 - cos(theta))
 
- public:
   /// Constructor with element of Lie algebra so(3)
   explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
@@ -151,34 +151,75 @@ class GTSAM_EXPORT ExpmapFunctor {
 
   /// Rodrigues formula
   SO3 expmap() const;
+
+ protected:
+  void init(bool nearZeroApprox = false);
 };
 
 /// Functor that implements Exponential map *and* its derivatives
-class DexpFunctor : public ExpmapFunctor {
+/// Math extends Ethan theme of elegant I + aW + bWW expressions.
+/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83).
+struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
   const Vector3 omega;
-  double a, b;
-  Matrix3 dexp_;
 
- public:
+  // Ethan's C constant used in Jacobians
+  double C;  // (1 - A) / theta^2
+
+  // Constant used in inverse Jacobians
+  double D;  // (1 - A/2B) / theta2
+
+  // Constants used in cross and doubleCross
+  double E;  // (2B - A) / theta2
+  double F;  // (3C - B) / theta2
+
   /// Constructor with element of Lie algebra so(3)
-  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
+  explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
 
   // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
   // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
   // Information Theory, and Lie Groups", Volume 2, 2008.
-  //   expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
-  // This maps a perturbation v in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v) */
-  const Matrix3& dexp() const { return dexp_; }
+  //   Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
+  // This maps a perturbation dxi=(w,v) in the tangent space to
+  // a perturbation on the manifold Expmap(dexp * xi)
+  Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; }
+
+  // Compute the left Jacobian for Exponential map in SO(3)
+  Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
+
+  /// Differential of expmap == right Jacobian
+  inline Matrix3 dexp() const { return rightJacobian(); }
+
+  /// Inverse of right Jacobian
+  Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
+
+  // Inverse of left Jacobian
+  Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
+
+  /// Synonym for rightJacobianInverse
+  inline Matrix3 invDexp() const { return rightJacobianInverse(); }
+
+  /// Computes B * (omega x v).
+  Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
+
+  /// Computes C * (omega x (omega x v)).
+  Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
 
   /// Multiplies with dexp(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+  Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
                     OptionalJacobian<3, 3> H2 = {}) const;
 
   /// Multiplies with dexp().inverse(), with optional derivatives
-  GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
-                       OptionalJacobian<3, 3> H1 = {},
+  Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
                        OptionalJacobian<3, 3> H2 = {}) const;
+
+  /// Multiplies with leftJacobian(), with optional derivatives
+  Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
+                            OptionalJacobian<3, 3> H2 = {}) const;
+
+  /// Multiplies with leftJacobianInverse(), with optional derivatives
+  Vector3 applyLeftJacobianInverse(const Vector3& v,
+                                   OptionalJacobian<3, 3> H1 = {},
+                                   OptionalJacobian<3, 3> H2 = {}) const;
 };
 }  //  namespace so3
 
diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h
index 834cab746..43b74f7aa 100644
--- a/gtsam/geometry/SO4.h
+++ b/gtsam/geometry/SO4.h
@@ -78,7 +78,7 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {});
  */
 GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {});
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 template <class Archive>
 /** Serialization function */
 void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index 7e1762b3f..e8e1f641d 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -24,7 +24,7 @@
 #include <gtsam/dllexport.h>
 #include <Eigen/Core>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -33,6 +33,7 @@
 #include <type_traits>
 #include <vector>
 #include <random>
+#include <cassert>
 
 namespace gtsam {
 
@@ -53,7 +54,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
 template <int N>
 class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
  public:
-  enum { dimension = internal::DimensionSO(N) };
+  inline constexpr static auto dimension = internal::DimensionSO(N);
   using MatrixNN = Eigen::Matrix<double, N, N>;
   using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
   using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
@@ -325,7 +326,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   /// @name Serialization
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   template <class Archive>
   friend void save(Archive&, SO&, const unsigned int);
   template <class Archive>
@@ -379,7 +380,7 @@ template <>
 GTSAM_EXPORT
 typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 /** Serialization function */
 template<class Archive>
 void serialize(
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 05bd0431e..8f7f30be8 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -203,7 +203,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
 
  private:
 
-  #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+  #if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h
index ef20aa7fe..786295d7b 100644
--- a/gtsam/geometry/SphericalCamera.h
+++ b/gtsam/geometry/SphericalCamera.h
@@ -26,7 +26,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -41,7 +41,7 @@ namespace gtsam {
  */
 class GTSAM_EXPORT EmptyCal {
  public:
-  enum { dimension = 0 };
+  inline constexpr static auto dimension = 0;
   EmptyCal() {}
   virtual ~EmptyCal() = default;
   using shared_ptr = std::shared_ptr<EmptyCal>;
@@ -54,7 +54,7 @@ class GTSAM_EXPORT EmptyCal {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class Archive>
@@ -73,7 +73,7 @@ class GTSAM_EXPORT EmptyCal {
  */
 class GTSAM_EXPORT SphericalCamera {
  public:
-  enum { dimension = 6 };
+  inline constexpr static auto dimension = 6;
 
   using Measurement = Unit3;
   using MeasurementVector = std::vector<Unit3>;
@@ -223,7 +223,7 @@ class GTSAM_EXPORT SphericalCamera {
   static size_t Dim() { return 6; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index da71dd070..315e4d267 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -61,9 +61,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 6
-  };
+  inline constexpr static auto dimension = 6;
 
   /// @name Standard Constructors
   /// @{
@@ -179,7 +177,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 4383e212e..26c14750b 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -20,7 +20,7 @@
 
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/base/VectorSpace.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -37,7 +37,7 @@ private:
   double uL_, uR_, v_;
 
 public:
-  enum { dimension = 3 };
+  inline constexpr static auto dimension = 3;
   /// @name Standard Constructors
   /// @{
 
@@ -150,7 +150,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h
index 18bc5d9f0..65a304d62 100644
--- a/gtsam/geometry/Unit3.h
+++ b/gtsam/geometry/Unit3.h
@@ -53,9 +53,7 @@ private:
 
 public:
 
-  enum {
-    dimension = 2
-  };
+  inline constexpr static auto dimension = 2;
 
   /// @name Constructors
   /// @{
@@ -200,7 +198,7 @@ private:
 
   /// @name Advanced Interface
   /// @{
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp
index ff2a9a6a4..df4a4dc51 100644
--- a/gtsam/geometry/tests/testBearingRange.cpp
+++ b/gtsam/geometry/tests/testBearingRange.cpp
@@ -56,7 +56,7 @@ TEST(BearingRange, 3D) {
   EXPECT(assert_equal(expected, actual));
 }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 using namespace serializationTestHelpers;
 /* ************************************************************************* */
 TEST(BearingRange, Serialization2D) {
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index dcfb99105..b3b751ea1 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -154,6 +154,17 @@ TEST( Point3, cross2) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Point3, doubleCross) {
+  Matrix aH1, aH2;
+  std::function<Point3(const Point3&, const Point3&)> f =
+      [](const Point3& p, const Point3& q) { return doubleCross(p, q); };
+  const Point3 omega(1, 2, 3), theta(4, 5, 6);
+  doubleCross(omega, theta, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
+}
+
 //*************************************************************************
 TEST (Point3, normalize) {
   Matrix actualH;
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index c9851f761..b04fb1982 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -835,17 +835,7 @@ TEST(Pose3, Align2) {
 }
 
 /* ************************************************************************* */
-TEST( Pose3, ExpmapDerivative1) {
-  Matrix6 actualH;
-  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
-  Pose3::Expmap(w,actualH);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  EXPECT(assert_equal(expectedH, actualH));
-}
-
-/* ************************************************************************* */
-TEST(Pose3, ExpmapDerivative2) {
+TEST(Pose3, ExpmapDerivative) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@@ -879,26 +869,71 @@ TEST(Pose3, ExpmapDerivative2) {
   }
 }
 
-TEST( Pose3, ExpmapDerivativeQr) {
-  Vector6 w = Vector6::Random();
-  w.head<3>().normalize();
-  w.head<3>() = w.head<3>() * 0.9e-2;
-  Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
-  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
-  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
+//******************************************************************************
+namespace test_cases {
+std::vector<Vector3> small{{0, 0, 0},                                 //
+                           {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
+                           {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
+std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0},    {0, 1, 0},
+                           {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
+auto omegas = [](bool nearZero) { return nearZero ? small : large; };
+std::vector<Vector3> vs{{1, 0, 0},    {0, 1, 0}, {0, 0, 1},
+                        {.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}};
+}  // namespace test_cases
+
+//******************************************************************************
+TEST(Pose3, ExpmapDerivatives) {
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& w : test_cases::omegas(nearZero)) {
+      for (Vector3 v : test_cases::vs) {
+        const Vector6 xi = (Vector6() << w, v).finished();
+        const Matrix6 expectedH =
+            numericalDerivative21<Pose3, Vector6, OptionalJacobian<6, 6> >(
+                &Pose3::Expmap, xi, {});
+        Matrix actualH;
+        Pose3::Expmap(xi, actualH);
+        EXPECT(assert_equal(expectedH, actualH));
+      }
+    }
+  }
 }
 
-/* ************************************************************************* */
-TEST( Pose3, LogmapDerivative) {
-  Matrix6 actualH;
-  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
-  Pose3 p = Pose3::Expmap(w);
-  EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
-  Matrix expectedH = numericalDerivative21<Vector6, Pose3,
-      OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
-  EXPECT(assert_equal(expectedH, actualH));
+//******************************************************************************
+// Check logmap for all small values, as we don't want wrapping.
+TEST(Pose3, Logmap) {
+  static constexpr bool nearZero = true;
+  for (const Vector3& w : test_cases::omegas(nearZero)) {
+    for (Vector3 v : test_cases::vs) {
+      const Vector6 xi = (Vector6() << w, v).finished();
+      Pose3 pose = Pose3::Expmap(xi);
+      EXPECT(assert_equal(xi, Pose3::Logmap(pose)));
+    }
+  }
+}
+
+//******************************************************************************
+// Check logmap derivatives for all values
+TEST(Pose3, LogmapDerivatives) {
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& w : test_cases::omegas(nearZero)) {
+      for (Vector3 v : test_cases::vs) {
+        const Vector6 xi = (Vector6() << w, v).finished();
+        Pose3 pose = Pose3::Expmap(xi);
+        const Matrix6 expectedH =
+            numericalDerivative21<Vector6, Pose3, OptionalJacobian<6, 6> >(
+                &Pose3::Logmap, pose, {});
+        Matrix actualH;
+        Pose3::Logmap(pose, actualH);
+#ifdef GTSAM_USE_QUATERNIONS
+        // TODO(Frank): Figure out why quaternions are not as accurate.
+        // Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS.
+        EXPECT(assert_equal(expectedH, actualH, 1e-7));
+#else
+        EXPECT(assert_equal(expectedH, actualH));
+#endif
+      }
+    }
+  }
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 27c143d31..c6fd52161 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -304,18 +304,77 @@ TEST(SO3, JacobianLogmap) {
 }
 
 //******************************************************************************
-TEST(SO3, ApplyDexp) {
-  Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+namespace test_cases {
+std::vector<Vector3> small{{0, 0, 0},                                 //
+                           {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
+                           {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
+std::vector<Vector3> large{
+    {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
+auto omegas = [](bool nearZero) { return nearZero ? small : large; };
+std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
+}  // namespace test_cases
+
+//******************************************************************************
+TEST(SO3, JacobianInverses) {
+  Matrix HR, HL;
+  for (bool nearZero : {true, false}) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
+                                   local.rightJacobianInverse()));
+      EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
+                                   local.leftJacobianInverse()));
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, CrossB) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
+          return so3::DexpFunctor(omega, nearZero).crossB(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (const Vector3& v : test_cases::vs) {
+        local.crossB(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, DoubleCrossC) {
+  Matrix aH1;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (const Vector3& v : test_cases::vs) {
+        local.doubleCrossC(v, aH1);
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, ApplyDexp) {
+  Matrix aH1, aH2;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).applyDexp(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (const Vector3& v : test_cases::vs) {
         EXPECT(assert_equal(Vector3(local.dexp() * v),
                             local.applyDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
@@ -329,22 +388,63 @@ TEST(SO3, ApplyDexp) {
 //******************************************************************************
 TEST(SO3, ApplyInvDexp) {
   Matrix aH1, aH2;
-  for (bool nearZeroApprox : {true, false}) {
+  for (bool nearZero : {true, false}) {
     std::function<Vector3(const Vector3&, const Vector3&)> f =
         [=](const Vector3& omega, const Vector3& v) {
-          return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
+          return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
         };
-    for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
-      so3::DexpFunctor local(omega, nearZeroApprox);
-      Matrix invDexp = local.dexp().inverse();
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
-                        Vector3(0.4, 0.3, 0.2)}) {
-        EXPECT(assert_equal(Vector3(invDexp * v),
-                            local.applyInvDexp(v, aH1, aH2)));
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      Matrix invJr = local.rightJacobianInverse();
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(
+            assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
         EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
         EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
-        EXPECT(assert_equal(invDexp, aH2));
+        EXPECT(assert_equal(invJr, aH2));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, ApplyLeftJacobian) {
+  Matrix aH1, aH2;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
+                            local.applyLeftJacobian(v, aH1, aH2)));
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(local.leftJacobian(), aH2));
+      }
+    }
+  }
+}
+
+//******************************************************************************
+TEST(SO3, ApplyLeftJacobianInverse) {
+  Matrix aH1, aH2;
+  for (bool nearZero : {true, false}) {
+    std::function<Vector3(const Vector3&, const Vector3&)> f =
+        [=](const Vector3& omega, const Vector3& v) {
+          return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
+        };
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
+      so3::DexpFunctor local(omega, nearZero);
+      Matrix invJl = local.leftJacobianInverse();
+      for (const Vector3& v : test_cases::vs) {
+        EXPECT(assert_equal(Vector3(invJl * v),
+                            local.applyLeftJacobianInverse(v, aH1, aH2)));
+        EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
+        EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
+        EXPECT(assert_equal(invJl, aH2));
       }
     }
   }
diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index 0c4c2c725..855e50a42 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -499,7 +499,7 @@ TEST(Unit3, CopyAssign) {
 }
 
 /* ************************************************************************* */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 TEST(actualH, Serialization) {
   Unit3 p(0, 1, 0);
   EXPECT(serializationTestHelpers::equalsObj(p));
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index d82da3459..749824845 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -20,7 +20,7 @@
 
 #pragma once
 
-#include "gtsam/geometry/Point3.h"
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
@@ -317,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements) {
   const size_t nrMeasurements = measurements.size();
-  assert(nrMeasurements == cameras.size());
+#ifndef NDEBUG
+  if (nrMeasurements != cameras.size()) {
+    throw;
+  }
+#endif
   typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
   for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
@@ -618,7 +622,7 @@ struct GTSAM_EXPORT TriangulationParameters {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -683,7 +687,7 @@ class TriangulationResult : public std::optional<Point3> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 96afb87d6..3e07c71ce 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -268,7 +268,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 0fe9ca6ea..06d880f02 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -115,7 +115,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index f3df725ef..a08b3a6ee 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -217,7 +217,7 @@ class GTSAM_EXPORT HybridConditional
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 8d6fef3f4..9fc280322 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -140,7 +140,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
index 0d2b1323c..e769662ed 100644
--- a/gtsam/hybrid/HybridGaussianConditional.h
+++ b/gtsam/hybrid/HybridGaussianConditional.h
@@ -253,7 +253,7 @@ class GTSAM_EXPORT HybridGaussianConditional
   /// Check whether `given` has values for all frontal keys.
   bool allFrontalsGiven(const VectorValues &given) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 6d1064647..efbba9e51 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -176,7 +176,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   // Private constructor using ConstructorHelper above.
   HybridGaussianFactor(const ConstructorHelper &helper);
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/hybrid/HybridGaussianProductFactor.h b/gtsam/hybrid/HybridGaussianProductFactor.h
index 60a58a3a5..f5e75aa86 100644
--- a/gtsam/hybrid/HybridGaussianProductFactor.h
+++ b/gtsam/hybrid/HybridGaussianProductFactor.h
@@ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianProductFactor
   ///@}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class Archive>
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 22d3c7dd2..b9bdcbed2 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -22,6 +22,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 95d475a02..fdfa94d2b 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -27,7 +27,7 @@
 
 #include <fstream>
 #include <queue>
-
+#include <cassert>
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index d0ebd916e..03f79c8cf 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -262,7 +262,7 @@ namespace gtsam {
     template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h
index adffa2f14..0ccb04e90 100644
--- a/gtsam/inference/BayesTreeCliqueBase.h
+++ b/gtsam/inference/BayesTreeCliqueBase.h
@@ -208,7 +208,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h
index 318624608..2a5f0b455 100644
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/gtsam/inference/ClusterTree-inst.h
@@ -19,6 +19,7 @@
 #include <mutex>
 #endif
 #include <queue>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h
index f9da36b7b..31e451714 100644
--- a/gtsam/inference/Conditional.h
+++ b/gtsam/inference/Conditional.h
@@ -233,7 +233,7 @@ namespace gtsam {
     // Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
     const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index e9acc12e1..16449d0f4 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/gtsam/inference/EliminationTree-inst.h
@@ -19,6 +19,7 @@
 
 #include <stack>
 #include <queue>
+#include <cassert>
 
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index 074151e16..19741b22e 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -21,7 +21,7 @@
 
 #pragma once
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <gtsam/base/FastVector.h>
@@ -193,7 +193,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /// @name Serialization
     /// @{
     /** Serialization function */
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index b6046d0bb..ca7321359 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -29,7 +29,7 @@
 
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
 #endif
@@ -420,7 +420,7 @@ class FactorGraph {
   inline bool exists(size_t idx) const { return idx < size() && at(idx); }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h
index 0f79c2a9a..6a8d6918a 100644
--- a/gtsam/inference/JunctionTree-inst.h
+++ b/gtsam/inference/JunctionTree-inst.h
@@ -25,6 +25,8 @@
 #include <gtsam/symbolic/SymbolicConditional.h>
 #include <gtsam/symbolic/SymbolicFactor-inst.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 template<class BAYESTREE, class GRAPH, class ETREE_NODE>
diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h
index 927fb7669..16ac7bccc 100644
--- a/gtsam/inference/LabeledSymbol.h
+++ b/gtsam/inference/LabeledSymbol.h
@@ -142,7 +142,7 @@ class GTSAM_EXPORT LabeledSymbol {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index abdf11c5f..e051b9ae9 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -92,7 +92,11 @@ public:
     return nKeys_;
   }
   Key intToKey(int32_t value) const {
-    assert(value >= 0);
+#ifndef NDEBUG
+    if (value < 0) {
+      throw;
+    }
+#endif
     return intKeyBMap_.right.find(value)->second;
   }
 
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index cb2ca752d..95eb17a1c 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -18,6 +18,7 @@
 
 #include <vector>
 #include <limits>
+#include <cassert>
 
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index c3df1b8a0..fced5d4f2 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -252,7 +252,7 @@ private:
   static Ordering ColamdConstrained(
       const VariableIndex& variableIndex, std::vector<int>& cmember);
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h
index 9f2133707..a62c22fac 100644
--- a/gtsam/inference/Symbol.h
+++ b/gtsam/inference/Symbol.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <cstdint>
@@ -124,7 +124,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
index 110c0bba4..2f872b25d 100644
--- a/gtsam/inference/VariableIndex.h
+++ b/gtsam/inference/VariableIndex.h
@@ -192,7 +192,7 @@ protected:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index e858bbe43..538d1532e 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -267,7 +267,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 27270fece..14b1ce87f 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -278,7 +278,7 @@ namespace gtsam {
     /// @}
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index c5133c6b2..253bbff0f 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -179,7 +179,7 @@ namespace gtsam {
     /// @}
     
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 6a7848f51..67313c086 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -400,7 +400,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 3c3050f90..4e8ef804c 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -25,13 +25,14 @@
 #include <gtsam/base/cholesky.h>
 #include <gtsam/base/debug.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/base/timing.h>
 
 #include <sstream>
+#include <cassert>
 #include <limits>
-#include "gtsam/base/Vector.h"
 
 using namespace std;
 
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 8cbabcd5d..189020712 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -23,7 +23,6 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
 
-
 namespace gtsam {
 
   // Forward declarations
@@ -241,14 +240,18 @@ namespace gtsam {
      * use, for example, begin() + 2 to get the 3rd variable in this factor.
      * @return The linear term \f$ g \f$ */
     SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
-      assert(!empty());
+#ifndef NDEBUG
+      if(empty()) throw;
+#endif
       return info_.aboveDiagonalBlock(j - begin(), size());
     }
 
     /** Return the complete linear term \f$ g \f$ as described above.
      * @return The linear term \f$ g \f$ */
     SymmetricBlockMatrix::constBlock linearTerm() const {
-      assert(!empty());
+#ifndef NDEBUG
+      if(empty()) throw;
+#endif
       // get the last column (except the bottom right block)
       return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
     }
@@ -256,7 +259,9 @@ namespace gtsam {
     /** Return the complete linear term \f$ g \f$ as described above.
      * @return The linear term \f$ g \f$ */
     SymmetricBlockMatrix::Block linearTerm() {
-      assert(!empty());
+#ifndef NDEBUG
+      if(empty()) throw;
+#endif
       return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
     }
 
@@ -325,7 +330,9 @@ namespace gtsam {
      * @param other the HessianFactor to be updated
      */
     void updateHessian(HessianFactor* other) const {
-      assert(other);
+#ifndef NDEBUG
+      if(!other) throw;
+#endif
       updateHessian(other->keys_, &other->info_);
     }
 
@@ -363,7 +370,7 @@ namespace gtsam {
     friend class NonlinearFactorGraph;
     friend class NonlinearClusterTree;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 3cfb5ce7b..51d513e33 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -32,6 +32,7 @@
 #include <gtsam/base/cholesky.h>
 
 #include <cmath>
+#include <cassert>
 #include <sstream>
 #include <stdexcept>
 
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 407ed1e27..4a5b6d5d1 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -24,7 +24,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/inference/VariableSlots.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/version.hpp>
 #include <boost/serialization/split_member.hpp>
 #endif
@@ -421,7 +421,7 @@ namespace gtsam {
     // be very selective on who can access these private methods:
     template<typename T> friend class ExpressionFactor;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -468,7 +468,7 @@ struct traits<JacobianFactor> : public Testable<JacobianFactor> {
 
 } // \ namespace gtsam
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
 #endif
 
diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index ded6bc5e3..ebc4d583e 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -20,26 +20,44 @@
  * @author Frank Dellaert
  */
 
+
 #include <gtsam/linear/KalmanFilter.h>
+
+#include <gtsam/base/Testable.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
-#include <gtsam/base/Testable.h>
 
+#ifndef NDEBUG
+#include <cassert>
+#endif
 
-using namespace std;
+// In the code below we often get a covariance matrix Q, and we need to create a
+// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as
+//   Q = L L^T
+// and hence
+//   Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1}
+// We then have
+//   0.5 * |Ax - b|^T Q^{-1} |Ax - b|
+// = 0.5 * |Ax - b|^T M^T M |Ax - b|
+// = 0.5 * |MAx - Mb|^T |MAx - Mb|
+// The functor below efficiently multiplies with M by calling L.solve().
+namespace {
+using Matrix = gtsam::Matrix;
+struct InverseL : public Eigen::LLT<Matrix> {
+  InverseL(const Matrix& Q) : Eigen::LLT<Matrix>(Q) {}
+  Matrix operator*(const Matrix& A) const { return matrixL().solve(A); }
+};
+}  // namespace
 
 namespace gtsam {
-
 /* ************************************************************************* */
-// Auxiliary function to solve factor graph and return pointer to root conditional
-KalmanFilter::State //
-KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
-
+// Auxiliary function to solve factor graph and return pointer to root
+// conditional
+KalmanFilter::State KalmanFilter::solve(
+    const GaussianFactorGraph& factorGraph) const {
   // Eliminate the graph using the provided Eliminate function
   Ordering ordering(factorGraph.keys());
-  const auto bayesNet = //
-      factorGraph.eliminateSequential(ordering, function_);
+  const auto bayesNet = factorGraph.eliminateSequential(ordering, function_);
 
   // As this is a filter, all we need is the posterior P(x_t).
   // This is the last GaussianConditional in the resulting BayesNet
@@ -49,9 +67,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
 
 /* ************************************************************************* */
 // Auxiliary function to create a small graph for predict or update and solve
-KalmanFilter::State //
-KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
-
+KalmanFilter::State KalmanFilter::fuse(
+    const State& p, GaussianFactor::shared_ptr newFactor) const {
   // Create a factor graph
   GaussianFactorGraph factorGraph;
   factorGraph.push_back(p);
@@ -63,45 +80,54 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::init(const Vector& x0,
-    const SharedDiagonal& P0) const {
-
+                                       const SharedDiagonal& P0) const {
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
-  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0, P0); // |x-x0|^2_diagSigma
+  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0,
+                                             P0);  // |x-x0|^2_P0
   return solve(factorGraph);
 }
 
 /* ************************************************************************* */
-KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {
-
+KalmanFilter::State KalmanFilter::init(const Vector& x0,
+                                       const Matrix& P0) const {
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
-  factorGraph.emplace_shared<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
+
+  // Perform Cholesky decomposition of P0 = LL^T
+  InverseL M(P0);
+
+  // Premultiply I and x0 with M=L^{-1}
+  const Matrix A = M * I_;  // = M
+  const Vector b = M * x0;  // = M*x0
+  factorGraph.emplace_shared<JacobianFactor>(0, A, b);
   return solve(factorGraph);
 }
 
 /* ************************************************************************* */
-void KalmanFilter::print(const string& s) const {
-  cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
+void KalmanFilter::print(const std::string& s) const {
+  std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl;
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const SharedDiagonal& model) const {
-
+                                          const Matrix& B, const Vector& u,
+                                          const SharedDiagonal& model) const {
   // The factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
+  // f2(x_{k},x_{k+1}) =
+  // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T
   Key k = step(p);
   return fuse(p,
-      std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
+              std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
-    const Matrix& B, const Vector& u, const Matrix& Q) const {
-
+                                           const Matrix& B, const Vector& u,
+                                           const Matrix& Q) const {
 #ifndef NDEBUG
-  DenseIndex n = F.cols();
+  const DenseIndex n = dim();
+  assert(F.cols() == n);
   assert(F.rows() == n);
   assert(B.rows() == n);
   assert(B.cols() == u.size());
@@ -109,50 +135,52 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
   assert(Q.cols() == n);
 #endif
 
-  // The factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
-  // See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
-  // TODO: starts to seem more elaborate than straight-up KF equations?
-  Matrix M = Q.inverse(), Ft = trans(F);
-  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
-  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
-  double f = dot(b, g2);
-  Key k = step(p);
-  return fuse(p,
-      std::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
+  // Perform Cholesky decomposition of Q = LL^T
+  InverseL M(Q);
+
+  // Premultiply -F, I, and B * u with M=L^{-1}
+  const Matrix A1 = -(M * F);
+  const Matrix A2 = M * I_;
+  const Vector b = M * (B * u);
+  return predict2(p, A1, A2, b);
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
-    const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
+                                           const Matrix& A1, const Vector& b,
+                                           const SharedDiagonal& model) const {
   // Nhe factor related to the motion model is defined as
-  // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
+  // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2
   Key k = step(p);
   return fuse(p, std::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
-    const Vector& z, const SharedDiagonal& model) const {
+                                         const Vector& z,
+                                         const SharedDiagonal& model) const {
   // The factor related to the measurements would be defined as
-  // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
-  //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
+  // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T
+  //    = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T
   Key k = step(p);
   return fuse(p, std::make_shared<JacobianFactor>(k, H, z, model));
 }
 
 /* ************************************************************************* */
 KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
-    const Vector& z, const Matrix& Q) const {
+                                          const Vector& z,
+                                          const Matrix& Q) const {
   Key k = step(p);
-  Matrix M = Q.inverse(), Ht = trans(H);
-  Matrix G = Ht * M * H;
-  Vector g = Ht * M * z;
-  double f = dot(z, M * z);
-  return fuse(p, std::make_shared<HessianFactor>(k, G, g, f));
+
+  // Perform Cholesky decomposition of Q = LL^T
+  InverseL M(Q);
+
+  // Pre-multiply H and z with M=L^{-1}, respectively
+  const Matrix A = M * H;
+  const Vector b = M * z;
+  return fuse(p, std::make_shared<JacobianFactor>(k, A, b));
 }
 
 /* ************************************************************************* */
 
-} // \namespace gtsam
-
+}  // namespace gtsam
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index 65fac877a..2bf0549fa 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -11,10 +11,10 @@
 
 /**
  * @file KalmanFilter.h
- * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
+ * @brief Simple linear Kalman filter implemented using factor graphs, i.e.,
+ * performs Cholesky or QR-based SRIF (Square-Root Information Filter).
  * @date Sep 3, 2011
- * @author Stephen Williams
- * @author Frank Dellaert
+ * @authors Stephen Williams, Frank Dellaert
  */
 
 #pragma once
@@ -32,120 +32,186 @@ namespace gtsam {
 /**
  * Kalman Filter class
  *
- * Knows how to maintain a Gaussian density under linear-Gaussian motion and
- * measurement models. It uses the square-root information form, as usual in GTSAM.
+ * Maintains a Gaussian density under linear-Gaussian motion and
+ * measurement models using the square-root information form.
  *
- * The filter is functional, in that it does not have state: you call init() to create
- * an initial state, then predict() and update() that create new states out of an old state.
+ * The filter is functional; it does not maintain internal state. Instead:
+ * - Use `init()` to create an initial filter state,
+ * - Call `predict()` and `update()` to create new states.
  */
 class GTSAM_EXPORT KalmanFilter {
-
-public:
-
+ public:
   /**
-   *  This Kalman filter is a Square-root Information filter
-   *  The type below allows you to specify the factorization variant.
+   * @enum Factorization
+   * @brief Specifies the factorization variant to use.
    */
-  enum Factorization {
-    QR, CHOLESKY
-  };
+  enum Factorization { QR, CHOLESKY };
 
   /**
-   * The Kalman filter state is simply a GaussianDensity
+   * @typedef State
+   * @brief The Kalman filter state, represented as a shared pointer to a
+   * GaussianDensity.
    */
   typedef GaussianDensity::shared_ptr State;
 
-private:
-
-  const size_t n_; /** dimensionality of state */
-  const Matrix I_; /** identity matrix of size n*n */
-  const GaussianFactorGraph::Eliminate function_; /** algorithm */
-
-  State solve(const GaussianFactorGraph& factorGraph) const;
-  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
-
-public:
-
-  // Constructor
-  KalmanFilter(size_t n, Factorization method =
-      KALMANFILTER_DEFAULT_FACTORIZATION) :
-      n_(n), I_(Matrix::Identity(n_, n_)), function_(
-          method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
-              GaussianFactorGraph::Eliminate(EliminateCholesky)) {
-  }
+ private:
+  const size_t n_;  ///< Dimensionality of the state.
+  const Matrix I_;  ///< Identity matrix of size \f$ n \times n \f$.
+  const GaussianFactorGraph::Eliminate
+      function_;  ///< Elimination algorithm used.
 
   /**
-   * Create initial state, i.e., prior density at time k=0
-   * In Kalman Filter notation, these are x_{0|0} and P_{0|0}
-   * @param x0 estimate at time 0
-   * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
+   * Solve the factor graph.
+   * @param factorGraph The Gaussian factor graph to solve.
+   * @return The resulting Kalman filter state.
+   */
+  State solve(const GaussianFactorGraph& factorGraph) const;
+
+  /**
+   * Fuse two states.
+   * @param p The prior state.
+   * @param newFactor The new factor to incorporate.
+   * @return The resulting fused state.
+   */
+  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
+
+ public:
+  /**
+   * Constructor.
+   * @param n Dimensionality of the state.
+   * @param method Factorization method (default: QR unless compile-flag set).
+   */
+  KalmanFilter(size_t n,
+               Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION)
+      : n_(n),
+        I_(Matrix::Identity(n_, n_)),
+        function_(method == QR
+                      ? GaussianFactorGraph::Eliminate(EliminateQR)
+                      : GaussianFactorGraph::Eliminate(EliminateCholesky)) {}
+
+  /**
+   * Create the initial state (prior density at time \f$ k=0 \f$).
+   *
+   * In Kalman Filter notation:
+   * - \f$ x_{0|0} \f$: Initial state estimate.
+   * - \f$ P_{0|0} \f$: Initial covariance matrix.
+   *
+   * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$).
+   * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian
+   * model.
+   * @return Initial Kalman filter state.
    */
   State init(const Vector& x0, const SharedDiagonal& P0) const;
 
-  /// version of init with a full covariance matrix
+  /**
+   * Create the initial state with a full covariance matrix.
+   * @param x0 Initial state estimate.
+   * @param P0 Full covariance matrix.
+   * @return Initial Kalman filter state.
+   */
   State init(const Vector& x0, const Matrix& P0) const;
 
-  /// print
+  /**
+   * Print the Kalman filter details.
+   * @param s Optional string prefix.
+   */
   void print(const std::string& s = "") const;
 
-  /** Return step index k, starts at 0, incremented at each predict. */
-  static Key step(const State& p) {
-    return p->firstFrontalKey();
-  }
+  /**
+   * Return the step index \f$ k \f$ (starts at 0, incremented at each predict
+   * step).
+   * @param p The current state.
+   * @return Step index.
+   */
+  static Key step(const State& p) { return p->firstFrontalKey(); }
 
   /**
-   * Predict the state P(x_{t+1}|Z^t)
-   *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
-   * Details and parameters:
-   *   In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
-   *   where F is the state transition model/matrix, B is the control input model,
-   *   and w is zero-mean, Gaussian white noise with covariance Q.
+   * Predict the next state \f$ P(x_{k+1}|Z^k) \f$.
+   *
+   * In Kalman Filter notation:
+   * - \f$ x_{k+1|k} \f$: Predicted state.
+   * - \f$ P_{k+1|k} \f$: Predicted covariance.
+   *
+   * Motion model:
+   * \f[
+   * x_{k+1} = F \cdot x_k + B \cdot u_k + w
+   * \f]
+   * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$.
+   *
+   * @param p Previous state (\f$ x_k \f$).
+   * @param F State transition matrix (\f$ F \f$).
+   * @param B Control input matrix (\f$ B \f$).
+   * @param u Control vector (\f$ u_k \f$).
+   * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian).
+   * @return Predicted state (\f$ x_{k+1|k} \f$).
    */
   State predict(const State& p, const Matrix& F, const Matrix& B,
-      const Vector& u, const SharedDiagonal& modelQ) const;
+                const Vector& u, const SharedDiagonal& modelQ) const;
 
-  /*
-   *  Version of predict with full covariance
-   *  Q is normally derived as G*w*G^T where w models uncertainty of some
-   *  physical property, such as velocity or acceleration, and G is derived from physics.
-   *  This version allows more realistic models than a diagonal covariance matrix.
+  /**
+   * Predict the next state with a full covariance matrix.
+   *
+   *@note Q is normally derived as G*w*G^T where w models uncertainty of some
+   * physical property, such as velocity or acceleration, and G is derived from
+   * physics. This version allows more realistic models than a diagonal matrix.
+   *
+   * @param p Previous state.
+   * @param F State transition matrix.
+   * @param B Control input matrix.
+   * @param u Control vector.
+   * @param Q Full covariance matrix (\f$ Q \f$).
+   * @return Predicted state.
    */
   State predictQ(const State& p, const Matrix& F, const Matrix& B,
-      const Vector& u, const Matrix& Q) const;
+                 const Vector& u, const Matrix& Q) const;
 
   /**
-   * Predict the state P(x_{t+1}|Z^t)
-   *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
-   *   After the call, that is the density that can be queried.
-   * Details and parameters:
-   *   This version of predict takes GaussianFactor motion model [A0 A1 b]
-   *   with an optional noise model.
+   * Predict the next state using a GaussianFactor motion model.
+   * @param p Previous state.
+   * @param A0 Factor matrix.
+   * @param A1 Factor matrix.
+   * @param b Constant term vector.
+   * @param model Noise model (optional).
+   * @return Predicted state.
    */
   State predict2(const State& p, const Matrix& A0, const Matrix& A1,
-      const Vector& b, const SharedDiagonal& model) const;
+                 const Vector& b, const SharedDiagonal& model = nullptr) const;
 
   /**
-   * Update Kalman filter with a measurement
-   * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t}
-   * will be of the form h(x_{t}) = H*x_{t} + v
-   * where H is the observation model/matrix, and v is zero-mean,
-   * Gaussian white noise with covariance R.
+   * Update the Kalman filter with a measurement.
+   *
+   * Observation model:
+   * \f[
+   * z_k = H \cdot x_k + v
+   * \f]
+   * where \f$ v \f$ is zero-mean Gaussian noise with covariance R.
    * In this version, R is restricted to diagonal Gaussians (model parameter)
+   *
+   * @param p Previous state.
+   * @param H Observation matrix.
+   * @param z Measurement vector.
+   * @param model Noise model (diagonal Gaussian).
+   * @return Updated state.
    */
   State update(const State& p, const Matrix& H, const Vector& z,
-      const SharedDiagonal& model) const;
+               const SharedDiagonal& model) const;
 
-  /*
-   *  Version of update with full covariance
-   *  Q is normally derived as G*w*G^T where w models uncertainty of some
-   *  physical property, such as velocity or acceleration, and G is derived from physics.
-   *  This version allows more realistic models than a diagonal covariance matrix.
+  /**
+   * Update the Kalman filter with a measurement using a full covariance matrix.
+   * @param p Previous state.
+   * @param H Observation matrix.
+   * @param z Measurement vector.
+   * @param R Full covariance matrix.
+   * @return Updated state.
    */
   State updateQ(const State& p, const Matrix& H, const Vector& z,
-      const Matrix& Q) const;
+                const Matrix& R) const;
+
+  /**
+   * Return the dimensionality of the state.
+   * @return Dimensionality of the state.
+   */
+  size_t dim() const { return n_; }
 };
 
-} // \namespace gtsam
-
-/* ************************************************************************* */
-
+}  // namespace gtsam
diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h
index a8902e11b..989557d87 100644
--- a/gtsam/linear/LossFunctions.h
+++ b/gtsam/linear/LossFunctions.h
@@ -24,7 +24,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/dllexport.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/extended_type_info.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/version.hpp>
@@ -129,7 +129,7 @@ class GTSAM_EXPORT Base {
   void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -161,7 +161,7 @@ class GTSAM_EXPORT Null : public Base {
   static shared_ptr Create();
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -195,7 +195,7 @@ class GTSAM_EXPORT Fair : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -230,7 +230,7 @@ class GTSAM_EXPORT Huber : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -270,7 +270,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -306,7 +306,7 @@ class GTSAM_EXPORT Tukey : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -341,7 +341,7 @@ class GTSAM_EXPORT Welsch : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -380,7 +380,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   double c_;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -420,7 +420,7 @@ class GTSAM_EXPORT DCS : public Base {
   double c_;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -460,7 +460,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -496,7 +496,7 @@ class GTSAM_EXPORT AsymmetricTukey : public Base {
   double modelParameter() const { return c_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -532,7 +532,7 @@ class GTSAM_EXPORT AsymmetricCauchy : public Base {
   double modelParameter() const { return k_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -577,7 +577,7 @@ class GTSAM_EXPORT Custom : public Base {
   inline Custom() = default;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 884c87270..39666b46b 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <limits>
 #include <stdexcept>
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 34fa63e4c..47a84654a 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -24,7 +24,7 @@
 #include <gtsam/dllexport.h>
 #include <gtsam/linear/LossFunctions.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/extended_type_info.hpp>
 #include <boost/serialization/singleton.hpp>
@@ -141,7 +141,7 @@ namespace gtsam {
       virtual double weight(const Vector& v) const { return 1.0; }
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -280,7 +280,7 @@ namespace gtsam {
       double negLogConstant() const;
 
      private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -375,7 +375,7 @@ namespace gtsam {
       }
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -520,7 +520,7 @@ namespace gtsam {
       shared_ptr unit() const;
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -593,7 +593,7 @@ namespace gtsam {
       inline double sigma() const { return sigma_; }
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -648,7 +648,7 @@ namespace gtsam {
       void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
@@ -739,7 +739,7 @@ namespace gtsam {
         const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
 
     private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
       /** Serialization function */
       friend class boost::serialization::access;
       template<class ARCHIVE>
diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp
index 20d4c955b..68f21d723 100644
--- a/gtsam/linear/Sampler.cpp
+++ b/gtsam/linear/Sampler.cpp
@@ -17,6 +17,9 @@
  */
 
 #include <gtsam/linear/Sampler.h>
+
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
index 834fc8d12..9520b826b 100644
--- a/gtsam/linear/SubgraphBuilder.cpp
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -25,7 +25,7 @@
 #include <gtsam/linear/SubgraphBuilder.h>
 #include <gtsam/base/kruskal.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/serialization/vector.hpp>
@@ -71,7 +71,7 @@ vector<size_t> Subgraph::edgeIndices() const {
   return eid;
 }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 /****************************************************************************/
 void Subgraph::save(const std::string &fn) const {
   std::ofstream os(fn.c_str());
diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h
index f9ddd4c9a..df77ea9de 100644
--- a/gtsam/linear/SubgraphBuilder.h
+++ b/gtsam/linear/SubgraphBuilder.h
@@ -21,7 +21,7 @@
 #include <gtsam/base/types.h>
 #include <gtsam/dllexport.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/version.hpp>
 #include <boost/serialization/nvp.hpp>
 #endif
@@ -51,7 +51,7 @@ class GTSAM_EXPORT Subgraph {
     friend std::ostream &operator<<(std::ostream &os, const Edge &edge);
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     friend class boost::serialization::access;
     template <class Archive>
     void serialize(Archive &ar, const unsigned int /*version*/) {
@@ -87,7 +87,7 @@ class GTSAM_EXPORT Subgraph {
   friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive &ar, const unsigned int /*version*/) {
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 96f4847b5..53ea94d6e 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -25,6 +25,7 @@
 #include <gtsam/base/Vector.h>
 
 #include <stdexcept>
+#include <cassert>
 
 using std::cout;
 using std::endl;
diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h
index 7fbd43ffc..e60209a83 100644
--- a/gtsam/linear/VectorValues.h
+++ b/gtsam/linear/VectorValues.h
@@ -371,7 +371,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h
index 05439bafc..496fcde3d 100644
--- a/gtsam/navigation/AHRSFactor.h
+++ b/gtsam/navigation/AHRSFactor.h
@@ -120,7 +120,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
   PreintegratedAhrsMeasurements _PIM_;
 
-  /** Default constructor - only use for serialization */
-  AHRSFactor() {}
-
 public:
 
   // Provide access to the Matrix& version of evaluateError:
@@ -154,6 +151,9 @@ public:
   typedef std::shared_ptr<AHRSFactor> shared_ptr;
 #endif
 
+  /** Default constructor - only use for serialization */
+  AHRSFactor() {}
+
   /**
    * Constructor
    * @param rot_i previous rot key
@@ -208,7 +208,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 2fada724d..e342e451e 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -73,7 +73,7 @@ class AttitudeFactor {
   const Unit3& bRef() const { return bMeasured_; }
 #endif
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -138,7 +138,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN<Rot3>,
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -220,7 +220,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN<Pose3>,
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h
index 70cae8d36..545928ca9 100644
--- a/gtsam/navigation/BarometricFactor.h
+++ b/gtsam/navigation/BarometricFactor.h
@@ -97,7 +97,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
     }
 
    private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template <class ARCHIVE>
diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp
index a2eb9e0c0..64ee86be9 100644
--- a/gtsam/navigation/CombinedImuFactor.cpp
+++ b/gtsam/navigation/CombinedImuFactor.cpp
@@ -21,7 +21,7 @@
  **/
 
 #include <gtsam/navigation/CombinedImuFactor.h>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/export.hpp>
 #endif
 
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 0ffb386a0..b4c3ae8dc 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -169,7 +169,7 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -281,7 +281,7 @@ class GTSAM_EXPORT CombinedImuFactor
                        OptionalMatrixType H6) const override;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index cd89dd464..6af184360 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -97,7 +97,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -166,7 +166,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h
index 6df87eda6..02a020c42 100644
--- a/gtsam/navigation/ImuBias.h
+++ b/gtsam/navigation/ImuBias.h
@@ -20,7 +20,7 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/VectorSpace.h>
 #include <iosfwd>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -142,7 +142,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 88e45d318..5697d54d4 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -23,6 +23,7 @@
 
 /* External or standard includes */
 #include <ostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 7254838fd..b19989a77 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -142,7 +142,7 @@ public:
 #endif
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -244,7 +244,7 @@ public:
 
  private:
   /** Serialization function */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -316,7 +316,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h
index ba9a08d78..ea0b30c75 100644
--- a/gtsam/navigation/MagPoseFactor.h
+++ b/gtsam/navigation/MagPoseFactor.h
@@ -132,7 +132,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function.
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h
index 40691c445..43228044a 100644
--- a/gtsam/navigation/ManifoldPreintegration.h
+++ b/gtsam/navigation/ManifoldPreintegration.h
@@ -113,7 +113,7 @@ public:
   /// @}
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp
index 3e2817752..bd482132e 100644
--- a/gtsam/navigation/NavState.cpp
+++ b/gtsam/navigation/NavState.cpp
@@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
 }
 
 //------------------------------------------------------------------------------
-Matrix7 NavState::matrix() const {
+Matrix5 NavState::matrix() const {
   Matrix3 R = this->R();
-  Matrix7 T;
-  T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
+
+  Matrix5 T = Matrix5::Identity();
+  T.block<3, 3>(0, 0) = R;
+  T.block<3, 1>(0, 3) = t_;
+  T.block<3, 1>(0, 4) = v_;
   return T;
 }
 
@@ -103,6 +106,160 @@ bool NavState::equals(const NavState& other, double tol) const {
       && equal_with_abs_tol(v_, other.v_, tol);
 }
 
+//------------------------------------------------------------------------------
+NavState NavState::inverse() const {
+  Rot3 Rt = R_.inverse();
+  return NavState(Rt, Rt * (-t_), Rt * -(v_));
+}
+
+//------------------------------------------------------------------------------
+NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
+  // Get angular velocity w and components rho (for t) and nu (for v) from xi
+  Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
+
+  // Compute rotation using Expmap
+  Matrix3 Jr;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
+
+  // Compute translations and optionally their Jacobians Q in w
+  // The Jacobians with respect to rho and nu are equal to Jr
+  Matrix3 Qt, Qv;
+  Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
+  Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
+
+  if (Hxi) {
+    *Hxi << Jr, Z_3x3, Z_3x3,  //
+        Qt, Jr, Z_3x3,         //
+        Qv, Z_3x3, Jr;
+  }
+
+  return NavState(R, t, v);
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) {
+  if (Hstate) *Hstate = LogmapDerivative(state);
+
+  const Vector3 phi = Rot3::Logmap(state.rotation());
+  const Vector3& p = state.position();
+  const Vector3& v = state.velocity();
+  const double t = phi.norm();
+  if (t < 1e-8) {
+    Vector9 log;
+    log << phi, p, v;
+    return log;
+
+  } else {
+    const Matrix3 W = skewSymmetric(phi / t);
+
+    const double Tan = tan(0.5 * t);
+    const Vector3 Wp = W * p;
+    const Vector3 Wv = W * v;
+    const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp);
+    const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv);
+    Vector9 log;
+    // Order is ω, p, v
+    log << phi, rho, nu;
+    return log;
+  }
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::AdjointMap() const {
+  const Matrix3 R = R_.matrix();
+  Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
+  Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R;
+  // Eqn 2 in Barrau20icra
+  Matrix9 adj;
+  adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state,
+                          OptionalJacobian<9, 9> H_xib) const {
+  const Matrix9 Ad = AdjointMap();
+
+  // Jacobians
+  if (H_state) *H_state = -Ad * adjointMap(xi_b);
+  if (H_xib) *H_xib = Ad;
+
+  return Ad * xi_b;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::adjointMap(const Vector9& xi) {
+  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
+  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
+  Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8));
+  Matrix9 adj;
+  adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat;
+  return adj;
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y,
+                          OptionalJacobian<9, 9> Hxi,
+                          OptionalJacobian<9, 9> H_y) {
+  if (Hxi) {
+    Hxi->setZero();
+    for (int i = 0; i < 9; ++i) {
+      Vector9 dxi;
+      dxi.setZero();
+      dxi(i) = 1.0;
+      Matrix9 Gi = adjointMap(dxi);
+      Hxi->col(i) = Gi * y;
+    }
+  }
+
+  const Matrix9& ad_xi = adjointMap(xi);
+  if (H_y) *H_y = ad_xi;
+
+  return ad_xi * y;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
+  Matrix9 J;
+  Expmap(xi, J);
+  return J;
+}
+
+//------------------------------------------------------------------------------
+Matrix9 NavState::LogmapDerivative(const NavState& state) {
+  const Vector9 xi = Logmap(state);
+  const Vector3 w = xi.head<3>();
+  Vector3 rho = xi.segment<3>(3);
+  Vector3 nu = xi.tail<3>();
+  
+  Matrix3 Qt, Qv;
+  const Rot3 R = Rot3::Expmap(w);
+  Pose3::ExpmapTranslation(w, rho, Qt);
+  Pose3::ExpmapTranslation(w,  nu, Qv);
+  const Matrix3 Jw = Rot3::LogmapDerivative(w);
+  const Matrix3 Qt2 = -Jw * Qt * Jw;
+  const Matrix3 Qv2 = -Jw * Qv * Jw;
+
+  Matrix9 J;
+  J <<  Jw, Z_3x3, Z_3x3, 
+       Qt2,    Jw, Z_3x3,
+       Qv2, Z_3x3,    Jw;
+  return J;
+}
+
+
+//------------------------------------------------------------------------------
+NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
+                                          ChartJacobian Hxi) {
+  return Expmap(xi, Hxi);
+}
+
+//------------------------------------------------------------------------------
+Vector9 NavState::ChartAtOrigin::Local(const NavState& state,
+                                       ChartJacobian Hstate) {
+  return Logmap(state, Hstate);
+}
+
 //------------------------------------------------------------------------------
 NavState NavState::retract(const Vector9& xi, //
     OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
@@ -142,15 +299,16 @@ Vector9 NavState::localCoordinates(const NavState& g, //
   Matrix3 D_xi_R;
   xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV;
   if (H1) {
-    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
-    D_dt_R, -I_3x3, Z_3x3, //
-    D_dv_R, Z_3x3, -I_3x3;
+    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3,  //
+        D_dt_R, -I_3x3, Z_3x3,             //
+        D_dv_R, Z_3x3, -I_3x3;
   }
   if (H2) {
-    *H2 << D_xi_R, Z_3x3, Z_3x3, //
-    Z_3x3, dR.matrix(), Z_3x3, //
-    Z_3x3, Z_3x3, dR.matrix();
+    *H2 << D_xi_R, Z_3x3, Z_3x3,    //
+        Z_3x3, dR.matrix(), Z_3x3,  //
+        Z_3x3, Z_3x3, dR.matrix();
   }
+
   return xi;
 }
 
@@ -213,7 +371,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
 //------------------------------------------------------------------------------
 Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
     OptionalJacobian<9, 9> H) const {
-  auto [nRb, n_t, n_v] = (*this);
+  Rot3 nRb = R_;
+  Point3 n_t = t_, n_v = v_;
 
   const double dt2 = dt * dt;
   const Vector3 omega_cross_vel = omega.cross(n_v);
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index f778a7123..e246cbe27 100644
--- a/gtsam/navigation/NavState.h
+++ b/gtsam/navigation/NavState.h
@@ -12,7 +12,7 @@
 /**
  * @file    NavState.h
  * @brief   Navigation state composing of attitude, position, and velocity
- * @author  Frank Dellaert
+ * @authors Frank Dellaert, Varun Agrawal, Fan Jiang
  * @date    July 2015
  **/
 
@@ -25,14 +25,18 @@
 namespace gtsam {
 
 /// Velocity is currently typedef'd to Vector3
-typedef Vector3 Velocity3;
+using Velocity3 = Vector3;
 
 /**
  * Navigation state: Pose (rotation, translation) + velocity
- * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
+ * Following Barrau20icra, this class belongs to the Lie group SE_2(3).
+ * This group is also called "double direct isometries”.
+ *
+ * NOTE: While Barrau20icra follow a R,v,t order,
+ * we use a R,t,v order to maintain backwards compatibility.
  */
-class GTSAM_EXPORT NavState {
-private:
+class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
+ private:
 
   // TODO(frank):
   // - should we rename t_ to p_? if not, we should rename dP do dT
@@ -42,11 +46,8 @@ private:
 
 public:
 
-  enum {
-    dimension = 9
-  };
+  inline constexpr static auto dimension = 9;
 
-  typedef std::pair<Point3, Velocity3> PositionAndVelocity;
 
   /// @name Constructors
   /// @{
@@ -69,11 +70,14 @@ public:
   }
   /// Named constructor with derivatives
   static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
-      OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
-      OptionalJacobian<9, 3> H3);
+                         OptionalJacobian<9, 3> H1 = {},
+                         OptionalJacobian<9, 3> H2 = {},
+                         OptionalJacobian<9, 3> H3 = {});
+
   /// Named constructor with derivatives
   static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
-      OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
+                                   OptionalJacobian<9, 6> H1 = {},
+                                   OptionalJacobian<9, 3> H2 = {});
 
   /// @}
   /// @name Component Access
@@ -111,9 +115,8 @@ public:
   Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const;
 
   /// Return matrix group representation, in MATLAB notation:
-  /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
-  /// With this embedding in GL(3), matrix product agrees with compose
-  Matrix7 matrix() const;
+  /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1]
+  Matrix5 matrix() const;
 
   /// @}
   /// @name Testable
@@ -130,7 +133,29 @@ public:
   bool equals(const NavState& other, double tol = 1e-8) const;
 
   /// @}
-  /// @name Manifold
+  /// @name Group
+  /// @{
+
+  /// identity for group operation
+  static NavState Identity() {
+    return NavState();
+  }
+
+  /// inverse transformation with derivatives
+  NavState inverse() const;
+
+  using LieGroup<NavState, 9>::inverse;  // version with derivative
+
+  /// compose syntactic sugar
+  NavState operator*(const NavState& T) const {
+    return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_);
+  }
+
+  /// Syntactic sugar
+  const Rot3& rotation() const { return attitude(); };
+
+  /// @}
+  /// @name Lie Group
   /// @{
 
   // Tangent space sugar.
@@ -164,6 +189,59 @@ public:
       OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
           {}) const;
 
+  /**
+   * Exponential map at identity - create a NavState from canonical coordinates
+   * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$
+   */
+  static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {});
+
+  /**
+   * Log map at identity - return the canonical coordinates \f$
+   * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState
+   */
+  static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {});
+
+  /**
+   * Calculate Adjoint map, transforming a twist in this pose's (i.e, body)
+   * frame to the world spatial frame.
+   */
+  Matrix9 AdjointMap() const;
+
+  /**
+   * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
+   * body-fixed velocity, transforming it to the spatial frame
+   * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
+   * Note that H_xib = AdjointMap()
+   */
+  Vector9 Adjoint(const Vector9& xi_b,
+                  OptionalJacobian<9, 9> H_this = {},
+                  OptionalJacobian<9, 9> H_xib = {}) const;
+  
+  /**
+   * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
+   * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^]
+   */
+  static Matrix9 adjointMap(const Vector9& xi);
+
+  /**
+   * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
+   */
+  static Vector9 adjoint(const Vector9& xi, const Vector9& y,
+                         OptionalJacobian<9, 9> Hxi = {},
+                         OptionalJacobian<9, 9> H_y = {});
+
+  /// Derivative of Expmap
+  static Matrix9 ExpmapDerivative(const Vector9& xi);
+
+  /// Derivative of Logmap
+  static Matrix9 LogmapDerivative(const NavState& xi);
+
+  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
+  struct GTSAM_EXPORT ChartAtOrigin {
+    static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {});
+    static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
+  };
+
   /// @}
   /// @name Dynamics
   /// @{
@@ -171,8 +249,9 @@ public:
   /// Integrate forward in time given angular velocity and acceleration in body frame
   /// Uses second order integration for position, returns derivatives except dt.
   NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
-      const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
-      OptionalJacobian<9, 3> G2) const;
+                  const double dt, OptionalJacobian<9, 9> F = {},
+                  OptionalJacobian<9, 3> G1 = {},
+                  OptionalJacobian<9, 3> G2 = {}) const;
 
   /// Compute tangent space contribution due to Coriolis forces
   Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
@@ -190,7 +269,7 @@ public:
 private:
   /// @{
   /// serialization
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
@@ -203,8 +282,10 @@ private:
 };
 
 // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
-template<>
-struct traits<NavState> : internal::Manifold<NavState> {
-};
+template <>
+struct traits<NavState> : public internal::LieGroup<NavState> {};
+
+template <>
+struct traits<const NavState> : public internal::LieGroup<NavState> {};
 
 } // namespace gtsam
diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h
index 49b1aa4c1..a887ef321 100644
--- a/gtsam/navigation/PreintegratedRotation.h
+++ b/gtsam/navigation/PreintegratedRotation.h
@@ -85,7 +85,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
   std::optional<Pose3>   getBodyPSensor()   const { return body_P_sensor; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -227,7 +227,7 @@ class GTSAM_EXPORT PreintegratedRotation {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 7f998987b..dd86b829c 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -20,9 +20,11 @@
  *  @author Varun Agrawal
  **/
 
-#include "PreintegrationBase.h"
+#include <gtsam/navigation/PreintegrationBase.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <cassert>
+
 using namespace std;
 
 namespace gtsam {
diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h
index e4a790b9d..de3a380cf 100644
--- a/gtsam/navigation/PreintegrationBase.h
+++ b/gtsam/navigation/PreintegrationBase.h
@@ -173,7 +173,7 @@ class GTSAM_EXPORT PreintegrationBase {
       OptionalJacobian<9, 6> H5 = {}) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h
index 6be05c082..b384a36e7 100644
--- a/gtsam/navigation/PreintegrationCombinedParams.h
+++ b/gtsam/navigation/PreintegrationCombinedParams.h
@@ -84,7 +84,7 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
   const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h
index 8435d0bad..6ea9e73b3 100644
--- a/gtsam/navigation/PreintegrationParams.h
+++ b/gtsam/navigation/PreintegrationParams.h
@@ -71,7 +71,7 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
 
 protected:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h
index e16ad9310..7f5c63d50 100644
--- a/gtsam/navigation/TangentPreintegration.h
+++ b/gtsam/navigation/TangentPreintegration.h
@@ -127,7 +127,7 @@ public:
   /// @}
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i
index 831788073..adb8bf2bb 100644
--- a/gtsam/navigation/navigation.i
+++ b/gtsam/navigation/navigation.i
@@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams {
 
   std::optional<gtsam::Vector> getOmegaCoriolis() const;
   std::optional<gtsam::Pose3> getBodyPSensor() const;
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/PreintegrationParams.h>
@@ -148,6 +151,9 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
   gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
       const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
       const gtsam::imuBias::ConstantBias& bias);
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/CombinedImuFactor.h>
@@ -170,22 +176,26 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
   gtsam::Matrix getBiasAccCovariance() const ;
   gtsam::Matrix getBiasOmegaCovariance() const ;
   gtsam::Matrix getBiasAccOmegaInit() const;
- 
+
+  // enabling serialization functionality
+  void serialize() const;
 };
 
 class PreintegratedCombinedMeasurements {
-// Constructors
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params);
-  PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
-				    const gtsam::imuBias::ConstantBias& bias);
+  // Constructors
+  PreintegratedCombinedMeasurements(
+      const gtsam::PreintegrationCombinedParams* params);
+  PreintegratedCombinedMeasurements(
+      const gtsam::PreintegrationCombinedParams* params,
+      const gtsam::imuBias::ConstantBias& bias);
   // Testable
   void print(string s = "Preintegrated Measurements:") const;
   bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
-      double tol);
+              double tol);
 
   // Standard Interface
-  void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
-      double deltaT);
+  void integrateMeasurement(gtsam::Vector measuredAcc,
+                            gtsam::Vector measuredOmega, double deltaT);
   void resetIntegration();
   void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
 
@@ -197,7 +207,10 @@ class PreintegratedCombinedMeasurements {
   gtsam::imuBias::ConstantBias biasHat() const;
   gtsam::Vector biasHatVector() const;
   gtsam::NavState predict(const gtsam::NavState& state_i,
-      const gtsam::imuBias::ConstantBias& bias) const;
+                          const gtsam::imuBias::ConstantBias& bias) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
@@ -211,6 +224,9 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
       const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
       const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::imuBias::ConstantBias& bias_j);
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/AHRSFactor.h>
@@ -237,6 +253,9 @@ class PreintegratedAhrsMeasurements {
   // Standard Interface
   void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT);
   void resetIntegration() ;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class AHRSFactor : gtsam::NonlinearFactor {
@@ -253,6 +272,9 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
   gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias,
       const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
       gtsam::Vector omegaCoriolis) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/AttitudeFactor.h>
@@ -266,6 +288,9 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor {
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nRef() const;
   gtsam::Unit3 bMeasured() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
@@ -280,6 +305,9 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nRef() const;
   gtsam::Unit3 bMeasured() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/GPSFactor.h>
@@ -294,6 +322,9 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 virtual class GPSFactor2 : gtsam::NonlinearFactor {
@@ -307,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::Point3 measurementIn() const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/BarometricFactor.h>
@@ -324,6 +358,9 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
   const double& measurementIn() const;
   double heightOut(double n) const;
   double baroOut(const double& meters) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/Scenario.h>
diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp
index b8f081518..427531415 100644
--- a/gtsam/navigation/tests/testNavState.cpp
+++ b/gtsam/navigation/tests/testNavState.cpp
@@ -12,13 +12,16 @@
 /**
  * @file    testNavState.cpp
  * @brief   Unit tests for NavState
- * @author  Frank Dellaert
+ * @authors Frank Dellaert, Varun Agrawal, Fan Jiang
  * @date    July 2015
  */
 
 #include <gtsam/navigation/NavState.h>
-#include <gtsam/base/TestableAssertions.h>
+
+#include <gtsam/base/lieProxies.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/testLie.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -26,6 +29,9 @@ using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
+GTSAM_CONCEPT_TESTABLE_INST(NavState)
+GTSAM_CONCEPT_LIE_INST(NavState)
+
 static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
 static const Point3 kPosition(1.0, 2.0, 3.0);
 static const Pose3 kPose(kAttitude, kPosition);
@@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
 static const Vector3 kGravity(0, 0, 9.81);
 static const Vector9 kZeroXi = Vector9::Zero();
 
+static const Point3 V(3, 0.4, -2.2);
+static const Point3 P(0.2, 0.7, -2);
+static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
+static const Point3 V2(-6.5, 3.5, 6.2);
+static const Point3 P2(3.5, -8.2, 4.2);
+static const NavState T(R, P2, V2);
+static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2);
+static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7),
+                         Point3(1, 2, 3));
+
 /* ************************************************************************* */
 TEST(NavState, Constructor) {
   std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
@@ -46,14 +62,14 @@ TEST(NavState, Constructor) {
       assert_equal(kState1,
           NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
   EXPECT(
-      assert_equal(
-          numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
+assert_equal(
+      numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
   EXPECT(
-      assert_equal(
-          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+assert_equal(
+      numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
   EXPECT(
-      assert_equal(
-          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+assert_equal(
+      numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
 }
 
 /* ************************************************************************* */
@@ -64,7 +80,7 @@ TEST(NavState, Constructor2) {
   Matrix aH1, aH2;
   EXPECT(
       assert_equal(kState1,
-          NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
+                      NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
   EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
   EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
 }
@@ -127,8 +143,8 @@ TEST( NavState, Manifold ) {
   Point3 dt = Point3(xi.segment<3>(3));
   Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
   NavState state2 = NavState(kState1.attitude() * drot,
-      kState1.position() + kState1.attitude() * dt,
-      kState1.velocity() + kState1.attitude() * dvel);
+                             kState1.position() + kState1.attitude() * dt,
+                             kState1.velocity() + kState1.attitude() * dvel);
   EXPECT(assert_equal(state2, kState1.retract(xi)));
   EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
 
@@ -169,6 +185,143 @@ TEST( NavState, Manifold ) {
   EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
 }
 
+/* ************************************************************************* */
+TEST(NavState, Equals) {
+  NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3));
+  NavState pose2 = T3;
+  EXPECT(T3.equals(pose2));
+  NavState origin;
+  EXPECT(!T3.equals(origin));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Compose) {
+  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
+  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
+  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
+                       {3.0, -1.0, 1.0});
+
+  auto ab_c = (nav_state_a * nav_state_b) * nav_state_c;
+  auto a_bc = nav_state_a * (nav_state_b * nav_state_c);
+  CHECK(assert_equal(ab_c, a_bc));
+
+  Matrix actual = (T2 * T2).matrix();
+
+  Matrix expected = T2.matrix() * T2.matrix();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix actualDcompose1, actualDcompose2;
+  T2.compose(T2, actualDcompose1, actualDcompose2);
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::compose<NavState>, T2, T2);
+
+  EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
+  EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::compose<NavState>, T2, T2);
+  EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4));
+}
+
+/* ************************************************************************* */
+// Check compose and its push-forward, another case
+TEST(NavState, Compose2) {
+  const NavState& T1 = T;
+  Matrix actual = (T1 * T2).matrix();
+  Matrix expected = T1.matrix() * T2.matrix();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix actualDcompose1, actualDcompose2;
+  T1.compose(T2, actualDcompose1, actualDcompose2);
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::compose<NavState>, T1, T2);
+  EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3));
+  EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::compose<NavState>, T1, T2);
+  EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Inverse) {
+  NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0});
+  NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0});
+  NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0},
+                       {3.0, -1.0, 1.0});
+
+  auto a_inv = nav_state_a.inverse();
+  auto a_a_inv = nav_state_a * a_inv;
+  CHECK(assert_equal(a_a_inv, NavState()));
+
+  auto b_inv = nav_state_b.inverse();
+  auto b_b_inv = nav_state_b * b_inv;
+  CHECK(assert_equal(b_b_inv, NavState()));
+
+  Matrix actualDinverse;
+  Matrix actual = T.inverse(actualDinverse).matrix();
+  Matrix expected = T.matrix().inverse();
+  EXPECT(assert_equal(actual, expected, 1e-8));
+
+  Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
+  EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
+  EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
+}
+
+/* ************************************************************************* */
+TEST(NavState, InverseDerivatives) {
+  Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5);
+  Vector3 v(3.5, -8.2, 4.2);
+  Point3 p(3.5, -8.2, 4.2);
+  NavState T(R, p, v);
+
+  Matrix numericalH = numericalDerivative11(testing::inverse<NavState>, T);
+  Matrix actualDinverse;
+  T.inverse(actualDinverse);
+  EXPECT(assert_equal(numericalH, actualDinverse, 5e-3));
+  EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Compose_Inverse) {
+  NavState actual = T * T.inverse();
+  NavState expected;
+  EXPECT(assert_equal(actual, expected, 1e-8));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Between) {
+  NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0));
+
+  NavState actual = s1.compose(s2);
+  EXPECT(assert_equal(s2, actual));
+
+  NavState between = s2.between(s1);
+  NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0));
+  EXPECT(assert_equal(expected_between, between));
+
+  NavState expected = T2.inverse() * T3;
+  Matrix actualDBetween1, actualDBetween2;
+  actual = T2.between(T3, actualDBetween1, actualDBetween2);
+  EXPECT(assert_equal(expected, actual));
+
+  Matrix numericalH1 =
+      numericalDerivative21(testing::between<NavState>, T2, T3);
+  EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3));
+
+  Matrix numericalH2 =
+      numericalDerivative22(testing::between<NavState>, T2, T3);
+  EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, interpolate) {
+  EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0)));
+  EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0)));
+}
+
 /* ************************************************************************* */
 static const double dt = 2.0;
 std::function<Vector9(const NavState&, const bool&)> coriolis =
@@ -189,7 +342,7 @@ TEST(NavState, Coriolis) {
 TEST(NavState, Coriolis2) {
   Matrix9 aH;
   NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
-      Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
+                  Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
 
   // first-order
   state2.coriolis(dt, kOmegaCoriolis, false, aH);
@@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) {
 }
 
 TEST(NavState, Coriolis3) {
-  /** Consider a massless planet with an attached nav frame at 
-   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with 
+  /** Consider a massless planet with an attached nav frame at
+   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with
    *  velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
-   *  aligned with the nav frame (i.e., nRb != I_3x3). Test that first and 
+   *  aligned with the nav frame (i.e., nRb != I_3x3). Test that first and
    *  second order Coriolis corrections are as expected.
    */
 
@@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) {
        bRn = nRb.inverse();
 
   // Get expected first and second order corrections in the nav frame
-  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, 
+  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1,
           n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
-          n_dV1e = dt * n_aCorr1, 
+          n_dV1e = dt * n_aCorr1,
           n_dV2e = dt * (n_aCorr1 + n_aCorr2);
 
   // Get expected first and second order corrections in the body frame
@@ -271,6 +424,262 @@ TEST(NavState, Stream)
   EXPECT(os.str() == expected);
 }
 
+/* ************************************************************************* */
+TEST(NavState, Print) {
+  NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3));
+
+  // Generate the expected output
+  std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
+  std::string p = "p: 1 2 3\n";
+  std::string v = "v: 1 2 3\n";
+  std::string expected = R + p + v;
+
+  EXPECT(assert_print_equal(expected, state));
+}
+
+/* ************************************************************************* */
+#ifndef GTSAM_POSE3_EXPMAP
+TEST(NavState, Retract_first_order) {
+  NavState id;
+  Vector v = Z_9x1;
+  v(0) = 0.3;
+  EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)),
+                      id.retract(v), 1e-2));
+  v(3) = 0.2;
+  v(4) = 0.7;
+  v(5) = -2;
+  v(6) = 3;
+  v(7) = 0.4;
+  v(8) = -2.2;
+  EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2));
+}
+#endif
+
+/* ************************************************************************* */
+TEST(NavState, RetractExpmap) {
+  Vector xi = Z_9x1;
+  xi(0) = 0.3;
+  NavState pose = NavState::Expmap(xi),
+           expected(R, Point3(0, 0, 0), Point3(0, 0, 0));
+  EXPECT(assert_equal(expected, pose, 1e-2));
+  EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Expmap_A_Full) {
+  NavState id;
+  Vector xi = Z_9x1;
+  xi(0) = 0.3;
+  EXPECT(assert_equal(expmap_default<NavState>(id, xi),
+                      NavState(R, Point3(0, 0, 0), Point3(0, 0, 0))));
+  xi(3) = -0.2;
+  xi(4) = -0.394742;
+  xi(5) = 2.08998;
+  xi(6) = 0.2;
+  xi(7) = 0.394742;
+  xi(8) = -2.08998;
+
+  NavState expected(R, -P, P);
+  EXPECT(assert_equal(expected, expmap_default<NavState>(id, xi), 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Expmap_b) {
+  NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0));
+  NavState p2 = p1.retract(
+      (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished());
+  NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0),
+                    Point3(100.0, 0.0, 0.0));
+  EXPECT(assert_equal(expected, p2));
+}
+
+/* ************************************************************************* */
+// test case for screw motion in the plane
+namespace screwNavState {
+double a = 0.3, c = cos(a), s = sin(a), w = 0.3;
+Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished();
+Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1);
+Point3 expectedV(0.29552, 0.0446635, 1);
+Point3 expectedP(0.29552, 0.0446635, 1);
+NavState expected(expectedR, expectedV, expectedP);
+}  // namespace screwNavState
+
+/* ************************************************************************* */
+// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi))
+TEST(NavState, Adjoint_full) {
+  NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse();
+  Vector xiPrime = T.Adjoint(screwNavState::xi);
+  EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6));
+
+  NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse();
+  Vector xiPrime2 = T2.Adjoint(screwNavState::xi);
+  EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6));
+
+  NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse();
+  Vector xiPrime3 = T3.Adjoint(screwNavState::xi);
+  EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Adjoint_compose_full) {
+  // To debug derivatives of compose, assert that
+  // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2
+  const NavState& T1 = T;
+  Vector x =
+      (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished();
+  NavState expected = T1 * NavState::Expmap(x) * T2;
+  Vector y = T2.inverse().Adjoint(x);
+  NavState actual = T1 * T2 * NavState::Expmap(y);
+  EXPECT(assert_equal(expected, actual, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(NavState, Retract_LocalCoordinates) {
+  Vector9 d;
+  d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  d /= 10;
+  const Rot3 R = Rot3::Retract(d.head<3>());
+  NavState t = NavState::Retract(d);
+  EXPECT(assert_equal(d, NavState::LocalCoordinates(t)));
+}
+/* ************************************************************************* */
+TEST(NavState, retract_localCoordinates) {
+  Vector9 d12;
+  d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  d12 /= 10;
+  NavState t1 = T, t2 = t1.retract(d12);
+  EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
+}
+/* ************************************************************************* */
+TEST(NavState, expmap_logmap) {
+  Vector d12 = Vector9::Constant(0.1);
+  NavState t1 = T, t2 = t1.expmap(d12);
+  EXPECT(assert_equal(d12, t1.logmap(t2)));
+}
+
+/* ************************************************************************* */
+TEST(NavState, retract_localCoordinates2) {
+  NavState t1 = T;
+  NavState t2 = T3;
+  NavState origin;
+  Vector d12 = t1.localCoordinates(t2);
+  EXPECT(assert_equal(t2, t1.retract(d12)));
+  Vector d21 = t2.localCoordinates(t1);
+  EXPECT(assert_equal(t1, t2.retract(d21)));
+  // NOTE(FRANK): d12 !== -d21 for arbitrary retractions.
+}
+/* ************************************************************************* */
+TEST(NavState, manifold_expmap) {
+  NavState t1 = T;
+  NavState t2 = T3;
+  NavState origin;
+  Vector d12 = t1.logmap(t2);
+  EXPECT(assert_equal(t2, t1.expmap(d12)));
+  Vector d21 = t2.logmap(t1);
+  EXPECT(assert_equal(t1, t2.expmap(d21)));
+
+  // Check that log(t1,t2)=-log(t2,t1)
+  EXPECT(assert_equal(d12, -d21));
+}
+
+/* ************************************************************************* */
+TEST(NavState, subgroups) {
+  // Frank - Below only works for correct "Agrawal06iros style expmap
+  // lines in canonical coordinates correspond to Abelian subgroups in SE(3)
+  Vector d =
+      (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished();
+  // exp(-d)=inverse(exp(d))
+  EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse()));
+  // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
+  NavState T2 = NavState::Expmap(2 * d);
+  NavState T3 = NavState::Expmap(3 * d);
+  NavState T5 = NavState::Expmap(5 * d);
+  EXPECT(assert_equal(T5, T2 * T3));
+  EXPECT(assert_equal(T5, T3 * T2));
+}
+
+/* ************************************************************************* */
+TEST(NavState, adjointMap) {
+  Matrix res = NavState::adjointMap(screwNavState::xi);
+  Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1),
+                            screwNavState::xi(2));
+  Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4),
+                            screwNavState::xi(5));
+  Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7),
+                            screwNavState::xi(8));
+  Matrix9 expected;
+  expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh;
+  EXPECT(assert_equal(expected, res, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(NavState, ExpmapDerivative1) {
+  Matrix9 actualH;
+  Vector9 w;
+  w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  NavState::Expmap(w, actualH);
+
+  std::function<NavState(const Vector9&)> f = [](const Vector9& w) {
+    return NavState::Expmap(w);
+  };
+  Matrix expectedH =
+      numericalDerivative21<NavState, Vector9, OptionalJacobian<9, 9> >(
+          &NavState::Expmap, w, {});
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+/* ************************************************************************* */
+TEST(NavState, LogmapDerivative) {
+  Matrix9 actualH;
+  Vector9 w;
+  w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  NavState p = NavState::Expmap(w);
+  EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5));
+
+  std::function<Vector9(const NavState&)> f = [](const NavState& p) {
+    return NavState::Logmap(p);
+  };
+  Matrix expectedH =
+      numericalDerivative21<Vector9, NavState, OptionalJacobian<9, 9> >(
+          &NavState::Logmap, p, {});
+  EXPECT(assert_equal(expectedH, actualH));
+}
+
+//******************************************************************************
+TEST(NavState, Invariants) {
+  NavState id;
+
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, T3));
+  EXPECT(check_group_invariants(T2, id));
+  EXPECT(check_group_invariants(T2, T3));
+
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, T3));
+  EXPECT(check_manifold_invariants(T2, id));
+  EXPECT(check_manifold_invariants(T2, T3));
+}
+
+//******************************************************************************
+TEST(NavState, LieGroupDerivatives) {
+  NavState id;
+
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T3);
+}
+
+//******************************************************************************
+TEST(NavState, ChartDerivatives) {
+  NavState id;
+  if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
+    CHECK_CHART_DERIVATIVES(id, id);
+    CHECK_CHART_DERIVATIVES(id,T2);
+    CHECK_CHART_DERIVATIVES(T2,id);
+    CHECK_CHART_DERIVATIVES(T2,T3);
+  }
+}
 
 /* ************************************************************************* */
 int main() {
diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h
index c4015db37..5f8a19ff0 100644
--- a/gtsam/nonlinear/CustomFactor.h
+++ b/gtsam/nonlinear/CustomFactor.h
@@ -88,7 +88,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
index 4f1c6fb54..e82a513ca 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <cmath>
+#include <cassert>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
 using namespace std;
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
index 8ce9b361e..1038dd522 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.h
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -17,6 +17,7 @@
 #pragma once
 
 #include <iomanip>
+#include <cassert>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/Ordering.h>
diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index 71ef1d840..b09bcb3a5 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -26,6 +26,7 @@
 #include <set>
 #include <string>
 #include <vector>
+#include <cassert>
 
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index b3e34d079..4f6648849 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -200,7 +200,7 @@ protected:
  }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
  /// Save to an archive: just saves the base class
  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
@@ -224,9 +224,9 @@ private:
 #endif
 
  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
- enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
-  public:
-	  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+ inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0;
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };
 // ExpressionFactor
 
@@ -287,7 +287,7 @@ private:
     return expression(keys);
   }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 5bf643c12..072f40b58 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -23,6 +23,8 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
index 128de5847..ca253179b 100644
--- a/gtsam/nonlinear/FunctorizedFactor.h
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -119,7 +119,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -231,7 +231,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
   /// @}
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 20874e2dc..d0b27f3f3 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -25,6 +25,7 @@
 #include <functional>
 #include <limits>
 #include <string>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h
index e9a9696eb..bb0210237 100644
--- a/gtsam/nonlinear/ISAM2-impl.h
+++ b/gtsam/nonlinear/ISAM2-impl.h
@@ -33,6 +33,7 @@
 #include <string>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 47857a2a2..0eb2deef5 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -29,6 +29,7 @@
 #include <map>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index d3abd95fd..f71b4b7da 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -340,7 +340,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   void updateDelta(bool forceFullSolve = false) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp
index d4b050f84..64ab7c66e 100644
--- a/gtsam/nonlinear/ISAM2Clique.cpp
+++ b/gtsam/nonlinear/ISAM2Clique.cpp
@@ -22,6 +22,7 @@
 
 #include <stack>
 #include <utility>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h
index fde9dd3f2..39bce432a 100644
--- a/gtsam/nonlinear/ISAM2Clique.h
+++ b/gtsam/nonlinear/ISAM2Clique.h
@@ -146,7 +146,7 @@ class GTSAM_EXPORT ISAM2Clique
   void restoreFromOriginals(const Vector& originalValues,
                             VectorValues* delta) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h
index dfbf221c3..f63ecb0c6 100644
--- a/gtsam/nonlinear/LinearContainerFactor.h
+++ b/gtsam/nonlinear/LinearContainerFactor.h
@@ -163,7 +163,7 @@ public:
   void initializeLinearizationPoint(const Values& linearizationPoint);
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index f81486b30..538a95208 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -180,7 +180,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -273,7 +273,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -344,7 +344,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 7312bf6d9..5eabfac91 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -19,6 +19,8 @@
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 2d3a13766..1af01f60d 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -29,7 +29,7 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/utilities.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/base_object.hpp>
 #endif
 #include <cstddef>
@@ -303,7 +303,7 @@ public:
   shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -434,7 +434,7 @@ class NoiseModelFactorN
       public detail::NoiseModelFactorAliases<ValueTypes...> {
  public:
   /// N is the number of variables (N-way factor)
-  enum { N = sizeof...(ValueTypes) };
+  inline constexpr static auto N = sizeof...(ValueTypes);
 
   using NoiseModelFactor::unwhitenedError;
 
@@ -715,7 +715,7 @@ protected:
     }
   }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index bf56d835f..8e3443dc7 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -250,7 +250,7 @@ namespace gtsam {
     std::shared_ptr<HessianFactor> linearizeToHessianFactor(
         const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index ab2a66a77..0979be3df 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -105,7 +105,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -118,7 +118,7 @@ namespace gtsam {
 #endif
 
   // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
+  inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0;
   public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 6af9f2b6a..1caaa9db1 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -28,6 +28,7 @@
 #include <list>
 #include <memory>
 #include <sstream>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 2fe64d9c9..7e950479a 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -29,7 +29,7 @@
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/VectorSpace.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/unique_ptr.hpp>
 #endif
 
@@ -393,7 +393,7 @@ namespace gtsam {
       return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
     }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h
index de0c9721e..25587f511 100644
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@@ -26,6 +26,7 @@
 #include <typeinfo>       // operator typeid
 #include <ostream>
 #include <map>
+#include <cassert>
 
 class ExpressionFactorBinaryTest;
 // Forward declare for testing
diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp
index 6f5fc53f5..67815e262 100644
--- a/gtsam/nonlinear/tests/testExpression.cpp
+++ b/gtsam/nonlinear/tests/testExpression.cpp
@@ -115,7 +115,7 @@ TEST(Expression, Unary3) {
 // Simple test class that implements the `VectorSpace` protocol.
 class Class : public Point3 {
  public:
-  enum {dimension = 3};
+  inline constexpr static auto dimension = 3;
   using Point3::Point3;
   const Vector3& vector() const { return *this; }
   inline static Class Identity() { return Class(0,0,0); }
diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp
index a2b265a56..06eb46d4c 100644
--- a/gtsam/nonlinear/tests/testValues.cpp
+++ b/gtsam/nonlinear/tests/testValues.cpp
@@ -54,7 +54,7 @@ int TestValueData::DestructorCount = 0;
 class TestValue {
   TestValueData data_;
 public:
-  enum {dimension = 0};
+  inline constexpr static auto dimension = 0;
   void print(const std::string& str = "") const {}
   bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
   size_t dim() const { return 0; }
diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h
index 01b2baf18..97f23ed8f 100644
--- a/gtsam/sam/BearingFactor.h
+++ b/gtsam/sam/BearingFactor.h
@@ -75,7 +75,7 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
 
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index d5467be47..aa08fea17 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -92,7 +92,7 @@ class BearingRangeFactor
 
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index 193a89a5a..0ccc54afd 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -80,7 +80,7 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
@@ -164,7 +164,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /** Serialization function */
   template <typename ARCHIVE>
diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h
index 7161ab2f9..dbe336979 100644
--- a/gtsam/sfm/SfmData.h
+++ b/gtsam/sfm/SfmData.h
@@ -125,7 +125,7 @@ struct GTSAM_EXPORT SfmData {
   /// @name Serialization
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /** Serialization function */
   template <class Archive>
diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h
index 2b494ed7c..bdfa79cef 100644
--- a/gtsam/sfm/SfmTrack.h
+++ b/gtsam/sfm/SfmTrack.h
@@ -160,7 +160,7 @@ struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
   /// @name Serialization
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 830f1c719..6c9f91e90 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -37,6 +37,7 @@
 #include <random>
 #include <set>
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h
index cfb9cff20..20bf005c1 100644
--- a/gtsam/sfm/TranslationFactor.h
+++ b/gtsam/sfm/TranslationFactor.h
@@ -78,7 +78,7 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
@@ -149,7 +149,7 @@ class BilinearAngleTranslationFactor
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template <class ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h
index 3b031b334..11a0b1530 100644
--- a/gtsam/slam/AntiFactor.h
+++ b/gtsam/slam/AntiFactor.h
@@ -106,7 +106,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 6eaa5c01b..297703d4a 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -135,7 +135,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -147,10 +147,10 @@ namespace gtsam {
     }
 #endif
 
-	  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
-	  enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
-    public:
-      GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+    // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
+    inline constexpr static auto NeedsToAlign = (sizeof(VALUE) % 16) == 0;
+  public:
+    GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   }; // \class BetweenFactor
 
   /// traits
@@ -176,7 +176,7 @@ namespace gtsam {
   private:
 
     /** Serialization function */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index a496971e2..85d1f6255 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -83,7 +83,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -161,7 +161,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index 45e54aa5d..6b57a8f27 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -91,7 +91,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 5fdf138cc..5d38cb234 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -70,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
                         const SharedNoiseModel& model,
                         std::shared_ptr<CALIBRATION> K)
       : Base(model, key) {
-    assert(K);
+#ifndef NDEBUG
+    if (!K) throw;
+#endif
     vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
     vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
   }
diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h
index a000a1514..37efa5f24 100644
--- a/gtsam/slam/FrobeniusFactor.h
+++ b/gtsam/slam/FrobeniusFactor.h
@@ -49,7 +49,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
  */
 template <class Rot>
 class FrobeniusPrior : public NoiseModelFactorN<Rot> {
-  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
   using MatrixNN = typename Rot::MatrixNN;
   Eigen::Matrix<double, Dim, 1> vecM_;  ///< vectorized matrix to approximate
 
@@ -79,7 +79,7 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
  */
 template <class Rot>
 class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
-  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
 
  public:
 
@@ -111,7 +111,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
   Rot R12_;  ///< measured rotation between R1 and R2
   Eigen::Matrix<double, Rot::dimension, Rot::dimension>
       R2hat_H_R1_;  ///< fixed derivative of R2hat wrpt R1
-  enum { Dim = Rot::VectorN2::RowsAtCompileTime };
+  inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
 
  public:
 
diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h
index 581624b38..06c832eb4 100644
--- a/gtsam/slam/GeneralSFMFactor.h
+++ b/gtsam/slam/GeneralSFMFactor.h
@@ -36,7 +36,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/timing.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 #include <iostream>
@@ -182,7 +182,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
@@ -283,7 +283,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h
index 4e82ca1d9..6b25dd261 100644
--- a/gtsam/slam/KarcherMeanFactor.h
+++ b/gtsam/slam/KarcherMeanFactor.h
@@ -45,7 +45,7 @@ template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
  * */
 template <class T> class KarcherMeanFactor : public NonlinearFactor {
   // Compile time dimension: can be -1
-  enum { D = traits<T>::dimension };
+  inline constexpr static auto D = traits<T>::dimension;
 
   // Runtime dimension: always >=0
   size_t d_;
diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
new file mode 100644
index 000000000..d53cd0ae1
--- /dev/null
+++ b/gtsam/slam/PlanarProjectionFactor.h
@@ -0,0 +1,298 @@
+/**
+ * ProjectionFactor, but with pose2 (robot on the floor)
+ *
+ * This factor is useful for high-school robotics competitions,
+ * which run robots on the floor, with use fixed maps and fiducial
+ * markers.
+ *
+ * @see https://www.firstinspires.org/
+ *
+ * @file PlanarProjectionFactor.h
+ * @brief for planar smoothing
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+
+    /**
+     * @class PlanarProjectionFactorBase
+     * @brief Camera projection for robot on the floor.  Measurement is camera pixels.
+     */
+    class PlanarProjectionFactorBase {
+    protected:
+        PlanarProjectionFactorBase() {}
+
+        /**
+         * @param measured pixels in the camera frame
+         */
+        PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}
+
+        /**
+         * Predict the projection of the landmark in camera pixels.
+         *
+         * @param landmark point3 of the target
+         * @param wTb "world to body": planar pose2 of vehicle body frame in world frame
+         * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x
+         * @param calib camera calibration with distortion
+         * @param Hlandmark jacobian
+         * @param HwTb jacobian
+         * @param HbTc jacobian
+         * @param Hcalib jacobian
+         */
+        Point2 predict(
+            const Point3& landmark,
+            const Pose2& wTb,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z)
+            OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta)
+            OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta)
+            OptionalJacobian<2, 9> Hcalib = {}
+        ) const {
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
+            try {
+#endif
+                Matrix63 Hp; // 6x3
+                Matrix66 H0; // 6x6
+                Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr);
+                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(wTc, calib);
+                if (HwTb || HbTc) {
+                    // Dpose is for pose3 (R,t)
+                    Matrix26 Dpose;
+                    Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
+                    if (HbTc)
+                        *HbTc = Dpose;
+                    if (HwTb)
+                        *HwTb = Dpose * H0 * Hp;
+                    return result;
+                } else {
+                    return camera.project(landmark, {}, {}, {});
+                }
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
+            } catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                if (Hlandmark) Hlandmark->setZero();
+                if (HwTb) HwTb->setZero();
+                if (HbTc) HbTc->setZero();
+                if (Hcalib) Hcalib->setZero();
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib.fx());
+            }
+#endif
+        }
+
+        Point2 measured_; // pixel measurement
+    };
+
+
+    /**
+     * @class PlanarProjectionFactor1
+     * @brief One variable: the pose.
+     * Landmark, camera offset, camera calibration are constant.
+     * This is intended for localization within a known map.
+     */
+    class PlanarProjectionFactor1
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2> {
+    public:
+        typedef NoiseModelFactorN<Pose2> Base;
+        using Base::evaluateError;
+        PlanarProjectionFactor1() {}
+
+        ~PlanarProjectionFactor1() override {}
+
+        /// @return a deep copy of this factor
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
+        }
+
+
+        /**
+         * @brief constructor for known landmark, offset, and calibration
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param landmark point3 in the world
+         * @param measured corresponding point2 in the camera frame
+         * @param bTc "body to camera": constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         */
+        PlanarProjectionFactor1(
+            Key poseKey,
+            const Point3& landmark,
+            const Point2& measured,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model = {})
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, poseKey),
+            landmark_(landmark),
+            bTc_(bTc),
+            calib_(calib) {}
+
+        /**
+         * @param wTb "world to body": estimated pose2
+         * @param HwTb jacobian
+         */
+        Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override {
+            return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
+        }
+
+    private:
+        Point3 landmark_; // landmark
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor1> :
+        public Testable<PlanarProjectionFactor1> {};
+
+    /**
+     * @class PlanarProjectionFactor2
+     * @brief Two unknowns: the pose and the landmark.
+     * Camera offset and calibration are constant.
+     * This is similar to GeneralSFMFactor, used for SLAM.
+    */
+    class PlanarProjectionFactor2
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
+    public:
+        typedef NoiseModelFactorN<Pose2, Point3> Base;
+        using Base::evaluateError;
+
+        PlanarProjectionFactor2() {}
+
+        ~PlanarProjectionFactor2() override {}
+
+        /// @return a deep copy of this factor
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
+        }
+
+        /**
+         * @brief constructor for variable landmark, known offset and calibration
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param landmarkKey index of the landmark point3
+         * @param measured corresponding point in the camera frame
+         * @param bTc "body to camera": constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         */
+        PlanarProjectionFactor2(
+            Key poseKey,
+            Key landmarkKey,
+            const Point2& measured,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model = {})
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, landmarkKey, poseKey),
+            bTc_(bTc),
+            calib_(calib) {}
+
+        /**
+         * @param wTb "world to body": estimated pose2
+         * @param landmark estimated landmark
+         * @param HwTb jacobian
+         * @param Hlandmark jacobian
+         */
+        Vector evaluateError(
+            const Pose2& wTb,
+            const Point3& landmark,
+            OptionalMatrixType HwTb,
+            OptionalMatrixType Hlandmark) const override {
+            return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
+        }
+
+    private:
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor2> :
+        public Testable<PlanarProjectionFactor2> {};
+
+    /**
+     * @class PlanarProjectionFactor3
+     * @brief Three unknowns: the pose, the camera offset, and the camera calibration.
+     * Landmark is constant.
+     * This is intended to be used for camera calibration.
+    */
+    class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
+    public:
+        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;
+        using Base::evaluateError;
+
+        PlanarProjectionFactor3() {}
+
+        ~PlanarProjectionFactor3() override {}
+
+        /// @return a deep copy of this factor
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
+        }
+
+        /**
+         * @brief constructor for variable pose, offset, and calibration, known landmark.
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param offsetKey index of camera offset from pose
+         * @param calibKey index of camera calibration
+         * @param landmark point3 in the world
+         * @param measured corresponding point2 in the camera frame
+         * @param model stddev of the measurements, ~one pixel?
+         */
+        PlanarProjectionFactor3(
+            Key poseKey,
+            Key offsetKey,
+            Key calibKey,
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model = {})
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
+            landmark_(landmark) {}
+
+        /**
+         * @param wTb "world to body": estimated pose2
+         * @param bTc "body to camera": pose3 offset from pose2 +x
+         * @param calib calibration
+         * @param HwTb pose jacobian
+         * @param HbTc offset jacobian
+         * @param Hcalib calibration jacobian
+         */
+        Vector evaluateError(
+            const Pose2& wTb,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            OptionalMatrixType HwTb,
+            OptionalMatrixType HbTc,
+            OptionalMatrixType Hcalib) const override {
+            return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
+        }
+
+    private:
+        Point3 landmark_; // landmark
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor3> :
+        public Testable<PlanarProjectionFactor3> {};
+
+} // namespace gtsam
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index 8ce90988b..47ea4c69f 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -91,7 +91,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h
index c5257f45c..378610825 100644
--- a/gtsam/slam/PoseTranslationPrior.h
+++ b/gtsam/slam/PoseTranslationPrior.h
@@ -90,7 +90,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index f40d8f5bb..baea29eed 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -188,7 +188,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h
index 0ae6ea88a..21617e27a 100644
--- a/gtsam/slam/ReferenceFrameFactor.h
+++ b/gtsam/slam/ReferenceFrameFactor.h
@@ -122,7 +122,7 @@ public:
   Key local_key() const { return this->key3(); }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 5d6ec4445..e641bf5d1 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -30,7 +30,7 @@
 #include <gtsam/geometry/CameraSet.h>
 
 #include <optional>
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
@@ -135,7 +135,11 @@ protected:
 
   /// Add a bunch of measurements, together with the camera keys.
   void add(const ZVector& measurements, const KeyVector& cameraKeys) {
-    assert(measurements.size() == cameraKeys.size());
+#ifndef NDEBUG
+    if (measurements.size() != cameraKeys.size()) {
+      throw std::runtime_error("Number of measurements and camera keys do not match");
+    }
+#endif
     for (size_t i = 0; i < measurements.size(); i++) {
       this->add(measurements[i], cameraKeys[i]);
     }
@@ -446,7 +450,7 @@ protected:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION///
 /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h
index 4523d0e0e..de3245038 100644
--- a/gtsam/slam/SmartFactorParams.h
+++ b/gtsam/slam/SmartFactorParams.h
@@ -118,7 +118,7 @@ struct SmartProjectionParams {
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index c54efecc4..3c8843b39 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -466,7 +466,7 @@ protected:
 
  private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h
index b88c1c628..855ae8c08 100644
--- a/gtsam/slam/SmartProjectionPoseFactor.h
+++ b/gtsam/slam/SmartProjectionPoseFactor.h
@@ -148,7 +148,7 @@ public:
 
  private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h
index 6007c84e2..dd4237299 100644
--- a/gtsam/slam/SmartProjectionRigFactor.h
+++ b/gtsam/slam/SmartProjectionRigFactor.h
@@ -349,7 +349,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 5adafcf3a..5fd171851 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -175,7 +175,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h
index 3b8486a59..f36e9b385 100644
--- a/gtsam/slam/TriangulationFactor.h
+++ b/gtsam/slam/TriangulationFactor.h
@@ -187,7 +187,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index a226f06ec..1761a6cc3 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -24,6 +24,38 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/PlanarProjectionFactor.h>
+virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor1(
+    size_t poseKey,
+    const gtsam::Point3& landmark,
+    const gtsam::Point2& measured,
+    const gtsam::Pose3& bTc,
+    const gtsam::Cal3DS2& calib,
+    const gtsam::noiseModel::Base* model);
+  void serialize() const;
+};
+virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor2(
+    size_t poseKey,
+    size_t landmarkKey,
+    const gtsam::Point2& measured,
+    const gtsam::Pose3& bTc,
+    const gtsam::Cal3DS2& calib,
+    const gtsam::noiseModel::Base* model);
+  void serialize() const;
+};
+virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor3(
+    size_t poseKey,
+    size_t offsetKey,
+    size_t calibKey,
+    const gtsam::Point3& landmark,
+    const gtsam::Point2& measured,
+    const gtsam::noiseModel::Base* model);
+  void serialize() const;
+};
+
 #include <gtsam/slam/ProjectionFactor.h>
 template <POSE, LANDMARK, CALIBRATION>
 virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
new file mode 100644
index 000000000..5f65c9b5e
--- /dev/null
+++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
@@ -0,0 +1,437 @@
+/**
+ * @file testPlanarProjectionFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarProjectionFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarProjectionFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+using symbol_shorthand::C;
+using symbol_shorthand::K;
+using symbol_shorthand::L;
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, Error1) {
+    // Example: center projection and Jacobian
+    Point3 landmark(1, 0, 0);
+    Point2 measured(200, 200);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
+    Pose2 pose(0, 0, 0);
+    Matrix H;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
+    CHECK(assert_equal((Matrix23() << //
+        0, 200, 200, //
+        0, 0, 0).finished(), H, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, Error2) {
+    // Example: upper left corner projection and Jacobian
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
+    Pose2 pose(0, 0, 0);
+    Matrix H;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
+    CHECK(assert_equal((Matrix23() << //
+        -200, 200, 400, //
+        -200, 0, 200).finished(), H, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, Error3) {
+    // Example: upper left corner projection and Jacobian with distortion
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
+    Pose2 pose(0, 0, 0);
+    Matrix H;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
+    CHECK(assert_equal((Matrix23() << //
+        -360, 280, 640, //
+        -360, 80, 440).finished(), H, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, Jacobian) {
+    // Verify Jacobians with numeric derivative
+    std::default_random_engine rng(42);
+    std::uniform_real_distribution<double> dist(-0.3, 0.3);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // center of the random camera poses
+    Pose3 centerOffset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
+        Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
+        Pose3 offset = centerOffset.compose(
+            Pose3(
+                Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
+                Point3(dist(rng), dist(rng), dist(rng))));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+        PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
+        Pose2 pose(dist(rng), dist(rng), dist(rng));
+        Matrix H1;
+        factor.evaluateError(pose, H1);
+        auto expectedH1 = numericalDerivative11<Vector, Pose2>(
+            [&factor](const Pose2& p) {
+                return factor.evaluateError(p, {});},
+                pose);
+        CHECK(assert_equal(expectedH1, H1, 5e-6));
+    }
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, Solve) {
+    // Example localization
+
+    SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // pose model is wide, so the solver finds the right answer.
+    SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10));
+
+    // landmarks
+    Point3 l0(1, 0.1, 1);
+    Point3 l1(1, -0.1, 1);
+
+    // camera pixels
+    Point2 p0(180, 0);
+    Point2 p1(220, 0);
+
+    // body
+    Pose2 x0(0, 0, 0);
+
+    // camera z looking at +x with (xy) antiparallel to (yz)
+    Pose3 c0(
+        Rot3(0, 0, 1, //
+            -1, 0, 0, //
+            0, -1, 0), //
+        Vector3(0, 0, 0));
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    NonlinearFactorGraph graph;
+    graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel));
+    graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel));
+    graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
+
+    Values initialEstimate;
+    initialEstimate.insert(X(0), x0);
+
+    // run the optimizer
+    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
+
+    // verify that the optimizer found the right pose.
+    CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
+
+    // covariance
+    Marginals marginals(graph, result);
+    Matrix cov = marginals.marginalCovariance(X(0));
+    CHECK(assert_equal((Matrix33() << //
+        0.000012, 0.000000, 0.000000, //
+        0.000000, 0.001287, -.001262, //
+        0.000000, -.001262, 0.001250).finished(), cov, 3e-6));
+
+    // pose stddev
+    Vector3 sigma = cov.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector3() << //
+        0.0035,
+        0.0359,
+        0.0354
+        ).finished(), sigma, 1e-4));
+
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, Error1) {
+    // Example: center projection and Jacobian
+    Point3 landmark(1, 0, 0);
+    Point2 measured(200, 200);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
+    Pose2 pose(0, 0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
+    CHECK(assert_equal((Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        0, -200, 0, -200, 0, 0,//
+        200, -0, 0, 0, -200, 0).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
+        0, 0, 0, 1, 0, 0, 0, 0, 0,//
+        0, 0, 0, 0, 1, 0, 0, 0, 0).finished(), H3, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, Error2) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
+    Pose2 pose(0, 0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3);
+    CHECK(assert_equal(Vector2(0, 0), actual));
+    CHECK(assert_equal((Matrix23() <<//
+        -200, 200, 400,//
+        -200, 0, 200).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        200, -400, -200, -200, 0, -200,//
+        400, -200, 200, 0, -200, -200).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, Error3) {
+    Point3 landmark(1, 1, 1);
+    Point2 measured(0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
+    Pose2 pose(0, 0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
+    CHECK(assert_equal((Matrix23() <<//
+        -360, 280, 640,//
+        -360, 80, 440).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        440, -640, -200, -280, -80, -360,//
+        640, -440, 200, -80, -280, -360).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
+        -1, 0, -1, 1, 0, -400, -800, 400, 800,//
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, Jacobian) {
+    // Verify Jacobians with numeric derivative
+
+    std::default_random_engine rng(42);
+    std::uniform_real_distribution<double> dist(-0.3, 0.3);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // center of the random camera poses
+    Pose3 centerOffset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
+        Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
+        Pose3 offset = centerOffset.compose(
+            Pose3(
+                Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
+                Point3(dist(rng), dist(rng), dist(rng))));
+        Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+
+        PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
+
+        Pose2 pose(dist(rng), dist(rng), dist(rng));
+
+        // actual H
+        Matrix H1, H2, H3;
+        factor.evaluateError(pose, offset, calib, H1, H2, H3);
+
+        Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c, {}, {}, {});},
+                pose, offset, calib);
+        Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c, {}, {}, {});},
+                pose, offset, calib);
+        Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c, {}, {}, {});},
+                pose, offset, calib);
+        CHECK(assert_equal(expectedH1, H1, 5e-6));
+        CHECK(assert_equal(expectedH2, H2, 5e-6));
+        CHECK(assert_equal(expectedH3, H3, 5e-6));
+    }
+}
+
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, SolveOffset) {
+    // Example localization
+    SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
+    // offset model is wide, so the solver finds the right answer.
+    SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10));
+    SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001));
+
+    // landmarks
+    Point3 l0(1, 0, 1);
+    Point3 l1(1, 0, 0);
+    Point3 l2(1, -1, 1);
+    Point3 l3(2, 2, 1);
+
+    // camera pixels
+    Point2 p0(200, 200);
+    Point2 p1(200, 400);
+    Point2 p2(400, 200);
+    Point2 p3(0, 200);
+
+    // body
+    Pose2 x0(0, 0, 0);
+
+    // camera z looking at +x with (xy) antiparallel to (yz)
+    Pose3 c0(
+        Rot3(0, 0, 1, //
+            -1, 0, 0, //
+            0, -1, 0), //
+        Vector3(0, 0, 1)); // note z offset
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    NonlinearFactorGraph graph;
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel));
+    graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
+    graph.add(PriorFactor<Pose3>(C(0), c0, cNoise));
+    graph.add(PriorFactor<Cal3DS2>(K(0), calib, kNoise));
+
+    Values initialEstimate;
+    initialEstimate.insert(X(0), x0);
+    initialEstimate.insert(C(0), c0);
+    initialEstimate.insert(K(0), calib);
+
+    // run the optimizer
+    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
+
+    // verify that the optimizer found the right pose.
+    CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
+
+    // verify the camera is pointing at +x
+    Pose3 cc0 = result.at<Pose3>(C(0));
+    CHECK(assert_equal(c0, cc0, 5e-3));
+
+    // verify the calibration
+    CHECK(assert_equal(calib, result.at<Cal3DS2>(K(0)), 2e-3));
+
+    Marginals marginals(graph, result);
+    Matrix x0cov = marginals.marginalCovariance(X(0));
+
+    // narrow prior => ~zero cov
+    CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4));
+
+    Matrix c0cov = marginals.marginalCovariance(C(0));
+
+    // invert the camera offset to get covariance in body coordinates
+    Matrix66 HcTb = cc0.inverse().AdjointMap().inverse();
+    Matrix c0cov2 = HcTb * c0cov * HcTb.transpose();
+
+    // camera-frame stddev
+    Vector6 c0sigma = c0cov.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector6() << //
+        0.009,
+        0.011,
+        0.004,
+        0.012,
+        0.012,
+        0.011
+        ).finished(), c0sigma, 1e-3));
+
+    // body frame stddev
+    Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector6() << //
+        0.004,
+        0.009,
+        0.011,
+        0.012,
+        0.012,
+        0.012
+        ).finished(), bTcSigma, 1e-3));
+
+    // narrow prior => ~zero cov
+    CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3));
+}
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h
index 3102191dc..4bba7cb69 100644
--- a/gtsam/symbolic/SymbolicBayesNet.h
+++ b/gtsam/symbolic/SymbolicBayesNet.h
@@ -101,7 +101,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h
index 275b9560d..2b9e2f7ad 100644
--- a/gtsam/symbolic/SymbolicBayesTree.h
+++ b/gtsam/symbolic/SymbolicBayesTree.h
@@ -62,7 +62,7 @@ namespace gtsam {
     bool equals(const This& other, double tol = 1e-9) const;
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h
index 5cc4e9cfc..3cc3e60cc 100644
--- a/gtsam/symbolic/SymbolicConditional.h
+++ b/gtsam/symbolic/SymbolicConditional.h
@@ -126,7 +126,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class Archive>
diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h
index 190609ecb..fd19740de 100644
--- a/gtsam/symbolic/SymbolicFactor.h
+++ b/gtsam/symbolic/SymbolicFactor.h
@@ -151,7 +151,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h
index 4122031d0..bbcd59f5e 100644
--- a/gtsam/symbolic/SymbolicFactorGraph.h
+++ b/gtsam/symbolic/SymbolicFactorGraph.h
@@ -145,7 +145,7 @@ namespace gtsam {
     /// @}
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp
index 0de4d32c4..53be25d55 100644
--- a/gtsam_unstable/discrete/Scheduler.cpp
+++ b/gtsam_unstable/discrete/Scheduler.cpp
@@ -14,6 +14,7 @@
 #include <cmath>
 #include <fstream>
 #include <iomanip>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h
index d4ebcba19..2dc0d552b 100644
--- a/gtsam_unstable/dynamics/DynamicsPriors.h
+++ b/gtsam_unstable/dynamics/DynamicsPriors.h
@@ -12,6 +12,8 @@
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 #include <gtsam_unstable/slam/PartialPriorFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 // Indices of relevant variables in the PoseRTV tangent vector:
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 02da899b7..eea019cd2 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index ee09600e5..241b6b497 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp
index cf21c315b..9457c99e8 100644
--- a/gtsam_unstable/dynamics/PoseRTV.cpp
+++ b/gtsam_unstable/dynamics/PoseRTV.cpp
@@ -7,6 +7,8 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Vector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;
diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h
index 0e0d9ae27..95b7cfff3 100644
--- a/gtsam_unstable/dynamics/PoseRTV.h
+++ b/gtsam_unstable/dynamics/PoseRTV.h
@@ -157,7 +157,7 @@ public:
   /// @}
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index 2bb70e1b5..dbdd27259 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 namespace dynamics {
diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h
index 9f2ec89d2..9751b30a7 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint3.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint3.h
@@ -51,7 +51,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp
index 9dfed612c..23afd158b 100644
--- a/gtsam_unstable/geometry/BearingS2.cpp
+++ b/gtsam_unstable/geometry/BearingS2.cpp
@@ -9,6 +9,8 @@
 
 #include <gtsam_unstable/geometry/BearingS2.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;
diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h
index cb1d055df..0c3f40df8 100644
--- a/gtsam_unstable/geometry/BearingS2.h
+++ b/gtsam_unstable/geometry/BearingS2.h
@@ -90,7 +90,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
   // Serialization function
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h
index a3c907646..471ada35b 100644
--- a/gtsam_unstable/geometry/Event.h
+++ b/gtsam_unstable/geometry/Event.h
@@ -39,7 +39,7 @@ class GTSAM_UNSTABLE_EXPORT Event {
   Point3 location_;  ///< Location at time event was generated
 
  public:
-  enum { dimension = 4 };
+  inline constexpr static auto dimension = 4;
 
   /// Default Constructor
   Event() : time_(0), location_(0, 0, 0) {}
diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h
index 45b27efe3..d1486209e 100644
--- a/gtsam_unstable/geometry/InvDepthCamera3.h
+++ b/gtsam_unstable/geometry/InvDepthCamera3.h
@@ -19,7 +19,7 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
 
@@ -175,7 +175,7 @@ private:
   /// @name Advanced Interface
   /// @{
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp
index 9cd9f78f5..c5ef0d70f 100644
--- a/gtsam_unstable/geometry/Pose3Upright.cpp
+++ b/gtsam_unstable/geometry/Pose3Upright.cpp
@@ -6,8 +6,9 @@
  */
 
 #include <iostream>
-#include "gtsam/base/OptionalJacobian.h"
-#include "gtsam/base/Vector.h"
+#include <cassert>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/base/Vector.h>
 
 #include <gtsam_unstable/geometry/Pose3Upright.h>
 
diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h
index a77715fc6..20e0fde5d 100644
--- a/gtsam_unstable/geometry/Pose3Upright.h
+++ b/gtsam_unstable/geometry/Pose3Upright.h
@@ -130,7 +130,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  //
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
   // Serialization function
   friend class boost::serialization::access;
   template<class Archive>
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 36cb6165f..8dfb15aa2 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/debug.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
index 2d404005d..375c49341 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam_unstable/nonlinear/LinearizedFactor.h>
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h
index 1ff45ef5f..7aa596be1 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.h
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.h
@@ -59,7 +59,7 @@ public:
   const Values& linearizationPoint() const { return lin_points_; }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -148,7 +148,7 @@ public:
   Vector error_vector(const Values& c) const;
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /** Serialization function */
   template<class ARCHIVE>
@@ -279,7 +279,7 @@ public:
 
 private:
   /** Serialization function */
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index f4718f7a9..21ed91604 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -14,6 +14,7 @@
 #include <iostream>
 #include <vector>
 #include <optional>
+#include <cassert>
 #include <boost/shared_array.hpp>
 
 #include <gtsam/base/timing.h>
diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h
index f1337c0e8..6cdce415e 100644
--- a/gtsam_unstable/slam/BetweenFactorEM.h
+++ b/gtsam_unstable/slam/BetweenFactorEM.h
@@ -415,7 +415,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h
index f2b669f4f..1c278b111 100644
--- a/gtsam_unstable/slam/BiasedGPSFactor.h
+++ b/gtsam_unstable/slam/BiasedGPSFactor.h
@@ -94,7 +94,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
index 0accb8f42..b759ceb56 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
@@ -704,7 +704,7 @@ public:
     }
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
index a94c95335..56f2e2cee 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
@@ -573,7 +573,7 @@ public:
     }
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
index b91ed2afe..c9c4141b1 100644
--- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
+++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
@@ -113,7 +113,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
index 8fda5456b..95fde2f75 100644
--- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
+++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
@@ -411,7 +411,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h
index 64a1366a2..f47c56bc9 100644
--- a/gtsam_unstable/slam/InvDepthFactor3.h
+++ b/gtsam_unstable/slam/InvDepthFactor3.h
@@ -113,7 +113,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h
index a49b2090c..638b77440 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant1.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h
@@ -133,7 +133,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h
index 7abbbe89a..01f087df0 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant2.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h
@@ -140,7 +140,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h
index 8664002e8..f71df8425 100644
--- a/gtsam_unstable/slam/InvDepthFactorVariant3.h
+++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h
@@ -137,7 +137,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
@@ -269,7 +269,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   friend class boost::serialization::access;
   /// Serialization function
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h
index 8e2ea0cf7..28e9c0215 100644
--- a/gtsam_unstable/slam/MultiProjectionFactor.h
+++ b/gtsam_unstable/slam/MultiProjectionFactor.h
@@ -213,7 +213,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h
index 7722e5d82..645d0305a 100644
--- a/gtsam_unstable/slam/PartialPriorFactor.h
+++ b/gtsam_unstable/slam/PartialPriorFactor.h
@@ -20,6 +20,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Lie.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /**
@@ -140,7 +142,7 @@ namespace gtsam {
     const std::vector<size_t>& indices() const { return indices_; }
 
   private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h
index aa383967d..1e0526e22 100644
--- a/gtsam_unstable/slam/PoseBetweenFactor.h
+++ b/gtsam_unstable/slam/PoseBetweenFactor.h
@@ -119,7 +119,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h
index 4a201ad89..60df7e948 100644
--- a/gtsam_unstable/slam/PosePriorFactor.h
+++ b/gtsam_unstable/slam/PosePriorFactor.h
@@ -101,7 +101,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h
index be454cb70..911445fa5 100644
--- a/gtsam_unstable/slam/PoseToPointFactor.h
+++ b/gtsam_unstable/slam/PoseToPointFactor.h
@@ -92,7 +92,7 @@ class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
   const POINT& measured() const { return measured_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h
index 38467c6cc..126900bc2 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPP.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPP.h
@@ -171,7 +171,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h
index df63330df..cea74842f 100644
--- a/gtsam_unstable/slam/ProjectionFactorPPPC.h
+++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h
@@ -151,7 +151,7 @@ class ProjectionFactorPPPC
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h
index c5eb58f14..778713733 100644
--- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h
+++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h
@@ -194,7 +194,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
   inline bool throwCheirality() const { return throwCheirality_; }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h
index d24415c3b..13bd5eba0 100644
--- a/gtsam_unstable/slam/RelativeElevationFactor.h
+++ b/gtsam_unstable/slam/RelativeElevationFactor.h
@@ -65,7 +65,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
index 4a2047ee1..f28ce642b 100644
--- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
+++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
@@ -453,7 +453,7 @@ class SmartProjectionPoseFactorRollingShutter
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
index 52d6eda05..7b06e8827 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h
@@ -30,7 +30,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam_unstable/dllexport.h>
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/optional.hpp>
 #endif
 
@@ -504,7 +504,7 @@ public:
 
 private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
index c4a473154..6d78bdb7a 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
@@ -18,6 +18,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
index 189c501bb..7482682fa 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
@@ -285,7 +285,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
   }
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
index 57913385a..52129daa8 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
@@ -21,6 +21,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
index 77a15c246..93d8877a7 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
@@ -138,7 +138,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
   Base::Cameras cameras(const Values& values) const override;
 
  private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
index 2a257ff57..cbc8eba52 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h
@@ -216,7 +216,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
index c47cf3b7d..352a9ff37 100644
--- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
+++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
@@ -414,7 +414,7 @@ namespace gtsam {
 
   private:
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py
new file mode 100644
index 000000000..4f5c57b0a
--- /dev/null
+++ b/python/gtsam/tests/test_Serialization.py
@@ -0,0 +1,64 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Serialization and deep copy tests.
+
+Author: Varun Agrawal
+"""
+import unittest
+
+import numpy as np
+from gtsam.symbol_shorthand import B, V, X
+from gtsam.utils.test_case import GtsamTestCase
+
+import gtsam
+
+
+class TestDeepCopy(GtsamTestCase):
+    """Tests for deep copy of various GTSAM objects."""
+
+    def test_PreintegratedImuMeasurements(self):
+        """
+        Test the deep copy of `PreintegratedImuMeasurements`.
+        """
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81)
+        pim = gtsam.PreintegratedImuMeasurements(params)
+
+        self.assertDeepCopyEquality(pim)
+
+    def test_ImuFactor(self):
+        """
+        Test the deep copy of `ImuFactor`.
+        """
+        params = gtsam.PreintegrationParams.MakeSharedD(9.81)
+        params.setAccelerometerCovariance(1e-7 * np.eye(3))
+        params.setGyroscopeCovariance(1e-8 * np.eye(3))
+        params.setIntegrationCovariance(1e-9 * np.eye(3))
+        priorBias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3))
+        pim = gtsam.PreintegratedImuMeasurements(params, priorBias)
+
+        # Preintegrate a single measurement for serialization to work.
+        pim.integrateMeasurement(measuredAcc=np.zeros(3),
+                                 measuredOmega=np.zeros(3),
+                                 deltaT=0.005)
+
+        factor = gtsam.ImuFactor(0, 1, 2, 3, 4, pim)
+
+        self.assertDeepCopyEquality(factor)
+
+    def test_PreintegratedCombinedMeasurements(self):
+        """
+        Test the deep copy of `PreintegratedCombinedMeasurements`.
+        """
+        params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81]))
+        pim = gtsam.PreintegratedCombinedMeasurements(params)
+
+        self.assertDeepCopyEquality(pim)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py
index a6a5745bc..e51617b00 100644
--- a/python/gtsam/tests/test_pickle.py
+++ b/python/gtsam/tests/test_pickle.py
@@ -9,24 +9,26 @@ Unit tests to check pickling.
 
 Author: Ayush Baid
 """
-from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 
-
 from gtsam.utils.test_case import GtsamTestCase
 
+from gtsam import (Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3,
+                   Pose3, Rot3, SfmTrack, Unit3)
+
+
 class TestPickle(GtsamTestCase):
     """Tests pickling on some of the classes."""
 
     def test_cal3Bundler_roundtrip(self):
         obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70)
         self.assertEqualityOnPickleRoundtrip(obj)
-    
+
     def test_pinholeCameraCal3Bundler_roundtrip(self):
         obj = PinholeCameraCal3Bundler(
             Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)),
             Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70),
         )
         self.assertEqualityOnPickleRoundtrip(obj)
-    
+
     def test_rot3_roundtrip(self):
         obj = Rot3.RzRyRx(0, 0.05, 0.1)
         self.assertEqualityOnPickleRoundtrip(obj)
diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py
index 50af004f4..74eaff1db 100644
--- a/python/gtsam/utils/test_case.py
+++ b/python/gtsam/utils/test_case.py
@@ -10,6 +10,7 @@ Author: Frank Dellaert
 """
 import pickle
 import unittest
+from copy import deepcopy
 
 
 class GtsamTestCase(unittest.TestCase):
@@ -28,8 +29,8 @@ class GtsamTestCase(unittest.TestCase):
         else:
             equal = actual.equals(expected, tol)
         if not equal:
-            raise self.failureException(
-                "Values are not equal:\n{}!={}".format(actual, expected))
+            raise self.failureException("Values are not equal:\n{}!={}".format(
+                actual, expected))
 
     def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None:
         """ Performs a round-trip using pickle and asserts equality.
@@ -41,3 +42,14 @@ class GtsamTestCase(unittest.TestCase):
         """
         roundTripObj = pickle.loads(pickle.dumps(obj))
         self.gtsamAssertEquals(roundTripObj, obj)
+
+    def assertDeepCopyEquality(self, obj):
+        """Perform assertion by checking if a
+        deep copied version of `obj` is equal to itself.
+
+        Args:
+            obj: The object to check is deep-copyable.
+        """
+        # If deep copy failed, then this will throw an error
+        obj2 = deepcopy(obj)
+        self.gtsamAssertEquals(obj, obj2)
diff --git a/tests/simulated2D.h b/tests/simulated2D.h
index 612dd1891..78fc26708 100644
--- a/tests/simulated2D.h
+++ b/tests/simulated2D.h
@@ -160,7 +160,7 @@ namespace simulated2D {
     /// Default constructor
     GenericPrior() { }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -210,7 +210,7 @@ namespace simulated2D {
     /// Default constructor
     GenericOdometry() { }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
@@ -261,7 +261,7 @@ namespace simulated2D {
     /// Default constructor
     GenericMeasurement() { }
 
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    ///
+#if GTSAM_ENABLE_BOOST_SERIALIZATION    ///
     /// Serialization function
     friend class boost::serialization::access;
     template<class ARCHIVE>
diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp
index 4c2ac99b1..845c28e5a 100644
--- a/tests/testExpressionFactor.cpp
+++ b/tests/testExpressionFactor.cpp
@@ -688,7 +688,7 @@ public:
   }
 
 private:
-#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 721ed51c0..3b507d806 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -25,6 +25,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
 
 using namespace std;
 using namespace gtsam;
diff --git a/wrap/pybind11/docs/requirements.txt b/wrap/pybind11/docs/requirements.txt
index 293db6a06..8dffd36b5 100644
--- a/wrap/pybind11/docs/requirements.txt
+++ b/wrap/pybind11/docs/requirements.txt
@@ -130,9 +130,9 @@ imagesize==1.4.1 \
     --hash=sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b \
     --hash=sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a
     # via sphinx
-jinja2==3.1.4 \
-    --hash=sha256:4a3aee7acbbe7303aede8e9648d13b8bf88a429282aa6122a993f0ac800cb369 \
-    --hash=sha256:bc5dd2abb727a5319567b7a813e6a2e7318c39f4f487cfe6c89c6f9c7d25197d
+jinja2==3.1.5 \
+    --hash=sha256:8fefff8dc3034e27bb80d67c671eb8a9bc424c0ef4c0826edbff304cceff43bb \
+    --hash=sha256:aba0f4dc9ed8013c424088f68a5c226f7d6097ed89b246d7749c2ec4175c6adb
     # via sphinx
 markupsafe==2.1.5 \
     --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \