From a5826dff935ca2335fd5fad0d515083b2a89d68b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 16 Aug 2013 22:17:28 +0000
Subject: [PATCH] First pass at parallelizing linearization

---
 gtsam/nonlinear/NonlinearFactorGraph.cpp | 40 ++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 555e7a22d..fef36a685 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -28,6 +28,10 @@
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
+#ifdef GTSAM_USE_TBB
+#  include <tbb/parallel_for.h>
+#endif
+
 using namespace std;
 
 namespace gtsam {
@@ -257,6 +261,31 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>
 //  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
 {
@@ -264,6 +293,15 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
 
   // create an empty linear FG
   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());
 
   // linearize all factors
@@ -274,6 +312,8 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
       (*linearFG) += GaussianFactor::shared_ptr();
   }
 
+#endif
+
   return linearFG;
 }