diff --git a/gtsam.h b/gtsam.h index 4f64bf155..0160c292f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1689,6 +1689,23 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { }; // \class LinearContainerFactor +// Summarization functionality +#include +pair + +summarize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::KeySet& saved_keys, bool useQR); +pair +summarize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::KeySet& saved_keys); // Defaults to QR; + +gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( + const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::KeySet& saved_keys, bool useQR); +gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( + const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::KeySet& saved_keys); // Defaults to QR + //************************************************************************* // Nonlinear optimizers //************************************************************************* diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index c416315f4..7b4aae485 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -8,28 +8,11 @@ #include #include #include -//#include using namespace std; namespace gtsam { -///* ************************************************************************* */ -//GaussianFactorGraph::shared_ptr summarizeGraphSequential( -// const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR) { -// GaussianSequentialSolver solver(full_graph, useQR); -// return solver.jointFactorGraph(indices); -//} -// -///* ************************************************************************* */ -//GaussianFactorGraph::shared_ptr summarizeGraphSequential( -// const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) { -// std::vector indices; -// BOOST_FOREACH(const Key& k, saved_keys) -// indices.push_back(ordering[k]); -// return summarizeGraphSequential(full_graph, indices, useQR); -//} - /* ************************************************************************* */ std::pair summarize(const NonlinearFactorGraph& graph, const Values& values, @@ -58,37 +41,6 @@ summarize(const NonlinearFactorGraph& graph, const Values& values, return make_pair(summarized_system, ordering); } -///* ************************************************************************* */ -//std::pair -//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 -// GaussianFactorGraph linearSystem = *graph.linearize(values, ordering); -// -// // Eliminate frontals -// GaussianFactorGraph summarized_system; -// summarized_system.push_back(EliminateCholesky(linearSystem, eliminatedKeys.size()).second); -// return make_pair(summarized_system, ordering); -//} -// -//std::pair GTSAM_EXPORT -//summarize(const NonlinearFactorGraph& graph, const Values& values, -// const KeySet& saved_keys, bool useQR = true); - - /* ************************************************************************* */ NonlinearFactorGraph summarizeAsNonlinearContainer( const NonlinearFactorGraph& graph, const Values& values, diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index d92651235..9d3c7aa17 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -31,20 +31,6 @@ std::pair GTSAM_EXPORT summarize(const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, bool useQR = true); -///** -// * 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 GTSAM_EXPORT -//partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, -// const KeySet& saved_keys); - /** * Performs the same summarization technique used in summarize(), but returns the * result as a NonlinearFactorGraph comprised of LinearContainerFactors. @@ -59,18 +45,6 @@ NonlinearFactorGraph GTSAM_EXPORT summarizeAsNonlinearContainer( const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, bool useQR = true); -/** - * Summarization function that eliminates a set of variables (does not convert to Jacobians) - * NOTE: uses sequential solver - requires fully constrained system - */ -//GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( -// const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); -// -///** Summarization that also converts keys to indices */ -//GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( -// const GaussianFactorGraph& full_graph, const Ordering& ordering, -// const KeySet& saved_keys, bool useQR = false); - } // \namespace gtsam diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 33c3a0b6d..43155e72c 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -438,14 +438,6 @@ gtsam::GaussianFactorGraph* summarizeGraphSequential( gtsam::GaussianFactorGraph* summarizeGraphSequential( const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); -pair -partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, - const gtsam::KeySet& saved_keys); - -pair -sequentialSummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, - const gtsam::KeySet& saved_keys); - #include class FixedLagSmootherKeyTimestampMapValue { FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); diff --git a/gtsam_unstable/nonlinear/summarization.cpp b/gtsam_unstable/nonlinear/summarization.cpp new file mode 100644 index 000000000..d691574b0 --- /dev/null +++ b/gtsam_unstable/nonlinear/summarization.cpp @@ -0,0 +1,35 @@ +/** + * @file summarization.cpp + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR) { + GaussianSequentialSolver solver(full_graph, useQR); + return solver.jointFactorGraph(indices); +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) { + std::vector indices; + BOOST_FOREACH(const Key& k, saved_keys) + indices.push_back(ordering[k]); + return summarizeGraphSequential(full_graph, indices, useQR); +} + +/* ************************************************************************* */ +} // \namespace gtsam + diff --git a/gtsam_unstable/nonlinear/summarization.h b/gtsam_unstable/nonlinear/summarization.h new file mode 100644 index 000000000..4d783ebe1 --- /dev/null +++ b/gtsam_unstable/nonlinear/summarization.h @@ -0,0 +1,34 @@ +/** + * @file summarization.h + * + * @brief Types and utility functions for summarization + * + * @date Jun 22, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + +/** + * Summarization function that eliminates a set of variables (does not convert to Jacobians) + * NOTE: uses sequential solver - requires fully constrained system + */ +GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const std::vector& indices, bool useQR = false); + +/** Summarization that also converts keys to indices */ +GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential( + const GaussianFactorGraph& full_graph, const Ordering& ordering, + const KeySet& saved_keys, bool useQR = false); + +} // \namespace gtsam + + diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index 5ea83a843..5204bfe1c 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -24,15 +24,9 @@ #include #include -//#include -// -//#include - using namespace std; using namespace boost::assign; using namespace gtsam; -//using namespace ddfsam; -//using namespace planarDDF; const double tol=1e-5; @@ -51,7 +45,7 @@ TEST( testSummarization, example_from_ddf1 ) { gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2); gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); - SharedDiagonal model = noiseModel::Unit::Create(2); + SharedDiagonal model = noiseModel::Unit::Create(4); Pose2 pose0; Pose2 pose1(1.0, 0.0, 0.0); @@ -86,23 +80,22 @@ TEST( testSummarization, example_from_ddf1 ) { Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; EXPECT(assert_equal(expSumOrdering, actOrdering)); + // Does not split out subfactors where possible GaussianFactorGraph expLinGraph; expLinGraph.add( expSumOrdering[lA3], - Matrix_(2,2, - 0.595867, 0.605092, - 0.0, 0.406109), + Matrix_(4,2, + 0.595867, 0.605092, + 0.0, -0.406109, + 0.0, 0.0, + 0.0, 0.0), expSumOrdering[lA5], - Matrix_(2,2, + Matrix_(4,2, -0.125971, -0.160052, - -0.13586, -0.301096), - zero(2), model); - expLinGraph.add( - expSumOrdering[lA5], - Matrix_(2,2, - 0.268667, 0.31703, - 0.0, 0.131698), - zero(2), model); + 0.13586, 0.301096, + 0.268667, 0.31703, + 0.0, -0.131698), + zero(4), model); EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); // Summarize directly from a nonlinear graph to another nonlinear graph