diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 7a26b0076..4f9dafacc 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -91,3 +91,11 @@ Values NonlinearISAM::estimate() { else return linPoint_; } + +/* ************************************************************************* */ +template +Matrix NonlinearISAM::marginalCovariance(const Symbol& key) { + Matrix covariance; Vector mean; + boost::tie(mean, covariance) = isam_.marginal(ordering_[key]); + return covariance; +} diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 8c6dc6bbd..2b7a8a8e4 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -77,6 +77,9 @@ public: /** replace the current ordering */ void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; } + /** find the marginal covariance for a single variable */ + Matrix marginalCovariance(const Symbol& key); + // access /** access the underlying bayes tree */