1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010-2025, 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 IncrementalFixedLagExample.cpp
14* @brief Example of incremental fixed-lag smoother using real-world data.
15* @author Xiangcheng Hu (xhubd@connect.ust.hk), Frank Dellaert, Kevin Doherty
16* @date Janaury 15, 2025
17*
18* Key objectives:
19* - Validate `IncrementalFixedLagSmoother` functionality with real-world data.
20* - Showcase how setting `findUnusedFactorSlots = true` addresses the issue #1452 in GTSAM, ensuring
21* that unused factor slots (nullptrs) are correctly released when old factors are marginalized.
22*
23* This example leverages pose measurements from a real scenario. The test data (named "IncrementalFixedLagSmootherTestData.txt") is
24* based on the corridor_day sequence from the FusionPortable dataset (https://fusionportable.github.io/dataset/fusionportable/).
25* - 1 prior factor derived from point cloud ICP alignment with a prior map.
26* - 199 relative pose factors derived from FAST-LIO2 odometry.
27*
28* Data Format (IncrementalFixedLagSmootherTestData.txt):
29* 1) PRIOR factor line:
30* @code
31* 0 timestamp key x y z roll pitch yaw cov_6x6
32* @endcode
33* - "0" indicates PRIOR factor.
34* - "timestamp" is the observation time (in seconds).
35* - "key" is the integer ID for the Pose3 variable.
36* - (x, y, z, roll, pitch, yaw) define the pose.
37* - "cov_6x6" is the full 6x6 covariance matrix (row-major).
38*
39* 2) BETWEEN factor line:
40* @code
41* 1 timestamp key1 key2 x y z roll pitch yaw cov_6x6
42* @endcode
43* - "1" indicates BETWEEN factor.
44* - "timestamp" is the observation time (in seconds).
45* - "key1" and "key2" are the integer IDs for the connected Pose3 variables.
46* - (x, y, z, roll, pitch, yaw) define the relative pose between these variables.
47* - "cov_6x6" is the full 6x6 covariance matrix (row-major).
48*
49* See also:
50* - GTSAM Issue #1452: https://github.com/borglab/gtsam/issues/1452
51*/
52
53// STL
54#include <iostream>
55#include <string>
56// GTSAM
57#include <gtsam/geometry/Pose3.h>
58#include <gtsam/nonlinear/ISAM2.h>
59#include <gtsam/nonlinear/Values.h>
60#include <gtsam/nonlinear/NonlinearFactorGraph.h>
61#include <gtsam/slam/BetweenFactor.h>
62#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
63#include <gtsam/inference/Symbol.h>
64#include <gtsam/slam/dataset.h> // for writeG2o
65
66using namespace std;
67using namespace gtsam;
68// Shorthand for symbols
69using symbol_shorthand::X; // Pose3 (x,y,z, roll, pitch, yaw)
70
71// Factor types
72enum FactorType {
73 PRIOR = 0,
74 BETWEEN = 1
75};
76
77typedef Eigen::Matrix<double, 6, 6> Matrix6;
78
79/* ************************************************************************* */
80/**
81 * @brief Read a 6x6 covariance matrix from an input string stream.
82 *
83 * @param iss Input string stream containing covariance entries.
84 * @return 6x6 covariance matrix.
85 */
86Matrix6 readCovarianceMatrix(istringstream &iss) {
87 Matrix6 cov;
88 for (int r = 0; r < 6; ++r) {
89 for (int c = 0; c < 6; ++c) {
90 iss >> cov(r, c);
91 }
92 }
93 return cov;
94}
95
96/* ************************************************************************* */
97/**
98 * @brief Create a Pose3 object from individual pose parameters.
99 *
100 * @param x Translation in x
101 * @param y Translation in y
102 * @param z Translation in z
103 * @param roll Rotation about x-axis
104 * @param pitch Rotation about y-axis
105 * @param yaw Rotation about z-axis
106 * @return Constructed Pose3 object
107 */
108Pose3 createPose(double x, double y, double z, double roll, double pitch, double yaw) {
109 return Pose3(Rot3::RzRyRx(x: roll, y: pitch, z: yaw), Point3(x, y, z));
110}
111
112/* ************************************************************************* */
113/**
114 * @brief Save the factor graph and estimates to a .g2o file (for visualization/debugging).
115 *
116 * @param graph The factor graph
117 * @param estimate Current estimates of all variables
118 * @param filename Base filename for saving
119 * @param iterCount Iteration count to differentiate successive outputs
120 */
121void saveG2oGraph(const NonlinearFactorGraph &graph, const Values &estimate,
122 const string &filename, int iterCount) {
123 // Create zero-padded iteration count
124 string countStr = to_string(val: iterCount);
125 string paddedCount = string(5 - countStr.length(), '0') + countStr;
126 string fullFilename = filename + "_" + paddedCount + ".g2o";
127 // Write graph and estimates to g2o file
128 writeG2o(graph, estimate, filename: fullFilename);
129 cout << "\nSaved graph to: " << fullFilename << endl;
130}
131
132/* ************************************************************************* */
133/**
134 * @brief Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing.
135 *
136 * Data Flow:
137 * 1) Parse lines from "IncrementalFixedLagSmootherTestData.txt".
138 * 2) For each line:
139 * - If it's a PRIOR factor, add a PriorFactor with a specified pose and covariance.
140 * - If it's a BETWEEN factor, add a BetweenFactor with a relative pose and covariance.
141 * - Insert new variables with initial guesses into the current solution if they don't exist.
142 * 3) Update the fixed-lag smoother (with iSAM2 inside) to incrementally optimize and marginalize out old poses
143 * beyond the specified lag window.
144 * 4) Repeat until all lines are processed.
145 * 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file.
146 */
147int main() {
148 string factor_loc = findExampleDataFile(name: "issue1452.txt");
149 ifstream factor_file(factor_loc.c_str());
150 cout << "Reading factors data file: " << factor_loc << endl;
151
152 // Configure ISAM2 parameters for the fixed-lag smoother
153 ISAM2Params isamParameters;
154 isamParameters.relinearizeThreshold = 0.1;
155 isamParameters.relinearizeSkip = 1;
156
157 // Important!!!!!! Key parameter to ensure old factors are released after marginalization
158 isamParameters.findUnusedFactorSlots = true;
159 // Initialize fixed-lag smoother with a 1-second lag window
160 const double lag = 1.0;
161 IncrementalFixedLagSmoother smoother(lag, isamParameters);
162 // Print the iSAM2 parameters (optional)
163 isamParameters.print();
164
165 // Containers for incremental updates
166 NonlinearFactorGraph newFactors;
167 Values newValues;
168 FixedLagSmoother::KeyTimestampMap newTimestamps;
169 // For tracking the latest estimate of all states in the sliding window
170 Values currentEstimate;
171 Pose3 lastPose;
172
173 // Read and process each line in the factor graph file
174 string line;
175 int lineCount = 0;
176 while (getline(is&: factor_file, str&: line)) {
177 if (line.empty()) continue; // Skip empty lines
178 cout << "\n======================== Processing line " << ++lineCount
179 << " =========================" << endl;
180
181 istringstream iss(line);
182 int factorType;
183 iss >> factorType;
184 // Check if the factor is PRIOR or BETWEEN
185 if (factorType == PRIOR) {
186 /**
187 * Format: PRIOR factor
188 * factor_type timestamp key pose(x y z roll pitch yaw) cov(6x6)
189 */
190 double timestamp;
191 int key;
192 double x, y, z, roll, pitch, yaw;
193 iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw;
194 Pose3 pose = createPose(x, y, z, roll, pitch, yaw);
195 Matrix6 cov = readCovarianceMatrix(iss);
196 auto noise = noiseModel::Gaussian::Covariance(covariance: cov);
197 // Add prior factor
198 newFactors.addPrior(key: X(j: key), prior: pose, model: noise);
199 cout << "Add PRIOR factor on key " << key << endl;
200 // Provide initial guess if not already in the graph
201 if (!newValues.exists(j: X(j: key))) {
202 newValues.insert(j: X(j: key), val: pose);
203 newTimestamps[X(j: key)] = timestamp;
204 }
205 } else if (factorType == BETWEEN) {
206 /**
207 * Format: BETWEEN factor
208 * factor_type timestamp key1 key2 pose(x y z roll pitch yaw) cov(6x6)
209 */
210 double timestamp;
211 int key1, key2;
212 iss >> timestamp >> key1 >> key2;
213 double x1, y1, z1, roll1, pitch1, yaw1;
214 iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1;
215 Pose3 relativePose = createPose(x: x1, y: y1, z: z1, roll: roll1, pitch: pitch1, yaw: yaw1);
216 Matrix6 cov = readCovarianceMatrix(iss);
217 auto noise = noiseModel::Gaussian::Covariance(covariance: cov);
218 // Add between factor
219 newFactors.emplace_shared<BetweenFactor<Pose3>>(args: X(j: key1), args: X(j: key2), args&: relativePose, args&: noise);
220 cout << "Add BETWEEN factor: " << key1 << " -> " << key2 << endl;
221 // Provide an initial guess if needed
222 if (!newValues.exists(j: X(j: key2))) {
223 newValues.insert(j: X(j: key2), val: lastPose.compose(g: relativePose));
224 newTimestamps[X(j: key2)] = timestamp;
225 }
226 } else {
227 cerr << "Unknown factor type: " << factorType << endl;
228 continue;
229 }
230
231 // Print some intermediate statistics
232 cout << "Before update - Graph has " << smoother.getFactors().size()
233 << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl;
234 cout << "New factors: " << newFactors.size()
235 << ", New values: " << newValues.size() << endl;
236
237 // Attempt to update the smoother with new factors and values
238 try {
239 smoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
240 // Optional: Perform extra internal iterations if needed
241 size_t maxExtraIterations = 3;
242 for (size_t i = 1; i < maxExtraIterations; ++i) {
243 smoother.update();
244 }
245 // you may not get expected results if you use the gtsam version lower than 4.3
246 cout << "After update - Graph has " << smoother.getFactors().size()
247 << " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl;
248
249 // Retrieve the latest estimate
250 currentEstimate = smoother.calculateEstimate();
251 if (!currentEstimate.empty()) {
252 // Update lastPose to the last key's estimate
253 Key lastKey = currentEstimate.keys().back();
254 lastPose = currentEstimate.at<Pose3>(j: lastKey);
255 }
256
257 // Clear containers for the next iteration
258 newFactors.resize(size: 0);
259 newValues.clear();
260 newTimestamps.clear();
261 } catch (const exception &e) {
262 cerr << "Smoother update failed: " << e.what() << endl;
263 }
264 }
265
266 // The results of the last sliding window are saved to a g2o file for visualization or further analysis.
267 saveG2oGraph(graph: smoother.getFactors(), estimate: currentEstimate, filename: "isam", iterCount: lineCount);
268 factor_file.close();
269
270 return 0;
271}
272
273