Upgrade to work with latest GTSAM

CMakeLists.txt

@@ -3,26 +3,27 @@ project(dcsam)
 
 # Set some compilation options.
 set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 if (NOT CMAKE_BUILD_TYPE)
   # Options: Debug, Release, MinSizeRel, RelWithDebInfo
   message(STATUS "No build type selected, default to Release")
   set(CMAKE_BUILD_TYPE "Release")
 endif()
 
 # Add option to enable testing
-option(DCSAM_ENABLE_TESTS "Enable tests" OFF)
+option(DCSAM_ENABLE_TESTS "Enable tests" ON)
 
 # External package dependencies.
-find_package(GTSAM 4.2 REQUIRED)
-find_package(Eigen3 3.3 REQUIRED)
+find_package(GTSAM REQUIRED)
+# We use Eigen bundled with GTSAM, so we don't need to find it separately.
 
 # add_definitions(-march=native)
 # add_definitions(-std=c++1z)
 
 add_library(dcsam SHARED)
 target_sources(dcsam PRIVATE src/DCSAM.cpp src/HybridFactorGraph.cpp)
 target_include_directories(dcsam PUBLIC include)
-target_link_libraries(dcsam PUBLIC Eigen3::Eigen gtsam)
+target_link_libraries(dcsam PUBLIC gtsam)
 target_compile_options(dcsam PRIVATE -Wall -Wpedantic -Wextra)
 
 # Make library accessible to other cmake projects

README.md

@@ -5,11 +5,16 @@
 
 This library, built using GTSAM, provides factor type definitions and a new solver to perform approximate inference on discrete-continuous (hybrid) factor graph models typically encountered in robotics applications.
 
-**NOTE: As of 1/30/2023 the latest version of DC-SAM on `main` depends on GTSAM release 4.2a8.** If you are using GTSAM 4.1.1, check out our [pre-4.2 release tag](https://github.com/MarineRoboticsGroup/dcsam/releases/tag/pre-4.2). This is the version of DC-SAM you would have used if you cloned the repository prior to 1/30/2023. Many thanks to [Parker Lusk](https://github.com/plusk01) for bringing us into the future.
+**NOTE: As of 3/01/2026 the latest version of DC-SAM on `main` depends on GTSAM `develop`**
 
-### References
+Thanks to [Varun Agrawal](https://github.com/varunagrawal) for bringing DC-SAM up to date with GTSAM.
+
+If you are using GTSAM 4.1.1, check out our [pre-4.2 release tag](https://github.com/MarineRoboticsGroup/dcsam/releases/tag/pre-4.2). This is the version of DC-SAM you would have used if you cloned the repository prior to 1/30/2023. Many thanks to [Parker Lusk](https://github.com/plusk01) for bringing us into the future.
+
+## References
 
 A technical report describing this library and our solver can be found [here](https://arxiv.org/abs/2204.11936). If you found this code useful, please cite it as:
+
 ```bibtex
 @article{doherty2022discrete,
   author={Doherty, Kevin J. and Lu, Ziqi and Singh, Kurran and Leonard, John J.},
@@ -25,16 +30,17 @@ A technical report describing this library and our solver can be found [here](ht
 
 ## Prerequisites
 
-- [GTSAM](https://github.com/borglab/gtsam) @ `4.2a8`
+- [GTSAM](https://github.com/borglab/gtsam) @ `develop`
 
 To retrieve the appropriate version of GTSAM:
+
 ```sh
 ~ $ git clone https://github.com/borglab/gtsam
 ~ $ cd gtsam
 ~/gtsam $ git checkout 4.2a8
 ```
-Follow instructions in the GTSAM repository to build and install with your desired configuration.
 
+Follow instructions in the GTSAM repository to build and install with your desired configuration.
 
 ### Optional
 
@@ -56,6 +62,7 @@ To build using `cmake`:
 ### Run tests
 
 To run unit tests, first build with testing enabled:
+
 ```bash
 ~/dcsam $ mkdir build
 ~/dcsam $ cd build
@@ -76,13 +83,17 @@ For example usage, check out [the DC-SAM examples repo](https://github.com/Marin
 ## Developing
 
 We're using [pre-commit](https://pre-commit.com/) for automatic linting. To install `pre-commit` run:
-```
+
+```sh
 pip3 install pre-commit
 ```
+
 You can verify your installation went through by running `pre-commit --version` and you should see something like `pre-commit 2.7.1`.
 
 To get started using `pre-commit` with this codebase, from the project repo run:
-```
+
+```sh
 pre-commit install
 ```
+
 Now, each time you `git add` new files and try to `git commit` your code will automatically be run through a variety of linters. You won't be able to commit anything until the linters are happy with your code.

include/dcsam/DCContinuousFactor.h

@@ -40,15 +40,15 @@ namespace dcsam {
 class DCContinuousFactor : public gtsam::NonlinearFactor {
  private:
   gtsam::DiscreteKeys discreteKeys_;
-  boost::shared_ptr<DCFactor> dcfactor_;
+  std::shared_ptr<DCFactor> dcfactor_;
   DiscreteValues discreteVals_;
 
  public:
   using Base = gtsam::NonlinearFactor;
 
   DCContinuousFactor() = default;
 
-  explicit DCContinuousFactor(boost::shared_ptr<DCFactor> dcfactor)
+  explicit DCContinuousFactor(std::shared_ptr<DCFactor> dcfactor)
       : discreteKeys_(dcfactor->discreteKeys()), dcfactor_(dcfactor) {
     keys_ = dcfactor->keys();
   }
@@ -58,7 +58,7 @@ class DCContinuousFactor : public gtsam::NonlinearFactor {
     return dcfactor_->error(continuousVals, discreteVals_);
   }
 
-  boost::shared_ptr<gtsam::GaussianFactor> linearize(
+  std::shared_ptr<gtsam::GaussianFactor> linearize(
       const gtsam::Values& continuousVals) const override {
     assert(allInitialized());
     return dcfactor_->linearize(continuousVals, discreteVals_);

include/dcsam/DCDiscreteFactor.h

@@ -42,7 +42,7 @@ class DCDiscreteFactor : public gtsam::DiscreteFactor {
  private:
   gtsam::DiscreteKeys discreteKeys_;
   gtsam::KeyVector continuousKeys_;
-  boost::shared_ptr<DCFactor> dcfactor_;
+  std::shared_ptr<DCFactor> dcfactor_;
   gtsam::Values continuousVals_;
   DiscreteValues discreteVals_;
 
@@ -52,7 +52,7 @@ class DCDiscreteFactor : public gtsam::DiscreteFactor {
   DCDiscreteFactor() = default;
 
   DCDiscreteFactor(const gtsam::DiscreteKeys& discreteKeys,
-                   boost::shared_ptr<DCFactor> dcfactor)
+                   std::shared_ptr<DCFactor> dcfactor)
       : discreteKeys_(discreteKeys),
         continuousKeys_(dcfactor->keys()),
         dcfactor_(dcfactor) {
@@ -61,7 +61,7 @@ class DCDiscreteFactor : public gtsam::DiscreteFactor {
     for (const gtsam::DiscreteKey& k : discreteKeys_) keys_.push_back(k.first);
   }
 
-  explicit DCDiscreteFactor(boost::shared_ptr<DCFactor> dcfactor)
+  explicit DCDiscreteFactor(std::shared_ptr<DCFactor> dcfactor)
       : discreteKeys_(dcfactor->discreteKeys()),
         continuousKeys_(dcfactor->keys()),
         dcfactor_(dcfactor) {
@@ -82,7 +82,7 @@ class DCDiscreteFactor : public gtsam::DiscreteFactor {
 
   virtual ~DCDiscreteFactor() = default;
 
-  bool equals(const DiscreteFactor& other, double tol = 1e-9) const override {
+  bool equals(const DiscreteFactor& other, [[maybe_unused]] double tol = 1e-9) const override {
     if (!dynamic_cast<const DCDiscreteFactor*>(&other)) return false;
     const DCDiscreteFactor& f(static_cast<const DCDiscreteFactor&>(other));
     return (dcfactor_->equals(*f.dcfactor_) &&
@@ -102,11 +102,79 @@ class DCDiscreteFactor : public gtsam::DiscreteFactor {
     return dcfactor_->conditionalTimes(f, continuousVals_, discreteVals_);
   }
 
-  double operator()(const DiscreteValues& values) const override {
+  virtual DiscreteFactor::shared_ptr operator*(double s) const override {
+    return toDecisionTreeFactor() * s;
+  }
+
+  double operator()(const DiscreteValues& values) const {
     assert(allInitialized());
     return exp(-dcfactor_->error(continuousVals_, values));
   }
 
+  void print(const std::string& s = "DCDiscreteFactor:\n",
+             const gtsam::KeyFormatter& formatter =
+                 gtsam::DefaultKeyFormatter) const override {
+    toDecisionTreeFactor().print(s, formatter);
+    continuousVals_.print("Continuous values: ", formatter);
+    discreteVals_.print("Discrete values: ", formatter);
+  }
+
+  double evaluate(const gtsam::Assignment<gtsam::Key>& values) const override {
+    return this->operator()(DiscreteValues(values));
+  }
+
+  virtual DiscreteFactor::shared_ptr multiply(
+      const DiscreteFactor::shared_ptr& df) const override {
+    return toDecisionTreeFactor().multiply(df);
+  }
+
+  /// divide by DiscreteFactor::shared_ptr f (safely)
+  virtual DiscreteFactor::shared_ptr operator/(
+      const DiscreteFactor::shared_ptr& df) const override {
+    throw toDecisionTreeFactor() / df;
+  }
+
+  /// Create new factor by summing all values with the same separator values
+  virtual DiscreteFactor::shared_ptr sum(size_t nrFrontals) const override {
+    return toDecisionTreeFactor().sum(nrFrontals);
+  }
+
+  /// Create new factor by summing all values with the same separator values
+  virtual DiscreteFactor::shared_ptr sum(
+      const gtsam::Ordering& keys) const override {
+    return toDecisionTreeFactor().sum(keys);
+  }
+
+  /// Find the maximum value in the factor.
+  virtual double max() const override {
+    return toDecisionTreeFactor().max();
+  }
+
+  /// Create new factor by maximizing over all values with the same separator.
+  virtual DiscreteFactor::shared_ptr max(size_t nrFrontals) const override {
+    return toDecisionTreeFactor().max(nrFrontals);
+  }
+
+  /// Create new factor by maximizing over all values with the same separator.
+  virtual DiscreteFactor::shared_ptr max(
+      const gtsam::Ordering& keys) const override {
+    return toDecisionTreeFactor().max(keys);
+  }
+
+  /**
+   * Get the number of non-zero values contained in this factor.
+   * It could be much smaller than `prod_{key}(cardinality(key))`.
+   */
+  virtual uint64_t nrValues() const override {
+    return toDecisionTreeFactor().nrValues();
+  }
+
+  /// Restrict the factor to the given assignment.
+  virtual DiscreteFactor::shared_ptr restrict(
+      const DiscreteValues& assignment) const override {
+    return toDecisionTreeFactor().restrict(assignment);
+  }
+
   void updateContinuous(const gtsam::Values& continuousVals) {
     for (const gtsam::Key& k : continuousKeys_) {
       // If key `k` is not set continuousVals, skip it.

include/dcsam/DCEMFactor.h

@@ -150,10 +150,10 @@ class DCEMFactor : public DCFactor {
   /*
    * Jacobian magic
    */
-  boost::shared_ptr<gtsam::GaussianFactor> linearize(
+  std::shared_ptr<gtsam::GaussianFactor> linearize(
       const gtsam::Values& continuousVals,
       const DiscreteValues& discreteVals) const override {
-    std::vector<boost::shared_ptr<gtsam::GaussianFactor>> gfs;
+    std::vector<std::shared_ptr<gtsam::GaussianFactor>> gfs;
 
     // Start by computing all errors, so we can get the component weights.
     std::vector<double> errors =
@@ -169,7 +169,7 @@ class DCEMFactor : public DCFactor {
     for (size_t i = 0; i < factors_.size(); i++) {
       // std::cout << "i = " << i << std::endl;
       // First get the GaussianFactor obtained by linearizing `factors_[i]`
-      boost::shared_ptr<gtsam::GaussianFactor> gf =
+      std::shared_ptr<gtsam::GaussianFactor> gf =
           factors_[i].linearize(continuousVals, discreteVals);
 
       gtsam::JacobianFactor jf_component(*gf);
@@ -197,7 +197,7 @@ class DCEMFactor : public DCFactor {
 
     // Stack Jacobians to build combined factor.
 
-    return boost::make_shared<gtsam::JacobianFactor>(gfg);
+    return std::make_shared<gtsam::JacobianFactor>(gfg);
   }
 
   gtsam::DecisionTreeFactor toDecisionTreeFactor(

include/dcsam/DCFactor.h

@@ -86,6 +86,8 @@ class DCFactor : public gtsam::Factor {
       const gtsam::Values& continuousVals,
       const gtsam::DiscreteFactor::Values& discreteVals) const = 0;
 
+  using Base::error;  // Removes warning about hiding the base class error function.
+
   /**
    * Linearize the error function with respect to the continuous
    * variables (given in `keys_`) at the point specified by `continuousVals`.
@@ -99,7 +101,7 @@ class DCFactor : public gtsam::Factor {
    * @param discreteVals - Likewise, assignment to the discrete variables in
    * `discreteKeys__`.
    */
-  virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
+  virtual std::shared_ptr<gtsam::GaussianFactor> linearize(
       const gtsam::Values& continuousVals,
       const DiscreteValues& discreteVals) const = 0;
 
@@ -155,7 +157,7 @@ class DCFactor : public gtsam::Factor {
    */
   virtual gtsam::DecisionTreeFactor toDecisionTreeFactor(
       const gtsam::Values& continuousVals,
-      const DiscreteValues& discreteVals) const {
+      [[maybe_unused]] const DiscreteValues& discreteVals) const {
     gtsam::DecisionTreeFactor converted;
     for (const gtsam::DiscreteKey& dkey : discreteKeys_) {
       std::vector<double> probs = evalProbs(dkey, continuousVals);
@@ -175,7 +177,7 @@ class DCFactor : public gtsam::Factor {
    * TODO(Kurran) is this the cleanest way to do this? Seems necessary for the
    * DCMaxMixtureFactor implementations etc...
    */
-  virtual double logNormalizingConstant(const gtsam::Values& values) const {
+  virtual double logNormalizingConstant([[maybe_unused]] const gtsam::Values& values) const {
     throw std::logic_error(
         "Normalizing constant not implemented."
         "One or more of the factors in use requires access to the normalization"
@@ -196,22 +198,22 @@ class DCFactor : public gtsam::Factor {
     gtsam::Matrix infoMat;
 
     // NOTE: This is sloppy, is there a cleaner way?
-    boost::shared_ptr<NonlinearFactorType> fPtr =
-        boost::make_shared<NonlinearFactorType>(factor);
-    boost::shared_ptr<NonlinearFactorType> factorPtr(fPtr);
+    std::shared_ptr<NonlinearFactorType> fPtr =
+        std::make_shared<NonlinearFactorType>(factor);
+    std::shared_ptr<NonlinearFactorType> factorPtr(fPtr);
 
     // If this is a NoiseModelFactor, we'll use its noiseModel to
     // otherwise noiseModelFactor will be nullptr
-    boost::shared_ptr<gtsam::NoiseModelFactor> noiseModelFactor =
-        boost::dynamic_pointer_cast<gtsam::NoiseModelFactor>(factorPtr);
+    std::shared_ptr<gtsam::NoiseModelFactor> noiseModelFactor =
+        std::dynamic_pointer_cast<gtsam::NoiseModelFactor>(factorPtr);
     if (noiseModelFactor) {
       // If dynamic cast to NoiseModelFactor succeeded, see if the noise model
       // is Gaussian
       gtsam::noiseModel::Base::shared_ptr noiseModel =
           noiseModelFactor->noiseModel();
 
-      boost::shared_ptr<gtsam::noiseModel::Gaussian> gaussianNoiseModel =
-          boost::dynamic_pointer_cast<gtsam::noiseModel::Gaussian>(noiseModel);
+      std::shared_ptr<gtsam::noiseModel::Gaussian> gaussianNoiseModel =
+          std::dynamic_pointer_cast<gtsam::noiseModel::Gaussian>(noiseModel);
       if (gaussianNoiseModel) {
         // If the noise model is Gaussian, retrieve the information matrix
         infoMat = gaussianNoiseModel->information();
@@ -220,7 +222,7 @@ class DCFactor : public gtsam::Factor {
         // something with a normalized noise model
         // TODO(kevin): does this make sense to do? I think maybe not in
         // general? Should we just yell at the user?
-        boost::shared_ptr<gtsam::GaussianFactor> gaussianFactor =
+        std::shared_ptr<gtsam::GaussianFactor> gaussianFactor =
             factor.linearize(values);
         infoMat = gaussianFactor->information();
       }

include/dcsam/DCMaxMixtureFactor.h

@@ -120,7 +120,7 @@ class DCMaxMixtureFactor : public DCFactor {
     return ((log_weights_ == f.log_weights_) && (normalized_ == f.normalized_));
   }
 
-  boost::shared_ptr<gtsam::GaussianFactor> linearize(
+  std::shared_ptr<gtsam::GaussianFactor> linearize(
       const gtsam::Values& continuousVals,
       const DiscreteValues& discreteVals) const override {
     size_t min_error_idx = getActiveFactorIdx(continuousVals, discreteVals);

include/dcsam/DCMixtureFactor.h

@@ -102,7 +102,7 @@ class DCMixtureFactor : public DCFactor {
             (normalized_ == f.normalized_));
   }
 
-  boost::shared_ptr<gtsam::GaussianFactor> linearize(
+  std::shared_ptr<gtsam::GaussianFactor> linearize(
       const gtsam::Values& continuousVals,
       const DiscreteValues& discreteVals) const override {
     // Retrieve the assignment to our discrete key.
@@ -112,6 +112,12 @@ class DCMixtureFactor : public DCFactor {
     // error.
     return factors_[assignment].linearize(continuousVals);
   }
+
+  void print(const std::string& s = "DCMixtureFactor:\n",
+             const gtsam::KeyFormatter& formatter =
+                 gtsam::DefaultKeyFormatter) const override {
+    Base::print(s, formatter);
+  }
 };
 
 }  // namespace dcsam

include/dcsam/DiscreteMarginalsOrdered.h

@@ -54,7 +54,9 @@ class DiscreteMarginalsOrdered : public gtsam::DiscreteMarginals {
     }
 
     // sum out frontals to get the factor on the separator.
-    gtsam::DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
+    gtsam::DecisionTreeFactor::shared_ptr sum =
+        std::dynamic_pointer_cast<gtsam::DecisionTreeFactor>(
+            product.sum(frontalKeys));
 
     // NOTE: Sum keys seems to be empty often - is this normal?
     // Ordering keys for the conditional so that frontalKeys is in front.