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

Changed interface to allow for different modes

release/4.3a0
Alex Cunningham 2013-05-08 10:14:10 +00:00
parent acd6e629e8
commit c74de0136c
3 changed files with 67 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
gtsam/nonlinear/summarization.cpp
View File

@ -16,38 +16,73 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<GaussianFactorGraph,Ordering> std::pair<GaussianFactorGraph,Ordering>
summarize(const NonlinearFactorGraph& graph, const Values& values, summarize(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, bool useQR) { const KeySet& saved_keys, SummarizationMode mode) {
// compute a new ordering with non-saved keys first const size_t nrEliminatedKeys = values.size() - saved_keys.size();
Ordering ordering; // // compute a new ordering with non-saved keys first
KeySet eliminatedKeys; // Ordering ordering;
BOOST_FOREACH(const Key& key, values.keys()) { // KeySet eliminatedKeys;
if (!saved_keys.count(key)) { // BOOST_FOREACH(const Key& key, values.keys()) {
eliminatedKeys.insert(key); // if (!saved_keys.count(key)) {
ordering += key; // eliminatedKeys.insert(key);
} // ordering += key;
} // }
// }
//
// BOOST_FOREACH(const Key& key, saved_keys)
// ordering += key;
BOOST_FOREACH(const Key& key, saved_keys) // Compute a constrained ordering with variables grouped to end
ordering += key; std::map<gtsam::Key, int> 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 // Linearize the system
GaussianFactorGraph full_graph = *graph.linearize(values, ordering); GaussianFactorGraph full_graph = *graph.linearize(values, ordering);
GaussianFactorGraph summarized_system; 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<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_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); return make_pair(summarized_system, ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph summarizeAsNonlinearContainer( NonlinearFactorGraph summarizeAsNonlinearContainer(
const NonlinearFactorGraph& graph, const Values& values, const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, bool useQR) { const KeySet& saved_keys, SummarizationMode mode) {
GaussianFactorGraph summarized_graph; GaussianFactorGraph summarized_graph;
Ordering ordering; 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); return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering);
} }

14
gtsam/nonlinear/summarization.h
View File

@ -17,6 +17,14 @@
namespace gtsam { 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 * 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. * 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 graph A full nonlinear graph
* @param values The chosen linearization point * @param values The chosen linearization point
* @param saved_keys is the set of keys for variables that should remain * @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 * @return a pair of the remaining graph and the ordering used for linearization
*/ */
std::pair<GaussianFactorGraph,Ordering> GTSAM_EXPORT std::pair<GaussianFactorGraph,Ordering> GTSAM_EXPORT
summarize(const NonlinearFactorGraph& graph, const Values& values, 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 * 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( NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer(
const NonlinearFactorGraph& graph, const Values& values, const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, bool useQR = true); const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);
} // \namespace gtsam } // \namespace gtsam

6
tests/testSummarization.cpp
View File

@ -74,8 +74,8 @@ TEST( testSummarization, example_from_ddf1 ) {
// Summarize to a linear system // Summarize to a linear system
GaussianFactorGraph actLinGraph; Ordering actOrdering; GaussianFactorGraph actLinGraph; Ordering actOrdering;
bool useQR = true; SummarizationMode mode = PARTIAL_QR;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, useQR); boost::tie(actLinGraph, actOrdering) = 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));
@ -99,7 +99,7 @@ TEST( testSummarization, example_from_ddf1 ) {
EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// 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, useQR); NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));