diff --git a/gtsam.h b/gtsam.h index a6326e23c..47930ad6b 100644 --- a/gtsam.h +++ b/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; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index c86e3c728..7d0913d86 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/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 > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e52321cea..e12c892de 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/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 ///@{ diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 2d5c32e25..4afd303ba 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/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(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);}