12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Some speedups in back-substitution

release/4.3a0
Richard Roberts 2013-07-26 18:00:37 +00:00
parent d3afcf9770
commit da10417cfc
1 changed files with 47 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

@ -33,7 +33,7 @@ namespace gtsam {
struct OptimizeData {
boost::optional<OptimizeData&> parentData;
VectorValuesUnordered ancestorResults;
VectorValuesUnordered results;
//VectorValuesUnordered results;
};
/* ************************************************************************* */
@ -43,42 +43,48 @@ namespace gtsam {
* solution that will finally be returned to the user. The reason we pass the individual
* clique solutions between nodes is to avoid log(n) lookups over all variables, they instead
* then are only over a node's parent variables. */
//struct OptimizeClique
struct OptimizeClique
{
VectorValuesUnordered collectedResult;
OptimizeData operator()(
const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
OptimizeData& parentData)
{
OptimizeData myData;
myData.parentData = parentData;
// Take any ancestor results we'll need
//BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// Solve and store in our results
VectorValuesUnordered result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/);
collectedResult.insert(result);
//myData.ancestorResults.insert(result);
return myData;
}
};
/* ************************************************************************* */
//OptimizeData OptimizePreVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& parentData)
//{
// VectorValuesUnordered collectedResult;
// VectorValuesUnordered operator()(
// const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
// const VectorValuesUnordered& parentSolution)
// {
// // parents are assumed to already be solved and available in result
// VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution);
// collectedResult.insert(cliqueSolution);
// return cliqueSolution;
// }
//};
// // Create data - holds a pointer to our parent, a copy of parent solution, and our results
// OptimizeData myData;
// myData.parentData = parentData;
// // Take any ancestor results we'll need
// BOOST_FOREACH(Key parent, clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// // Solve and store in our results
// myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
// myData.ancestorResults.insert(myData.results);
// return myData;
//}
/* ************************************************************************* */
OptimizeData OptimizePreVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& parentData)
{
// Create data - holds a pointer to our parent, a copy of parent solution, and our results
OptimizeData myData;
myData.parentData = parentData;
// Take any ancestor results we'll need
BOOST_FOREACH(Key parent, clique->conditional_->parents())
myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// Solve and store in our results
myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
myData.ancestorResults.insert(myData.results);
return myData;
}
/* ************************************************************************* */
void OptimizePostVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& myData)
{
// Conglomerate our results to the parent
myData.parentData->results.insert(myData.results);
}
//void OptimizePostVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& myData)
//{
// // Conglomerate our results to the parent
// myData.parentData->results.insert(myData.results);
//}
/* ************************************************************************* */
double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
@ -110,9 +116,13 @@ namespace gtsam {
VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const
{
gttic(GaussianBayesTree_optimize);
internal::OptimizeData rootData; // Will hold final solution
treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
return rootData.results;
//internal::OptimizeData rootData; // Will hold final solution
//treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
//return rootData.results;
internal::OptimizeData rootData;
internal::OptimizeClique preVisitor;
treeTraversal::DepthFirstForest(*this, rootData, preVisitor);
return preVisitor.collectedResult;
}
///* ************************************************************************* */