diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 6ff22c3a1..32a27f6fb 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -593,11 +593,11 @@ Values ISAM2::calculateEstimate() const { /* ************************************************************************* */ template -template -typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { +template +VALUE ISAM2::calculateEstimate(Key key) const { const Index index = getOrdering()[key]; const SubVector delta = getDelta()[index]; - return getLinearizationPoint()[key].retract(delta); + return theta_.at(key).retract(delta); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 4c8df2997..4a4bdea2c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -390,8 +390,8 @@ public: * @param key * @return */ - template - typename KEY::Value calculateEstimate(const KEY& key) const; + template + VALUE calculateEstimate(Key key) const; /// @name Public members for non-typical usage //@{