Merge branch 'develop' into discrete-elimination-refactor

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)

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>`.

INSTALL.md

@@ -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

examples/CombinedImuFactorsExample.cpp

@@ -48,6 +48,7 @@
 #include <cstring>
 #include <fstream>
 #include <iostream>
+#include <cassert>
 
 using namespace gtsam;
 using namespace std;

examples/ImuFactorsExample.cpp

@@ -49,6 +49,7 @@
 #include <gtsam/slam/dataset.h>
 
 #include <cstring>
+#include <cassert>
 #include <fstream>
 #include <iostream>
 

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;
 

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(

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>

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>

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>

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>

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)
 };

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;
 

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;

gtsam/base/Matrix.cpp

@@ -24,6 +24,7 @@
 #include <Eigen/LU>
 
 #include <cstdarg>
+#include <cassert>
 #include <cstring>
 #include <iomanip>
 #include <list>

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;
 

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>

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};
+  constexpr static const size_t dimension1 = traits<G>::dimension;
-  enum {dimension2 = traits<H>::dimension};
+  constexpr static const size_t dimension2 = traits<H>::dimension;
 
 public:
   /// Default constructor yields identity
@@ -67,7 +67,7 @@ public:
 
   /// @name Manifold
   /// @{
-  enum {dimension = dimension1 + dimension2};
+  inline constexpr static auto dimension = dimension1 + dimension2;
   inline static size_t Dim() { return dimension; }
   inline size_t dim() const { return dimension; }
 

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>

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

gtsam/base/Vector.cpp

@@ -24,6 +24,7 @@
 #include <iostream>
 #include <fstream>
 #include <sstream>
+#include <cassert>
 #include <iomanip>
 #include <cmath>
 #include <cstdio>

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>

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;

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>

gtsam/base/cholesky.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 
 using namespace std;
 

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>

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>

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>
 

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;
 

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:

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);

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) {
+  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;
-  }
 
   /// All Chebyshev points
   static Vector Points(size_t N) {

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

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>

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>

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);
     }
   }
 

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>

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>

gtsam/discrete/DiscreteConditional.cpp

@@ -30,6 +30,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <cassert>
 
 using namespace std;
 using std::pair;

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>

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;

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>

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>

gtsam/discrete/DiscreteLookupDAG.cpp

@@ -23,6 +23,7 @@
 #include <iterator>
 #include <string>
 #include <utility>
+#include <cassert>
 
 using std::pair;
 using std::vector;

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>

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
+ * We visit each leaf in the tree, and using the cardinalities of the keys,
- * the cardinality of the innermost key `n`, and we go up to modulo n.
+ * 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,
+static Eigen::SparseVector<double> ComputeSparseTable(
-                                        const DecisionTreeFactor& dt) {
+    const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) {
-  std::vector<double> probs = dt.probabilities();
+  // SparseVector needs to know the maximum possible index,
-  std::vector<double> ordered;
+  // 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) {
+   * @brief Functor which is called by the DecisionTree for each leaf.
-      if (idx % n == k) {
+   * For each leaf value, we use the corresponding assignment to compute a
-        ordered.push_back(probs[idx]);
+   * 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);
     }
-  // NOTE(Varun): This constructor is really expensive!!
+    // k will be in reverse key order already
-  DecisionTreeFactor f(dkeys, table);
+    uint64_t k;
+    ss >> k;
+    pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i)));
+  }
+
+  // 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;
 }

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}
+   * Convert probability table given as doubles to SparseVector.
-  static Eigen::SparseVector<double> Convert(const std::vector<double>& table);
+   * 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;
 

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(
+  std::string expected_values =
-      X & Y,
+      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667";
-      "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()));
 }
 
 /* ************************************************************************* */

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 };
+  constexpr static const size_t dimB = traits<B>::dimension;
-  enum { dimR = traits<R>::dimension };
+  constexpr static const size_t dimR = traits<R>::dimension;
-  enum { dimension = dimB + dimR };
+  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 {
+  inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0;
-    NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
-  };
 public:
   GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 };

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>

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>

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>

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>

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>

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>

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>

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>

gtsam/geometry/Cal3f.cpp

@@ -28,6 +28,7 @@
 #include <gtsam/geometry/Cal3f.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 

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>

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 {
+  inline constexpr static auto dimension = 6;
-    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>

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>

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>

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; }
 

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() :

gtsam/geometry/OrientedPlane3.h

@@ -39,9 +39,7 @@ private:
   double d_;    ///< The perpendicular distance to this plane
 
 public:
-  enum {
+  inline constexpr static auto dimension = 3;
-    dimension = 3
-  };
 
   /// @name Constructors
   /// @{

gtsam/geometry/PinholeCamera.h

@@ -51,9 +51,7 @@ private:
 
 public:
 
-  enum {
+  inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration
-    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>

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 {
+  inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for
-    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>

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>

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
 

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();

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 = {},

gtsam/geometry/Pose2.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/base/concepts.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <iomanip>
 

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) :
+  Pose2(const Matrix &T)
-    r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
+      : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
-    assert(T.rows() == 3 && T.cols() == 3);
+#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>

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();
@@ -165,23 +177,27 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
 }
 
 /* ************************************************************************* */
-/** 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
+  // Compute rotation using Expmap
-  Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
+  Matrix3 Jr;
+  Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
 
-  Rot3 R = Rot3::Expmap(omega);
+  // Compute translation and optionally its Jacobian Q in w
-  double theta2 = omega.dot(omega);
+  // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
-  if (theta2 > std::numeric_limits<double>::epsilon()) {
+  Matrix3 Q;
-    Vector3 t_parallel = omega * omega.dot(v);  // translation parallel to axis
+  const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
-    Vector3 omega_cross_v = omega.cross(v);     // points towards axis
+
-    Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
+  if (Hxi) {
-    return Pose3(R, t);
+    *Hxi << Jr, Z_3x3,  //
-  } else {
+        Q, Jr;
-    return Pose3(R, v);
   }
+
+  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;
-
+  ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
-#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
-
   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;

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>

gtsam/geometry/Quaternion.h

@@ -45,9 +45,7 @@ struct traits<QUATERNION_TYPE> {
   /// @}
   /// @name Basic manifold traits
   /// @{
-  enum {
+  inline constexpr static auto dimension = 3;
-    dimension = 3
-  };
   typedef OptionalJacobian<3, 3> ChartJacobian;
   typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
 

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>

gtsam/geometry/Rot3.cpp

@@ -23,6 +23,7 @@
 #include <gtsam/geometry/SO3.h>
 
 #include <cmath>
+#include <cassert>
 #include <random>
 
 using namespace std;

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>

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)) {
+    : theta2(omega.dot(omega)),
-  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
+      theta(std::sqrt(theta2)),
-  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+      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) {
+    : theta2(angle * angle),
-  const double ax = axis.x(), ay = axis.y(), az = axis.z();
+      theta(angle),
-  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
+      W(skewSymmetric(axis * angle)),
-  W = K * angle;
+      WW(W * W) {
   init(nearZeroApprox);
-  if (!nearZero) {
-    KK = K * K;
-  }
 }
 
-SO3 ExpmapFunctor::expmap() const {
+SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
-  if (nearZero)
-    return SO3(I_3x3 + W);
-  else
-    return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
-}
 
 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
     : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
-  if (nearZero) {
+  if (!nearZero) {
-    dexp_ = I_3x3 - 0.5 * W;
+    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;
+    // Taylor expansion at 0
-    b = 1.0 - sin_theta / theta;
+    // TODO(Frank): flipping signs here does not trigger any tests: harden!
-    dexp_ = I_3x3 - a * K + b * KK;
+    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) {
+  Matrix3 D_BWv_w, D_CWWv_w;
-    if (nearZero) {
+  const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
-      *H1 = 0.5 * skewSymmetric(v);
+  const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
-    } else {
+  if (H1) *H1 = -D_BWv_w + D_CWWv_w;
-      // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
+  if (H2) *H2 = rightJacobian();
-      const Vector3 Kv = K * v;
+  return v - BWv + CWWv;
-      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;
 }
 
 Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
                                   OptionalJacobian<3, 3> H2) const {
-  const Matrix3 invDexp = dexp_.inverse();
+  const Matrix3 invJr = rightJacobianInverse();
-  const Vector3 c = invDexp * v;
+  const Vector3 c = invJr * v;
   if (H1) {
-    Matrix3 D_dexpv_omega;
+    Matrix3 H;
-    applyDexp(c, D_dexpv_omega);  // get derivative H of forward mapping
+    applyDexp(c, H);  // get derivative H of forward mapping
-    *H1 = -invDexp * D_dexpv_omega;
+    *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^
+  return skewSymmetric(xi);
-  Matrix3 Y = Z_3x3;
-  Y(0, 1) = -xi(2);
-  Y(0, 2) = +xi(1);
-  Y(1, 2) = -xi(0);
-  return Y - Y.transpose();
 }
 
 //******************************************************************************

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 {
+/// Math is based on Ethan Eade's elegant Lie group document, at
- protected:
+/// https://www.ethaneade.org/lie.pdf.
-  const double theta2;
+struct GTSAM_EXPORT ExpmapFunctor {
-  Matrix3 W, K, KK;
+  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,32 +151,73 @@ 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)
+  //   Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi)
-  // This maps a perturbation v in the tangent space to
+  // This maps a perturbation dxi=(w,v) in the tangent space to
-  // a perturbation on the manifold Expmap(dexp * v) */
+  // a perturbation on the manifold Expmap(dexp * xi)
-  const Matrix3& dexp() const { return dexp_; }
+  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,
+  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;
 };

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*/) {

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(

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>

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>

gtsam/geometry/StereoCamera.h

@@ -61,9 +61,7 @@ private:
 
 public:
 
-  enum {
+  inline constexpr static auto dimension = 6;
-    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*/) {

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>

gtsam/geometry/Unit3.h

@@ -53,9 +53,7 @@ private:
 
 public:
 
-  enum {
+  inline constexpr static auto dimension = 2;
-    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>

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) {

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;

gtsam/geometry/tests/testPose3.cpp

@@ -835,17 +835,7 @@ TEST(Pose3, Align2) {
 }
 
 /* ************************************************************************* */
-TEST( Pose3, ExpmapDerivative1) {
+TEST(Pose3, ExpmapDerivative) {
-  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) {
   // 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();
+namespace test_cases {
-  w.head<3>().normalize();
+std::vector<Vector3> small{{0, 0, 0},                                 //
-  w.head<3>() = w.head<3>() * 0.9e-2;
+                           {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5},  //,
-  Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
+                           {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
-  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
+std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0},    {0, 1, 0},
-      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
+                           {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
-  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
+auto omegas = [](bool nearZero) { return nearZero ? small : large; };
-  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
+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) {
+// Check logmap for all small values, as we don't want wrapping.
-  Matrix6 actualH;
+TEST(Pose3, Logmap) {
-  Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
+  static constexpr bool nearZero = true;
-  Pose3 p = Pose3::Expmap(w);
+  for (const Vector3& w : test_cases::omegas(nearZero)) {
-  EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
+    for (Vector3 v : test_cases::vs) {
-  Matrix expectedH = numericalDerivative21<Vector6, Pose3,
+      const Vector6 xi = (Vector6() << w, v).finished();
-      OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {});
+      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
+      }
+    }
+  }
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testSO3.cpp

@@ -304,18 +304,77 @@ TEST(SO3, JacobianLogmap) {
 }
 
 //******************************************************************************
-TEST(SO3, ApplyDexp) {
+namespace test_cases {
-  Matrix aH1, aH2;
+std::vector<Vector3> small{{0, 0, 0},                                 //
-  for (bool nearZeroApprox : {true, false}) {
+                           {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),
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      for (const Vector3& v : test_cases::vs) {
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+        local.crossB(v, aH1);
-                        Vector3(0.4, 0.3, 0.2)}) {
+        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),
+    for (const Vector3& omega : test_cases::omegas(nearZero)) {
-                          Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
+      so3::DexpFunctor local(omega, nearZero);
-      so3::DexpFunctor local(omega, nearZeroApprox);
+      Matrix invJr = local.rightJacobianInverse();
-      Matrix invDexp = local.dexp().inverse();
+      for (const Vector3& v : test_cases::vs) {
-      for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
+        EXPECT(
-                        Vector3(0.4, 0.3, 0.2)}) {
+            assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
-        EXPECT(assert_equal(Vector3(invDexp * 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));
       }
     }
   }

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));

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>

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>

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>

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>

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>

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>

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>