diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index bc18abfb4..dc764c164 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -1018,6 +1018,13 @@ Values ISAM2::calculateEstimate() const { return ret; } +/* ************************************************************************* */ +Matrix ISAM2::marginalCovariance(Index key) const { + return marginalFactor(ordering_[key], + params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) + ->information().inverse(); +} + /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3015a51c8..687723392 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -585,6 +585,9 @@ public: template VALUE calculateEstimate(Key key) const; + /** Return marginal on any variable as a covariance matrix */ + GTSAM_EXPORT Matrix marginalCovariance(Index key) const; + /// @name Public members for non-typical usage /// @{ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f3f26900e..aa169c3a0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -1027,6 +1028,18 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves5) EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); } +/* ************************************************************************* */ +TEST(ISAM2, marginalCovariance) +{ + // Create isam2 + ISAM2 isam = createSlamlikeISAM2(); + + // Check marginal + Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5); + Matrix actual = isam.marginalCovariance(5); + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */