Added ISAM2::marginalCovariance function.

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_));

gtsam/nonlinear/ISAM2.h

@@ -585,6 +585,9 @@ public:
   template<class VALUE>
   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
   /// @{
 

tests/testGaussianISAM2.cpp

@@ -18,6 +18,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
@@ -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);}
 /* ************************************************************************* */