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

Added optional template parameter for NonlinearISAM to specify a specialized graph

release/4.3a0
Alex Cunningham 2011-11-17 14:09:04 +00:00
parent f0b5e5ea3b
commit 6d961844c4
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -30,8 +30,8 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES, class GRAPH>
void NonlinearISAM<Values>::update(const Factors& newFactors, void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
const Values& initialValues) { const Values& initialValues) {
if(newFactors.size() > 0) { if(newFactors.size() > 0) {
@ -63,8 +63,8 @@ void NonlinearISAM<Values>::update(const Factors& newFactors,
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES, class GRAPH>
void NonlinearISAM<Values>::reorder_relinearize() { void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl; // cout << "Reordering, relinearizing..." << endl;
@ -90,8 +90,8 @@ void NonlinearISAM<Values>::reorder_relinearize() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES, class GRAPH>
Values NonlinearISAM<Values>::estimate() const { VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
if(isam_.size() > 0) if(isam_.size() > 0)
return linPoint_.retract(optimize(isam_), ordering_); return linPoint_.retract(optimize(isam_), ordering_);
else else
@ -99,14 +99,14 @@ Values NonlinearISAM<Values>::estimate() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES, class GRAPH>
Matrix NonlinearISAM<Values>::marginalCovariance(const Symbol& key) const { Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
return isam_.marginalCovariance(ordering_[key]); return isam_.marginalCovariance(ordering_[key]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Values> template<class VALUES, class GRAPH>
void NonlinearISAM<Values>::print(const std::string& s) const { void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
cout << "ISAM - " << s << ":" << endl; cout << "ISAM - " << s << ":" << endl;
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
isam_.print("GaussianISAM"); isam_.print("GaussianISAM");

5
gtsam/nonlinear/NonlinearISAM.h
View File

@ -25,11 +25,12 @@ namespace gtsam {
/** /**
* Wrapper class to manage ISAM in a nonlinear context * Wrapper class to manage ISAM in a nonlinear context
*/ */
template<class Values> template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
class NonlinearISAM { class NonlinearISAM {
public: public:
typedef gtsam::NonlinearFactorGraph<Values> Factors; typedef VALUES Values;
typedef GRAPH Factors;
protected: protected: