Added negate() and clone() to GaussianFactorGraph

gtsam.h

@@ -1269,6 +1269,9 @@ class GaussianFactorGraph {
   double error(const gtsam::VectorValues& c) const;
   double probPrime(const gtsam::VectorValues& c) const;
 
+  gtsam::GaussianFactorGraph clone() const;
+  gtsam::GaussianFactorGraph negate() const;
+
   // Optimizing and linear algebra  
   gtsam::VectorValues optimize() const;
   gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;

gtsam/linear/GaussianFactorGraph.cpp

@@ -54,6 +54,30 @@ namespace gtsam {
     return keys;
   }
 
+  /* ************************************************************************* */
+  GaussianFactorGraph GaussianFactorGraph::clone() const {
+    GaussianFactorGraph result;
+    BOOST_FOREACH(const sharedFactor& f, *this) {
+      if (f)
+        result.push_back(f->clone());
+      else
+        result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
+    }
+    return result;
+  }
+
+  /* ************************************************************************* */
+  GaussianFactorGraph GaussianFactorGraph::negate() const {
+    GaussianFactorGraph result;
+    BOOST_FOREACH(const sharedFactor& f, *this) {
+      if (f)
+        result.push_back(f->negate());
+      else
+        result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
+    }
+    return result;
+  }
+
   /* ************************************************************************* */
   std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
     // First find dimensions of each variable

gtsam/linear/GaussianFactorGraph.h

@@ -148,6 +148,21 @@ namespace gtsam {
       return exp(-0.5 * error(c));
     }
 
+    /**
+     * Clone() performs a deep-copy of the graph, including all of the factors.
+     * Cloning preserves null factors so indices for the original graph are still
+     * valid for the cloned graph.
+     */
+    GaussianFactorGraph clone() const;
+
+    /**
+     * Returns the negation of all factors in this graph - corresponds to antifactors.
+     * Will convert all factors to HessianFactors due to negation of information.
+     * Cloning preserves null factors so indices for the original graph are still
+     * valid for the cloned graph.
+     */
+    GaussianFactorGraph negate() const;
+
     ///@name Linear Algebra
     ///@{
 

gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp

@@ -322,6 +322,37 @@ TEST( GaussianFactorGraph, gradientAtZero )
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, clone ) {
+  GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
+  init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
+  GaussianFactorGraph exp_graph = createGaussianFactorGraphWithHessianFactor(); // Created separately
+  exp_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
+  GaussianFactorGraph actCloned = init_graph.clone();
+  EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version
+
+  // Apply an in-place change to init_graph and compare
+  JacobianFactor::shared_ptr jacFactor0 = boost::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0));
+  CHECK(jacFactor0);
+  jacFactor0->getA(jacFactor0->begin()) *= 7.;
+  EXPECT(assert_inequal(init_graph, exp_graph));
+  EXPECT(assert_equal(exp_graph, actCloned));
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, negate ) {
+  GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
+  init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
+  GaussianFactorGraph actNegation = init_graph.negate();
+  GaussianFactorGraph expNegation;
+  expNegation.push_back(init_graph.at(0)->negate());
+  expNegation.push_back(init_graph.at(1)->negate());
+  expNegation.push_back(init_graph.at(2)->negate());
+  expNegation.push_back(init_graph.at(3)->negate());
+  expNegation.push_back(init_graph.at(4)->negate());
+  expNegation.push_back(GaussianFactor::shared_ptr());
+  EXPECT(assert_equal(expNegation, actNegation));
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}