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

Documentation and formatting

release/4.3a0
dellaert 2015-06-21 14:55:30 -07:00
parent b546a1f0a7
commit f9bf63b71c
2 changed files with 34 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/inference/ClusterTree-inst.h
View File

@ -63,14 +63,19 @@ struct EliminationData {
// Elimination post-order visitor - combine the child factors with our own factors, add the
// resulting conditional to the BayesTree, and add the remaining factor to the parent.
struct EliminationPostOrderVisitor {
const typename CLUSTERTREE::Eliminate& eliminationFunction;
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex;
class EliminationPostOrderVisitor {
const typename CLUSTERTREE::Eliminate& eliminationFunction_;
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_;
public:
// Construct functor
EliminationPostOrderVisitor(
const typename CLUSTERTREE::Eliminate& eliminationFunction,
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {
eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) {
}
// Function that does the HEAVY lifting
void operator()(const typename CLUSTERTREE::sharedNode& node,
EliminationData& myData) {
assert(node);
@ -90,10 +95,11 @@ struct EliminationData {
}
}
// Do dense elimination step
// >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>>
std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
eliminationFunction_(gatheredFactors, node->orderedFrontalKeys);
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
myData.bayesTreeNode->setEliminationResult(eliminationResult);
@ -102,7 +108,7 @@ struct EliminationData {
// putting orphan subtrees in the index - they'll already be in the index of the ISAM2
// object they're added to.
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode));
nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
// Store remaining factor in parent's gathered factors
if (!eliminationResult.second->empty())

39
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -276,23 +276,26 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const
namespace {
#ifdef GTSAM_USE_TBB
struct _LinearizeOneFactor {
const NonlinearFactorGraph& graph;
const Values& linearizationPoint;
GaussianFactorGraph& result;
_LinearizeOneFactor(const NonlinearFactorGraph& graph, const Values& linearizationPoint, GaussianFactorGraph& result) :
graph(graph), linearizationPoint(linearizationPoint), result(result) {}
void operator()(const tbb::blocked_range<size_t>& r) const
{
for(size_t i = r.begin(); i != r.end(); ++i)
{
if(graph[i])
result[i] = graph[i]->linearize(linearizationPoint);
else
result[i] = GaussianFactor::shared_ptr();
}
class _LinearizeOneFactor {
const NonlinearFactorGraph& nonlinearGraph_;
const Values& linearizationPoint_;
GaussianFactorGraph& result_;
public:
// Create functor with constant parameters
_LinearizeOneFactor(const NonlinearFactorGraph& graph,
const Values& linearizationPoint, GaussianFactorGraph& result) :
nonlinearGraph_(graph), linearizationPoint_(linearizationPoint), result_(result) {
}
// Operator that linearizes a given range of the factors
void operator()(const tbb::blocked_range<size_t>& blocked_range) const {
for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) {
if (nonlinearGraph_[i])
result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_);
else
result_[i] = GaussianFactor::shared_ptr();
}
};
}
};
#endif
}
@ -319,9 +322,9 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
// linearize all factors
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) {
(*linearFG) += factor->linearize(linearizationPoint);
(*linearFG) += factor->linearize(linearizationPoint_);
} else
(*linearFG) += GaussianFactor::shared_ptr();
(*linearFG) += GaussianFactor::shared_ptr();
}
#endif