12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Re-enabled summarization test/implementation. Sequential versions re-implemented, but tests don't pass

release/4.3a0
Alex Cunningham 2013-08-09 19:59:14 +00:00
parent 5b7d7b3793
commit 073ea4fa0f
3 changed files with 50 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam/nonlinear/summarization.cpp
View File

@ -5,8 +5,6 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#if 0
#include <gtsam/nonlinear/summarization.h> #include <gtsam/nonlinear/summarization.h>
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
@ -15,35 +13,29 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<GaussianFactorGraph,Ordering> GaussianFactorGraph summarize(const NonlinearFactorGraph& graph, const Values& values,
summarize(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode) { const KeySet& saved_keys, SummarizationMode mode) {
const size_t nrEliminatedKeys = values.size() - saved_keys.size(); 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 we aren't eliminating anything, linearize and return
if (!nrEliminatedKeys || saved_keys.empty()) { if (!nrEliminatedKeys || saved_keys.empty())
Ordering ordering = *values.orderingArbitrary(); return full_graph;
GaussianFactorGraph linear_graph = *graph.linearize(values, ordering);
return make_pair(linear_graph, ordering);
}
// Compute a constrained ordering with variables grouped to end std::vector<Key> saved_keys_vec(saved_keys.begin(), saved_keys.end());
std::map<gtsam::Key, int> ordering_constraints;
// group all saved variables together // // Compute a constrained ordering with variables grouped to end
BOOST_FOREACH(const gtsam::Key& key, saved_keys) // std::map<gtsam::Key, int> ordering_constraints;
ordering_constraints.insert(make_pair(key, 1)); //
// // group all saved variables together
Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints); // 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 // Linearize the system
GaussianFactorGraph full_graph = *graph.linearize(values, ordering);
GaussianFactorGraph summarized_system; GaussianFactorGraph summarized_system;
std::vector<Index> 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_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 // 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_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) { switch (mode) {
case PARTIAL_QR: { case PARTIAL_QR: {
summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second); // summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second);
break; break;
} }
case PARTIAL_CHOLESKY: { case PARTIAL_CHOLESKY: {
summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second); // summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second);
break; break;
} }
case SEQUENTIAL_QR: { case SEQUENTIAL_QR: {
GaussianSequentialSolver solver(full_graph, true); summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateQR));
summarized_system.push_back(*solver.jointFactorGraph(indices)); // GaussianSequentialSolver solver(full_graph, true);
// summarized_system.push_back(*solver.jointFactorGraph(indices));
break; break;
} }
case SEQUENTIAL_CHOLESKY: { case SEQUENTIAL_CHOLESKY: {
GaussianSequentialSolver solver(full_graph, false); summarized_system.push_back(*full_graph.marginal(saved_keys_vec, EliminateCholesky));
summarized_system.push_back(*solver.jointFactorGraph(indices)); // GaussianSequentialSolver solver(full_graph, false);
// summarized_system.push_back(*solver.jointFactorGraph(indices));
break; break;
} }
} }
return make_pair(summarized_system, ordering); return summarized_system;
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph summarizeAsNonlinearContainer( NonlinearFactorGraph summarizeAsNonlinearContainer(
const NonlinearFactorGraph& graph, const Values& values, const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode) { const KeySet& saved_keys, SummarizationMode mode) {
GaussianFactorGraph summarized_graph; GaussianFactorGraph summarized_graph = summarize(graph, values, saved_keys, mode);
Ordering ordering; return LinearContainerFactor::convertLinearGraph(summarized_graph);
boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode);
return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // \namespace gtsam
#endif

11
gtsam/nonlinear/summarization.h
View File

@ -9,13 +9,9 @@
#pragma once #pragma once
#if 0
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam { namespace gtsam {
@ -40,9 +36,8 @@ typedef enum {
* @param mode controls what elimination technique and requirements to use * @param mode controls what elimination technique and requirements to use
* @return a pair of the remaining graph and the ordering used for linearization * @return a pair of the remaining graph and the ordering used for linearization
*/ */
std::pair<GaussianFactorGraph,Ordering> GTSAM_EXPORT GaussianFactorGraph GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph,
summarize(const NonlinearFactorGraph& graph, const Values& values, const Values& values, const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
/** /**
* Performs the same summarization technique used in summarize(), but returns the * 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); const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
} // \namespace gtsam } // \namespace gtsam
#endif

66
tests/testSummarization.cpp
View File

@ -9,8 +9,6 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#if 0
#include <boost/assign/std/set.hpp> #include <boost/assign/std/set.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
@ -78,23 +76,23 @@ TEST( testSummarization, example_from_ddf1 ) {
{ {
// Summarize to a linear system // Summarize to a linear system
GaussianFactorGraph actLinGraph; Ordering actOrdering; GaussianFactorGraph actLinGraph;
SummarizationMode mode = PARTIAL_QR; 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; // Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering)); // EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible // Does not split out subfactors where possible
GaussianFactorGraph expLinGraph; GaussianFactorGraph expLinGraph;
expLinGraph += JacobianFactor( expLinGraph += JacobianFactor(
expSumOrdering[lA3], lA3,
Matrix_(4,2, Matrix_(4,2,
0.595867, 0.605092, 0.595867, 0.605092,
0.0, -0.406109, 0.0, -0.406109,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0), 0.0, 0.0),
expSumOrdering[lA5], lA5,
Matrix_(4,2, Matrix_(4,2,
-0.125971, -0.160052, -0.125971, -0.160052,
0.13586, 0.301096, 0.13586, 0.301096,
@ -105,30 +103,27 @@ TEST( testSummarization, example_from_ddf1 ) {
// Summarize directly from a nonlinear graph to another nonlinear graph // Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); 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)); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
} }
{ {
// Summarize to a linear system using cholesky - compare to previous version // Summarize to a linear system using cholesky - compare to previous version
GaussianFactorGraph actLinGraph; Ordering actOrdering; GaussianFactorGraph actLinGraph;
SummarizationMode mode = PARTIAL_CHOLESKY; SummarizationMode mode = PARTIAL_CHOLESKY;
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));
// Does not split out subfactors where possible // Does not split out subfactors where possible
GaussianFactorGraph expLinGraph; GaussianFactorGraph expLinGraph;
expLinGraph += HessianFactor(JacobianFactor( expLinGraph += HessianFactor(JacobianFactor(
expSumOrdering[lA3], lA3,
Matrix_(4,2, Matrix_(4,2,
0.595867, 0.605092, 0.595867, 0.605092,
0.0, -0.406109, 0.0, -0.406109,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0), 0.0, 0.0),
expSumOrdering[lA5], lA5,
Matrix_(4,2, Matrix_(4,2,
-0.125971, -0.160052, -0.125971, -0.160052,
0.13586, 0.301096, 0.13586, 0.301096,
@ -139,35 +134,31 @@ TEST( testSummarization, example_from_ddf1 ) {
// Summarize directly from a nonlinear graph to another nonlinear graph // Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); 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)); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
} }
{ {
// Summarize to a linear system with joint factor graph version // Summarize to a linear system with joint factor graph version
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = SEQUENTIAL_QR; SummarizationMode mode = SEQUENTIAL_QR;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible // Does not split out subfactors where possible
GaussianFactorGraph expLinGraph; GaussianFactorGraph expLinGraph;
expLinGraph += JacobianFactor( expLinGraph += JacobianFactor(
expSumOrdering[lA3], lA3,
Matrix_(2,2, Matrix_(2,2,
0.595867, 0.605092, 0.595867, 0.605092,
0.0, 0.406109), 0.0, 0.406109),
expSumOrdering[lA5], lA5,
Matrix_(2,2, Matrix_(2,2,
-0.125971, -0.160052, -0.125971, -0.160052,
-0.13586, -0.301096), -0.13586, -0.301096),
zero(2), diagmodel2); zero(2), diagmodel2);
expLinGraph += JacobianFactor( expLinGraph += JacobianFactor(
expSumOrdering[lA5], lA5,
Matrix_(2,2, Matrix_(2,2,
0.268667, 0.31703, 0.268667, 0.31703,
0.0, 0.131698), 0.0, 0.131698),
@ -177,35 +168,31 @@ TEST( testSummarization, example_from_ddf1 ) {
// Summarize directly from a nonlinear graph to another nonlinear graph // Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); 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)); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
} }
{ {
// Summarize to a linear system with joint factor graph version // Summarize to a linear system with joint factor graph version
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = SEQUENTIAL_CHOLESKY; SummarizationMode mode = SEQUENTIAL_CHOLESKY;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible // Does not split out subfactors where possible
GaussianFactorGraph expLinGraph; GaussianFactorGraph expLinGraph;
expLinGraph += JacobianFactor( expLinGraph += JacobianFactor(
expSumOrdering[lA3], lA3,
Matrix_(2,2, Matrix_(2,2,
0.595867, 0.605092, 0.595867, 0.605092,
0.0, 0.406109), 0.0, 0.406109),
expSumOrdering[lA5], lA5,
Matrix_(2,2, Matrix_(2,2,
-0.125971, -0.160052, -0.125971, -0.160052,
-0.13586, -0.301096), -0.13586, -0.301096),
zero(2), diagmodel2); zero(2), diagmodel2);
expLinGraph += JacobianFactor( expLinGraph += JacobianFactor(
expSumOrdering[lA5], lA5,
Matrix_(2,2, Matrix_(2,2,
0.268667, 0.31703, 0.268667, 0.31703,
0.0, 0.131698), 0.0, 0.131698),
@ -215,7 +202,7 @@ TEST( testSummarization, example_from_ddf1 ) {
// Summarize directly from a nonlinear graph to another nonlinear graph // Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); 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)); 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)); values.insert(key, Pose2(0.0, 0.0, 0.1));
SummarizationMode mode = SEQUENTIAL_CHOLESKY; SummarizationMode mode = SEQUENTIAL_CHOLESKY;
GaussianFactorGraph actLinGraph; Ordering actOrdering; GaussianFactorGraph actLinGraph = summarize(graph, values, saved_keys, mode);
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); GaussianFactorGraph expLinGraph = *graph.linearize(values);
Ordering expOrdering; expOrdering += key;
GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering);
EXPECT(assert_equal(expOrdering, actOrdering));
EXPECT(assert_equal(expLinGraph, actLinGraph)); EXPECT(assert_equal(expLinGraph, actLinGraph));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */