Moved older functions back to gtsam_unstable, cleanup. All tests pass. Rearranged summarization wrap interfaces.

gtsam.h

@@ -1689,6 +1689,23 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
 
 }; // \class LinearContainerFactor
 
+// Summarization functionality
+#include <gtsam/nonlinear/summarization.h>
+pair<gtsam::GaussianFactorGraph,gtsam::Ordering>
+
+summarize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
+    const gtsam::KeySet& saved_keys, bool useQR);
+pair<gtsam::GaussianFactorGraph,gtsam::Ordering>
+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
 //*************************************************************************

gtsam/nonlinear/summarization.cpp

@@ -8,28 +8,11 @@
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/nonlinear/summarization.h>
 #include <gtsam/nonlinear/LinearContainerFactor.h>
-//#include <gtsam_unstable/linear/bayesTreeOperations.h>
 
 using namespace std;
 
 namespace gtsam {
 
-///* ************************************************************************* */
-//GaussianFactorGraph::shared_ptr summarizeGraphSequential(
-//    const GaussianFactorGraph& full_graph, const std::vector<Index>& 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<Index> indices;
-//  BOOST_FOREACH(const Key& k, saved_keys)
-//    indices.push_back(ordering[k]);
-//  return summarizeGraphSequential(full_graph, indices, useQR);
-//}
-
 /* ************************************************************************* */
 std::pair<GaussianFactorGraph,Ordering>
 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<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
-//  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<GaussianFactorGraph,Ordering> GTSAM_EXPORT
-//summarize(const NonlinearFactorGraph& graph, const Values& values,
-//    const KeySet& saved_keys, bool useQR = true);
-
-
 /* ************************************************************************* */
 NonlinearFactorGraph summarizeAsNonlinearContainer(
     const NonlinearFactorGraph& graph, const Values& values,

gtsam/nonlinear/summarization.h

@@ -31,20 +31,6 @@ std::pair<GaussianFactorGraph,Ordering> 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<GaussianFactorGraph,Ordering> 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<Index>& 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
 
 

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<gtsam::GaussianFactorGraph,gtsam::Ordering>
-partialCholeskySummarization(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
-    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 {
   FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp);

gtsam_unstable/nonlinear/summarization.cpp

@@ -0,0 +1,35 @@
+/**
+ * @file summarization.cpp
+ *
+ * @date Jun 22, 2012
+ * @author Alex Cunningham
+ */
+
+#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam_unstable/nonlinear/summarization.h>
+
+#include <boost/foreach.hpp>
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+GaussianFactorGraph::shared_ptr summarizeGraphSequential(
+    const GaussianFactorGraph& full_graph, const std::vector<Index>& 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<Index> indices;
+  BOOST_FOREACH(const Key& k, saved_keys)
+    indices.push_back(ordering[k]);
+  return summarizeGraphSequential(full_graph, indices, useQR);
+}
+
+/* ************************************************************************* */
+} // \namespace gtsam
+

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 <gtsam_unstable/base/dllexport.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/nonlinear/Ordering.h>
+
+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<Index>& 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
+
+

tests/testSummarization.cpp

@@ -24,15 +24,9 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
 
-//#include <ddfsam/domains/planarDDFSystem.h>
-//
-//#include <ddfsam/ddf1/DDF1SummarizedMap.h>
-
 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,
+      Matrix_(4,2,
-          0.595867, 0.605092,
+          0.595867,  0.605092,
-          0.0, 0.406109),
+               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),
+            0.13586,  0.301096,
-      zero(2), model);
+           0.268667,   0.31703,
-  expLinGraph.add(
+                0.0, -0.131698),
-      expSumOrdering[lA5],
+      zero(4), model);
-      Matrix_(2,2,
-          0.268667,  0.31703,
-          0.0, 0.131698),
-      zero(2), model);
   EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
 
   // Summarize directly from a nonlinear graph to another nonlinear graph