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

Added methods to add JacobianFactor and HessianFactor, the templated push_back gives trouble...

release/4.3a0
Frank Dellaert 2012-01-20 04:33:30 +00:00
parent 3c1e5c0f04
commit fa200f511a
2 changed files with 28 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/inference/FactorGraph.h
View File

@ -83,18 +83,26 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** convert from a derived type */ /** convert from a derived type */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) { factors_.insert(end(), factors.begin(), factors.end()); } FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/** Add a factor */ /** Add a factor */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) { factors_.push_back(sharedFactor(factor)); } void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
/** push back many factors */ /** push back many factors */
void push_back(const FactorGraph<FACTOR>& factors) { factors_.insert(end(), factors.begin(), factors.end()); } void push_back(const FactorGraph<FACTOR>& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/** push back many factors with an iterator */ /** push back many factors with an iterator */
template<typename ITERATOR> template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor);
}
/** push back many factors stored in a vector*/ /** push back many factors stored in a vector*/
template<typename DERIVEDFACTOR> template<typename DERIVEDFACTOR>

21
gtsam/linear/GaussianFactorGraph.h
View File

@ -25,6 +25,7 @@
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
@ -84,22 +85,32 @@ namespace gtsam {
push_back(fg); push_back(fg);
} }
/** Add a Jacobian factor */
void add(const boost::shared_ptr<JacobianFactor>& factor) {
factors_.push_back(boost::shared_ptr<GaussianFactor>(factor));
}
/** Add a Hessian factor */
void add(const boost::shared_ptr<HessianFactor>& factor) {
factors_.push_back(boost::shared_ptr<GaussianFactor>(factor));
}
/** Add a null factor */ /** Add a null factor */
void add(const Vector& b) { void add(const Vector& b) {
push_back(sharedFactor(new JacobianFactor(b))); add(JacobianFactor::shared_ptr(new 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) {
push_back(sharedFactor(new JacobianFactor(key1,A1,b,model))); add(JacobianFactor::shared_ptr(new 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) {
push_back(sharedFactor(new JacobianFactor(key1,A1,key2,A2,b,model))); add(JacobianFactor::shared_ptr(new JacobianFactor(key1,A1,key2,A2,b,model)));
} }
/** Add a ternary factor */ /** Add a ternary factor */
@ -107,13 +118,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) {
push_back(sharedFactor(new JacobianFactor(key1,A1,key2,A2,key3,A3,b,model))); add(JacobianFactor::shared_ptr(new 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) {
push_back(sharedFactor(new JacobianFactor(terms,b,model))); add(JacobianFactor::shared_ptr(new JacobianFactor(terms,b,model)));
} }
/** /**