simple ISAM2 based example for City10000 dataset

examples/Hybrid_City10000.cpp

@@ -0,0 +1,206 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Hybrid_City10000.cpp
+ * @brief  Example of using hybrid estimation
+ *         with multiple odometry measurements.
+ * @author Varun Agrawal
+ * @date   January 22, 2025
+ */
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/dataset.h>
+#include <time.h>
+
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <fstream>
+#include <string>
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::algorithm;
+
+using symbol_shorthand::X;
+
+// Testing params
+const size_t max_loop_count = 8000;  // 200 //2000 //8000
+
+const bool is_with_ambiguity = false;  // run original iSAM2 without ambiguities
+// const bool is_with_ambiguity = true;  // run original iSAM2 with ambiguities
+
+noiseModel::Diagonal::shared_ptr prior_noise_model =
+    noiseModel::Diagonal::Sigmas(
+        (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
+
+noiseModel::Diagonal::shared_ptr pose_noise_model =
+    noiseModel::Diagonal::Sigmas(
+        (Vector(3) << 1.0 / 50.0, 1.0 / 50.0, 1.0 / 100.0).finished());
+
+/* ************************************************************************* */
+
+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 loop_count = 0;
+
+  std::list<double> time_list;
+
+  ISAM2Params parameters;
+
+  parameters.optimizationParams =
+      gtsam::ISAM2GaussNewtonParams(0.0);  //_wildfireThreshold = 0.001
+
+  parameters.relinearizeThreshold = 0.01;
+
+  parameters.relinearizeSkip = 1;
+  ISAM2* isam2 = new ISAM2(parameters);  //[2] MHISAM2
+
+  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->add(PriorFactor<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) && loop_count < 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 m_num = stoi(parts[5]);
+    vector<double> x_arr(m_num);
+    vector<double> y_arr(m_num);
+    vector<double> rad_arr(m_num);
+    for (int i = 0; i < m_num; ++i) {
+      x_arr[i] = stod(parts[6 + 3 * i]);
+      y_arr[i] = stod(parts[7 + 3 * i]);
+      rad_arr[i] = stod(parts[8 + 3 * i]);
+    }
+
+    Pose2 odom_pose;
+    if (is_with_ambiguity) {
+      // Get wrong intentionally
+      int id = loop_count % m_num;
+      odom_pose = Pose2(x_arr[id], y_arr[id], rad_arr[id]);
+    } else {
+      odom_pose = Pose2(x_arr[0], y_arr[0], rad_arr[0]);
+    }
+
+    if (key_s == key_t - 1) {  // new X(key)
+      init_values.insert(X(key_t), results.at<Pose2>(X(key_s)) * odom_pose);
+      pose_count++;
+    } else {  // loop
+      loop_count++;
+    }
+    graph->add(
+        BetweenFactor<Pose2>(X(key_s), X(key_t), odom_pose, pose_noise_model));
+
+    isam2->update(*graph, init_values);
+    graph->resize(0);
+    init_values.clear();
+    results = isam2->calculateBestEstimate();
+
+    //*
+    if (loop_count % 50 == 0 && key_s != key_t - 1) {
+      std::cout << "loop_count: " << loop_count << std::endl;
+      std::cout << "acc_time:  " << time_list.back() << std::endl;
+    }
+    // */
+
+    if (key_s == key_t - 1) {
+      clock_t cur_time = clock();
+      time_list.push_back(cur_time - start_time);
+    }
+
+    if (time_list.size() % 100 == 0 && (key_s == key_t - 1)) {
+      string step_file_idx = std::to_string(100000 + time_list.size());
+
+      ofstream step_outfile;
+      string step_file_name =
+          "step_files/ISAM2_TEST_city10000_S" + step_file_idx;
+      step_outfile.open(step_file_name + ".txt");
+      for (size_t i = 0; i < (key_t + 1); ++i) {
+        Pose2 out_pose = results.at<Pose2>(X(i));
+        step_outfile << out_pose.x() << " " << out_pose.y() << " "
+                     << out_pose.theta() << endl;
+      }
+      step_outfile.close();
+    }
+  }
+
+  clock_t end_time = clock();
+  clock_t total_time = end_time - start_time;
+  cout << "total_time: " << total_time << endl;
+  //*
+
+  ofstream outfile;
+  string file_name = "ISAM2_TEST_city10000";
+  outfile.open(file_name + ".txt");
+
+  for (size_t i = 0; i < (key_t + 1); ++i) {
+    Pose2 out_pose = results.at<Pose2>(X(i));
+
+    outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta()
+            << endl;
+  }
+  outfile.close();
+  cout << "output " << file_name << ".txt file." << endl;
+  // */
+
+  //*
+  ofstream outfile_time;
+  string time_file_name = "ISAM2_TEST_city10000_time";
+  outfile_time.open(time_file_name + ".txt");
+  for (auto acc_time : time_list) {
+    outfile_time << acc_time << endl;  // WRONG FORMAT... JUST FOR SIMPLE SHOW
+  }
+  outfile_time.close();
+  cout << "output " << time_file_name << ".txt file." << endl;
+  // */
+  return 0;
+}