Merge pull request #2010 from borglab/city10000

Updates to City10000 Example

examples/City10000.h

@@ -0,0 +1,110 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   City10000.h
+ * @brief  Class for City10000 dataset
+ * @author Varun Agrawal
+ * @date   February 3, 2025
+ */
+
+#include <gtsam/geometry/Pose2.h>
+
+#include <fstream>
+
+using namespace gtsam;
+
+using symbol_shorthand::X;
+
+auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10);
+const double kOpenLoopConstant = kOpenLoopModel->negLogConstant();
+
+auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
+    (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
+
+auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas(
+    (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
+const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant();
+
+class City10000Dataset {
+  std::ifstream in_;
+
+  /// Read a `line` from the dataset, separated by the `delimiter`.
+  std::vector<std::string> readLine(const std::string& line,
+                                    const std::string& delimiter = " ") const {
+    std::vector<std::string> parts;
+    auto start = 0U;
+    auto end = line.find(delimiter);
+    while (end != std::string::npos) {
+      parts.push_back(line.substr(start, end - start));
+      start = end + delimiter.length();
+      end = line.find(delimiter, start);
+    }
+    return parts;
+  }
+
+ public:
+  City10000Dataset(const std::string& filename) : in_(filename) {
+    if (!in_.is_open()) {
+      std::cerr << "Failed to open file: " << filename << std::endl;
+    }
+  }
+
+  /// Parse line from file
+  std::pair<std::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
+      const std::string& line) const {
+    std::vector<std::string> parts = readLine(line);
+
+    size_t keyS = stoi(parts[1]);
+    size_t keyT = stoi(parts[3]);
+
+    int numMeasurements = stoi(parts[5]);
+    std::vector<Pose2> poseArray(numMeasurements);
+    for (int i = 0; i < numMeasurements; ++i) {
+      double x = stod(parts[6 + 3 * i]);
+      double y = stod(parts[7 + 3 * i]);
+      double rad = stod(parts[8 + 3 * i]);
+      poseArray[i] = Pose2(x, y, rad);
+    }
+    return {poseArray, {keyS, keyT}};
+  }
+
+  /// Read and parse the next line.
+  bool next(std::vector<Pose2>* poseArray, std::pair<size_t, size_t>* keys) {
+    std::string line;
+    if (getline(in_, line)) {
+      std::tie(*poseArray, *keys) = parseLine(line);
+      return true;
+    } else
+      return false;
+  }
+};
+
+/**
+ * @brief Write the result of optimization to file.
+ *
+ * @param result The Values object with the final result.
+ * @param num_poses The number of poses to write to the file.
+ * @param filename The file name to save the result to.
+ */
+void writeResult(const Values& result, size_t numPoses,
+                 const std::string& filename = "Hybrid_city10000.txt") {
+  std::ofstream outfile;
+  outfile.open(filename);
+
+  for (size_t i = 0; i < numPoses; ++i) {
+    Pose2 outPose = result.at<Pose2>(X(i));
+    outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta()
+            << std::endl;
+  }
+  outfile.close();
+  std::cout << "Output written to " << filename << std::endl;
+}

examples/Hybrid_City10000.cpp

@@ -29,36 +29,28 @@
 #include <gtsam/slam/dataset.h>
 #include <time.h>
 
-#include <boost/algorithm/string/classification.hpp>
-#include <boost/algorithm/string/split.hpp>
 #include <cstdlib>
 #include <fstream>
 #include <iostream>
 #include <string>
 #include <vector>
 
+#include "City10000.h"
+
 using namespace gtsam;
-using namespace boost::algorithm;
 
 using symbol_shorthand::L;
 using symbol_shorthand::M;
 using symbol_shorthand::X;
 
-auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10);
-const double kOpenLoopConstant = kOpenLoopModel->negLogConstant();
-
-auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
-    (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
-
-auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas(
-    (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
-const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant();
-
 // Experiment Class
 class Experiment {
+  /// The City10000 dataset
+  City10000Dataset dataset_;
+
  public:
   // Parameters with default values
-  size_t maxLoopCount = 3000;
+  size_t maxLoopCount = 8000;
 
   // 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5: 39s} No DT optimizations
   // 3000: {1: 65s, 2: 20s, 3: 16s, 4: 21s, 5: 28s} With DT optimizations
@@ -73,32 +65,10 @@ class Experiment {
   double marginalThreshold = 0.9999;
 
  private:
-  std::string filename_;
   HybridSmoother smoother_;
   HybridNonlinearFactorGraph newFactors_, allFactors_;
   Values initial_;
 
-  /**
-   * @brief Write the result of optimization to file.
-   *
-   * @param result The Values object with the final result.
-   * @param num_poses The number of poses to write to the file.
-   * @param filename The file name to save the result to.
-   */
-  void writeResult(const Values& result, size_t numPoses,
-                   const std::string& filename = "Hybrid_city10000.txt") const {
-    std::ofstream outfile;
-    outfile.open(filename);
-
-    for (size_t i = 0; i < numPoses; ++i) {
-      Pose2 outPose = result.at<Pose2>(X(i));
-      outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta()
-              << std::endl;
-    }
-    outfile.close();
-    std::cout << "Output written to " << filename << std::endl;
-  }
-
   /**
    * @brief Create a hybrid loop closure factor where
    * 0 - loose noise model and 1 - loop noise model.
@@ -135,7 +105,7 @@ class Experiment {
   }
 
   /// @brief Perform smoother update and optimize the graph.
-  auto smootherUpdate(size_t maxNrHypotheses) {
+  clock_t smootherUpdate(size_t maxNrHypotheses) {
     std::cout << "Smoother update: " << newFactors_.size() << std::endl;
     gttic_(SmootherUpdate);
     clock_t beforeUpdate = clock();
@@ -148,7 +118,7 @@ class Experiment {
   }
 
   /// @brief Re-linearize, solve ALL, and re-initialize smoother.
-  auto reInitialize() {
+  clock_t reInitialize() {
     std::cout << "================= Re-Initialize: " << allFactors_.size()
               << std::endl;
     clock_t beforeUpdate = clock();
@@ -164,40 +134,13 @@ class Experiment {
     return afterUpdate - beforeUpdate;
   }
 
-  // Parse line from file
-  std::pair<std::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
-      const std::string& line) const {
-    std::vector<std::string> parts;
-    split(parts, line, is_any_of(" "));
-
-    size_t keyS = stoi(parts[1]);
-    size_t keyT = stoi(parts[3]);
-
-    int numMeasurements = stoi(parts[5]);
-    std::vector<Pose2> poseArray(numMeasurements);
-    for (int i = 0; i < numMeasurements; ++i) {
-      double x = stod(parts[6 + 3 * i]);
-      double y = stod(parts[7 + 3 * i]);
-      double rad = stod(parts[8 + 3 * i]);
-      poseArray[i] = Pose2(x, y, rad);
-    }
-    return {poseArray, {keyS, keyT}};
-  }
-
  public:
   /// Construct with filename of experiment to run
   explicit Experiment(const std::string& filename)
-      : filename_(filename), smoother_(marginalThreshold) {}
+      : dataset_(filename), smoother_(marginalThreshold) {}
 
   /// @brief Run the main experiment with a given maxLoopCount.
   void run() {
-    // Prepare reading
-    std::ifstream in(filename_);
-    if (!in.is_open()) {
-      std::cerr << "Failed to open file: " << filename_ << std::endl;
-      return;
-    }
-
     // Initialize local variables
     size_t discreteCount = 0, index = 0, loopCount = 0, updateCount = 0;
 
@@ -221,9 +164,11 @@ class Experiment {
     Values result;
     size_t keyS = 0, keyT = 0;
     clock_t startTime = clock();
-    std::string line;
+
-    while (getline(in, line) && index < maxLoopCount) {
+    std::vector<Pose2> poseArray;
-      auto [poseArray, keys] = parseLine(line);
+    std::pair<size_t, size_t> keys;
+
+    while (dataset_.next(&poseArray, &keys) && index < maxLoopCount) {
       keyS = keys.first;
       keyT = keys.second;
       size_t numMeasurements = poseArray.size();
@@ -312,13 +257,6 @@ class Experiment {
     // Write results to file
     writeResult(result, keyT + 1, "Hybrid_City10000.txt");
 
-    // TODO Write to file
-    //  for (size_t i = 0; i < smoother_update_times.size(); i++) {
-    //    auto p = smoother_update_times.at(i);
-    //    std::cout << p.first << ", " << p.second / CLOCKS_PER_SEC <<
-    //    std::endl;
-    //  }
-
     // Write timing info to file
     std::ofstream outfileTime;
     std::string timeFileName = "Hybrid_City10000_time.txt";

examples/ISAM2_City10000.cpp

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   Hybrid_City10000.cpp
+ * @file   ISAM2_City10000.cpp
- * @brief  Example of using hybrid estimation
+ * @brief  Example of using ISAM2 estimation
  *         with multiple odometry measurements.
  * @author Varun Agrawal
  * @date   January 22, 2025
@@ -20,6 +20,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/ISAM2Params.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/BetweenFactor.h>
@@ -32,178 +33,189 @@
 #include <string>
 #include <vector>
 
-using namespace std;
+#include "City10000.h"
+
 using namespace gtsam;
-using namespace boost::algorithm;
 
 using symbol_shorthand::X;
 
-// Testing params
+// Experiment Class
-const size_t max_loop_count = 2000;  // 200 //2000 //8000
+class Experiment {
+  /// The City10000 dataset
+  City10000Dataset dataset_;
 
-const bool is_with_ambiguity = false;  // run original iSAM2 without ambiguities
+ public:
-// const bool is_with_ambiguity = true;  // run original iSAM2 with ambiguities
+  // Parameters with default values
+  size_t maxLoopCount = 2000;  // 200 //2000 //8000
 
-noiseModel::Diagonal::shared_ptr prior_noise_model =
+  // false: run original iSAM2 without ambiguities
-    noiseModel::Diagonal::Sigmas(
+  // true: run original iSAM2 with ambiguities
-        (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
+  bool isWithAmbiguity;
 
-noiseModel::Diagonal::shared_ptr pose_noise_model =
+ private:
-    noiseModel::Diagonal::Sigmas(
+  ISAM2 isam2_;
-        (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
+  NonlinearFactorGraph graph_;
-
+  Values initial_;
-/**
+  Values results;
- * @brief Write the results of optimization to filename.
- *
- * @param results The Values object with the final results.
- * @param num_poses The number of poses to write to the file.
- * @param filename The file name to save the results to.
- */
-void write_results(const Values& results, size_t num_poses,
-                   const std::string& filename = "ISAM2_city10000.txt") {
-  ofstream outfile;
-  outfile.open(filename);
-
-  for (size_t i = 0; i < num_poses; ++i) {
-    Pose2 out_pose = results.at<Pose2>(X(i));
-
-    outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta()
-            << std::endl;
-  }
-  outfile.close();
-  std::cout << "output written to " << filename << std::endl;
-}
-
-/* ************************************************************************* */
-int main(int argc, char* argv[]) {
-  ifstream in(findExampleDataFile("T1_city10000_04.txt"));
-  // ifstream in("../data/mh_T1_city10000_04.txt"); //Type #1 only
-  // ifstream in("../data/mh_T3b_city10000_10.txt"); //Type #3 only
-  // ifstream in("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3
-
-  // ifstream in("../data/mh_All_city10000_groundtruth.txt");
-
-  size_t pose_count = 0;
-  size_t index = 0;
-
-  std::list<double> time_list;
 
+ public:
+  /// Construct with filename of experiment to run
+  explicit Experiment(const std::string& filename, bool isWithAmbiguity = false)
+      : dataset_(filename), isWithAmbiguity(isWithAmbiguity) {
     ISAM2Params parameters;
     parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
     parameters.relinearizeThreshold = 0.01;
     parameters.relinearizeSkip = 1;
-
+    isam2_ = ISAM2(parameters);
-  ISAM2* isam2 = new ISAM2(parameters);
-
-  NonlinearFactorGraph* graph = new NonlinearFactorGraph();
-
-  Values init_values;
-  Values results;
-
-  double x = 0.0;
-  double y = 0.0;
-  double rad = 0.0;
-
-  Pose2 prior_pose(x, y, rad);
-
-  init_values.insert(X(0), prior_pose);
-  pose_count++;
-
-  graph->addPrior<Pose2>(X(0), prior_pose, prior_noise_model);
-
-  isam2->update(*graph, init_values);
-  graph->resize(0);
-  init_values.clear();
-  results = isam2->calculateBestEstimate();
-
-  //*
-  size_t key_s = 0;
-  size_t key_t = 0;
-
-  clock_t start_time = clock();
-  string str;
-  while (getline(in, str) && index < max_loop_count) {
-    // cout << str << endl;
-    vector<string> parts;
-    split(parts, str, is_any_of(" "));
-
-    key_s = stoi(parts[1]);
-    key_t = stoi(parts[3]);
-
-    int num_measurements = stoi(parts[5]);
-    vector<Pose2> pose_array(num_measurements);
-    for (int i = 0; i < num_measurements; ++i) {
-      x = stod(parts[6 + 3 * i]);
-      y = stod(parts[7 + 3 * i]);
-      rad = stod(parts[8 + 3 * i]);
-      pose_array[i] = Pose2(x, y, rad);
   }
 
-    Pose2 odom_pose;
+  /// @brief Run the main experiment with a given maxLoopCount.
-    if (is_with_ambiguity) {
+  void run() {
+    // Initialize local variables
+    size_t index = 0;
+
+    std::list<double> timeList;
+
+    // Set up initial prior
+    Pose2 priorPose(0, 0, 0);
+    initial_.insert(X(0), priorPose);
+    graph_.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
+
+    // Initial update
+    isam2_.update(graph_, initial_);
+    graph_.resize(0);
+    initial_.clear();
+    results = isam2_.calculateBestEstimate();
+
+    // Start main loop
+    size_t keyS = 0;
+    size_t keyT = 0;
+    clock_t startTime = clock();
+
+    std::vector<Pose2> poseArray;
+    std::pair<size_t, size_t> keys;
+
+    while (dataset_.next(&poseArray, &keys) && index < maxLoopCount) {
+      keyS = keys.first;
+      keyT = keys.second;
+      size_t numMeasurements = poseArray.size();
+
+      Pose2 odomPose;
+      if (isWithAmbiguity) {
         // Get wrong intentionally
-      int id = index % num_measurements;
+        int id = index % numMeasurements;
-      odom_pose = Pose2(pose_array[id]);
+        odomPose = Pose2(poseArray[id]);
       } else {
-      odom_pose = pose_array[0];
+        odomPose = poseArray[0];
       }
 
-    if (key_s == key_t - 1) {  // new X(key)
+      if (keyS == keyT - 1) {  // new X(key)
-      init_values.insert(X(key_t), results.at<Pose2>(X(key_s)) * odom_pose);
+        initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose);
-      pose_count++;
+        graph_.add(
+            BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel));
+
       } else {  // loop
+        int id = index % numMeasurements;
+        if (isWithAmbiguity && id % 2 == 0) {
+          graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
+                                          kPoseNoiseModel));
+        } else {
+          graph_.add(BetweenFactor<Pose2>(
+              X(keyS), X(keyT), odomPose,
+              noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
+        }
         index++;
       }
-    graph->add(
-        BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, pose_noise_model));
 
-    isam2->update(*graph, init_values);
+      isam2_.update(graph_, initial_);
-    graph->resize(0);
+      graph_.resize(0);
-    init_values.clear();
+      initial_.clear();
-    results = isam2->calculateBestEstimate();
+      results = isam2_.calculateBestEstimate();
 
       // Print loop index and time taken in processor clock ticks
-    if (index % 50 == 0 && key_s != key_t - 1) {
+      if (index % 50 == 0 && keyS != keyT - 1) {
         std::cout << "index: " << index << std::endl;
-      std::cout << "acc_time:  " << time_list.back() / CLOCKS_PER_SEC
+        std::cout << "accTime:  " << timeList.back() / CLOCKS_PER_SEC
                   << std::endl;
       }
 
-    if (key_s == key_t - 1) {
+      if (keyS == keyT - 1) {
-      clock_t cur_time = clock();
+        clock_t curTime = clock();
-      time_list.push_back(cur_time - start_time);
+        timeList.push_back(curTime - startTime);
       }
 
-    if (time_list.size() % 100 == 0 && (key_s == key_t - 1)) {
+      if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) {
-      string step_file_idx = std::to_string(100000 + time_list.size());
+        std::string stepFileIdx = std::to_string(100000 + timeList.size());
 
-      ofstream step_outfile;
+        std::ofstream stepOutfile;
-      string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx;
+        std::string stepFileName = "step_files/ISAM2_City10000_S" + stepFileIdx;
-      step_outfile.open(step_file_name + ".txt");
+        stepOutfile.open(stepFileName + ".txt");
-      for (size_t i = 0; i < (key_t + 1); ++i) {
+        for (size_t i = 0; i < (keyT + 1); ++i) {
-        Pose2 out_pose = results.at<Pose2>(X(i));
+          Pose2 outPose = results.at<Pose2>(X(i));
-        step_outfile << out_pose.x() << " " << out_pose.y() << " "
+          stepOutfile << outPose.x() << " " << outPose.y() << " "
-                     << out_pose.theta() << endl;
+                      << outPose.theta() << std::endl;
         }
-      step_outfile.close();
+        stepOutfile.close();
       }
     }
 
-  clock_t end_time = clock();
+    clock_t endTime = clock();
-  clock_t total_time = end_time - start_time;
+    clock_t totalTime = endTime - startTime;
-  cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl;
+    std::cout << "totalTime: " << totalTime / CLOCKS_PER_SEC << std::endl;
 
     /// Write results to file
-  write_results(results, (key_t + 1));
+    writeResult(results, (keyT + 1), "ISAM2_City10000.txt");
 
-  ofstream outfile_time;
+    std::ofstream outfileTime;
-  std::string time_file_name = "ISAM2_city10000_time.txt";
+    std::string timeFileName = "ISAM2_City10000_time.txt";
-  outfile_time.open(time_file_name);
+    outfileTime.open(timeFileName);
-  for (auto acc_time : time_list) {
+    for (auto accTime : timeList) {
-    outfile_time << acc_time << std::endl;
+      outfileTime << accTime << std::endl;
     }
-  outfile_time.close();
+    outfileTime.close();
-  cout << "output " << time_file_name << " file." << endl;
+    std::cout << "Written cumulative time to: " << timeFileName << " file."
+              << std::endl;
+  }
+};
+
+/* ************************************************************************* */
+// Function to parse command-line arguments
+void parseArguments(int argc, char* argv[], size_t& maxLoopCount,
+                    bool& isWithAmbiguity) {
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg == "--max-loop-count" && i + 1 < argc) {
+      maxLoopCount = std::stoul(argv[++i]);
+    } else if (arg == "--is-with-ambiguity" && i + 1 < argc) {
+      isWithAmbiguity = bool(std::stoul(argv[++i]));
+    } else if (arg == "--help") {
+      std::cout << "Usage: " << argv[0] << " [options]\n"
+                << "Options:\n"
+                << "  --max-loop-count <value>       Set the maximum loop "
+                   "count (default: 2000)\n"
+                << "  --is-with-ambiguity <value=0/1>     Set whether to use "
+                   "ambiguous measurements "
+                   "(default: false)\n"
+                << "  --help                         Show this help message\n";
+      std::exit(0);
+    }
+  }
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  Experiment experiment(findExampleDataFile("T1_City10000_04.txt"));
+  // Experiment experiment("../data/mh_T1_City10000_04.txt"); //Type #1 only
+  // Experiment experiment("../data/mh_T3b_City10000_10.txt"); //Type #3 only
+  // Experiment experiment("../data/mh_T1_T3_City10000_04.txt"); //Type #1 +
+  // Type #3
+
+  // Parse command-line arguments
+  parseArguments(argc, argv, experiment.maxLoopCount,
+                 experiment.isWithAmbiguity);
+
+  // Run the experiment
+  experiment.run();
 
   return 0;
 }

examples/plot_city10000.m

@@ -2,12 +2,13 @@ clear;
 
 gt = dlmread('Data/ISAM2_GT_city10000.txt');
 
-eh_poses = dlmread('../build/examples/ISAM2_city10000.txt');
+% Generate by running `make ISAM2_City10000.run`
+eh_poses = dlmread('../build/examples/ISAM2_City10000.txt');
 
-h_poses = dlmread('../build/examples/HybridISAM_city10000.txt');
+% Generate by running `make Hybrid_City10000.run`
+h_poses = dlmread('../build/examples/Hybrid_City10000.txt');
 
 % Plot the same number of GT poses as estimated ones
-% gt = gt(1:size(eh_poses, 1), :);
 gt = gt(1:size(h_poses, 1), :);
 eh_poses = eh_poses(1:size(h_poses, 1), :);
 
@@ -16,13 +17,18 @@ figure(1)
 hold on;
 axis equal;
 axis([-65 65 -75 60])
+% title('City10000 result with Hybrid Factor Graphs');
 plot(gt(:,1), gt(:,2), '--', 'LineWidth', 4, 'color', [0.1 0.7 0.1 0.5]);
-% hold off;
-
-% figure(2)
-% hold on;
-% axis equal;
-% axis([-65 65 -75 60])
-plot(eh_poses(:,1), eh_poses(:,2), '-', 'LineWidth', 2, 'color', [0.9 0.1 0. 0.4]);
 plot(h_poses(:,1), h_poses(:,2), '-', 'LineWidth', 2, 'color', [0.1 0.1 0.9 0.4]);
+legend('Ground truth', 'Hybrid Factor Graphs');
+hold off;
+
+figure(2)
+hold on;
+axis equal;
+axis([-65 65 -75 60])
+% title('City10000 result with ISAM2');
+plot(gt(:,1), gt(:,2), '--', 'LineWidth', 4, 'color', [0.1 0.7 0.1 0.5]);
+plot(eh_poses(:,1), eh_poses(:,2), '-', 'LineWidth', 2, 'color', [0.9 0.1 0. 0.4]);
+legend('Ground truth', 'ISAM2');
 hold off;

gtsam/discrete/DecisionTreeFactor.cpp

@@ -523,6 +523,10 @@ namespace gtsam {
 
       // Check if value is less than the threshold and
       // we haven't exceeded the maximum number of leaves.
+      // TODO(Varun): Bug since we can have a case where we need to prune higher
+      // probabilities after we have reached N.
+      // E.g. N=3 for [0.2, 0.2, 0.1, 0.2, 0.3]
+      // will give [0.2, 0.2, 0.0, 0.2, 0.0]
       if (value < threshold || total >= N) {
         return 0.0;
       } else {

gtsam/hybrid/HybridBayesNet.cpp

@@ -95,6 +95,10 @@ HybridBayesNet HybridBayesNet::prune(
           "HybrdiBayesNet::prune: Unknown HybridConditional type.");
   }
 
+#if GTSAM_HYBRID_TIMING
+  gttoc_(HybridPruning);
+#endif
+
   // Add the pruned discrete conditionals to the result.
   for (const DiscreteConditional::shared_ptr &discrete : prunedBN)
     result.push_back(discrete);

gtsam/hybrid/HybridSmoother.cpp

@@ -89,8 +89,14 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
     ordering = *given_ordering;
   }
 
+#if GTSAM_HYBRID_TIMING
+  gttic_(HybridSmootherEliminate);
+#endif
   // Eliminate.
   HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering);
+#if GTSAM_HYBRID_TIMING
+  gttoc_(HybridSmootherEliminate);
+#endif
 
 #ifdef DEBUG_SMOOTHER_DETAIL
   for (auto conditional : bayesNetFragment) {
@@ -110,12 +116,18 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
 
   /// Prune
   if (maxNrLeaves) {
+#if GTSAM_HYBRID_TIMING
+    gttic_(HybridSmootherPrune);
+#endif
     // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in
     // all the conditionals with the same keys in bayesNetFragment.
     DiscreteValues newlyFixedValues;
     bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_,
                                               &newlyFixedValues);
     fixedValues_.insert(newlyFixedValues);
+#if GTSAM_HYBRID_TIMING
+    gttoc_(HybridSmootherPrune);
+#endif
   }
 
 #ifdef DEBUG_SMOOTHER
@@ -158,7 +170,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
     // in the previous `hybridBayesNet` to the graph
 
     // New conditionals to add to the graph
-    gtsam::HybridBayesNet newConditionals;
+    HybridBayesNet newConditionals;
 
     // NOTE(Varun) Using a for-range loop doesn't work since some of the
     // conditionals are invalid pointers