Merge pull request #1010 from borglab/feature/subgraph_refactor

Subgraph API change

gtsam/linear/SubgraphBuilder.cpp

@@ -446,30 +446,29 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(
 }
 
 /*****************************************************************************/
-GaussianFactorGraph::shared_ptr buildFactorSubgraph(
-    const GaussianFactorGraph &gfg, const Subgraph &subgraph,
-    const bool clone) {
-  auto subgraphFactors = boost::make_shared<GaussianFactorGraph>();
-  subgraphFactors->reserve(subgraph.size());
+GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
+                                        const Subgraph &subgraph,
+                                        const bool clone) {
+  GaussianFactorGraph subgraphFactors;
+  subgraphFactors.reserve(subgraph.size());
   for (const auto &e : subgraph) {
     const auto factor = gfg[e.index];
-    subgraphFactors->push_back(clone ? factor->clone() : factor);
+    subgraphFactors.push_back(clone ? factor->clone() : factor);
   }
   return subgraphFactors;
 }
 
 /**************************************************************************************************/
-std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>  //
-splitFactorGraph(const GaussianFactorGraph &factorGraph,
-                 const Subgraph &subgraph) {
+std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
+    const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) {
   // Get the subgraph by calling cheaper method
   auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false);
 
   // Now, copy all factors then set subGraph factors to zero
-  auto remaining = boost::make_shared<GaussianFactorGraph>(factorGraph);
+  GaussianFactorGraph remaining = factorGraph;
 
   for (const auto &e : subgraph) {
-    remaining->remove(e.index);
+    remaining.remove(e.index);
   }
 
   return std::make_pair(subgraphFactors, remaining);

gtsam/linear/SubgraphBuilder.h

@@ -172,12 +172,13 @@ class GTSAM_EXPORT SubgraphBuilder {
 };
 
 /** Select the factors in a factor graph according to the subgraph. */
-boost::shared_ptr<GaussianFactorGraph> buildFactorSubgraph(
-    const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
+GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
+                                        const Subgraph &subgraph,
+                                        const bool clone);
 
 /** Split the graph into a subgraph and the remaining edges. 
  * Note that the remaining factorgraph has null factors. */
-std::pair<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> > 
-splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
+std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
+    const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
 
 }  // namespace gtsam

gtsam/linear/SubgraphPreconditioner.cpp

@@ -77,16 +77,16 @@ static void setSubvector(const Vector &src, const KeyInfo &keyInfo,
 /* ************************************************************************* */
 // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with
 // Cholesky)
-static GaussianFactorGraph::shared_ptr convertToJacobianFactors(
+static GaussianFactorGraph convertToJacobianFactors(
     const GaussianFactorGraph &gfg) {
-  auto result = boost::make_shared<GaussianFactorGraph>();
+  GaussianFactorGraph result;
   for (const auto &factor : gfg) 
     if (factor) {
       auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
       if (!jf) {
         jf = boost::make_shared<JacobianFactor>(*factor);
       }
-      result->push_back(jf);
+      result.push_back(jf);
     }
   return result;
 }
@@ -96,42 +96,42 @@ SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParam
          parameters_(p) {}
 
 /* ************************************************************************* */
-SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
-    const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
-        Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
-        b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
+SubgraphPreconditioner::SubgraphPreconditioner(const GaussianFactorGraph& Ab2,
+    const GaussianBayesNet& Rc1, const VectorValues& xbar, const SubgraphPreconditionerParameters &p) :
+        Ab2_(convertToJacobianFactors(Ab2)), Rc1_(Rc1), xbar_(xbar),
+        b2bar_(-Ab2_.gaussianErrors(xbar)), parameters_(p) {
 }
 
 /* ************************************************************************* */
 // x = xbar + inv(R1)*y
 VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
-  return *xbar_ + Rc1_->backSubstitute(y);
+  return xbar_ + Rc1_.backSubstitute(y);
 }
 
 /* ************************************************************************* */
 double SubgraphPreconditioner::error(const VectorValues& y) const {
   Errors e(y);
   VectorValues x = this->x(y);
-  Errors e2 = Ab2()->gaussianErrors(x);
+  Errors e2 = Ab2_.gaussianErrors(x);
   return 0.5 * (dot(e, e) + dot(e2,e2));
 }
 
 /* ************************************************************************* */
 // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
 VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const {
-  VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
-  Errors e = (*Ab2() * x - *b2bar());        /* (A2*inv(R1)*y-b2bar) */
+  VectorValues x = Rc1_.backSubstitute(y); /* inv(R1)*y */
+  Errors e = Ab2_ * x - b2bar_;              /* (A2*inv(R1)*y-b2bar) */
   VectorValues v = VectorValues::Zero(x);
-  Ab2()->transposeMultiplyAdd(1.0, e, v);    /* A2'*(A2*inv(R1)*y-b2bar) */
-  return y + Rc1()->backSubstituteTranspose(v);
+  Ab2_.transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
+  return y + Rc1_.backSubstituteTranspose(v);
 }
 
 /* ************************************************************************* */
 // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
-Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
+Errors SubgraphPreconditioner::operator*(const VectorValues &y) const {
   Errors e(y);
-  VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
-  Errors e2 = *Ab2() * x;                      /* A2*x */
+  VectorValues x = Rc1_.backSubstitute(y); /* x=inv(R1)*y */
+  Errors e2 = Ab2_ * x;                      /* A2*x */
   e.splice(e.end(), e2);
   return e;
 }
@@ -147,8 +147,8 @@ void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) c
   }
 
   // Add A2 contribution
-  VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
-  Ab2()->multiplyInPlace(x, ei);                  // use iterator version
+  VectorValues x = Rc1_.backSubstitute(y);      // x=inv(R1)*y
+  Ab2_.multiplyInPlace(x, ei);                  // use iterator version
 }
 
 /* ************************************************************************* */
@@ -190,22 +190,22 @@ void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
   while (it != end) e2.push_back(*(it++));
 
   VectorValues x = VectorValues::Zero(y); // x = 0
-  Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
-  y += alpha * Rc1_->backSubstituteTranspose(x); // y += alpha*inv(R1')*x
+  Ab2_.transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
+  y += alpha * Rc1_.backSubstituteTranspose(x); // y += alpha*inv(R1')*x
 }
 
 /* ************************************************************************* */
 void SubgraphPreconditioner::print(const std::string& s) const {
   cout << s << endl;
-  Ab2_->print();
+  Ab2_.print();
 }
 
 /*****************************************************************************/
 void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
   assert(x.size() == y.size());
 
   /* back substitute */
-  for (const auto &cg : boost::adaptors::reverse(*Rc1_)) {
+  for (const auto &cg : boost::adaptors::reverse(Rc1_)) {
     /* collect a subvector of x that consists of the parents of cg (S) */
     const KeyVector parentKeys(cg->beginParents(), cg->endParents());
     const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
@@ -228,7 +228,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
   std::copy(y.data(), y.data() + y.rows(), x.data());
 
   /* in place back substitute */
-  for (const auto &cg : *Rc1_) {
+  for (const auto &cg : Rc1_) {
     const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
     const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
     const Vector solFrontal =
@@ -261,10 +261,10 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
   keyInfo_ = keyInfo;
 
   /* build factor subgraph */
-  GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);
+  auto gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);
 
   /* factorize and cache BayesNet */
-  Rc1_ = gfg_subgraph->eliminateSequential();
+  Rc1_ = *gfg_subgraph.eliminateSequential();
 }
 
 /*****************************************************************************/

gtsam/linear/SubgraphPreconditioner.h

@@ -19,6 +19,8 @@
 
 #include <gtsam/linear/SubgraphBuilder.h>
 #include <gtsam/linear/Errors.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/Preconditioner.h>
 #include <gtsam/linear/VectorValues.h>
@@ -53,16 +55,12 @@ namespace gtsam {
 
   public:
     typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
-    typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
-    typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
-    typedef boost::shared_ptr<const VectorValues> sharedValues;
-    typedef boost::shared_ptr<const Errors> sharedErrors;
 
   private:
-    sharedFG Ab2_;
-    sharedBayesNet Rc1_;
-    sharedValues xbar_;  ///< A1 \ b1
-    sharedErrors b2bar_; ///< A2*xbar - b2
+    GaussianFactorGraph Ab2_;
+    GaussianBayesNet Rc1_;
+    VectorValues xbar_;  ///< A1 \ b1
+    Errors b2bar_; ///< A2*xbar - b2
 
     KeyInfo keyInfo_;
     SubgraphPreconditionerParameters parameters_;
@@ -77,7 +75,7 @@ namespace gtsam {
      * @param Rc1: the Bayes Net R1*x=c1
      * @param xbar: the solution to R1*x=c1
      */
-    SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar,
+    SubgraphPreconditioner(const GaussianFactorGraph& Ab2, const GaussianBayesNet& Rc1, const VectorValues& xbar,
                            const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
 
     ~SubgraphPreconditioner() override {}
@@ -86,13 +84,13 @@ namespace gtsam {
     void print(const std::string& s = "SubgraphPreconditioner") const;
 
     /** Access Ab2 */
-    const sharedFG& Ab2() const { return Ab2_; }
+    const GaussianFactorGraph& Ab2() const { return Ab2_; }
 
     /** Access Rc1 */
-    const sharedBayesNet& Rc1() const { return Rc1_; }
+    const GaussianBayesNet& Rc1() const { return Rc1_; }
 
     /** Access b2bar */
-    const sharedErrors b2bar() const { return b2bar_; }
+    const Errors b2bar() const { return b2bar_; }
 
     /**
      * Add zero-mean i.i.d. Gaussian prior terms to each variable
@@ -104,8 +102,7 @@ namespace gtsam {
 
     /* A zero VectorValues with the structure of xbar */
     VectorValues zero() const {
-      assert(xbar_);
-      return VectorValues::Zero(*xbar_);
+      return VectorValues::Zero(xbar_);
     }
 
     /**

gtsam/linear/SubgraphSolver.cpp

@@ -34,35 +34,35 @@ namespace gtsam {
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
     const Parameters &parameters, const Ordering& ordering) :
     parameters_(parameters) {
-  GaussianFactorGraph::shared_ptr Ab1,Ab2;
+  GaussianFactorGraph Ab1, Ab2;
   std::tie(Ab1, Ab2) = splitGraph(Ab);
   if (parameters_.verbosity())
-    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
+    cout << "Split A into (A1) " << Ab1.size() << " and (A2) " << Ab2.size()
          << " factors" << endl;
 
-  auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR);
-  auto xbar = boost::make_shared<VectorValues>(Rc1->optimize());
+  auto Rc1 = *Ab1.eliminateSequential(ordering, EliminateQR);
+  auto xbar = Rc1.optimize();
   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
 /**************************************************************************************************/
 // Taking eliminated tree [R1|c] and constraint graph [A2|b2]
-SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
-                               const GaussianFactorGraph::shared_ptr &Ab2,
+SubgraphSolver::SubgraphSolver(const GaussianBayesNet &Rc1,
+                               const GaussianFactorGraph &Ab2,
                                const Parameters &parameters)
     : parameters_(parameters) {
-  auto xbar = boost::make_shared<VectorValues>(Rc1->optimize());
+  auto xbar = Rc1.optimize();
   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
 /**************************************************************************************************/
 // Taking subgraphs [A1|b1] and [A2|b2]
 // delegate up
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
-                               const GaussianFactorGraph::shared_ptr &Ab2,
+                               const GaussianFactorGraph &Ab2,
                                const Parameters &parameters,
                                const Ordering &ordering)
-    : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
+    : SubgraphSolver(*Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
                      parameters) {}
 
 /**************************************************************************************************/
@@ -78,7 +78,7 @@ VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
   return VectorValues();
 }
 /**************************************************************************************************/
-pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
+pair<GaussianFactorGraph, GaussianFactorGraph> //
 SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
 
   /* identify the subgraph structure */

gtsam/linear/SubgraphSolver.h

@@ -99,15 +99,13 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
    * eliminate Ab1. We take Ab1 as a const reference, as it will be transformed
    * into Rc1, but take Ab2 as a shared pointer as we need to keep it around.
    */
-  SubgraphSolver(const GaussianFactorGraph &Ab1,
-                 const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+  SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
                  const Parameters &parameters, const Ordering &ordering);
   /**
    * The same as above, but we assume A1 was solved by caller.
    * We take two shared pointers as we keep both around.
    */
-  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
-                 const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+  SubgraphSolver(const GaussianBayesNet &Rc1, const GaussianFactorGraph &Ab2,
                  const Parameters &parameters);
 
   /// Destructor
@@ -131,9 +129,8 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
   /// @{
 
   /// Split graph using Kruskal algorithm, treating binary factors as edges.
-  std::pair < boost::shared_ptr<GaussianFactorGraph>,
-      boost::shared_ptr<GaussianFactorGraph> > splitGraph(
-          const GaussianFactorGraph &gfg);
+  std::pair<GaussianFactorGraph, GaussianFactorGraph> splitGraph(
+      const GaussianFactorGraph &gfg);
 
   /// @}
 };

gtsam/linear/linear.i

@@ -624,7 +624,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
 
 virtual class SubgraphSolver  {
   SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
-  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
+  SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph& Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
   gtsam::VectorValues optimize() const;
 };
 

tests/smallExample.h

@@ -679,26 +679,25 @@ inline Ordering planarOrdering(size_t N) {
 }
 
 /* ************************************************************************* */
-inline std::pair<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr > splitOffPlanarTree(size_t N,
-    const GaussianFactorGraph& original) {
-  auto T = boost::make_shared<GaussianFactorGraph>(), C= boost::make_shared<GaussianFactorGraph>();
+inline std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(
+    size_t N, const GaussianFactorGraph& original) {
+  GaussianFactorGraph T, C;
 
   // Add the x11 constraint to the tree
-  T->push_back(original[0]);
+  T.push_back(original[0]);
 
   // Add all horizontal constraints to the tree
   size_t i = 1;
   for (size_t x = 1; x < N; x++)
-    for (size_t y = 1; y <= N; y++, i++)
-      T->push_back(original[i]);
+    for (size_t y = 1; y <= N; y++, i++) T.push_back(original[i]);
 
   // Add first vertical column of constraints to T, others to C
   for (size_t x = 1; x <= N; x++)
     for (size_t y = 1; y < N; y++, i++)
       if (x == 1)
-        T->push_back(original[i]);
+        T.push_back(original[i]);
       else
-        C->push_back(original[i]);
+        C.push_back(original[i]);
 
   return std::make_pair(T, C);
 }