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 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()() /** 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 * 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 * 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()( // VectorValuesUnordered operator()(
const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, // const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
const VectorValuesUnordered& parentSolution) // const VectorValuesUnordered& parentSolution)
{ // {
// parents are assumed to already be solved and available in result // // parents are assumed to already be solved and available in result
VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution); // VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution);
collectedResult.insert(cliqueSolution); // collectedResult.insert(cliqueSolution);
return 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) double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum)
{ {
parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum(); parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
assert(false);
return 0;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const { GaussianBayesTreeUnordered::GaussianBayesTreeUnordered(const GaussianBayesTreeUnordered& other) :
internal::OptimizeClique visitor; Base(other) {}
VectorValuesUnordered empty;
treeTraversal::DepthFirstForest(*this, empty, visitor); /* ************************************************************************* */
return visitor.collectedResult; 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;
} }
///* ************************************************************************* */ ///* ************************************************************************* */