From 1337ac8ab02daa7aab8d39259503681a173757b3 Mon Sep 17 00:00:00 2001
From: Luca <lcarlone@mit.edu>
Date: Wed, 24 Aug 2016 14:19:02 -0400
Subject: [PATCH] added method to access isam2 results in incremental fixed lag
 smoother

---
 gtsam_unstable/nonlinear/FixedLagSmoother.h              | 1 -
 gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp | 2 +-
 gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h   | 6 ++++++
 3 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h
index e57510be4..37fdce5a2 100644
--- a/gtsam_unstable/nonlinear/FixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h
@@ -68,7 +68,6 @@ public:
     }
   };
 
-
   /** default constructor */
   FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
 
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
index 98a3110aa..b4f09fc62 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
@@ -125,7 +125,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
   KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
 
   // Update iSAM2
-  ISAM2Result isamResult = isam_.update(newFactors, newTheta,
+  isamResult_ = isam_.update(newFactors, newTheta,
 		  factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys);
 
   if (debug) {
diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
index 00ad5d13b..39af8f27e 100644
--- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
+++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@@ -109,11 +109,17 @@ public:
     return isam_.marginalCovariance(key);
   }
 
+  /// Get results of latest isam2 update
+  const ISAM2Result getISAM2Result() const{ return isamResult_; }
+
 protected:
   /** An iSAM2 object used to perform inference. The smoother lag is controlled
    * by what factors are removed each iteration */
   ISAM2 isam_;
 
+  /** Store results of latest isam2 update */
+  ISAM2Result isamResult_;
+
   /** Erase any keys associated with timestamps before the provided time */
   void eraseKeysBefore(double timestamp);