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

Added a new summarization convenience function and cleaned up documentation

release/4.3a0
Alex Cunningham 2013-04-26 18:27:22 +00:00
parent 34730a92cc
commit 224be276e4
3 changed files with 59 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam_unstable/gtsam_unstable.h
View File

@ -424,7 +424,11 @@ gtsam::GaussianFactorGraph* summarizeGraphSequential(
pair<gtsam::GaussianFactorGraph,gtsam::Ordering>
partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
const gtsam::KeySet& overlap_keys);
const gtsam::KeySet& saved_keys);
pair<gtsam::GaussianFactorGraph,gtsam::Ordering>
sequentialSummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
const gtsam::KeySet& saved_keys);
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
class FixedLagSmootherKeyTimestampMapValue {

32
gtsam_unstable/nonlinear/summarization.cpp
View File

@ -31,19 +31,43 @@ GaussianFactorGraph::shared_ptr summarizeGraphSequential(
/* ************************************************************************* */
std::pair<GaussianFactorGraph,Ordering>
partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& overlap_keys) {
sequentialSummarization(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys) {
// compute a new ordering with non-saved keys first
Ordering ordering;
KeySet eliminatedKeys;
BOOST_FOREACH(const Key& key, values.keys()) {
if (!overlap_keys.count(key)) {
if (!saved_keys.count(key)) {
eliminatedKeys.insert(key);
ordering += key;
}
}
BOOST_FOREACH(const Key& key, overlap_keys)
BOOST_FOREACH(const Key& key, saved_keys)
ordering += key;
// Linearize the system
GaussianFactorGraph full_graph = *graph.linearize(values, ordering);
GaussianFactorGraph summarized_system;
summarized_system.push_back(EliminateQR(full_graph, eliminatedKeys.size()).second);
return make_pair(summarized_system, ordering);
}
/* ************************************************************************* */
std::pair<GaussianFactorGraph,Ordering>
partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys) {
// 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;
// reorder the system

40
gtsam_unstable/nonlinear/summarization.h
View File

@ -17,9 +17,32 @@
namespace gtsam {
// Sequential graph summarization
typedef FactorGraph<JacobianFactor> JacobianGraph;
typedef boost::shared_ptr<JacobianGraph> shared_jacobianGraph;
/**
* 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.
*
* @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
* @return a pair of the remaining graph and the ordering used for linearization
*/
std::pair<GaussianFactorGraph,Ordering> GTSAM_UNSTABLE_EXPORT
sequentialSummarization(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys);
/**
* Summarization function to remove a subset of variables from a system using
* a partial cholesky approach. This does not require that the system be fully constrained.
* Performs linearization to apply an ordering.
*
* @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
* * @return a pair of the remaining graph and the ordering used for linearization
*/
std::pair<GaussianFactorGraph,Ordering> GTSAM_UNSTABLE_EXPORT
partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys);
/**
* Summarization function that eliminates a set of variables (does not convert to Jacobians)
@ -33,17 +56,6 @@ GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const Ordering& ordering,
const KeySet& saved_keys, bool useQR = false);
/**
* Summarization function to remove a subset of variables from a system using
* a partial cholesky approach. This does not require that the system be fully constrained.
*
* Performs linearization to apply an ordering
*/
std::pair<GaussianFactorGraph,Ordering> GTSAM_UNSTABLE_EXPORT
partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& overlap_keys);
} // \namespace gtsam