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

Implemented Gaussian back-sub (also includes some cleanups from a few commits ago)

release/4.3a0
Richard Roberts 2013-07-09 17:50:46 +00:00
parent 2ce65f4cf8
commit 3def2db1a4
1 changed files with 66 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
gtsam/linear/GaussianBayesTreeUnordered.cpp
View File

@ -29,6 +29,13 @@ namespace gtsam {
/* ************************************************************************* */
namespace internal
{
/* ************************************************************************* */
struct OptimizeData {
boost::optional<OptimizeData&> parentData;
VectorValuesUnordered ancestorResults;
VectorValuesUnordered results;
};
/* ************************************************************************* */
/** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()()
* optimizes the clique given the solution for the parents, and returns the solution for the
@ -36,34 +43,75 @@ 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
{
VectorValuesUnordered collectedResult;
//struct OptimizeClique
//{
// 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;
}
};
// 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;
// }
//};
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
{
parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
assert(false);
return 0;
}
}
/* ************************************************************************* */
VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const {
internal::OptimizeClique visitor;
VectorValuesUnordered empty;
treeTraversal::DepthFirstForest(*this, empty, visitor);
return visitor.collectedResult;
GaussianBayesTreeUnordered::GaussianBayesTreeUnordered(const GaussianBayesTreeUnordered& other) :
Base(other) {}
/* ************************************************************************* */
GaussianBayesTreeUnordered& GaussianBayesTreeUnordered::operator=(const GaussianBayesTreeUnordered& other)
{
(void) Base::operator=(other);
return *this;
}
/* ************************************************************************* */
bool GaussianBayesTreeUnordered::equals(const This& other, double tol) const
{
return Base::equals(other, tol);
}
/* ************************************************************************* */
VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const
{
internal::OptimizeData rootData; // Will hold final solution
treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
return rootData.results;
}
///* ************************************************************************* */