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

72
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

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