From 073ea4fa0f25a3c3b0db55f451245bac7cb12cfa Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 9 Aug 2013 19:59:14 +0000 Subject: [PATCH] Re-enabled summarization test/implementation. Sequential versions re-implemented, but tests don't pass --- gtsam/nonlinear/summarization.cpp | 57 +++++++++++--------------- gtsam/nonlinear/summarization.h | 11 +----- tests/testSummarization.cpp | 66 +++++++++++-------------------- 3 files changed, 50 insertions(+), 84 deletions(-) diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index 4f9c7d126..e38226c9b 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -5,8 +5,6 @@ * @author Alex Cunningham */ -#if 0 - #include #include @@ -15,35 +13,29 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -std::pair -summarize(const NonlinearFactorGraph& graph, const Values& values, +GaussianFactorGraph summarize(const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, SummarizationMode mode) { const size_t nrEliminatedKeys = values.size() - saved_keys.size(); + GaussianFactorGraph full_graph = *graph.linearize(values); // 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); - } + if (!nrEliminatedKeys || saved_keys.empty()) + return full_graph; - // Compute a constrained ordering with variables grouped to end - std::map ordering_constraints; + std::vector saved_keys_vec(saved_keys.begin(), saved_keys.end()); - // 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); +// // 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; - 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) @@ -51,38 +43,37 @@ summarize(const NonlinearFactorGraph& graph, const Values& values, switch (mode) { case PARTIAL_QR: { - summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); +// summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); break; } case PARTIAL_CHOLESKY: { - summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second); +// 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)); + summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateQR)); +// 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)); + summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateCholesky)); +// GaussianSequentialSolver solver(full_graph, false); +// summarized_system.push_back(*solver.jointFactorGraph(indices)); break; } } - return make_pair(summarized_system, ordering); + return summarized_system; } /* ************************************************************************* */ NonlinearFactorGraph summarizeAsNonlinearContainer( const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, SummarizationMode mode) { - GaussianFactorGraph summarized_graph; - Ordering ordering; - boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode); - return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering); + GaussianFactorGraph summarized_graph = summarize(graph, values, saved_keys, mode); + return LinearContainerFactor::convertLinearGraph(summarized_graph); } /* ************************************************************************* */ } // \namespace gtsam -#endif diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index 75f09074d..cfed16330 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -9,13 +9,9 @@ #pragma once -#if 0 - #include #include - #include -#include namespace gtsam { @@ -40,9 +36,8 @@ typedef enum { * @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, SummarizationMode mode = PARTIAL_QR); +GaussianFactorGraph GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph, + const Values& values, const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); /** * Performs the same summarization technique used in summarize(), but returns the @@ -59,5 +54,3 @@ NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer( const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR); } // \namespace gtsam - -#endif diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index 82483359c..9db7586ef 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -9,8 +9,6 @@ #include -#if 0 - #include #include @@ -78,23 +76,23 @@ TEST( testSummarization, example_from_ddf1 ) { { // Summarize to a linear system - GaussianFactorGraph actLinGraph; Ordering actOrdering; + GaussianFactorGraph actLinGraph; SummarizationMode mode = PARTIAL_QR; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); + actLinGraph = summarize(graph, values, saved_keys, mode); - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); +// Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; +// EXPECT(assert_equal(expSumOrdering, actOrdering)); // Does not split out subfactors where possible GaussianFactorGraph expLinGraph; expLinGraph += JacobianFactor( - expSumOrdering[lA3], + lA3, Matrix_(4,2, 0.595867, 0.605092, 0.0, -0.406109, 0.0, 0.0, 0.0, 0.0), - expSumOrdering[lA5], + lA5, Matrix_(4,2, -0.125971, -0.160052, 0.13586, 0.301096, @@ -105,30 +103,27 @@ TEST( testSummarization, example_from_ddf1 ) { // Summarize directly from a nonlinear graph to another nonlinear graph NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); + NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); } { // Summarize to a linear system using cholesky - compare to previous version - GaussianFactorGraph actLinGraph; Ordering actOrdering; + GaussianFactorGraph actLinGraph; SummarizationMode mode = PARTIAL_CHOLESKY; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); + actLinGraph = summarize(graph, values, saved_keys, mode); // Does not split out subfactors where possible GaussianFactorGraph expLinGraph; expLinGraph += HessianFactor(JacobianFactor( - expSumOrdering[lA3], + lA3, Matrix_(4,2, 0.595867, 0.605092, 0.0, -0.406109, 0.0, 0.0, 0.0, 0.0), - expSumOrdering[lA5], + lA5, Matrix_(4,2, -0.125971, -0.160052, 0.13586, 0.301096, @@ -139,35 +134,31 @@ TEST( testSummarization, example_from_ddf1 ) { // Summarize directly from a nonlinear graph to another nonlinear graph NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); + NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); } { // Summarize to a linear system with joint factor graph version - GaussianFactorGraph actLinGraph; Ordering actOrdering; SummarizationMode mode = SEQUENTIAL_QR; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); + GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode); // Does not split out subfactors where possible GaussianFactorGraph expLinGraph; expLinGraph += JacobianFactor( - expSumOrdering[lA3], + lA3, Matrix_(2,2, 0.595867, 0.605092, 0.0, 0.406109), - expSumOrdering[lA5], + lA5, Matrix_(2,2, -0.125971, -0.160052, -0.13586, -0.301096), zero(2), diagmodel2); expLinGraph += JacobianFactor( - expSumOrdering[lA5], + lA5, Matrix_(2,2, 0.268667, 0.31703, 0.0, 0.131698), @@ -177,35 +168,31 @@ TEST( testSummarization, example_from_ddf1 ) { // Summarize directly from a nonlinear graph to another nonlinear graph NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); + NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); } { // Summarize to a linear system with joint factor graph version - GaussianFactorGraph actLinGraph; Ordering actOrdering; SummarizationMode mode = SEQUENTIAL_CHOLESKY; - boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); - - Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; - EXPECT(assert_equal(expSumOrdering, actOrdering)); + GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode); // Does not split out subfactors where possible GaussianFactorGraph expLinGraph; expLinGraph += JacobianFactor( - expSumOrdering[lA3], + lA3, Matrix_(2,2, 0.595867, 0.605092, 0.0, 0.406109), - expSumOrdering[lA5], + lA5, Matrix_(2,2, -0.125971, -0.160052, -0.13586, -0.301096), zero(2), diagmodel2); expLinGraph += JacobianFactor( - expSumOrdering[lA5], + lA5, Matrix_(2,2, 0.268667, 0.31703, 0.0, 0.131698), @@ -215,7 +202,7 @@ TEST( testSummarization, example_from_ddf1 ) { // Summarize directly from a nonlinear graph to another nonlinear graph NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); - NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); + NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); } @@ -233,16 +220,11 @@ TEST( testSummarization, no_summarize_case ) { 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)); + GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode); + GaussianFactorGraph expLinGraph = *graph.linearize(values); EXPECT(assert_equal(expLinGraph, actLinGraph)); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */