Added version of calculateEstimate(const KEY&) to calculate only a single variable

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 {

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
   //@{