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

Removed template argument from NonlinearISAM, now just uses NonlinearFactorGraph

release/4.3a0
Alex Cunningham 2012-06-07 17:45:05 +00:00
parent 8ba08135b2
commit 8bfe8571bc
4 changed files with 16 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/nonlinear/NonlinearISAM-inl.h → gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -15,7 +15,7 @@
* @author Viorela Ila and Richard Roberts * @author Viorela Ila and Richard Roberts
*/ */
#pragma once #include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/ISAM-inl.h> #include <gtsam/inference/ISAM-inl.h>
@ -28,14 +28,12 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> void NonlinearISAM::saveGraph(const std::string& s) const {
void NonlinearISAM<GRAPH>::saveGraph(const std::string& s) const {
isam_.saveGraph(s); isam_.saveGraph(s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
const Values& initialValues) { const Values& initialValues) {
if(newFactors.size() > 0) { if(newFactors.size() > 0) {
@ -54,12 +52,12 @@ void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
// Augment ordering // Augment ordering
// FIXME: should just loop over new values // FIXME: should just loop over new values
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors)
BOOST_FOREACH(Key key, factor->keys()) BOOST_FOREACH(Key key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors( boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
newFactors.linearize(linPoint_, ordering_)->template dynamicCastFactors<GaussianFactorGraph>()); newFactors.linearize(linPoint_, ordering_)->dynamicCastFactors<GaussianFactorGraph>());
// Update ISAM // Update ISAM
isam_.update(*linearizedNewFactors); isam_.update(*linearizedNewFactors);
@ -67,8 +65,7 @@ void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> void NonlinearISAM::reorder_relinearize() {
void NonlinearISAM<GRAPH>::reorder_relinearize() {
// std::cout << "Reordering, relinearizing..." << std::endl; // std::cout << "Reordering, relinearizing..." << std::endl;
@ -83,7 +80,7 @@ void NonlinearISAM<GRAPH>::reorder_relinearize() {
// Create a linear factor graph at the new linearization point // Create a linear factor graph at the new linearization point
boost::shared_ptr<GaussianFactorGraph> gfg( boost::shared_ptr<GaussianFactorGraph> gfg(
factors_.linearize(newLinPoint, ordering_)->template dynamicCastFactors<GaussianFactorGraph>()); factors_.linearize(newLinPoint, ordering_)->dynamicCastFactors<GaussianFactorGraph>());
// Just recreate the whole BayesTree // Just recreate the whole BayesTree
isam_.update(*gfg); isam_.update(*gfg);
@ -94,8 +91,7 @@ void NonlinearISAM<GRAPH>::reorder_relinearize() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> Values NonlinearISAM::estimate() const {
Values NonlinearISAM<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
@ -103,14 +99,12 @@ Values NonlinearISAM<GRAPH>::estimate() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> Matrix NonlinearISAM::marginalCovariance(Key key) const {
Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
return isam_.marginalCovariance(ordering_[key]); return isam_.marginalCovariance(ordering_[key]);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> void NonlinearISAM::print(const std::string& s) const {
void NonlinearISAM<GRAPH>::print(const std::string& s) const {
std::cout << "ISAM - " << s << ":" << std::endl; std::cout << "ISAM - " << s << ":" << std::endl;
std::cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << std::endl; std::cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << std::endl;
isam_.print("GaussianISAM"); isam_.print("GaussianISAM");
@ -120,8 +114,7 @@ void NonlinearISAM<GRAPH>::print(const std::string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GRAPH> void NonlinearISAM::printStats() const {
void NonlinearISAM<GRAPH>::printStats() const {
gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData(); gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData();
gtsam::GaussianISAM::CliqueStats stats = data.getStats(); gtsam::GaussianISAM::CliqueStats stats = data.getStats();
std::cout << "\navg Conditional Size: " << stats.avgConditionalSize; std::cout << "\navg Conditional Size: " << stats.avgConditionalSize;

12
gtsam/nonlinear/NonlinearISAM.h
View File

@ -24,12 +24,7 @@ namespace gtsam {
/** /**
* Wrapper class to manage ISAM in a nonlinear context * Wrapper class to manage ISAM in a nonlinear context
*/ */
template<class GRAPH = gtsam::NonlinearFactorGraph >
class NonlinearISAM { class NonlinearISAM {
public:
typedef GRAPH Factors;
protected: protected:
/** The internal iSAM object */ /** The internal iSAM object */
@ -42,7 +37,7 @@ protected:
gtsam::Ordering ordering_; gtsam::Ordering ordering_;
/** The original factors, used when relinearizing */ /** The original factors, used when relinearizing */
Factors factors_; NonlinearFactorGraph factors_;
/** The reordering interval and counter */ /** The reordering interval and counter */
int reorderInterval_; int reorderInterval_;
@ -84,7 +79,7 @@ public:
const gtsam::Ordering& getOrdering() const { return ordering_; } const gtsam::Ordering& getOrdering() const { return ordering_; }
/** get underlying nonlinear graph */ /** get underlying nonlinear graph */
const Factors& getFactorsUnsafe() const { return factors_; } const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
/** get counters */ /** get counters */
int reorderInterval() const { return reorderInterval_; } ///<TODO: comment int reorderInterval() const { return reorderInterval_; } ///<TODO: comment
@ -104,7 +99,7 @@ public:
/// @{ /// @{
/** Add new factors along with their initial linearization points */ /** Add new factors along with their initial linearization points */
void update(const Factors& newFactors, const Values& initialValues); void update(const NonlinearFactorGraph& newFactors, const Values& initialValues);
/** Relinearization and reordering of variables */ /** Relinearization and reordering of variables */
void reorder_relinearize(); void reorder_relinearize();
@ -121,4 +116,3 @@ public:
} // \namespace gtsam } // \namespace gtsam
#include <gtsam/nonlinear/NonlinearISAM-inl.h>

2
gtsam/slam/visualSLAM.h
View File

@ -203,6 +203,6 @@ namespace visualSLAM {
/** /**
* Non-linear ISAM for vanilla incremental visual SLAM inference * Non-linear ISAM for vanilla incremental visual SLAM inference
*/ */
typedef gtsam::NonlinearISAM<Graph> ISAM; typedef gtsam::NonlinearISAM ISAM;
} // namespaces } // namespaces

2
tests/testNonlinearISAM.cpp
View File

@ -13,7 +13,7 @@
using namespace gtsam; using namespace gtsam;
using namespace planarSLAM; using namespace planarSLAM;
typedef NonlinearISAM<> PlanarISAM; typedef NonlinearISAM PlanarISAM;
const double tol=1e-5; const double tol=1e-5;