1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, 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 ConcurrentFilteringAndSmoothingExample.cpp
14 * @brief Demonstration of the concurrent filtering and smoothing architecture using
15 * a planar robot example and multiple odometry-like sensors
16 * @author Stephen Williams
17 */
18
19/**
20 * A simple 2D pose slam example with multiple odometry-like measurements
21 * - The robot initially faces along the X axis (horizontal, to the right in 2D)
22 * - The robot moves forward at 2m/s
23 * - We have measurements between each pose from multiple odometry sensors
24 */
25
26// This example demonstrates the use of the Concurrent Filtering and Smoothing architecture in GTSAM unstable
27#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
28#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
29
30// We will compare the results to a similar Fixed-Lag Smoother
31#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
32
33// In GTSAM, measurement functions are represented as 'factors'. Several common factors
34// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
35// Here we will use Between factors for the relative motion described by odometry measurements.
36// Also, we will initialize the robot at the origin using a Prior factor.
37#include <gtsam/slam/BetweenFactor.h>
38
39// When the factors are created, we will add them to a Factor Graph. As the factors we are using
40// are nonlinear factors, we will need a Nonlinear Factor Graph.
41#include <gtsam/nonlinear/NonlinearFactorGraph.h>
42
43// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
44// nonlinear functions around an initial linearization point, then solve the linear system
45// to update the linearization point. This happens repeatedly until the solver converges
46// to a consistent set of variable values. This requires us to specify an initial guess
47// for each variable, held in a Values container.
48#include <gtsam/nonlinear/Values.h>
49
50// We will use simple integer Keys to uniquely identify each robot pose.
51#include <gtsam/inference/Key.h>
52
53// We will use Pose2 variables (x, y, theta) to represent the robot positions
54#include <gtsam/geometry/Pose2.h>
55
56#include <iomanip>
57
58using namespace std;
59using namespace gtsam;
60
61
62int main(int argc, char** argv) {
63
64 // Define the smoother lag (in seconds)
65 double lag = 2.0;
66
67 // Create a Concurrent Filter and Smoother
68 ConcurrentBatchFilter concurrentFilter;
69 ConcurrentBatchSmoother concurrentSmoother;
70
71 // And a fixed lag smoother with a short lag
72 BatchFixedLagSmoother fixedlagSmoother(lag);
73
74 // And a fixed lag smoother with very long lag (i.e. a full batch smoother)
75 BatchFixedLagSmoother batchSmoother(1000.0);
76
77
78 // Create containers to store the factors and linearization points that
79 // will be sent to the smoothers
80 NonlinearFactorGraph newFactors;
81 Values newValues;
82 FixedLagSmoother::KeyTimestampMap newTimestamps;
83
84 // Create a prior on the first pose, placing it at the origin
85 Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
86 auto priorNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.3, 0.3, 0.1));
87 Key priorKey = 0;
88 newFactors.addPrior(key: priorKey, prior: priorMean, model: priorNoise);
89 newValues.insert(j: priorKey, val: priorMean); // Initialize the first pose at the mean of the prior
90 newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;
91
92 // Now, loop through several time steps, creating factors from different "sensors"
93 // and adding them to the fixed-lag smoothers
94 double deltaT = 0.25;
95 for(double time = 0.0+deltaT; time <= 5.0; time += deltaT) {
96
97 // Define the keys related to this timestamp
98 Key previousKey(1000 * (time-deltaT));
99 Key currentKey(1000 * (time));
100
101 // Assign the current key to the current timestamp
102 newTimestamps[currentKey] = time;
103
104 // Add a guess for this pose to the new values
105 // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
106 // {This is not a particularly good way to guess, but this is just an example}
107 Pose2 currentPose(time * 2.0, 0.0, 0.0);
108 newValues.insert(j: currentKey, val: currentPose);
109
110 // Add odometry factors from two different sources with different error stats
111 Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
112 auto odometryNoise1 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.1, 0.1, 0.05));
113 newFactors.push_back(factor: BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
114
115 Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
116 auto odometryNoise2 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.05, 0.05, 0.05));
117 newFactors.push_back(factor: BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
118
119 // Unlike the fixed-lag versions, the concurrent filter implementation
120 // requires the user to supply the specify which keys to move to the smoother
121 FastList<Key> oldKeys;
122 if(time >= lag+deltaT) {
123 oldKeys.push_back(x: 1000 * (time-lag-deltaT));
124 }
125
126 // Update the various inference engines
127 concurrentFilter.update(newFactors, newTheta: newValues, keysToMove: oldKeys);
128 fixedlagSmoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
129 batchSmoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
130
131 // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
132 if(fmod(x: time, y: 1.0) < 0.01) {
133 // Synchronize the Filter and Smoother
134 concurrentSmoother.update();
135 synchronize(filter&: concurrentFilter, smoother&: concurrentSmoother);
136 }
137
138 // Print the optimized current pose
139 cout << setprecision(5) << "Timestamp = " << time << endl;
140 concurrentFilter.calculateEstimate<Pose2>(key: currentKey).print(s: "Concurrent Estimate: ");
141 fixedlagSmoother.calculateEstimate<Pose2>(key: currentKey).print(s: "Fixed Lag Estimate: ");
142 batchSmoother.calculateEstimate<Pose2>(key: currentKey).print(s: "Batch Estimate: ");
143 cout << endl;
144
145 // Clear contains for the next iteration
146 newTimestamps.clear();
147 newValues.clear();
148 newFactors.resize(size: 0);
149 }
150 cout << "******************************************************************" << endl;
151 cout << "All three versions should be identical." << endl;
152 cout << "Adding a loop closure factor to the Batch version only." << endl;
153 cout << "******************************************************************" << endl;
154 cout << endl;
155
156 // At the moment, all three versions produce the same output.
157 // Now lets create a "loop closure" factor between the first pose and the current pose
158 Key loopKey1(1000 * (0.0));
159 Key loopKey2(1000 * (5.0));
160 Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00);
161 auto loopNoise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.5, 0.5, 0.25));
162 NonlinearFactor::shared_ptr loopFactor(new BetweenFactor<Pose2>(loopKey1, loopKey2, loopMeasurement, loopNoise));
163
164 // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state
165 // This measurement can never be added to the fixed-lag smoother, as one of the poses has been permanently marginalized out
166 // This measurement can be incorporated into the full batch version though
167 newFactors.push_back(factor: loopFactor);
168 batchSmoother.update(newFactors, newTheta: Values(), timestamps: FixedLagSmoother::KeyTimestampMap());
169 newFactors.resize(size: 0);
170
171
172
173 // Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
174 for(double time = 5.0+deltaT; time <= 8.0; time += deltaT) {
175
176 // Define the keys related to this timestamp
177 Key previousKey(1000 * (time-deltaT));
178 Key currentKey(1000 * (time));
179
180 // Assign the current key to the current timestamp
181 newTimestamps[currentKey] = time;
182
183 // Add a guess for this pose to the new values
184 // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
185 // {This is not a particularly good way to guess, but this is just an example}
186 Pose2 currentPose(time * 2.0, 0.0, 0.0);
187 newValues.insert(j: currentKey, val: currentPose);
188
189 // Add odometry factors from two different sources with different error stats
190 Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
191 auto odometryNoise1 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.1, 0.1, 0.05));
192 newFactors.push_back(factor: BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
193
194 Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
195 auto odometryNoise2 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.05, 0.05, 0.05));
196 newFactors.push_back(factor: BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
197
198 // Unlike the fixed-lag versions, the concurrent filter implementation
199 // requires the user to supply the specify which keys to marginalize
200 FastList<Key> oldKeys;
201 if(time >= lag+deltaT) {
202 oldKeys.push_back(x: 1000 * (time-lag-deltaT));
203 }
204
205 // Update the various inference engines
206 concurrentFilter.update(newFactors, newTheta: newValues, keysToMove: oldKeys);
207 fixedlagSmoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
208 batchSmoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
209
210 // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
211 if(fmod(x: time, y: 1.0) < 0.01) {
212 // Synchronize the Filter and Smoother
213 concurrentSmoother.update();
214 synchronize(filter&: concurrentFilter, smoother&: concurrentSmoother);
215 }
216
217 // Print the optimized current pose
218 cout << setprecision(5) << "Timestamp = " << time << endl;
219 concurrentFilter.calculateEstimate<Pose2>(key: currentKey).print(s: "Concurrent Estimate: ");
220 fixedlagSmoother.calculateEstimate<Pose2>(key: currentKey).print(s: "Fixed Lag Estimate: ");
221 batchSmoother.calculateEstimate<Pose2>(key: currentKey).print(s: "Batch Estimate: ");
222 cout << endl;
223
224 // Clear contains for the next iteration
225 newTimestamps.clear();
226 newValues.clear();
227 newFactors.resize(size: 0);
228 }
229 cout << "******************************************************************" << endl;
230 cout << "The Concurrent system and the Fixed-Lag Smoother should be " << endl;
231 cout << "the same, but the Batch version has a loop closure." << endl;
232 cout << "Adding the loop closure factor to the Concurrent version." << endl;
233 cout << "This will not update the Concurrent Filter until the next " << endl;
234 cout << "synchronization, but the Concurrent solution should be identical " << endl;
235 cout << "to the Batch solution afterwards." << endl;
236 cout << "******************************************************************" << endl;
237 cout << endl;
238
239 // The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure.
240 newFactors.push_back(factor: loopFactor);
241 concurrentSmoother.update(newFactors, newTheta: Values());
242 newFactors.resize(size: 0);
243
244
245 // Now run for a few more seconds so the concurrent smoother and filter have to to re-sync
246 // Continue adding odometry factors until the loop closure may be incorporated into the concurrent smoother
247 for(double time = 8.0+deltaT; time <= 15.0; time += deltaT) {
248
249 // Define the keys related to this timestamp
250 Key previousKey(1000 * (time-deltaT));
251 Key currentKey(1000 * (time));
252
253 // Assign the current key to the current timestamp
254 newTimestamps[currentKey] = time;
255
256 // Add a guess for this pose to the new values
257 // Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s]
258 // {This is not a particularly good way to guess, but this is just an example}
259 Pose2 currentPose(time * 2.0, 0.0, 0.0);
260 newValues.insert(j: currentKey, val: currentPose);
261
262 // Add odometry factors from two different sources with different error stats
263 Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02);
264 auto odometryNoise1 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.1, 0.1, 0.05));
265 newFactors.push_back(factor: BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement1, odometryNoise1));
266
267 Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01);
268 auto odometryNoise2 = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.05, 0.05, 0.05));
269 newFactors.push_back(factor: BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
270
271 // Unlike the fixed-lag versions, the concurrent filter implementation
272 // requires the user to supply the specify which keys to marginalize
273 FastList<Key> oldKeys;
274 if(time >= lag+deltaT) {
275 oldKeys.push_back(x: 1000 * (time-lag-deltaT));
276 }
277
278 // Update the various inference engines
279 concurrentFilter.update(newFactors, newTheta: newValues, keysToMove: oldKeys);
280 fixedlagSmoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
281 batchSmoother.update(newFactors, newTheta: newValues, timestamps: newTimestamps);
282
283 // Manually synchronize the Concurrent Filter and Smoother every 1.0 s
284 if(fmod(x: time, y: 1.0) < 0.01) {
285 // Synchronize the Filter and Smoother
286 concurrentSmoother.update();
287 synchronize(filter&: concurrentFilter, smoother&: concurrentSmoother);
288 cout << "******************************************************************" << endl;
289 cout << "Syncing Concurrent Filter and Smoother." << endl;
290 cout << "******************************************************************" << endl;
291 cout << endl;
292 }
293
294 // Print the optimized current pose
295 cout << setprecision(5) << "Timestamp = " << time << endl;
296 concurrentFilter.calculateEstimate<Pose2>(key: currentKey).print(s: "Concurrent Estimate: ");
297 fixedlagSmoother.calculateEstimate<Pose2>(key: currentKey).print(s: "Fixed Lag Estimate: ");
298 batchSmoother.calculateEstimate<Pose2>(key: currentKey).print(s: "Batch Estimate: ");
299 cout << endl;
300
301 // Clear contains for the next iteration
302 newTimestamps.clear();
303 newValues.clear();
304 newFactors.resize(size: 0);
305 }
306
307
308 // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
309 cout << "After 15.0 seconds, each version contains to the following keys:" << endl;
310 cout << " Concurrent Filter Keys: " << endl;
311 for(const auto key: concurrentFilter.getLinearizationPoint().keys()) {
312 cout << setprecision(5) << " Key: " << key << endl;
313 }
314 cout << " Concurrent Smoother Keys: " << endl;
315 for(const auto key: concurrentSmoother.getLinearizationPoint().keys()) {
316 cout << setprecision(5) << " Key: " << key << endl;
317 }
318 cout << " Fixed-Lag Smoother Keys: " << endl;
319 for(const auto& key_timestamp: fixedlagSmoother.timestamps()) {
320 cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
321 }
322 cout << " Batch Smoother Keys: " << endl;
323 for(const auto& key_timestamp: batchSmoother.timestamps()) {
324 cout << setprecision(5) << " Key: " << key_timestamp.first << endl;
325 }
326
327 return 0;
328}
329