From 8bfe8571bcd00d62c20cd5fc6d9a918c513e90dd Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 7 Jun 2012 17:45:05 +0000 Subject: [PATCH] Removed template argument from NonlinearISAM, now just uses NonlinearFactorGraph --- ...{NonlinearISAM-inl.h => NonlinearISAM.cpp} | 29 +++++++------------ gtsam/nonlinear/NonlinearISAM.h | 12 ++------ gtsam/slam/visualSLAM.h | 2 +- tests/testNonlinearISAM.cpp | 2 +- 4 files changed, 16 insertions(+), 29 deletions(-) rename gtsam/nonlinear/{NonlinearISAM-inl.h => NonlinearISAM.cpp} (80%) diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM.cpp similarity index 80% rename from gtsam/nonlinear/NonlinearISAM-inl.h rename to gtsam/nonlinear/NonlinearISAM.cpp index ea62f1713..8c4424f05 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -15,7 +15,7 @@ * @author Viorela Ila and Richard Roberts */ -#pragma once +#include #include #include @@ -28,14 +28,12 @@ namespace gtsam { /* ************************************************************************* */ -template -void NonlinearISAM::saveGraph(const std::string& s) const { +void NonlinearISAM::saveGraph(const std::string& s) const { isam_.saveGraph(s); } /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, +void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -54,12 +52,12 @@ void NonlinearISAM::update(const Factors& newFactors, // Augment ordering // 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()) ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present boost::shared_ptr linearizedNewFactors( - newFactors.linearize(linPoint_, ordering_)->template dynamicCastFactors()); + newFactors.linearize(linPoint_, ordering_)->dynamicCastFactors()); // Update ISAM isam_.update(*linearizedNewFactors); @@ -67,8 +65,7 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +void NonlinearISAM::reorder_relinearize() { // std::cout << "Reordering, relinearizing..." << std::endl; @@ -83,7 +80,7 @@ void NonlinearISAM::reorder_relinearize() { // Create a linear factor graph at the new linearization point boost::shared_ptr gfg( - factors_.linearize(newLinPoint, ordering_)->template dynamicCastFactors()); + factors_.linearize(newLinPoint, ordering_)->dynamicCastFactors()); // Just recreate the whole BayesTree isam_.update(*gfg); @@ -94,8 +91,7 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -Values NonlinearISAM::estimate() const { +Values NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else @@ -103,14 +99,12 @@ Values NonlinearISAM::estimate() const { } /* ************************************************************************* */ -template -Matrix NonlinearISAM::marginalCovariance(Key key) const { +Matrix NonlinearISAM::marginalCovariance(Key key) const { return isam_.marginalCovariance(ordering_[key]); } /* ************************************************************************* */ -template -void NonlinearISAM::print(const std::string& s) const { +void NonlinearISAM::print(const std::string& s) const { std::cout << "ISAM - " << s << ":" << std::endl; std::cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << std::endl; isam_.print("GaussianISAM"); @@ -120,8 +114,7 @@ void NonlinearISAM::print(const std::string& s) const { } /* ************************************************************************* */ -template -void NonlinearISAM::printStats() const { +void NonlinearISAM::printStats() const { gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData(); gtsam::GaussianISAM::CliqueStats stats = data.getStats(); std::cout << "\navg Conditional Size: " << stats.avgConditionalSize; diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 145af7c17..d2ca75c42 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -24,12 +24,7 @@ namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ -template class NonlinearISAM { -public: - - typedef GRAPH Factors; - protected: /** The internal iSAM object */ @@ -42,7 +37,7 @@ protected: gtsam::Ordering ordering_; /** The original factors, used when relinearizing */ - Factors factors_; + NonlinearFactorGraph factors_; /** The reordering interval and counter */ int reorderInterval_; @@ -84,7 +79,7 @@ public: const gtsam::Ordering& getOrdering() const { return ordering_; } /** get underlying nonlinear graph */ - const Factors& getFactorsUnsafe() const { return factors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; } /** get counters */ int reorderInterval() const { return reorderInterval_; } /// diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 22e56b8a0..f1f5c3850 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -203,6 +203,6 @@ namespace visualSLAM { /** * Non-linear ISAM for vanilla incremental visual SLAM inference */ - typedef gtsam::NonlinearISAM ISAM; + typedef gtsam::NonlinearISAM ISAM; } // namespaces diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 28926ff5d..f063f9c7d 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -13,7 +13,7 @@ using namespace gtsam; using namespace planarSLAM; -typedef NonlinearISAM<> PlanarISAM; +typedef NonlinearISAM PlanarISAM; const double tol=1e-5;