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

First pass at parallelizing linearization

release/4.3a0
Richard Roberts 2013-08-16 22:17:28 +00:00
parent 09c4d8b712
commit a5826dff93
1 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -28,6 +28,10 @@
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#ifdef GTSAM_USE_TBB
# include <tbb/parallel_for.h>
#endif
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
@ -257,6 +261,31 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>
// return make_pair(symbolic(*ordering), ordering); // return make_pair(symbolic(*ordering), ordering);
//} //}
/* ************************************************************************* */
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();
}
}
};
#endif
}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& linearizationPoint) const
{ {
@ -264,6 +293,15 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
// create an empty linear FG // create an empty linear FG
GaussianFactorGraph::shared_ptr linearFG = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph::shared_ptr linearFG = boost::make_shared<GaussianFactorGraph>();
#ifdef GTSAM_USE_TBB
linearFG->resize(this->size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, this->size()),
_LinearizeOneFactor(*this, linearizationPoint, *linearFG));
#else
linearFG->reserve(this->size()); linearFG->reserve(this->size());
// linearize all factors // linearize all factors
@ -274,6 +312,8 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
(*linearFG) += GaussianFactor::shared_ptr(); (*linearFG) += GaussianFactor::shared_ptr();
} }
#endif
return linearFG; return linearFG;
} }