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