Added debugging function to IncrementalFixedLagSmoother

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp

@@ -22,6 +22,8 @@
 #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
 #include <gtsam/base/debug.h>
 
+#include <gtsam/linear/GaussianSequentialSolver.h>
+
 namespace gtsam {
 
 /* ************************************************************************* */
@@ -38,6 +40,96 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol
       && isam_.equals(e->isam_, tol);
 }
 
+/* ************************************************************************* */
+bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
+
+  Matrix expectedAugmentedHessian, expected3AugmentedHessian;
+
+  vector<Index> toKeep;
+
+  const Index lastVar = isam.getOrdering().size() - 1;
+
+  for (Index i = 0; i <= lastVar; ++i)
+
+    if (find(leafKeys.begin(), leafKeys.end(), isam.getOrdering().key(i))
+        == leafKeys.end())
+
+      toKeep.push_back(i);
+
+  // Calculate expected marginal from iSAM2 tree
+
+  GaussianFactorGraph isamAsGraph(isam);
+
+  GaussianSequentialSolver solver(isamAsGraph);
+
+  GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep);
+
+  expectedAugmentedHessian = marginalgfg.augmentedHessian();
+
+  //// Calculate expected marginal from cached linear factors
+
+  //assert(isam.params().cacheLinearizedFactors);
+
+  //GaussianSequentialSolver solver2(isam.linearFactors_, isam.params().factorization == ISAM2Params::QR);
+
+  //expected2AugmentedHessian = solver2.jointFactorGraph(toKeep)->augmentedHessian();
+
+  // Calculate expected marginal from original nonlinear factors
+
+  GaussianSequentialSolver solver3(
+
+      *isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint(),
+          isam.getOrdering()),
+
+      isam.params().factorization == ISAM2Params::QR);
+
+  expected3AugmentedHessian =
+      solver3.jointFactorGraph(toKeep)->augmentedHessian();
+
+  // Do marginalization
+
+  isam.experimentalMarginalizeLeaves(leafKeys);
+
+  // Check
+
+  GaussianFactorGraph actualMarginalGraph(isam);
+
+  Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
+
+  //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian();
+
+  Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize(
+
+  isam.getLinearizationPoint(), isam.getOrdering())->augmentedHessian();
+
+  assert(
+      actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite<double>)).all());
+
+  // Check full marginalization
+
+  bool treeEqual = assert_equal(expectedAugmentedHessian,
+      actualAugmentedHessian, 1e-6);
+
+  //bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
+
+  bool nonlinEqual = assert_equal(expected3AugmentedHessian,
+      actualAugmentedHessian, 1e-6);
+
+  //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6);
+
+  //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6);
+
+  actual3AugmentedHessian.bottomRightCorner(1, 1) =
+      expected3AugmentedHessian.bottomRightCorner(1, 1);
+  bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian,
+      actual3AugmentedHessian, 1e-6);
+
+  bool ok = treeEqual && /*linEqual &&*/nonlinEqual
+      && /*linCorrect &&*//*afterLinCorrect &&*/afterNonlinCorrect;
+
+  return ok;
+}
+
 /* ************************************************************************* */
 FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) {
 
@@ -94,7 +186,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
 
   // Marginalize out any needed variables
   FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
-  isam_.experimentalMarginalizeLeaves(leafKeys);
+//  isam_.experimentalMarginalizeLeaves(leafKeys);
+  checkMarginalizeLeaves(isam_, leafKeys);
 
   // Remove marginalized keys from the KeyTimestampMap
   eraseKeyTimestampMap(marginalizableKeys);