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 Hybrid_City10000.cpp
14 * @brief Example of using hybrid 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/hybrid/HybridNonlinearFactor.h>
22#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
23#include <gtsam/hybrid/HybridSmoother.h>
24#include <gtsam/hybrid/HybridValues.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/nonlinear/Values.h>
27#include <gtsam/slam/BetweenFactor.h>
28#include <gtsam/slam/PriorFactor.h>
29#include <gtsam/slam/dataset.h>
30#include <time.h>
31
32#include <cstdlib>
33#include <fstream>
34#include <iostream>
35#include <string>
36#include <vector>
37
38#include "City10000.h"
39
40using namespace gtsam;
41
42using symbol_shorthand::L;
43using symbol_shorthand::M;
44using symbol_shorthand::X;
45
46// Experiment Class
47class Experiment {
48 /// The City10000 dataset
49 City10000Dataset dataset_;
50
51 public:
52 // Parameters with default values
53 size_t maxLoopCount = 8000;
54
55 // 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5: 39s} No DT optimizations
56 // 3000: {1: 65s, 2: 20s, 3: 16s, 4: 21s, 5: 28s} With DT optimizations
57 // 3000: {1: 59s, 2: 19s, 3: 18s, 4: 26s, 5: 33s} With DT optimizations +
58 // merge
59 size_t updateFrequency = 3;
60
61 size_t maxNrHypotheses = 10;
62
63 size_t reLinearizationFrequency = 10;
64
65 double marginalThreshold = 0.9999;
66
67 private:
68 HybridSmoother smoother_;
69 HybridNonlinearFactorGraph newFactors_, allFactors_;
70 Values initial_;
71
72 /**
73 * @brief Create a hybrid loop closure factor where
74 * 0 - loose noise model and 1 - loop noise model.
75 */
76 HybridNonlinearFactor hybridLoopClosureFactor(
77 size_t loopCounter, size_t keyS, size_t keyT,
78 const Pose2& measurement) const {
79 DiscreteKey l(L(j: loopCounter), 2);
80
81 auto f0 = std::make_shared<BetweenFactor<Pose2>>(
82 args: X(j: keyS), args: X(j: keyT), args: measurement, args&: kOpenLoopModel);
83 auto f1 = std::make_shared<BetweenFactor<Pose2>>(
84 args: X(j: keyS), args: X(j: keyT), args: measurement, args&: kPoseNoiseModel);
85
86 std::vector<NonlinearFactorValuePair> factors{{f0, kOpenLoopConstant},
87 {f1, kPoseNoiseConstant}};
88 HybridNonlinearFactor mixtureFactor(l, factors);
89 return mixtureFactor;
90 }
91
92 /// @brief Create hybrid odometry factor with discrete measurement choices.
93 HybridNonlinearFactor hybridOdometryFactor(
94 size_t numMeasurements, size_t keyS, size_t keyT, const DiscreteKey& m,
95 const std::vector<Pose2>& poseArray) const {
96 auto f0 = std::make_shared<BetweenFactor<Pose2>>(
97 args: X(j: keyS), args: X(j: keyT), args: poseArray[0], args&: kPoseNoiseModel);
98 auto f1 = std::make_shared<BetweenFactor<Pose2>>(
99 args: X(j: keyS), args: X(j: keyT), args: poseArray[1], args&: kPoseNoiseModel);
100
101 std::vector<NonlinearFactorValuePair> factors{{f0, kPoseNoiseConstant},
102 {f1, kPoseNoiseConstant}};
103 HybridNonlinearFactor mixtureFactor(m, factors);
104 return mixtureFactor;
105 }
106
107 /// @brief Perform smoother update and optimize the graph.
108 clock_t smootherUpdate(size_t maxNrHypotheses) {
109 std::cout << "Smoother update: " << newFactors_.size() << std::endl;
110 gttic_(SmootherUpdate);
111 clock_t beforeUpdate = clock();
112 smoother_.update(graph: newFactors_, initial: initial_, maxNrLeaves: maxNrHypotheses);
113 clock_t afterUpdate = clock();
114 allFactors_.push_back(container: newFactors_);
115 newFactors_.resize(size: 0);
116 return afterUpdate - beforeUpdate;
117 }
118
119 /// @brief Re-linearize, solve ALL, and re-initialize smoother.
120 clock_t reInitialize() {
121 std::cout << "================= Re-Initialize: " << allFactors_.size()
122 << std::endl;
123 clock_t beforeUpdate = clock();
124 allFactors_ = allFactors_.restrict(assignment: smoother_.fixedValues());
125 auto linearized = allFactors_.linearize(continuousValues: initial_);
126 auto bayesNet = linearized->eliminateSequential();
127 HybridValues delta = bayesNet->optimize();
128 initial_ = initial_.retract(delta: delta.continuous());
129 smoother_.reInitialize(hybridBayesNet: std::move(*bayesNet));
130 clock_t afterUpdate = clock();
131 std::cout << "Took " << (afterUpdate - beforeUpdate) / CLOCKS_PER_SEC
132 << " seconds." << std::endl;
133 return afterUpdate - beforeUpdate;
134 }
135
136 public:
137 /// Construct with filename of experiment to run
138 explicit Experiment(const std::string& filename)
139 : dataset_(filename), smoother_(marginalThreshold) {}
140
141 /// @brief Run the main experiment with a given maxLoopCount.
142 void run() {
143 // Initialize local variables
144 size_t discreteCount = 0, index = 0, loopCount = 0, updateCount = 0;
145
146 std::list<double> timeList;
147
148 // Set up initial prior
149 Pose2 priorPose(0, 0, 0);
150 initial_.insert(j: X(j: 0), val: priorPose);
151 newFactors_.push_back(
152 factor: PriorFactor<Pose2>(X(j: 0), priorPose, kPriorNoiseModel));
153
154 // Initial update
155 auto time = smootherUpdate(maxNrHypotheses);
156 std::vector<std::pair<size_t, double>> smootherUpdateTimes;
157 smootherUpdateTimes.push_back(x: {index, time});
158
159 // Flag to decide whether to run smoother update
160 size_t numberOfHybridFactors = 0;
161
162 // Start main loop
163 Values result;
164 size_t keyS = 0, keyT = 0;
165 clock_t startTime = clock();
166
167 std::vector<Pose2> poseArray;
168 std::pair<size_t, size_t> keys;
169
170 while (dataset_.next(poseArray: &poseArray, keys: &keys) && index < maxLoopCount) {
171 keyS = keys.first;
172 keyT = keys.second;
173 size_t numMeasurements = poseArray.size();
174
175 // Take the first one as the initial estimate
176 Pose2 odomPose = poseArray[0];
177 if (keyS == keyT - 1) {
178 // Odometry factor
179 if (numMeasurements > 1) {
180 // Add hybrid factor
181 DiscreteKey m(M(j: discreteCount), numMeasurements);
182 HybridNonlinearFactor mixtureFactor =
183 hybridOdometryFactor(numMeasurements, keyS, keyT, m, poseArray);
184 newFactors_.push_back(factor: mixtureFactor);
185 discreteCount++;
186 numberOfHybridFactors += 1;
187 std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl;
188 } else {
189 newFactors_.add(factorOrContainer: BetweenFactor<Pose2>(X(j: keyS), X(j: keyT), odomPose,
190 kPoseNoiseModel));
191 }
192 // Insert next pose initial guess
193 initial_.insert(j: X(j: keyT), val: initial_.at<Pose2>(j: X(j: keyS)) * odomPose);
194 } else {
195 // Loop closure
196 HybridNonlinearFactor loopFactor =
197 hybridLoopClosureFactor(loopCounter: loopCount, keyS, keyT, measurement: odomPose);
198 // print loop closure event keys:
199 std::cout << "Loop closure: " << keyS << " " << keyT << std::endl;
200 newFactors_.add(factorOrContainer: loopFactor);
201 numberOfHybridFactors += 1;
202 loopCount++;
203 }
204
205 if (numberOfHybridFactors >= updateFrequency) {
206 auto time = smootherUpdate(maxNrHypotheses);
207 smootherUpdateTimes.push_back(x: {index, time});
208 numberOfHybridFactors = 0;
209 updateCount++;
210
211 if (updateCount % reLinearizationFrequency == 0) {
212 reInitialize();
213 }
214 }
215
216 // Record timing for odometry edges only
217 if (keyS == keyT - 1) {
218 clock_t curTime = clock();
219 timeList.push_back(x: curTime - startTime);
220 }
221
222 // Print some status every 100 steps
223 if (index % 100 == 0) {
224 std::cout << "Index: " << index << std::endl;
225 if (!timeList.empty()) {
226 std::cout << "Acc_time: " << timeList.back() / CLOCKS_PER_SEC
227 << " seconds" << std::endl;
228 // delta.discrete().print("The Discrete Assignment");
229 tictoc_finishedIteration_();
230 tictoc_print_();
231 }
232 }
233
234 index++;
235 }
236
237 // Final update
238 time = smootherUpdate(maxNrHypotheses);
239 smootherUpdateTimes.push_back(x: {index, time});
240
241 // Final optimize
242 gttic_(HybridSmootherOptimize);
243 HybridValues delta = smoother_.optimize();
244 gttoc_(HybridSmootherOptimize);
245
246 result.insert_or_assign(values: initial_.retract(delta: delta.continuous()));
247
248 std::cout << "Final error: " << smoother_.hybridBayesNet().error(values: delta)
249 << std::endl;
250
251 clock_t endTime = clock();
252 clock_t totalTime = endTime - startTime;
253 std::cout << "Total time: " << totalTime / CLOCKS_PER_SEC << " seconds"
254 << std::endl;
255
256 // Write results to file
257 writeResult(result, numPoses: keyT + 1, filename: "Hybrid_City10000.txt");
258
259 // Write timing info to file
260 std::ofstream outfileTime;
261 std::string timeFileName = "Hybrid_City10000_time.txt";
262 outfileTime.open(s: timeFileName);
263 for (auto accTime : timeList) {
264 outfileTime << accTime / CLOCKS_PER_SEC << std::endl;
265 }
266 outfileTime.close();
267 std::cout << "Output " << timeFileName << " file." << std::endl;
268
269 std::ofstream timingFile;
270 std::string timingFileName = "Hybrid_City10000_timing.txt";
271 timingFile.open(s: timingFileName);
272 for (size_t i = 0; i < smootherUpdateTimes.size(); i++) {
273 auto p = smootherUpdateTimes.at(n: i);
274 timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
275 }
276 timingFile.close();
277 std::cout << "Wrote timing information to " << timingFileName << std::endl;
278 }
279};
280
281/* ************************************************************************* */
282// Function to parse command-line arguments
283void parseArguments(int argc, char* argv[], size_t& maxLoopCount,
284 size_t& updateFrequency, size_t& maxNrHypotheses) {
285 for (int i = 1; i < argc; ++i) {
286 std::string arg = argv[i];
287 if (arg == "--max-loop-count" && i + 1 < argc) {
288 maxLoopCount = std::stoul(str: argv[++i]);
289 } else if (arg == "--update-frequency" && i + 1 < argc) {
290 updateFrequency = std::stoul(str: argv[++i]);
291 } else if (arg == "--max-nr-hypotheses" && i + 1 < argc) {
292 maxNrHypotheses = std::stoul(str: argv[++i]);
293 } else if (arg == "--help") {
294 std::cout << "Usage: " << argv[0] << " [options]\n"
295 << "Options:\n"
296 << " --max-loop-count <value> Set the maximum loop "
297 "count (default: 3000)\n"
298 << " --update-frequency <value> Set the update frequency "
299 "(default: 3)\n"
300 << " --max-nr-hypotheses <value> Set the maximum number of "
301 "hypotheses (default: 10)\n"
302 << " --help Show this help message\n";
303 std::exit(status: 0);
304 }
305 }
306}
307
308/* ************************************************************************* */
309// Main function
310int main(int argc, char* argv[]) {
311 Experiment experiment(findExampleDataFile(name: "T1_city10000_04.txt"));
312 // Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only
313 // Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only
314 // Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 +
315 // Type #3
316
317 // Parse command-line arguments
318 parseArguments(argc, argv, maxLoopCount&: experiment.maxLoopCount,
319 updateFrequency&: experiment.updateFrequency, maxNrHypotheses&: experiment.maxNrHypotheses);
320
321 // Run the experiment
322 experiment.run();
323
324 return 0;
325}
326