diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 4b4d874e7..cffa7ccde 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -424,7 +424,11 @@ gtsam::GaussianFactorGraph* summarizeGraphSequential( pair partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, - const gtsam::KeySet& overlap_keys); + const gtsam::KeySet& saved_keys); + +pair +sequentialSummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, + const gtsam::KeySet& saved_keys); #include class FixedLagSmootherKeyTimestampMapValue { diff --git a/gtsam_unstable/nonlinear/summarization.cpp b/gtsam_unstable/nonlinear/summarization.cpp index 1c14ea974..680719c82 100644 --- a/gtsam_unstable/nonlinear/summarization.cpp +++ b/gtsam_unstable/nonlinear/summarization.cpp @@ -31,19 +31,43 @@ GaussianFactorGraph::shared_ptr summarizeGraphSequential( /* ************************************************************************* */ std::pair -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 +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 diff --git a/gtsam_unstable/nonlinear/summarization.h b/gtsam_unstable/nonlinear/summarization.h index 6d1da9cad..7226c810b 100644 --- a/gtsam_unstable/nonlinear/summarization.h +++ b/gtsam_unstable/nonlinear/summarization.h @@ -17,9 +17,32 @@ namespace gtsam { -// Sequential graph summarization -typedef FactorGraph JacobianGraph; -typedef boost::shared_ptr 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 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 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 GTSAM_UNSTABLE_EXPORT -partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values, - const KeySet& overlap_keys); - - } // \namespace gtsam