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>
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<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
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<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);
}
/* ************************************************************************* */
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);
}

14
gtsam/nonlinear/summarization.h
View File

@ -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<GaussianFactorGraph,Ordering> 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

6
tests/testSummarization.cpp
View File

@ -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));