From 5087e4eabcde99db73e4c94e2a6321eb352d5f4f Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Thu, 11 Apr 2013 21:10:32 +0000
Subject: [PATCH] Modified Fixed-Lag Smoothing example to call iSAM2 multiple
 times to recover the same solution as batch

---
 gtsam_unstable/examples/FixedLagSmootherExample.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
index 9b48227d9..984e40d06 100644
--- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp
+++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp
@@ -111,6 +111,11 @@ int main(int argc, char** argv) {
     // Update the smoothers with the new factors
     smootherBatch.update(newFactors, newValues, newTimestamps);
     smootherISAM2.update(newFactors, newValues, newTimestamps);
+    // The iSAM2 algorithm only performs one optimization iteration per call to update.
+    // To get equivalent results to the full batch optimization each time, we need to call update multiple times
+    for(size_t i = 0; i < 10; ++i) {
+      smootherISAM2.update();
+    }
 
     // Print the optimized current pose
     cout << setprecision(5) << "Timestamp = " << time << endl;