diff --git a/gtsam.h b/gtsam.h index 00f5a9030..0e50ac9b6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -982,6 +982,7 @@ class NonlinearFactorGraph { void print(string s) const; double error(const gtsam::Values& c) const; double probPrime(const gtsam::Values& c) const; + void add(const gtsam::NonlinearFactor* factor); gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index b4878565e..fd1ec1f6c 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -83,32 +83,32 @@ namespace gtsam { push_back(fg); } - /** Add a Jacobian factor */ - void add(const boost::shared_ptr& factor) { - factors_.push_back(boost::shared_ptr(factor)); + /** Add a factor by value - makes a copy */ + void add(const GaussianFactor& factor) { + factors_.push_back(factor.clone()); } - /** Add a Hessian factor */ - void add(const boost::shared_ptr& factor) { - factors_.push_back(boost::shared_ptr(factor)); + /** Add a factor by pointer - stores pointer without copying the factor */ + void add(const sharedFactor& factor) { + factors_.push_back(factor); } /** Add a null factor */ void add(const Vector& b) { - add(JacobianFactor::shared_ptr(new JacobianFactor(b))); + add(JacobianFactor(b)); } /** Add a unary factor */ void add(Index key1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) { - add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,b,model))); + add(JacobianFactor(key1,A1,b,model)); } /** Add a binary factor */ void add(Index key1, const Matrix& A1, Index key2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) { - add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,key2,A2,b,model))); + add(JacobianFactor(key1,A1,key2,A2,b,model)); } /** Add a ternary factor */ @@ -116,13 +116,13 @@ namespace gtsam { Index key2, const Matrix& A2, Index key3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) { - add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,key2,A2,key3,A3,b,model))); + add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); } /** Add an n-ary factor */ void add(const std::vector > &terms, const Vector &b, const SharedDiagonal& model) { - add(JacobianFactor::shared_ptr(new JacobianFactor(terms,b,model))); + add(JacobianFactor(terms,b,model)); } /** diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 85cc0e63f..f7ebb42bc 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -55,9 +55,14 @@ namespace gtsam { /** Unnormalized probability. O(n) */ double probPrime(const Values& c) const; - template - void add(const F& factor) { - this->push_back(boost::shared_ptr(new F(factor))); + /// Add a factor by value - copies the factor object + void add(const NonlinearFactor& factor) { + this->push_back(factor.clone()); + } + + /// Add a factor by pointer - stores pointer without copying factor object + void add(const sharedFactor& factor) { + this->push_back(factor); } /**