Caught corner case in summarization causing ccolamd to segfault

gtsam/nonlinear/summarization.cpp

@@ -18,18 +18,13 @@ std::pair<GaussianFactorGraph,Ordering>
 summarize(const NonlinearFactorGraph& graph, const Values& values,
     const KeySet& saved_keys, SummarizationMode mode) {
   const size_t nrEliminatedKeys = values.size() - saved_keys.size();
-  //  // compute a new ordering with non-saved keys first
-  //  Ordering ordering;
-  //  KeySet eliminatedKeys;
-  //  BOOST_FOREACH(const Key& key, values.keys()) {
-  //    if (!saved_keys.count(key)) {
-  //      eliminatedKeys.insert(key);
-  //      ordering += key;
-  //    }
-  //  }
-  //
-  //  BOOST_FOREACH(const Key& key, saved_keys)
-  //    ordering += key;
+
+  // If we aren't eliminating anything, linearize and return
+  if (!nrEliminatedKeys || saved_keys.empty()) {
+    Ordering ordering = *values.orderingArbitrary();
+    GaussianFactorGraph linear_graph = *graph.linearize(values, ordering);
+    return make_pair(linear_graph, ordering);
+  }
 
   // Compute a constrained ordering with variables grouped to end
   std::map<gtsam::Key, int> ordering_constraints;

gtsam/nonlinear/summarization.h

@@ -29,6 +29,9 @@ typedef enum {
  * Summarization function to remove a subset of variables from a system with the
  * sequential solver. This does not require that the system be fully constrained.
  *
+ * Requirement: set of keys in the graph should match the set of keys in the
+ * values structure.
+ *
  * @param graph A full nonlinear graph
  * @param values The chosen linearization point
  * @param saved_keys is the set of keys for variables that should remain

tests/testSummarization.cpp

@@ -34,17 +34,16 @@ typedef gtsam::PriorFactor<gtsam::Pose2> PosePrior;
 typedef gtsam::BetweenFactor<gtsam::Pose2> PoseBetween;
 typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2> PosePointBearingRange;
 
+gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
+gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);
+
 /* ************************************************************************* */
 TEST( testSummarization, example_from_ddf1 ) {
-
   Key xA0 = LabeledSymbol('x', 'A', 0),
       xA1 = LabeledSymbol('x', 'A', 1),
       xA2 = LabeledSymbol('x', 'A', 2);
   Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5);
 
-  gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
-  gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);
-
   SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2);
   SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4);
 
@@ -218,6 +217,26 @@ TEST( testSummarization, example_from_ddf1 ) {
   }
 }
 
+/* ************************************************************************* */
+TEST( testSummarization, no_summarize_case ) {
+  // Checks a corner case in which no variables are being eliminated
+  gtsam::Key key = 7;
+  gtsam::KeySet saved_keys; saved_keys.insert(key);
+  NonlinearFactorGraph graph;
+  graph.add(PosePrior(key, Pose2(1.0, 2.0, 0.3), model3));
+  graph.add(PosePrior(key, Pose2(2.0, 3.0, 0.4), model3));
+  Values values;
+  values.insert(key, Pose2(0.0, 0.0, 0.1));
+
+  SummarizationMode mode = SEQUENTIAL_CHOLESKY;
+  GaussianFactorGraph actLinGraph; Ordering actOrdering;
+  boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
+  Ordering expOrdering; expOrdering += key;
+  GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering);
+  EXPECT(assert_equal(expOrdering, actOrdering));
+  EXPECT(assert_equal(expLinGraph, actLinGraph));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */