diff --git a/.cproject b/.cproject index c6c10d6db..6a32e5c08 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + diff --git a/.project b/.project index c4d531b04..e52e979df 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - + -j8 org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index 9cb327bec..4972b13bf 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -25,4 +25,17 @@ namespace gtsam { +/* ************************************************************************* */ +namespace internal { +template +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) { + // parents are assumed to already be solved and available in result + clique->conditional()->solveInPlace(result); + + // starting from the root, call optimize on each conditional + BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + optimizeInPlace(child, result); +} +} + } diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 4243657e5..ae76d958e 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -23,22 +23,15 @@ namespace gtsam { /* ************************************************************************* */ -namespace internal { -void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result) { - // parents are assumed to already be solved and available in result - clique->conditional()->solveInPlace(result); - - // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr >& child, clique->children_) - optimizeInPlace(child, result); -} +VectorValues optimize(const GaussianBayesTree& bayesTree) { + VectorValues result = *allocateVectorValues(bayesTree); + optimizeInPlace(bayesTree, result); + return result; } /* ************************************************************************* */ -VectorValues optimize(const GaussianBayesTree& bayesTree) { - VectorValues result = *allocateVectorValues(bayesTree); - internal::optimizeInPlace(bayesTree.root(), result); - return result; +void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { + internal::optimizeInPlace(bayesTree.root(), result); } /* ************************************************************************* */ @@ -77,11 +70,6 @@ void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& gr toc(4, "Compute point"); } -/* ************************************************************************* */ -void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) { - internal::optimizeInPlace(bayesTree.root(), result); -} - /* ************************************************************************* */ VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) { return gradient(FactorGraph(bayesTree), x0); diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index bb15eb063..8b581351c 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -34,7 +34,8 @@ VectorValues optimize(const GaussianBayesTree& bayesTree); void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result); namespace internal { -void optimizeInPlace(const boost::shared_ptr >& clique, VectorValues& result); +template +void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result); } /** diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index 2c2a44dbb..87063bb48 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -50,7 +50,7 @@ namespace gtsam { // back-substitution tic(3, "back-substitute"); - internal::optimizeInPlace(rootClique, result); + internal::optimizeInPlace(rootClique, result); toc(3, "back-substitute"); return result; } diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8dce6934a..b6d018796 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -16,13 +16,14 @@ */ #include +#include namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { + Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -54,9 +55,9 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { FastSet indices; - BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(Key key, factor->keys()) { indices.insert(ordering[key]); } @@ -93,7 +94,7 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permuted& d } /* ************************************************************************* */ -void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -107,7 +108,7 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSetprint("Key(s) marked in clique "); if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; } - BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children_) { FindAll(child, keys, markedMask); } } @@ -226,7 +227,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, // eliminate into a Bayes net tic(7,"eliminate"); - JunctionTree jt(factors, affectedFactorsIndex); + JunctionTree jt(factors, affectedFactorsIndex); result.bayesTree = jt.eliminate(EliminatePreferLDL); toc(7,"eliminate"); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 3dcc15aa8..f49a385bc 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -15,7 +15,10 @@ * @author Michael Kaess, Richard Roberts */ +#pragma once + #include +#include namespace gtsam { @@ -24,7 +27,7 @@ using namespace std; struct ISAM2::Impl { struct PartialSolveResult { - typename ISAM2::sharedClique bayesTree; + ISAM2::sharedClique bayesTree; Permutation fullReordering; Permutation fullReorderingInverse; }; @@ -46,7 +49,7 @@ struct ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, - Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -55,7 +58,7 @@ struct ISAM2::Impl { * @param factors The factors from which to extract the variables * @return The set of variables indices from the factors */ - static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); + static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -84,7 +87,7 @@ struct ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -120,7 +123,7 @@ struct ISAM2::Impl { static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode); - static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); }; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 01d6bb2cf..58cc4b23c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -26,7 +26,6 @@ using namespace boost::assign; #include #include -#include #include @@ -120,7 +119,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -FactorGraph +FactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -252,7 +251,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(2,"linearize"); tic(5,"eliminate"); - JunctionTree jt(factors, variableIndex_); + JunctionTree jt(factors, variableIndex_); sharedClique newRoot = jt.eliminate(EliminatePreferLDL); if(debug) newRoot->print("Eliminated: "); toc(5,"eliminate"); @@ -324,7 +323,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(1,"list to set"); tic(2,"PartialSolve"); - typename Impl::ReorderingMode reorderingMode; + Impl::ReorderingMode reorderingMode; reorderingMode.nFullSystemVars = ordering_.nVars(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; @@ -332,7 +331,7 @@ boost::shared_ptr > ISAM2::recalculate( reorderingMode.constrainedKeys = *constrainKeys; else reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); - typename Impl::PartialSolveResult partialSolveResult = + Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); toc(2,"PartialSolve"); @@ -605,7 +604,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); - optimize2(this->root(), delta); + internal::optimizeInPlace(this->root(), delta); return theta_.retract(delta, ordering_); } @@ -616,6 +615,13 @@ const Permuted& ISAM2::getDelta() const { return delta_; } +/* ************************************************************************* */ +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + internal::optimizeInPlace(isam.root(), delta); + return delta; +} + /* ************************************************************************* */ VectorValues optimizeGradientSearch(const ISAM2& isam) { tic(0, "Allocate VectorValues"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d5a1cac52..2d5b16d1b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -186,9 +186,9 @@ struct ISAM2Clique : public BayesTreeCliqueBase shared_ptr; typedef boost::weak_ptr weak_ptr; typedef GaussianConditional ConditionalType; - typedef typename ConditionalType::shared_ptr sharedConditional; + typedef ConditionalType::shared_ptr sharedConditional; - typename Base::FactorType::shared_ptr cachedFactor_; + Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; /** Construct from a conditional */ @@ -196,7 +196,7 @@ struct ISAM2Clique : public BayesTreeCliqueBase >& result) : + ISAM2Clique(const std::pair >& result) : Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { // Compute gradient contribution const ConditionalType& conditional(*result.first); @@ -208,13 +208,13 @@ struct ISAM2Clique : public BayesTreeCliqueBaseclone() : typename Base::FactorType::shared_ptr()))); + cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr()))); copy->gradientContribution_ = gradientContribution_; return copy; } /** Access the cached factor */ - typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } @@ -343,9 +343,9 @@ public: /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ ISAM2(); - typedef typename Base::Clique Clique; ///< A clique - typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class + typedef Base::Clique Clique; ///< A clique + typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef Base::Cliques Cliques; ///< List of Clique typedef from base class void cloneTo(boost::shared_ptr& newISAM2) const { boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); @@ -457,11 +457,7 @@ private: }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -VectorValues optimize(const ISAM2& isam) { - VectorValues delta = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), delta); - return delta; -} +VectorValues optimize(const ISAM2& isam); /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain @@ -541,3 +537,4 @@ void gradientAtZero(const BayesTree& bayesTree } /// namespace gtsam #include +#include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 21f2e62e9..831d17478 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -17,7 +17,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include #include @@ -52,7 +52,7 @@ TEST(ISAM2, AddVariables) { Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); - GaussianISAM2<>::Nodes nodes(2); + ISAM2::Nodes nodes(2); // Verify initial state LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]); @@ -82,11 +82,11 @@ TEST(ISAM2, AddVariables) { Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); - GaussianISAM2<>::Nodes nodesExpected( - 3, GaussianISAM2<>::sharedClique()); + ISAM2::Nodes nodesExpected( + 3, ISAM2::sharedClique()); // Expand initial state - GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); + ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); @@ -171,10 +171,10 @@ TEST(ISAM2, optimize2) { conditional->solveInPlace(expected); // Clique - GaussianISAM2<>::sharedClique clique( - GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); + ISAM2::sharedClique clique( + ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); VectorValues actual(theta.dims(ordering)); - internal::optimizeInPlace(clique, actual); + internal::optimizeInPlace(clique, actual); // expected.print("expected: "); // actual.print("actual: "); @@ -182,7 +182,7 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) { +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) { Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); @@ -212,7 +212,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -300,7 +300,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -345,7 +345,7 @@ TEST(ISAM2, slamlike_solution_dogleg) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -433,7 +433,7 @@ TEST(ISAM2, slamlike_solution_dogleg) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -473,7 +473,7 @@ TEST(ISAM2, clone) { SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); Values fullinit; planarSLAM::Graph fullgraph; @@ -558,8 +558,8 @@ TEST(ISAM2, clone) { } // CLONING... - boost::shared_ptr > isam2 - = boost::shared_ptr >(new GaussianISAM2<>()); + boost::shared_ptr isam2 + = boost::shared_ptr(new ISAM2()); isam.cloneTo(isam2); CHECK(assert_equal(isam, *isam2)); @@ -567,24 +567,23 @@ TEST(ISAM2, clone) { /* ************************************************************************* */ TEST(ISAM2, permute_cached) { - typedef ISAM2Clique Clique; - typedef boost::shared_ptr > sharedClique; + typedef boost::shared_ptr sharedISAM2Clique; // Construct expected permuted BayesTree (variable 2 has been changed to 1) - BayesTree expected; - expected.insert(sharedClique(new Clique(make_pair( + BayesTree expected; + expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (3, Matrix_(1,1,1.0)) (4, Matrix_(1,1,2.0)), 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) HessianFactor::shared_ptr())))); // Cached: empty - expected.insert(sharedClique(new Clique(make_pair( + expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (2, Matrix_(1,1,1.0)) (3, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - expected.insert(sharedClique(new Clique(make_pair( + expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (0, Matrix_(1,1,1.0)) (2, Matrix_(1,1,2.0)), @@ -595,20 +594,20 @@ TEST(ISAM2, permute_cached) { expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1; // Construct unpermuted BayesTree - BayesTree actual; - actual.insert(sharedClique(new Clique(make_pair( + BayesTree actual; + actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (3, Matrix_(1,1,1.0)) (4, Matrix_(1,1,2.0)), 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) HessianFactor::shared_ptr())))); // Cached: empty - actual.insert(sharedClique(new Clique(make_pair( + actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (2, Matrix_(1,1,1.0)) (3, Matrix_(1,1,2.0)), 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - actual.insert(sharedClique(new Clique(make_pair( + actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( boost::make_shared(pair_list_of (0, Matrix_(1,1,1.0)) (2, Matrix_(1,1,2.0)), @@ -646,7 +645,7 @@ TEST(ISAM2, removeFactors) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -740,7 +739,7 @@ TEST(ISAM2, removeFactors) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -785,7 +784,7 @@ TEST(ISAM2, constrained_ordering) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; @@ -883,7 +882,7 @@ TEST(ISAM2, constrained_ordering) (isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12)); // Check gradient at each node - typedef GaussianISAM2<>::sharedClique sharedClique; + typedef ISAM2::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg;