support GTSAM built with TBB

include/glim/mapping/global_mapping.hpp

@@ -14,6 +14,7 @@ class NonlinearFactorGraph;
 namespace gtsam_points {
 class ISAM2Ext;
 class StreamTempBufferRoundRobin;
+struct ISAM2ResultExt;
 }  // namespace gtsam_points
 
 namespace glim {
@@ -81,6 +82,7 @@ class GlobalMapping : public GlobalMappingBase {
   boost::shared_ptr<gtsam::NonlinearFactorGraph> create_matching_cost_factors(int current) const;
 
   void update_submaps();
+  gtsam_points::ISAM2ResultExt update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values);
 
 private:
   using Params = GlobalMappingParams;
@@ -98,5 +100,7 @@ class GlobalMapping : public GlobalMappingBase {
   std::unique_ptr<gtsam::NonlinearFactorGraph> new_factors;
 
   std::unique_ptr<gtsam_points::ISAM2Ext> isam2;
+
+  std::shared_ptr<void> tbb_task_arena;
 };
 }  // namespace glim

include/glim/mapping/global_mapping_pose_graph.hpp

@@ -129,5 +129,7 @@ class GlobalMappingPoseGraph : public GlobalMappingBase {
   std::unique_ptr<gtsam::NonlinearFactorGraph> new_factors;
 
   std::unique_ptr<gtsam_points::ISAM2Ext> isam2;
+
+  std::shared_ptr<void> tbb_task_arena;
 };
 }  // namespace glim

include/glim/mapping/sub_mapping.hpp

@@ -96,6 +96,8 @@ class SubMapping : public SubMappingBase {
   std::unique_ptr<gtsam::NonlinearFactorGraph> graph;
 
   std::vector<SubMap::Ptr> submap_queue;
+
+  std::shared_ptr<void> tbb_task_arena;
 };
 
 }  // namespace glim

include/glim/odometry/odometry_estimation_ct.hpp

@@ -82,6 +82,8 @@ class OdometryEstimationCT : public OdometryEstimationBase {
   // Optimizer
   using FixedLagSmootherExt = gtsam_points::IncrementalFixedLagSmootherExtWithFallback;
   std::unique_ptr<FixedLagSmootherExt> smoother;
+
+  std::shared_ptr<void> tbb_task_arena;
 };
 
 }  // namespace glim

include/glim/odometry/odometry_estimation_imu.hpp

@@ -1,12 +1,12 @@
 #pragma once
 
+#include <map>
 #include <memory>
 #include <random>
 
 #include <boost/shared_ptr.hpp>
 #include <glim/odometry/odometry_estimation_base.hpp>
 
-
 namespace gtsam {
 class Pose3;
 class Values;
@@ -59,7 +59,8 @@ struct OdometryEstimationIMUParams {
   // Logging params
   bool save_imu_rate_trajectory;
 
-  int num_threads;
+  int num_threads;                  // Number of threads for preprocessing and per-factor parallelism
+  int num_smoother_update_threads;  // Number of threads for TBB parallelism in smoother update (should be kept 1)
 };
 
 /**
@@ -83,6 +84,10 @@ class OdometryEstimationIMU : public OdometryEstimationBase {
   virtual void fallback_smoother() {}
   virtual void update_frames(const int current, const gtsam::NonlinearFactorGraph& new_factors);
 
+  virtual void
+  update_smoother(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values, const std::map<std::uint64_t, double>& new_stamp, int update_count = 0);
+  virtual void update_smoother(int update_count = 1);
+
 protected:
   std::unique_ptr<OdometryEstimationIMUParams> params;
 
@@ -103,6 +108,8 @@ class OdometryEstimationIMU : public OdometryEstimationBase {
   // Optimizer
   using FixedLagSmootherExt = gtsam_points::IncrementalFixedLagSmootherExtWithFallback;
   std::unique_ptr<FixedLagSmootherExt> smoother;
+
+  std::shared_ptr<void> tbb_task_arena;
 };
 
 }  // namespace glim

include/glim/viewer/interactive/manual_loop_close_modal.hpp

@@ -53,6 +53,8 @@ class ManualLoopCloseModal {
 
   glk::Drawable::ConstPtr target_drawable;
   glk::Drawable::ConstPtr source_drawable;
+
+  std::shared_ptr<void> tbb_task_arena;
 };
 
 }  // namespace glim

src/glim/mapping/global_mapping.cpp

@@ -30,6 +30,10 @@
 #include <glim/common/imu_integration.hpp>
 #include <glim/mapping/callbacks.hpp>
 
+#ifdef GTSAM_USE_TBB
+#include <tbb/task_arena.h>
+#endif
+
 namespace glim {
 
 using gtsam::symbol_shorthand::B;
@@ -96,6 +100,10 @@ GlobalMapping::GlobalMapping(const GlobalMappingParams& params) : params(params)
 #ifdef BUILD_GTSAM_POINTS_GPU
   stream_buffer_roundrobin = std::make_shared<gtsam_points::StreamTempBufferRoundRobin>(64);
 #endif
+
+#ifdef GTSAM_USE_TBB
+  tbb_task_arena = std::make_shared<tbb::task_arena>(1);
+#endif
 }
 
 GlobalMapping::~GlobalMapping() {}
@@ -202,7 +210,7 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) {
 
   Callbacks::on_smoother_update(*isam2, *new_factors, *new_values);
   try {
-    auto result = isam2->update(*new_factors, *new_values);
+    auto result = update_isam2(*new_factors, *new_values);
     Callbacks::on_smoother_update_result(*isam2, result);
   } catch (std::exception& e) {
     logger->error("an exception was caught during global map optimization!!");
@@ -323,7 +331,7 @@ void GlobalMapping::find_overlapping_submaps(double min_overlap) {
 
   gtsam::Values new_values;
   Callbacks::on_smoother_update(*isam2, new_factors, new_values);
-  auto result = isam2->update(new_factors, new_values);
+  auto result = update_isam2(new_factors, new_values);
   Callbacks::on_smoother_update_result(*isam2, result);
 
   update_submaps();
@@ -338,7 +346,7 @@ void GlobalMapping::optimize() {
   gtsam::NonlinearFactorGraph new_factors;
   gtsam::Values new_values;
   Callbacks::on_smoother_update(*isam2, new_factors, new_values);
-  auto result = isam2->update(new_factors, new_values);
+  auto result = update_isam2(new_factors, new_values);
 
   Callbacks::on_smoother_update_result(*isam2, result);
 
@@ -378,8 +386,16 @@ boost::shared_ptr<gtsam::NonlinearFactorGraph> GlobalMapping::create_between_fac
   lm_params.setMaxIterations(10);
   lm_params.callback = [this](const auto& status, const auto& values) { logger->debug(status.to_string()); };
 
-  gtsam_points::LevenbergMarquardtOptimizerExt optimizer(graph, values, lm_params);
-  values = optimizer.optimize();
+#ifdef GTSAM_USE_TBB
+  auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+  arena->execute([&] {
+#endif
+    gtsam_points::LevenbergMarquardtOptimizerExt optimizer(graph, values, lm_params);
+    values = optimizer.optimize();
+
+#ifdef GTSAM_USE_TBB
+  });
+#endif
 
   const gtsam::Pose3 estimated_delta = values.at<gtsam::Pose3>(X(1));
   const auto linearized = factor->linearize(values);
@@ -439,6 +455,21 @@ void GlobalMapping::update_submaps() {
   }
 }
 
+gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values) {
+  gtsam_points::ISAM2ResultExt result;
+
+#ifdef GTSAM_USE_TBB
+  auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+  arena->execute([&] {
+#endif
+    result = isam2->update(new_factors, new_values);
+#ifdef GTSAM_USE_TBB
+  });
+#endif
+
+  return result;
+}
+
 void GlobalMapping::save(const std::string& path) {
   optimize();
 
@@ -654,7 +685,7 @@ bool GlobalMapping::load(const std::string& path) {
 
   logger->info("optimize");
   Callbacks::on_smoother_update(*isam2, graph, values);
-  auto result = isam2->update(graph, values);
+  auto result = update_isam2(graph, values);
   Callbacks::on_smoother_update_result(*isam2, result);
 
   update_submaps();

src/glim/mapping/global_mapping_pose_graph.cpp

@@ -21,6 +21,10 @@
 #include <glim/util/serialization.hpp>
 #include <glim/mapping/callbacks.hpp>
 
+#ifdef GTSAM_USE_TBB
+#include <tbb/task_arena.h>
+#endif
+
 namespace glim {
 
 using gtsam::symbol_shorthand::B;
@@ -81,6 +85,10 @@ GlobalMappingPoseGraph::GlobalMappingPoseGraph(const GlobalMappingPoseGraphParam
     isam2.reset(new gtsam_points::ISAM2ExtDummy(isam2_params));
   }
 
+#ifdef GTSAM_USE_TBB
+  tbb_task_arena.reset(new tbb::task_arena(params.num_threads));
+#endif
+
   kill_switch = false;
   loop_detection_thread = std::thread([this] { loop_detection_task(); });
 }
@@ -134,8 +142,18 @@ void GlobalMappingPoseGraph::insert_submap(const SubMap::Ptr& submap) {
 
   Callbacks::on_smoother_update(*isam2, *new_factors, *new_values);
   try {
-    auto result = isam2->update(*new_factors, *new_values);
+    gtsam_points::ISAM2ResultExt result;
+#ifdef GTSAM_USE_TBB
+    auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+    arena->execute([&] {
+#endif
+      result = isam2->update(*new_factors, *new_values);
+#ifdef GTSAM_USE_TBB
+    });
+#endif
+
     Callbacks::on_smoother_update_result(*isam2, result);
+
   } catch (std::exception& e) {
     logger->error("an exception was caught during global map optimization!!");
     logger->error(e.what());
@@ -157,7 +175,16 @@ void GlobalMappingPoseGraph::optimize() {
   new_factors.add(*collect_detected_loops());
 
   Callbacks::on_smoother_update(*isam2, new_factors, new_values);
-  auto result = isam2->update(new_factors, new_values);
+
+  gtsam_points::ISAM2ResultExt result;
+#ifdef GTSAM_USE_TBB
+  auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+  arena->execute([&] {
+#endif
+    result = isam2->update(new_factors, new_values);
+#ifdef GTSAM_USE_TBB
+  });
+#endif
 
   Callbacks::on_smoother_update_result(*isam2, result);
 
@@ -372,7 +399,16 @@ void GlobalMappingPoseGraph::loop_detection_task() {
 
         gtsam_points::LevenbergMarquardtExtParams lm_params;
         lm_params.setMaxIterations(10);
-        values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
+
+#ifdef GTSAM_USE_TBB
+        auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+        arena->execute([&] {
+#endif
+          values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
+
+#ifdef GTSAM_USE_TBB
+        });
+#endif
 
         error = factor->error(values);
         inlier_fraction = factor->inlier_fraction();
@@ -384,7 +420,16 @@ void GlobalMappingPoseGraph::loop_detection_task() {
 
         gtsam_points::LevenbergMarquardtExtParams lm_params;
         lm_params.setMaxIterations(10);
-        values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
+
+#ifdef GTSAM_USE_TBB
+        auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+        arena->execute([&] {
+#endif
+          values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
+
+#ifdef GTSAM_USE_TBB
+        });
+#endif
 
         error = factor->error(values);
         inlier_fraction = factor->inlier_fraction();

src/glim/mapping/sub_mapping.cpp

@@ -25,6 +25,10 @@
 #include <glim/common/cloud_covariance_estimation.hpp>
 #include <glim/mapping/callbacks.hpp>
 
+#ifdef GTSAM_USE_TBB
+#include <tbb/task_arena.h>
+#endif
+
 namespace glim {
 
 using gtsam::symbol_shorthand::B;
@@ -81,6 +85,10 @@ SubMapping::SubMapping(const SubMappingParams& params) : params(params) {
   stream = std::make_shared<gtsam_points::CUDAStream>();
   stream_buffer_roundrobin = std::make_shared<gtsam_points::StreamTempBufferRoundRobin>(8);
 #endif
+
+#ifdef GTSAM_USE_TBB
+  tbb_task_arena = std::make_shared<tbb::task_arena>(1);
+#endif
 }
 
 SubMapping::~SubMapping() {}
@@ -133,7 +141,15 @@ void SubMapping::insert_frame(const EstimationFrame::ConstPtr& odom_frame_) {
     lm_params.setAbsoluteErrorTol(1e-6);
     lm_params.setRelativeErrorTol(1e-6);
     lm_params.setMaxIterations(5);
-    values = gtsam::LevenbergMarquardtOptimizer(graph, values, lm_params).optimize();
+
+#ifdef GTSAM_USE_TBB
+    auto arena = static_cast<tbb::task_arena*>(this->tbb_task_arena.get());
+    arena->execute([&] {
+#endif
+      values = gtsam::LevenbergMarquardtOptimizer(graph, values, lm_params).optimize();
+#ifdef GTSAM_USE_TBB
+    });
+#endif
 
     odom_frame->imu_rate_trajectory.resize(8, imu_stamps.size());
     for (int i = 0; i < imu_stamps.size(); i++) {
@@ -410,8 +426,16 @@ SubMap::Ptr SubMapping::create_submap(bool force_create) const {
   gtsam_points::LevenbergMarquardtOptimizerExt optimizer(*graph, *values, lm_params);
   if (params.enable_optimization) {
     try {
-      gtsam::Values optimized = optimizer.optimize();
-      *values = optimized;
+#ifdef GTSAM_USE_TBB
+      auto arena = static_cast<tbb::task_arena*>(this->tbb_task_arena.get());
+      arena->execute([&] {
+#endif
+        gtsam::Values optimized = optimizer.optimize();
+        *values = optimized;
+
+#ifdef GTSAM_USE_TBB
+      });
+#endif
     } catch (std::exception& e) {
       logger->error("an exception was caught during sub map optimization");
       logger->error(e.what());

src/glim/odometry/odometry_estimation_cpu.cpp

@@ -21,6 +21,10 @@
 
 #include <glim/odometry/callbacks.hpp>
 
+#ifdef GTSAM_USE_TBB
+#include <tbb/task_arena.h>
+#endif
+
 namespace glim {
 
 using Callbacks = OdometryEstimationCallbacks;
@@ -134,7 +138,15 @@ gtsam::NonlinearFactorGraph OdometryEstimationCPU::create_factors(const int curr
   // Optimize
   // lm_params.setDiagonalDamping(true);
   gtsam_points::LevenbergMarquardtOptimizerExt optimizer(graph, values, lm_params);
-  values = optimizer.optimize();
+
+#ifdef GTSAM_USE_TBB
+  auto arena = static_cast<tbb::task_arena*>(this->tbb_task_arena.get());
+  arena->execute([&] {
+#endif
+    values = optimizer.optimize();
+#ifdef GTSAM_USE_TBB
+  });
+#endif
 
   const Eigen::Isometry3d T_target_imu = Eigen::Isometry3d(values.at<gtsam::Pose3>(X(current)).matrix());
   Eigen::Isometry3d T_last_current = last_T_target_imu.inverse() * T_target_imu;