update testIncrementalFixedLagSmoother.cpp to reproduce the bug in marginalization

gtsam/inference/VariableIndex.h

@@ -87,9 +87,11 @@ class GTSAM_EXPORT VariableIndex {
   const FactorIndices& operator[](Key variable) const {
     KeyMap::const_iterator item = index_.find(variable);
     if(item == index_.end())
-      throw std::invalid_argument("Requested non-existent variable from VariableIndex");
+      throw std::invalid_argument("Requested non-existent variable '" +
+                                  DefaultKeyFormatter(variable) +
+                                  "' from VariableIndex");
     else
-    return item->second;
+      return item->second;
   }
 
   /// Return true if no factors associated with a variable

gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp

@@ -49,6 +49,41 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini
   return assert_equal(expected, actual);
 }
 
+/* ************************************************************************* */
+void PrintSymbolicTreeHelper(
+    const ISAM2Clique::shared_ptr& clique, const std::string indent = "") {
+
+  // Print the current clique
+  std::cout << indent << "P( ";
+  for(Key key: clique->conditional()->frontals()) {
+    std::cout << DefaultKeyFormatter(key) << " ";
+  }
+  if (clique->conditional()->nrParents() > 0)
+    std::cout << "| ";
+  for(Key key: clique->conditional()->parents()) {
+    std::cout << DefaultKeyFormatter(key) << " ";
+  }
+  std::cout << ")" << std::endl;
+
+  // Recursively print all of the children
+  for(const ISAM2Clique::shared_ptr& child: clique->children) {
+    PrintSymbolicTreeHelper(child, indent + " ");
+  }
+}
+
+/* ************************************************************************* */
+void PrintSymbolicTree(const ISAM2& isam,
+    const std::string& label) {
+  std::cout << label << std::endl;
+  if (!isam.roots().empty()) {
+    for(const ISAM2::sharedClique& root: isam.roots()) {
+      PrintSymbolicTreeHelper(root);
+    }
+  } else
+    std::cout << "{Empty Tree}" << std::endl;
+}
+
+
 /* ************************************************************************* */
 TEST( IncrementalFixedLagSmoother, Example )
 {
@@ -64,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example )
 
   // Create a Fixed-Lag Smoother
   typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
-  IncrementalFixedLagSmoother smoother(7.0, ISAM2Params());
+  IncrementalFixedLagSmoother smoother(9.0, ISAM2Params());
 
   // Create containers to keep the full graph
   Values fullinit;
@@ -158,6 +193,9 @@ TEST( IncrementalFixedLagSmoother, Example )
     Values newValues;
     Timestamps newTimestamps;
 
+    // Add the odometry factor twice to ensure the removeFactor test below works,
+    // where we need to keep the connectivity of the graph.
+    newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
     newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
     newValues.insert(key2, Point2(double(i)+0.1, -0.1));
     newTimestamps[key2] = double(i);
@@ -210,6 +248,10 @@ TEST( IncrementalFixedLagSmoother, Example )
 
 	  const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
 
+    std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl;
+    std::cout << "smootherFactorsBeforeRemove.size() = "
+              << smootherFactorsBeforeRemove.size() << std::endl;
+
 	  // remove factor
 	  smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
 
@@ -231,6 +273,59 @@ TEST( IncrementalFixedLagSmoother, Example )
 	    }
 	  }
   }
+
+  {
+    PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:");
+
+    i = 17;
+    while(i <= 200) {
+      Key key_0 = MakeKey(i);
+      Key key_1 = MakeKey(i-1);
+      Key key_2 = MakeKey(i-2);
+      Key key_3 = MakeKey(i-3);
+      Key key_4 = MakeKey(i-4);
+      Key key_5 = MakeKey(i-5);
+      Key key_6 = MakeKey(i-6);
+      Key key_7 = MakeKey(i-7);
+      Key key_8 = MakeKey(i-8);
+
+      NonlinearFactorGraph newFactors;
+      Values newValues;
+      Timestamps newTimestamps;
+
+      // To make a complex graph
+      newFactors.push_back(BetweenFactor<Point2>(key_1, key_0, Point2(1.0, 0.0), odometerNoise));
+      if (i % 2 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_2, key_1, Point2(1.0, 0.0), odometerNoise));
+      if (i % 3 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_3, key_2, Point2(1.0, 0.0), odometerNoise));
+      if (i % 4 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_4, key_3, Point2(1.0, 0.0), odometerNoise));
+      if (i % 5 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_5, key_4, Point2(1.0, 0.0), odometerNoise));
+      if (i % 6 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_6, key_5, Point2(1.0, 0.0), odometerNoise));
+      if (i % 7 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_7, key_6, Point2(1.0, 0.0), odometerNoise));
+      if (i % 8 == 0)
+        newFactors.push_back(BetweenFactor<Point2>(key_8, key_7, Point2(1.0, 0.0), odometerNoise));
+
+      newValues.insert(key_0, Point2(double(i)+0.1, -0.1));
+      newTimestamps[key_0] = double(i);
+
+      fullgraph.push_back(newFactors);
+      fullinit.insert(newValues);
+
+      // Update the smoother
+      smoother.update(newFactors, newValues, newTimestamps);
+
+      // Check
+      CHECK(check_smoother(fullgraph, fullinit, smoother, key_0));
+      PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i));
+
+      ++i;
+    }
+  }
 }
 
 /* ************************************************************************* */