From 2b78eb8bec7f8a4a09c6d7dd757d3cf278af721d Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 17 Sep 2013 03:13:32 +0000
Subject: [PATCH] Restored NonlinearFactorGraph::symbolic() and made a few
 other minor fixes.

---
 gtsam/nonlinear/NonlinearFactorGraph.cpp | 45 +++++++++---------------
 gtsam/nonlinear/NonlinearFactorGraph.h   | 14 ++------
 gtsam/symbolic/SymbolicFactor.h          |  9 +++--
 tests/testNonlinearFactorGraph.cpp       | 19 +++++++++-
 4 files changed, 42 insertions(+), 45 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index fef36a685..5a2fb8331 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -20,12 +20,13 @@
 #include <cmath>
 #include <limits>
 #include <boost/foreach.hpp>
-#include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/inference/FactorGraph-inst.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/inference/Ordering.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 #ifdef GTSAM_USE_TBB
@@ -233,33 +234,21 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>
 }
 
 /* ************************************************************************* */
-//SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
-//  gttic(NonlinearFactorGraph_symbolic_from_Ordering);
-//
-//  // Generate the symbolic factor graph
-//  SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
-//  symbolicfg->reserve(this->size());
-//
-//  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
-//    if(factor)
-//      symbolicfg->push_back(factor->symbolic(ordering));
-//    else
-//      symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
-//  }
-//
-//  return symbolicfg;
-//}
+SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const
+{
+  // Generate the symbolic factor graph
+  SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared<SymbolicFactorGraph>();
+  symbolic->reserve(this->size());
 
-/* ************************************************************************* */
-//pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
-//    const Values& config) const
-//{
-//  gttic(NonlinearFactorGraph_symbolic_from_Values);
-//
-//  // Generate an initial key ordering in iterator order
-//  Ordering::shared_ptr ordering(config.orderingArbitrary());
-//  return make_pair(symbolic(*ordering), ordering);
-//}
+  BOOST_FOREACH(const sharedFactor& factor, *this) {
+    if(factor)
+      *symbolic += SymbolicFactor(*factor);
+    else
+      *symbolic += SymbolicFactorGraph::sharedFactor();
+  }
+
+  return symbolic;
+}
 
 /* ************************************************************************* */
 namespace {
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 9746f33ad..dd40e9c1b 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -31,6 +31,7 @@ namespace gtsam {
   class Values;
   class Ordering;
   class GaussianFactorGraph;
+  class SymbolicFactorGraph;
 
   /**
    * Formatting options when saving in GraphViz format using
@@ -106,18 +107,9 @@ namespace gtsam {
     double probPrime(const Values& c) const;
 
     /**
-     * Create a symbolic factor graph using an existing ordering
+     * Create a symbolic factor graph
      */
-    //SymbolicFactorGraph::shared_ptr symbolic() const;
-
-    /**
-     * Create a symbolic factor graph and initial variable ordering that can
-     * be used for graph operations like determining a fill-reducing ordering.
-     * The graph and ordering should be permuted after such a fill-reducing
-     * ordering is found.
-     */
-    //std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
-    //  symbolic(const Values& config) const;
+    boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
 
     /**
      * Compute a fill-reducing ordering using COLAMD.
diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h
index a9551d5f9..94c3910ef 100644
--- a/gtsam/symbolic/SymbolicFactor.h
+++ b/gtsam/symbolic/SymbolicFactor.h
@@ -52,7 +52,7 @@ namespace gtsam {
     SymbolicFactor() {}
 
     /** Construct unary factor */
-    SymbolicFactor(Key j) :
+    explicit SymbolicFactor(Key j) :
       Base(boost::assign::cref_list_of<1>(j)) {}
 
     /** Construct binary factor */
@@ -75,6 +75,9 @@ namespace gtsam {
     SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
       Base(boost::assign::cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}
 
+    /** Create symbolic version of any factor */
+    explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {}
+
     virtual ~SymbolicFactor() {}
 
     /// Copy this object as its actual derived type.
@@ -91,10 +94,6 @@ namespace gtsam {
     
     /// @name Advanced Constructors
     /// @{
-  private:
-    explicit SymbolicFactor(const Base& base) :
-      Base(base) {}
-
   public:
     /** Constructor from a collection of keys */
     template<typename KEYITERATOR>
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 88b2a2b29..9c2eddcc3 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -31,8 +31,9 @@ using namespace boost::assign;
 #include <gtsam/base/Matrix.h>
 #include <tests/smallExample.h>
 #include <gtsam/inference/FactorGraph.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/symbolic/SymbolicFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
 using namespace gtsam;
 using namespace example;
@@ -148,6 +149,22 @@ TEST( NonlinearFactorGraph, rekey )
   EXPECT(assert_equal(expRekey, actRekey));
 }
 
+/* ************************************************************************* */
+TEST( NonlinearFactorGraph, symbolic )
+{
+  NonlinearFactorGraph graph = createNonlinearFactorGraph();
+
+  SymbolicFactorGraph expected;
+  expected.push_factor(X(1));
+  expected.push_factor(X(1), X(2));
+  expected.push_factor(X(1), L(1));
+  expected.push_factor(X(2), L(1));
+
+  SymbolicFactorGraph actual = *graph.symbolic();
+
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */