1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file ISAM2_City10000.cpp
14 * @brief Example of using ISAM2 estimation
15 * with multiple odometry measurements.
16 * @author Varun Agrawal
17 * @date January 22, 2025
18 */
19
20#include <gtsam/geometry/Pose2.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/nonlinear/ISAM2.h>
23#include <gtsam/nonlinear/ISAM2Params.h>
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
25#include <gtsam/nonlinear/Values.h>
26#include <gtsam/slam/BetweenFactor.h>
27#include <gtsam/slam/dataset.h>
28#include <time.h>
29
30#include <fstream>
31#include <string>
32#include <vector>
33
34#include "City10000.h"
35
36using namespace gtsam;
37
38using symbol_shorthand::X;
39
40// Experiment Class
41class Experiment {
42 /// The City10000 dataset
43 City10000Dataset dataset_;
44
45 public:
46 // Parameters with default values
47 size_t maxLoopCount = 2000; // 200 //2000 //8000
48
49 // false: run original iSAM2 without ambiguities
50 // true: run original iSAM2 with ambiguities
51 bool isWithAmbiguity;
52
53 private:
54 ISAM2 isam2_;
55 NonlinearFactorGraph graph_;
56 Values initial_;
57 Values results;
58
59 public:
60 /// Construct with filename of experiment to run
61 explicit Experiment(const std::string& filename, bool isWithAmbiguity = false)
62 : dataset_(filename), isWithAmbiguity(isWithAmbiguity) {
63 ISAM2Params parameters;
64 parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0);
65 parameters.relinearizeThreshold = 0.01;
66 parameters.relinearizeSkip = 1;
67 isam2_ = ISAM2(parameters);
68 }
69
70 /// @brief Run the main experiment with a given maxLoopCount.
71 void run() {
72 // Initialize local variables
73 size_t index = 0;
74
75 std::vector<std::pair<size_t, double>> smootherUpdateTimes;
76
77 std::list<double> timeList;
78
79 // Set up initial prior
80 Pose2 priorPose(0, 0, 0);
81 initial_.insert(j: X(j: 0), val: priorPose);
82 graph_.addPrior<Pose2>(key: X(j: 0), prior: priorPose, model: kPriorNoiseModel);
83
84 // Initial update
85 clock_t beforeUpdate = clock();
86 isam2_.update(newFactors: graph_, newTheta: initial_);
87 results = isam2_.calculateBestEstimate();
88 clock_t afterUpdate = clock();
89 smootherUpdateTimes.push_back(
90 x: std::make_pair(x&: index, y: afterUpdate - beforeUpdate));
91 graph_.resize(size: 0);
92 initial_.clear();
93 index += 1;
94
95 // Start main loop
96 size_t keyS = 0;
97 size_t keyT = 0;
98 clock_t startTime = clock();
99
100 std::vector<Pose2> poseArray;
101 std::pair<size_t, size_t> keys;
102
103 while (dataset_.next(poseArray: &poseArray, keys: &keys) && index < maxLoopCount) {
104 keyS = keys.first;
105 keyT = keys.second;
106 size_t numMeasurements = poseArray.size();
107
108 Pose2 odomPose;
109 if (isWithAmbiguity) {
110 // Get wrong intentionally
111 int id = index % numMeasurements;
112 odomPose = Pose2(poseArray[id]);
113 } else {
114 odomPose = poseArray[0];
115 }
116
117 if (keyS == keyT - 1) { // new X(key)
118 initial_.insert(j: X(j: keyT), val: results.at<Pose2>(j: X(j: keyS)) * odomPose);
119 graph_.add(
120 factorOrContainer: BetweenFactor<Pose2>(X(j: keyS), X(j: keyT), odomPose, kPoseNoiseModel));
121
122 } else { // loop
123 int id = index % numMeasurements;
124 if (isWithAmbiguity && id % 2 == 0) {
125 graph_.add(factorOrContainer: BetweenFactor<Pose2>(X(j: keyS), X(j: keyT), odomPose,
126 kPoseNoiseModel));
127 } else {
128 graph_.add(factorOrContainer: BetweenFactor<Pose2>(
129 X(j: keyS), X(j: keyT), odomPose,
130 noiseModel::Diagonal::Sigmas(sigmas: Vector3::Ones() * 10.0)));
131 }
132 index++;
133 }
134
135 clock_t beforeUpdate = clock();
136 isam2_.update(newFactors: graph_, newTheta: initial_);
137 results = isam2_.calculateBestEstimate();
138 clock_t afterUpdate = clock();
139 smootherUpdateTimes.push_back(
140 x: std::make_pair(x&: index, y: afterUpdate - beforeUpdate));
141 graph_.resize(size: 0);
142 initial_.clear();
143 index += 1;
144
145 // Print loop index and time taken in processor clock ticks
146 if (index % 50 == 0 && keyS != keyT - 1) {
147 std::cout << "index: " << index << std::endl;
148 std::cout << "accTime: " << timeList.back() / CLOCKS_PER_SEC
149 << std::endl;
150 }
151
152 if (keyS == keyT - 1) {
153 clock_t curTime = clock();
154 timeList.push_back(x: curTime - startTime);
155 }
156
157 if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) {
158 std::string stepFileIdx = std::to_string(val: 100000 + timeList.size());
159
160 std::ofstream stepOutfile;
161 std::string stepFileName = "step_files/ISAM2_City10000_S" + stepFileIdx;
162 stepOutfile.open(s: stepFileName + ".txt");
163 for (size_t i = 0; i < (keyT + 1); ++i) {
164 Pose2 outPose = results.at<Pose2>(j: X(j: i));
165 stepOutfile << outPose.x() << " " << outPose.y() << " "
166 << outPose.theta() << std::endl;
167 }
168 stepOutfile.close();
169 }
170 }
171
172 clock_t endTime = clock();
173 clock_t totalTime = endTime - startTime;
174 std::cout << "totalTime: " << totalTime / CLOCKS_PER_SEC << std::endl;
175
176 /// Write results to file
177 writeResult(result: results, numPoses: (keyT + 1), filename: "ISAM2_City10000.txt");
178
179 std::ofstream outfileTime;
180 std::string timeFileName = "ISAM2_City10000_time.txt";
181 outfileTime.open(s: timeFileName);
182 for (auto accTime : timeList) {
183 outfileTime << accTime << std::endl;
184 }
185 outfileTime.close();
186 std::cout << "Written cumulative time to: " << timeFileName << " file."
187 << std::endl;
188
189 std::ofstream timingFile;
190 std::string timingFileName = "ISAM2_City10000_timing.txt";
191 timingFile.open(s: timingFileName);
192 for (size_t i = 0; i < smootherUpdateTimes.size(); i++) {
193 auto p = smootherUpdateTimes.at(n: i);
194 timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
195 }
196 timingFile.close();
197 std::cout << "Wrote timing information to " << timingFileName << std::endl;
198 }
199};
200
201/* ************************************************************************* */
202// Function to parse command-line arguments
203void parseArguments(int argc, char* argv[], size_t& maxLoopCount,
204 bool& isWithAmbiguity) {
205 for (int i = 1; i < argc; ++i) {
206 std::string arg = argv[i];
207 if (arg == "--max-loop-count" && i + 1 < argc) {
208 maxLoopCount = std::stoul(str: argv[++i]);
209 } else if (arg == "--is-with-ambiguity" && i + 1 < argc) {
210 isWithAmbiguity = bool(std::stoul(str: argv[++i]));
211 } else if (arg == "--help") {
212 std::cout << "Usage: " << argv[0] << " [options]\n"
213 << "Options:\n"
214 << " --max-loop-count <value> Set the maximum loop "
215 "count (default: 2000)\n"
216 << " --is-with-ambiguity <value=0/1> Set whether to use "
217 "ambiguous measurements "
218 "(default: false)\n"
219 << " --help Show this help message\n";
220 std::exit(status: 0);
221 }
222 }
223}
224
225/* ************************************************************************* */
226int main(int argc, char* argv[]) {
227 Experiment experiment(findExampleDataFile(name: "T1_City10000_04.txt"));
228 // Experiment experiment("../data/mh_T1_City10000_04.txt"); //Type #1 only
229 // Experiment experiment("../data/mh_T3b_City10000_10.txt"); //Type #3 only
230 // Experiment experiment("../data/mh_T1_T3_City10000_04.txt"); //Type #1 +
231 // Type #3
232
233 // Parse command-line arguments
234 parseArguments(argc, argv, maxLoopCount&: experiment.maxLoopCount,
235 isWithAmbiguity&: experiment.isWithAmbiguity);
236
237 // Run the experiment
238 experiment.run();
239
240 return 0;
241}
242