From 1952337e23e62f486acddf3d2e2256c3515c6ef8 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 10 Apr 2013 21:08:52 +0000 Subject: [PATCH] Removed unused code from Concurrent Batch Smoother --- .../nonlinear/ConcurrentBatchSmoother.cpp | 143 ------------------ 1 file changed, 143 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index ede077665..6ba9a3bd0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -527,149 +527,6 @@ void ConcurrentBatchSmoother::EliminationForest::removeChildrenIndices(std::set< } } - - - -///* ************************************************************************* */ -//std::set ConcurrentBatchSmoother::findFactorsWithAny(const std::set& keys) const { -// // Find the set of factor slots for each specified key -// std::set factorSlots; -// BOOST_FOREACH(Key key, keys) { -// FactorIndex::const_iterator iter = factorIndex_.find(key); -// if(iter != factorIndex_.end()) { -// factorSlots.insert(iter->second.begin(), iter->second.end()); -// } -// } -// -// return factorSlots; -//} -// -///* ************************************************************************* */ -//std::set ConcurrentBatchSmoother::findFactorsWithOnly(const std::set& keys) const { -// // Find the set of factor slots with any of the provided keys -// std::set factorSlots = findFactorsWithAny(keys); -// // Test each factor for non-specified keys -// std::set::iterator slot = factorSlots.begin(); -// while(slot != factorSlots.end()) { -// const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); -// std::set factorKeys(factor->begin(), factor->end()); // ensure the keys are sorted -// if(!std::includes(keys.begin(), keys.end(), factorKeys.begin(), factorKeys.end())) { -// factorSlots.erase(slot++); -// } else { -// ++slot; -// } -// } -// -// return factorSlots; -//} -// -///* ************************************************************************* */ -//NonlinearFactor::shared_ptr ConcurrentBatchSmoother::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& keysToKeep, const Values& theta) const { -// -//factor->print("Factor Before:\n"); -// -// // Sort the keys for this factor -// std::set factorKeys; -// BOOST_FOREACH(Key key, *factor) { -// factorKeys.insert(key); -// } -// -// // Calculate the set of keys to marginalize -// std::set marginalizeKeys; -// std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); -// std::set remainingKeys; -// std::set_intersection(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(remainingKeys, remainingKeys.end())); -// -// // -// if(marginalizeKeys.size() == 0) { -// // No keys need to be marginalized out. Simply return the original factor. -// return factor; -// } else if(marginalizeKeys.size() == factor->size()) { -// // All keys need to be marginalized out. Return an empty factor -// return NonlinearFactor::shared_ptr(); -// } else { -// // (0) Create an ordering with the remaining keys last -// Ordering ordering; -// BOOST_FOREACH(Key key, marginalizeKeys) { -// ordering.push_back(key); -// } -// BOOST_FOREACH(Key key, remainingKeys) { -// ordering.push_back(key); -// } -//ordering.print("Ordering:\n"); -// -// // (1) construct a linear factor graph -// GaussianFactorGraph graph; -// graph.push_back( factor->linearize(theta, ordering) ); -//graph.at(0)->print("Linear Factor Before:\n"); -// -// // (2) solve for the marginal factor -// // Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables) -// // and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2 -// std::vector variables; -// BOOST_FOREACH(Key key, marginalizeKeys) { -// variables.push_back(ordering.at(key)); -// } -//// std::pair result = graph.eliminate(variables); -// GaussianFactorGraph::EliminationResult result = EliminateQR(graph, marginalizeKeys.size()); -//result.first->print("Resulting Conditional:\n"); -//result.second->print("Resulting Linear Factor:\n"); -//// graph = result.second; -// graph.replace(0, result.second); -// -// // (3) convert the marginal factors into Linearized Factors -// NonlinearFactor::shared_ptr marginalFactor; -// assert(graph.size() <= 1); -// if(graph.size() > 0) { -//graph.at(0)->print("Linear Factor After:\n"); -// marginalFactor.reset(new LinearContainerFactor(graph.at(0), ordering, theta)); -// } -//marginalFactor->print("Factor After:\n"); -// return marginalFactor; -// } -//} - - - -///* ************************************************************************* */ -//void ConcurrentBatchSmoother::PrintSingleClique(const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { -// std::cout << indent << "P( "; -// BOOST_FOREACH(Index index, clique->conditional()->frontals()){ -// std::cout << keyFormatter(ordering.key(index)) << " "; -// } -// if(clique->conditional()->nrParents() > 0){ -// std::cout << "| "; -// BOOST_FOREACH(Index index, clique->conditional()->parents()){ -// std::cout << keyFormatter(ordering.key(index)) << " "; -// } -// } -// std::cout << ")" << std::endl; -//} -// -///* ************************************************************************* */ -//void ConcurrentBatchSmoother::PrintRecursiveClique(const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { -// -// // Print this node -// PrintSingleClique(clique, ordering, indent, keyFormatter); -// -// // Print Children -// BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { -// PrintRecursiveClique(child, ordering, indent+" ", keyFormatter); -// } -//} -// -///* ************************************************************************* */ -//void ConcurrentBatchSmoother::PrintBayesTree(const ISAM2& bayesTree, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) { -// -// std::cout << indent << "Bayes Tree:" << std::endl; -// if (bayesTree.root().use_count() == 0) { -// std::cout << indent << " {EMPTY}" << std::endl; -// } else { -// std::cout << indent << " clique size == " << bayesTree.size() << ", node size == " << bayesTree.nodes().size() << std::endl; -// PrintRecursiveClique(bayesTree.root(), ordering, indent+" ", keyFormatter); -// } -//} - /* ************************************************************************* */ }/// namespace gtsam