From 2f13d0b961594a8309761eb9107f7d55f57e81ba Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 15 Nov 2011 17:06:34 +0000
Subject: [PATCH] Added version of calculateEstimate(const KEY&) to calculate
 only a single variable

---
 gtsam/nonlinear/ISAM2-inl.h |  9 +++++++++
 gtsam/nonlinear/ISAM2.h     | 12 +++++++++++-
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h
index 8a724cbcf..b5dfea527 100644
--- a/gtsam/nonlinear/ISAM2-inl.h
+++ b/gtsam/nonlinear/ISAM2-inl.h
@@ -564,6 +564,15 @@ Values ISAM2<Conditional, Values>::calculateEstimate() const {
   return ret;
 }
 
+/* ************************************************************************* */
+template<class CONDITIONAL, class VALUES>
+template<class KEY>
+typename KEY::Value ISAM2<CONDITIONAL, VALUES>::calculateEstimate(const KEY& key) const {
+  const Index index = getOrdering()[key];
+  const SubVector delta = getDelta()[index];
+  return getLinearizationPoint()[key].retract(delta);
+}
+
 /* ************************************************************************* */
 template<class Conditional, class Values>
 Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index 8159ebfb5..67335648e 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -205,10 +205,20 @@ public:
   const VALUES& getLinearizationPoint() const {return theta_;}
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
-   * This delta is incomplete because it was not updated below wildfire_threshold.
+   * This delta is incomplete because it was not updated below wildfire_threshold.  If only
+   * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
    */
   VALUES calculateEstimate() const;
 
+  /** Compute an estimate for a single variable using its incomplete linear delta computed
+   * during the last update.  This is faster than calling the no-argument version of
+   * calculateEstimate, which operates on all variables.
+   * @param key
+   * @return
+   */
+  template<class KEY>
+  typename KEY::Value calculateEstimate(const KEY& key) const;
+
   /// @name Public members for non-typical usage
   //@{