59 using namespace gtsam;
62 int main(
int argc,
char** argv) {
90 newTimestamps[priorKey] = 0.0;
102 newTimestamps[currentKey] =
time;
108 newValues.
insert(currentKey, currentPose);
111 Pose2 odometryMeasurement1 =
Pose2(0.61, -0.08, 0.02);
112 auto odometryNoise1 = noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 0.05));
115 Pose2 odometryMeasurement2 =
Pose2(0.47, 0.03, 0.01);
116 auto odometryNoise2 = noiseModel::Diagonal::Sigmas(
Vector3(0.05, 0.05, 0.05));
127 concurrentFilter.
update(newFactors, newValues, oldKeys);
128 fixedlagSmoother.
update(newFactors, newValues, newTimestamps);
129 batchSmoother.
update(newFactors, newValues, newTimestamps);
134 concurrentSmoother.
update();
139 cout << setprecision(5) <<
"Timestamp = " <<
time << endl;
146 newTimestamps.clear();
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;
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(
Vector3(0.5, 0.5, 0.25));
181 newTimestamps[currentKey] =
time;
187 newValues.
insert(currentKey, currentPose);
190 Pose2 odometryMeasurement1 =
Pose2(0.61, -0.08, 0.02);
191 auto odometryNoise1 = noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 0.05));
194 Pose2 odometryMeasurement2 =
Pose2(0.47, 0.03, 0.01);
195 auto odometryNoise2 = noiseModel::Diagonal::Sigmas(
Vector3(0.05, 0.05, 0.05));
206 concurrentFilter.
update(newFactors, newValues, oldKeys);
207 fixedlagSmoother.
update(newFactors, newValues, newTimestamps);
208 batchSmoother.
update(newFactors, newValues, newTimestamps);
213 concurrentSmoother.
update();
218 cout << setprecision(5) <<
"Timestamp = " <<
time << endl;
225 newTimestamps.clear();
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;
254 newTimestamps[currentKey] =
time;
260 newValues.
insert(currentKey, currentPose);
263 Pose2 odometryMeasurement1 =
Pose2(0.61, -0.08, 0.02);
264 auto odometryNoise1 = noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 0.05));
267 Pose2 odometryMeasurement2 =
Pose2(0.47, 0.03, 0.01);
268 auto odometryNoise2 = noiseModel::Diagonal::Sigmas(
Vector3(0.05, 0.05, 0.05));
279 concurrentFilter.
update(newFactors, newValues, oldKeys);
280 fixedlagSmoother.
update(newFactors, newValues, newTimestamps);
281 batchSmoother.
update(newFactors, newValues, newTimestamps);
286 concurrentSmoother.
update();
288 cout <<
"******************************************************************" << endl;
289 cout <<
"Syncing Concurrent Filter and Smoother." << endl;
290 cout <<
"******************************************************************" << endl;
295 cout << setprecision(5) <<
"Timestamp = " <<
time << endl;
302 newTimestamps.clear();
309 cout <<
"After 15.0 seconds, each version contains to the following keys:" << endl;
310 cout <<
" Concurrent Filter Keys: " << endl;
312 cout << setprecision(5) <<
" Key: " <<
key << endl;
314 cout <<
" Concurrent Smoother Keys: " << endl;
316 cout << setprecision(5) <<
" Key: " <<
key << endl;
318 cout <<
" Fixed-Lag Smoother Keys: " << endl;
319 for(
const auto& key_timestamp: fixedlagSmoother.
timestamps()) {
320 cout << setprecision(5) <<
" Key: " << key_timestamp.first << endl;
322 cout <<
" Batch Smoother Keys: " << endl;
323 for(
const auto& key_timestamp: batchSmoother.
timestamps()) {
324 cout << setprecision(5) <<
" Key: " << key_timestamp.first << endl;