12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small cleanup with factor graph 'add' functions

release/4.3a0
Richard Roberts 2012-07-12 22:28:23 +00:00
parent b1a58e4447
commit c3ed90c792
3 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam.h
View File

@ -982,6 +982,7 @@ class NonlinearFactorGraph {
void print(string s) const; void print(string s) const;
double error(const gtsam::Values& c) const; double error(const gtsam::Values& c) const;
double probPrime(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; gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const;
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;

22
gtsam/linear/GaussianFactorGraph.h
View File

@ -83,32 +83,32 @@ namespace gtsam {
push_back(fg); push_back(fg);
} }
/** Add a Jacobian factor */ /** Add a factor by value - makes a copy */
void add(const boost::shared_ptr<JacobianFactor>& factor) { void add(const GaussianFactor& factor) {
factors_.push_back(boost::shared_ptr<GaussianFactor>(factor)); factors_.push_back(factor.clone());
} }
/** Add a Hessian factor */ /** Add a factor by pointer - stores pointer without copying the factor */
void add(const boost::shared_ptr<HessianFactor>& factor) { void add(const sharedFactor& factor) {
factors_.push_back(boost::shared_ptr<GaussianFactor>(factor)); factors_.push_back(factor);
} }
/** Add a null factor */ /** Add a null factor */
void add(const Vector& b) { void add(const Vector& b) {
add(JacobianFactor::shared_ptr(new JacobianFactor(b))); add(JacobianFactor(b));
} }
/** Add a unary factor */ /** Add a unary factor */
void add(Index key1, const Matrix& A1, void add(Index key1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) { 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 */ /** Add a binary factor */
void add(Index key1, const Matrix& A1, void add(Index key1, const Matrix& A1,
Index key2, const Matrix& A2, Index key2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) { 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 */ /** Add a ternary factor */
@ -116,13 +116,13 @@ namespace gtsam {
Index key2, const Matrix& A2, Index key2, const Matrix& A2,
Index key3, const Matrix& A3, Index key3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model) { 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 */ /** Add an n-ary factor */
void add(const std::vector<std::pair<Index, Matrix> > &terms, void add(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) { const Vector &b, const SharedDiagonal& model) {
add(JacobianFactor::shared_ptr(new JacobianFactor(terms,b,model))); add(JacobianFactor(terms,b,model));
} }
/** /**

11
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -55,9 +55,14 @@ namespace gtsam {
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const Values& c) const; double probPrime(const Values& c) const;
template<class F> /// Add a factor by value - copies the factor object
void add(const F& factor) { void add(const NonlinearFactor& factor) {
this->push_back(boost::shared_ptr<F>(new F(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);
} }
/** /**