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 { 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;
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; // // Create data - holds a pointer to our parent, a copy of parent solution, and our results
// OptimizeData myData;
// VectorValuesUnordered operator()( // myData.parentData = parentData;
// const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, // // Take any ancestor results we'll need
// const VectorValuesUnordered& parentSolution) // BOOST_FOREACH(Key parent, clique->conditional_->parents())
// { // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// // parents are assumed to already be solved and available in result // // Solve and store in our results
// VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution); // myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
// collectedResult.insert(cliqueSolution); // myData.ancestorResults.insert(myData.results);
// return cliqueSolution; // return myData;
// } //}
//};
/* ************************************************************************* */ /* ************************************************************************* */
OptimizeData OptimizePreVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& parentData) //void OptimizePostVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& myData)
{ //{
// Create data - holds a pointer to our parent, a copy of parent solution, and our results // // Conglomerate our results to the parent
OptimizeData myData; // myData.parentData->results.insert(myData.results);
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;
} }
///* ************************************************************************* */ ///* ************************************************************************* */