Updates to work with latest GTSAM

.gitignore

@@ -84,3 +84,5 @@ build/
 .sass-cache/
 Gemfile.lock
 _site/
+
+output

CMakeLists.txt

@@ -6,8 +6,7 @@ set(CMAKE_CXX_STANDARD 17)
 add_compile_options(-Wall -Wpedantic -Wextra)
 
 # External package dependencies.
-find_package(GTSAM 4.2 REQUIRED)
-find_package(Eigen3 3.3 REQUIRED)
+find_package(GTSAM REQUIRED)  # Use Eigen bundled with GTSAM.
 find_package(dcsam REQUIRED)
 
 include_directories(include)

README.md

@@ -3,7 +3,11 @@
 This repository contains example code for the experiments provided in
 our paper "[Discrete-Continuous Smoothing and Mapping](https://arxiv.org/abs/2204.11936)."
 
-**NOTE: As of 1/30/2023 the latest version of DC-SAM (and dcsam-examples) 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-examples/releases/tag/pre-4.2) and the [corresponding pre-4.2 version of DC-SAM](https://github.com/MarineRoboticsGroup/dcsam/releases/tag/pre-4.2). This is the version of DC-SAM (and dcsam-examples) 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`**
+
+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-examples/releases/tag/pre-4.2) and the [corresponding pre-4.2 version of DC-SAM](https://github.com/MarineRoboticsGroup/dcsam/releases/tag/pre-4.2). This is the version of DC-SAM (and dcsam-examples) 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.
 
 The file `icp.cpp` implements the iterative closest point (ICP) method for point
 cloud registration.
@@ -21,11 +25,13 @@ To build the ICP and robust PGO examples:
 ```
 
 To run the ICP example:
+
 ```bash
 ~/build/examples $ ./icp (path to source cloud) (path to target cloud)
 ```
 
 To run the robust pose-graph optimization example:
+
 ```bash
 ~/build/examples $ ./robust_pgo_mc [.g2o file] [outlier rate (int >= 0; default 0)] [is3D (0/1; default 1)] [num trials (default 1)]
 ```

examples/icp.cpp

@@ -21,7 +21,6 @@ std::vector<gtsam::Point3> read_xyz_file(const std::string& path_to_file) {
   // Open file for reading.
   std::ifstream infile(path_to_file);
 
-  size_t i = 0;
   while (std::getline(infile, line)) {
     // Construct a stream from line.
     std::stringstream strstrm(line);

examples/robust_pgo_mc.cpp

@@ -8,6 +8,7 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/dataset.h>
+#include <filesystem>
 
 #include <chrono>
 #include <iostream>
@@ -29,6 +30,8 @@ void run_experiment3D(const gtsam::NonlinearFactorGraph& graph,
   /// 1. Build outlier graph.
   // Add prior on the pose having index (key) = 0
   gtsam::NonlinearFactorGraph graphWithOutliers = graph;
+  std::cout << "graph size: " << graph.size() << std::endl;
+  return;
 
   gtsam::Vector6 prior_sigmas;
   prior_sigmas << 1e-8, 1e-8, 1e-8, 1e-6, 1e-6, 1e-6;
@@ -66,8 +69,8 @@ void run_experiment3D(const gtsam::NonlinearFactorGraph& graph,
   // Add all good measurements.
   for (const auto& factor : graphWithOutliers) {
     // Ensure that we correctly retrieve a BetweenFactor
-    boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>> bwFactor =
-        boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
+    std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>> bwFactor =
+        std::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
     if (bwFactor && isLoopClosure(*bwFactor)) {
       // If the meaasurement is a loop closure, we add inlier and outlier
       // hypotheses.
@@ -135,8 +138,8 @@ void run_experiment3D(const gtsam::NonlinearFactorGraph& graph,
   // Add all outlier measurements.
   for (const auto& factor : outlierGraph) {
     // Ensure that we correctly retrieve a BetweenFactor
-    boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>> bwFactor =
-        boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
+    std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>> bwFactor =
+        std::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
     if (bwFactor && isLoopClosure(*bwFactor)) {
       // If the meaasurement is a loop closure, we add inlier and outlier
       // hypotheses.
@@ -356,11 +359,12 @@ void run_experiment3D(const gtsam::NonlinearFactorGraph& graph,
   std::cout << "Total time [GNC]: " << total_time_gnc << " seconds."
             << std::endl;
 
-  std::cout << "OUTLIER PCT" << std::to_string((int)(100 * outlier_pct))
+  std::cout << "OUTLIER PCT " << std::to_string((int)(100 * outlier_pct))
             << std::endl;
   std::string path_prefix = "../../output/robust_pgo_vanilla/" + dataset_name +
                             "/" + std::to_string((int)(100 * outlier_pct)) +
                             "/" + std::to_string(random_seed) + "/";
+  std::filesystem::create_directories(path_prefix);
 
   std::cout << "path_prefix: " << path_prefix << std::endl;
 
@@ -418,9 +422,6 @@ void run_experiment2D(const gtsam::NonlinearFactorGraph& graph,
   double outlier_prob = 0.5;
   std::vector<double> lc_probabilities{(1.0 - outlier_prob), outlier_prob};
 
-  // double outlier_prob = 0.5;
-  // std::vector<double> lc_probabilities{(1.0 - outlier_prob), outlier_prob};
-
   // We'll use this to save the inlier model so we can create the inlier
   // hypotheses.
   gtsam::SharedNoiseModel inlier_model;
@@ -438,8 +439,8 @@ void run_experiment2D(const gtsam::NonlinearFactorGraph& graph,
   // Add all good measurements.
   for (const auto& factor : graphWithOutliers) {
     // Ensure that we correctly retrieve a BetweenFactor
-    boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>> bwFactor =
-        boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose2>>(factor);
+    std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>> bwFactor =
+        std::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose2>>(factor);
     if (bwFactor && isLoopClosure(*bwFactor)) {
       // If the meaasurement is a loop closure, we add inlier and outlier
       // hypotheses.
@@ -508,8 +509,8 @@ void run_experiment2D(const gtsam::NonlinearFactorGraph& graph,
   // Add all outlier measurements.
   for (const auto& factor : outlierGraph) {
     // Ensure that we correctly retrieve a BetweenFactor
-    boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>> bwFactor =
-        boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose2>>(factor);
+    std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>> bwFactor =
+        std::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose2>>(factor);
     if (bwFactor && isLoopClosure(*bwFactor)) {
       // If the meaasurement is a loop closure, we add inlier and outlier
       // hypotheses.
@@ -546,6 +547,7 @@ void run_experiment2D(const gtsam::NonlinearFactorGraph& graph,
     }
   }
   std::cout << "HFG size: " << hfg.size() << std::endl;
+  std::cout << "Number of discrete factors:" << hfg.discreteGraph().size() << std::endl;
 
   // Currently DCSAM requires that we provide initial discrete values, but
   // they aren't actually used since we perform the discrete solve first, so
@@ -737,11 +739,12 @@ void run_experiment2D(const gtsam::NonlinearFactorGraph& graph,
   std::cout << "Total time [GNC]: " << total_time_gnc << " seconds."
             << std::endl;
 
-  std::cout << "OUTLIER PCT" << std::to_string((int)(100 * outlier_pct))
+  std::cout << "OUTLIER PCT " << std::to_string((int)(100 * outlier_pct))
             << std::endl;
   std::string path_prefix = "../../output/robust_pgo_vanilla/" + dataset_name +
                             "/" + std::to_string((int)(100 * outlier_pct)) +
                             "/" + std::to_string(random_seed) + "/";
+  std::filesystem::create_directories(path_prefix);
 
   std::cout << "path_prefix: " << path_prefix << std::endl;
 
@@ -811,7 +814,7 @@ int main(int argc, char** argv) {
   gtsam::NonlinearFactorGraph::shared_ptr graph;
   gtsam::Values::shared_ptr initial;
 
-  boost::tie(graph, initial) = gtsam::readG2o(path, is3D);
+  std::tie(graph, initial) = gtsam::readG2o(path, is3D);
 
   std::cout << "Loaded a graph of size: " << graph->size() << std::endl;
 

include/PosePointFactor.h

@@ -40,12 +40,12 @@ class PosePointFactor : public gtsam::NonlinearFactor {
 
   size_t dim() const override { return 3; };
 
-  boost::shared_ptr<gtsam::GaussianFactor> linearize(
+  std::shared_ptr<gtsam::GaussianFactor> linearize(
       const gtsam::Values &values) const override {
     gtsam::Pose3 T = values.at<gtsam::Pose3>(keys_[0]);
     Eigen::Matrix<double, 3, 6> H;
-    gtsam::Point3 transformed_p = T.transformFrom(p_, &H);
-    return boost::make_shared<gtsam::JacobianFactor>(keys_[0], H,
-                                                     -error_vector(values));
+    T.transformFrom(p_, &H);  // For obtaining the Jacobian
+    return std::make_shared<gtsam::JacobianFactor>(keys_[0], H,
+                                                   -error_vector(values));
   }
 };