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 // 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. // resulting conditional to the BayesTree, and add the remaining factor to the parent.
struct EliminationPostOrderVisitor { class EliminationPostOrderVisitor {
const typename CLUSTERTREE::Eliminate& eliminationFunction; const typename CLUSTERTREE::Eliminate& eliminationFunction_;
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_;
public:
// Construct functor
EliminationPostOrderVisitor( EliminationPostOrderVisitor(
const typename CLUSTERTREE::Eliminate& eliminationFunction, const typename CLUSTERTREE::Eliminate& eliminationFunction,
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : 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, void operator()(const typename CLUSTERTREE::sharedNode& node,
EliminationData& myData) { EliminationData& myData) {
assert(node); assert(node);
@ -90,10 +95,11 @@ struct EliminationData {
} }
} }
// Do dense elimination step // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>>
std::pair<boost::shared_ptr<ConditionalType>, std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> > eliminationResult = 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 // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
myData.bayesTreeNode->setEliminationResult(eliminationResult); 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 // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
// object they're added to. // object they're added to.
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) 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 // Store remaining factor in parent's gathered factors
if (!eliminationResult.second->empty()) if (!eliminationResult.second->empty())

33
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

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