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

2013-05-08 10:13:57 +00:00 · 2013-05-08 10:13:57 +00:00 · acd6e629e8
parent 81f63bcc0e
commit acd6e629e8
7 changed files with 98 additions and 101 deletions
--- a/gtsam.h
+++ b/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
 //*************************************************************************
--- a/gtsam/nonlinear/summarization.cpp
+++ b/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,
--- a/gtsam/nonlinear/summarization.h
+++ b/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


--- 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<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);
--- a/gtsam_unstable/nonlinear/summarization.cpp
+++ b/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
+
--- a/gtsam_unstable/nonlinear/summarization.h
+++ 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 <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
+
+
--- a/tests/testSummarization.cpp
+++ b/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,
-          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