diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index 5722f9c2c..acd222d43 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -18,18 +18,13 @@ std::pair 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 ordering_constraints; diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index faa2003ef..c25d5d934 100644 --- a/gtsam/nonlinear/summarization.h +++ b/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 diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index 2f3cfdf03..bf7342202 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -34,17 +34,16 @@ typedef gtsam::PriorFactor PosePrior; typedef gtsam::BetweenFactor PoseBetween; typedef gtsam::BearingRangeFactor 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); } /* ************************************************************************* */