Removed unused code from Concurrent Batch Smoother
Stephen Williams
parent
3e754ecd70
commit
1952337e23
|
|
@ -527,149 +527,6 @@ void ConcurrentBatchSmoother::EliminationForest::removeChildrenIndices(std::set<
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
///* ************************************************************************* */
|
|
||||||
//std::set<size_t> ConcurrentBatchSmoother::findFactorsWithAny(const std::set<Key>& keys) const {
|
|
||||||
// // Find the set of factor slots for each specified key
|
|
||||||
// std::set<size_t> 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<size_t> ConcurrentBatchSmoother::findFactorsWithOnly(const std::set<Key>& keys) const {
|
|
||||||
// // Find the set of factor slots with any of the provided keys
|
|
||||||
// std::set<size_t> factorSlots = findFactorsWithAny(keys);
|
|
||||||
// // Test each factor for non-specified keys
|
|
||||||
// std::set<size_t>::iterator slot = factorSlots.begin();
|
|
||||||
// while(slot != factorSlots.end()) {
|
|
||||||
// const NonlinearFactor::shared_ptr& factor = graph_.at(*slot);
|
|
||||||
// std::set<Key> 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<Key>& keysToKeep, const Values& theta) const {
|
|
||||||
//
|
|
||||||
//factor->print("Factor Before:\n");
|
|
||||||
//
|
|
||||||
// // Sort the keys for this factor
|
|
||||||
// std::set<Key> factorKeys;
|
|
||||||
// BOOST_FOREACH(Key key, *factor) {
|
|
||||||
// factorKeys.insert(key);
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// // Calculate the set of keys to marginalize
|
|
||||||
// std::set<Key> marginalizeKeys;
|
|
||||||
// std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
|
|
||||||
// std::set<Key> 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<Index> variables;
|
|
||||||
// BOOST_FOREACH(Key key, marginalizeKeys) {
|
|
||||||
// variables.push_back(ordering.at(key));
|
|
||||||
// }
|
|
||||||
//// std::pair<GaussianFactorGraph::sharedConditional, GaussianFactorGraph> 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
|
}/// namespace gtsam
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue