From c74de0136cafe112a03de3da96bcac0889e08f35 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 8 May 2013 10:14:10 +0000 Subject: [PATCH] Changed interface to allow for different modes --- gtsam/nonlinear/summarization.cpp | 71 +++++++++++++++++++++++-------- gtsam/nonlinear/summarization.h | 14 ++++-- tests/testSummarization.cpp | 6 +-- 3 files changed, 67 insertions(+), 24 deletions(-) diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index 7b4aae485..5722f9c2c 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -16,38 +16,73 @@ namespace gtsam { /* ************************************************************************* */ std::pair summarize(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, bool useQR) { - // 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; - } - } + 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; - BOOST_FOREACH(const Key& key, saved_keys) - ordering += key; + // Compute a constrained ordering with variables grouped to end + std::map ordering_constraints; + + // group all saved variables together + BOOST_FOREACH(const gtsam::Key& key, saved_keys) + ordering_constraints.insert(make_pair(key, 1)); + + Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints); // Linearize the system GaussianFactorGraph full_graph = *graph.linearize(values, ordering); GaussianFactorGraph summarized_system; - if (useQR) - summarized_system.push_back(EliminateQR(full_graph, eliminatedKeys.size()).second); - else - summarized_system.push_back(EliminateCholesky(full_graph, eliminatedKeys.size()).second); + std::vector indices; + BOOST_FOREACH(const Key& k, saved_keys) + indices.push_back(ordering[k]); + + // PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system + // PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system + // SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system) + // SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system) + + switch (mode) { + case PARTIAL_QR: { + summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); + break; + } + case PARTIAL_CHOLESKY: { + summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second); + break; + } + case SEQUENTIAL_QR: { + GaussianSequentialSolver solver(full_graph, true); + summarized_system.push_back(*solver.jointFactorGraph(indices)); + break; + } + case SEQUENTIAL_CHOLESKY: { + GaussianSequentialSolver solver(full_graph, false); + summarized_system.push_back(*solver.jointFactorGraph(indices)); + break; + } + } return make_pair(summarized_system, ordering); } /* ************************************************************************* */ NonlinearFactorGraph summarizeAsNonlinearContainer( const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, bool useQR) { + const KeySet& saved_keys, SummarizationMode mode) { GaussianFactorGraph summarized_graph; Ordering ordering; - boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, useQR); + boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode); return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering); } diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index 9d3c7aa17..43eb1d089 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -17,6 +17,14 @@ namespace gtsam { +// Flag to control how summarization should be performed +typedef enum { + PARTIAL_QR = 0, /// Uses QR solver to eliminate, does not require fully constrained system + PARTIAL_CHOLESKY = 1, /// Uses Cholesky solver, does not require fully constrained system + SEQUENTIAL_QR = 2, /// Uses QR to compute full joint graph (needs fully constrained system) + SEQUENTIAL_CHOLESKY = 3 /// Uses Cholesky to compute full joint graph (needs fully constrained system) +} SummarizationMode; + /** * 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. @@ -24,12 +32,12 @@ namespace gtsam { * @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 - * @param useQR uses QR as the elimination algorithm if true, Cholesky otherwise + * @param mode controls what elimination technique and requirements to use * @return a pair of the remaining graph and the ordering used for linearization */ std::pair GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, bool useQR = true); + const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); /** * Performs the same summarization technique used in summarize(), but returns the @@ -43,7 +51,7 @@ summarize(const NonlinearFactorGraph& graph, const Values& values, */ NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer( const NonlinearFactorGraph& graph, const Values& values, - const KeySet& saved_keys, bool useQR = true); + const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); } // \namespace gtsam diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index 5204bfe1c..99cb40cd2 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -74,8 +74,8 @@ TEST( testSummarization, example_from_ddf1 ) { // Summarize to a linear system GaussianFactorGraph actLinGraph; Ordering actOrdering; - bool useQR = true; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, useQR); + SummarizationMode mode = PARTIAL_QR; + boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; EXPECT(assert_equal(expSumOrdering, actOrdering)); @@ -99,7 +99,7 @@ TEST( testSummarization, example_from_ddf1 ) { EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); // Summarize directly from a nonlinear graph to another nonlinear graph - NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, useQR); + NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));