diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 152b02704..7d2cb18cd 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -19,8 +19,8 @@ add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-fail # Sources to remove from builds set (excluded_sources # "") "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentIncrementalFilter.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentIncrementalSmoother.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentIncrementalSmoother.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp" ) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 7a3cfe888..d83e08c09 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -93,15 +93,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No std::set markedKeys; BOOST_FOREACH(gtsam::Key key, *keysToMove) { if(isam2_.getLinearizationPoint().exists(key)) { - Index index = isam2_.getOrdering().at(key); - ISAM2Clique::shared_ptr clique = isam2_[index]; - GaussianConditional::const_iterator index_iter = clique->conditional()->begin(); - while(*index_iter != index) { - markedKeys.insert(isam2_.getOrdering().key(*index_iter)); - ++index_iter; + ISAM2Clique::shared_ptr clique = isam2_[key]; + GaussianConditional::const_iterator key_iter = clique->conditional()->begin(); + while(*key_iter != key) { + markedKeys.insert(*key_iter); + ++key_iter; } - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { - RecursiveMarkAffectedKeys(index, child, isam2_.getOrdering(), markedKeys); + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + RecursiveMarkAffectedKeys(key, child, markedKeys); } } } @@ -261,20 +260,21 @@ void ConcurrentIncrementalFilter::postsync() { gttoc(postsync); } + /* ************************************************************************* */ -void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys) { +void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { + if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique - BOOST_FOREACH(Index idx, clique->conditional()->frontals()) { - additionalKeys.insert(ordering.key(idx)); + BOOST_FOREACH(Key idx, clique->conditional()->frontals()) { + additionalKeys.insert(idx); } // Recursively mark all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { - RecursiveMarkAffectedKeys(index, child, ordering, additionalKeys); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + RecursiveMarkAffectedKeys(key, child, additionalKeys); } } // If the key was not found in the separator/parents, then none of its children can have it either @@ -288,7 +288,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 std::vector removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { - const FastList& slots = variableIndex[isam2.getOrdering().at(key)]; + const FastList& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } @@ -345,27 +345,23 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // Find all cliques that contain any separator variables std::set separatorCliques; BOOST_FOREACH(Key key, separatorKeys) { - Index index = isam2_.getOrdering().at(key); - ISAM2Clique::shared_ptr clique = isam2_[index]; + ISAM2Clique::shared_ptr clique = isam2_[key]; separatorCliques.insert( clique ); } - // Create the set of clique keys - std::vector cliqueIndices; + // Create the set of clique keys LC: std::vector cliqueKeys; BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - BOOST_FOREACH(Index index, clique->conditional()->frontals()) { - cliqueIndices.push_back(index); - cliqueKeys.push_back(isam2_.getOrdering().key(index)); + BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + cliqueKeys.push_back(key); } } - std::sort(cliqueIndices.begin(), cliqueIndices.end()); std::sort(cliqueKeys.begin(), cliqueKeys.end()); // Gather all factors that involve only clique keys std::set cliqueFactorSlots; - BOOST_FOREACH(Index index, cliqueIndices) { - BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[index]) { + BOOST_FOREACH(Key key, cliqueKeys) { + BOOST_FOREACH(size_t slot, isam2_.getVariableIndex()[key]) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { std::set factorKeys(factor->begin(), factor->end()); @@ -391,7 +387,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() std::set childCliques; // Add all of the children BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { - childCliques.insert(clique->children().begin(), clique->children().end()); + childCliques.insert(clique->children.begin(), clique->children.end()); } // Remove any separator cliques that were added because they were children of other separator cliques BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, separatorCliques) { @@ -400,7 +396,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // Augment the factor graph with cached factors from the children BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, childCliques) { - LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getOrdering(), isam2_.getLinearizationPoint())); + LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint())); graph.push_back( factor ); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 6e5216642..364d30c78 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -73,11 +73,6 @@ public: return isam2_.getLinearizationPoint(); } - /** Access the current ordering */ - const Ordering& getOrdering() const { - return isam2_.getOrdering(); - } - /** Access the current set of deltas to the linearization point */ const VectorValues& getDelta() const { return isam2_.getDelta(); @@ -171,7 +166,7 @@ protected: private: /** Traverse the iSAM2 Bayes Tree, inserting all descendants of the provided index/key into 'additionalKeys' */ - static void RecursiveMarkAffectedKeys(Index index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys); + static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys); /** Find the set of iSAM2 factors adjacent to 'keys' */ static std::vector FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore); diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 2341fc449..b5556994c 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,19 +25,19 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) { + if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique - BOOST_FOREACH(Index i, clique->conditional()->frontals()) { - additionalKeys.insert(ordering.key(i)); + BOOST_FOREACH(Key i, clique->conditional()->frontals()) { + additionalKeys.insert(i); } // Recursively mark all of the children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { - recursiveMarkAffectedKeys(index, child, ordering, additionalKeys); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + recursiveMarkAffectedKeys(key, child, additionalKeys); } } // If the key was not found in the separator/parents, then none of its children can have it either @@ -106,10 +106,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Mark additional keys between the marginalized keys and the leaves std::set additionalKeys; BOOST_FOREACH(gtsam::Key key, marginalizableKeys) { - gtsam::Index index = isam_.getOrdering().at(key); - gtsam::ISAM2Clique::shared_ptr clique = isam_[index]; - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { - recursiveMarkAffectedKeys(index, child, isam_.getOrdering(), additionalKeys); + gtsam::ISAM2Clique::shared_ptr clique = isam_[key]; + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + recursiveMarkAffectedKeys(key, child, additionalKeys); } } KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); @@ -184,48 +183,52 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { std::cout << "f("; - BOOST_FOREACH(Index index, factor->keys()) { - std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; + BOOST_FOREACH(gtsam::Key key, factor->keys()) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); } std::cout << " )" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { - PrintSymbolicFactor(factor, ordering); + PrintSymbolicFactor(factor); } } /* ************************************************************************* */ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { std::cout << label << std::endl; - if(isam.root()) - PrintSymbolicTreeHelper(isam.root(), isam.getOrdering()); + if(!isam.roots().empty()) + { + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){ + PrintSymbolicTreeHelper(root); + } + } else std::cout << "{Empty Tree}" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; - BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { + std::cout << gtsam::DefaultKeyFormatter(key) << " "; } if(clique->conditional()->nrParents() > 0) std::cout << "| "; - BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { - std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; + BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { + std::cout << gtsam::DefaultKeyFormatter(key) << " "; } std::cout << ")" << std::endl; // Recursively print all of the children - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { - PrintSymbolicTreeHelper(child, ordering, indent+" "); + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + PrintSymbolicTreeHelper(child, indent+" "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 9434e3ecb..ceb15f1f1 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -89,11 +89,6 @@ public: return isam_.getLinearizationPoint(); } - /** Access the current ordering */ - const Ordering& getOrdering() const { - return isam_.getOrdering(); - } - /** Access the current set of deltas to the linearization point */ const VectorValues& getDelta() const { return isam_.getDelta(); @@ -113,10 +108,10 @@ protected: private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); - static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:"); + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:"); static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent = ""); + static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = ""); }; // IncrementalFixedLagSmoother diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index a2941acaa..e67f1a346 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -83,7 +83,7 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newFactors.push_back(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); newValues.insert(key0, Point2(0.01, 0.01)); newTimestamps[key0] = 0.0; @@ -108,7 +108,7 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -134,8 +134,8 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); - newFactors.add(BetweenFactor(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -160,7 +160,7 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; - newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i);