From ac7f1f3f9738714b66e7f3e2534c863c3e4d2442 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Wed, 14 Aug 2024 14:17:23 +0900 Subject: [PATCH] support GTSAM built with TBB --- include/glim/mapping/global_mapping.hpp | 4 ++ .../mapping/global_mapping_pose_graph.hpp | 2 + include/glim/mapping/sub_mapping.hpp | 2 + .../glim/odometry/odometry_estimation_ct.hpp | 2 + .../glim/odometry/odometry_estimation_imu.hpp | 11 +++- .../interactive/manual_loop_close_modal.hpp | 2 + src/glim/mapping/global_mapping.cpp | 43 ++++++++++++--- .../mapping/global_mapping_pose_graph.cpp | 53 ++++++++++++++++-- src/glim/mapping/sub_mapping.cpp | 30 +++++++++-- src/glim/odometry/odometry_estimation_cpu.cpp | 14 ++++- src/glim/odometry/odometry_estimation_ct.cpp | 28 +++++++++- src/glim/odometry/odometry_estimation_imu.cpp | 54 +++++++++++++++++-- .../interactive/manual_loop_close_modal.cpp | 18 ++++++- 13 files changed, 239 insertions(+), 24 deletions(-) diff --git a/include/glim/mapping/global_mapping.hpp b/include/glim/mapping/global_mapping.hpp index 2b994206..6e4c8eb7 100644 --- a/include/glim/mapping/global_mapping.hpp +++ b/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 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 new_factors; std::unique_ptr isam2; + + std::shared_ptr tbb_task_arena; }; } // namespace glim \ No newline at end of file diff --git a/include/glim/mapping/global_mapping_pose_graph.hpp b/include/glim/mapping/global_mapping_pose_graph.hpp index aab966de..4e424f15 100644 --- a/include/glim/mapping/global_mapping_pose_graph.hpp +++ b/include/glim/mapping/global_mapping_pose_graph.hpp @@ -129,5 +129,7 @@ class GlobalMappingPoseGraph : public GlobalMappingBase { std::unique_ptr new_factors; std::unique_ptr isam2; + + std::shared_ptr tbb_task_arena; }; } // namespace glim \ No newline at end of file diff --git a/include/glim/mapping/sub_mapping.hpp b/include/glim/mapping/sub_mapping.hpp index 6c1b1a30..2ac38b24 100644 --- a/include/glim/mapping/sub_mapping.hpp +++ b/include/glim/mapping/sub_mapping.hpp @@ -96,6 +96,8 @@ class SubMapping : public SubMappingBase { std::unique_ptr graph; std::vector submap_queue; + + std::shared_ptr tbb_task_arena; }; } // namespace glim diff --git a/include/glim/odometry/odometry_estimation_ct.hpp b/include/glim/odometry/odometry_estimation_ct.hpp index 5c684a5d..fe891d0e 100644 --- a/include/glim/odometry/odometry_estimation_ct.hpp +++ b/include/glim/odometry/odometry_estimation_ct.hpp @@ -82,6 +82,8 @@ class OdometryEstimationCT : public OdometryEstimationBase { // Optimizer using FixedLagSmootherExt = gtsam_points::IncrementalFixedLagSmootherExtWithFallback; std::unique_ptr smoother; + + std::shared_ptr tbb_task_arena; }; } // namespace glim \ No newline at end of file diff --git a/include/glim/odometry/odometry_estimation_imu.hpp b/include/glim/odometry/odometry_estimation_imu.hpp index 0ed83931..4246f3d3 100644 --- a/include/glim/odometry/odometry_estimation_imu.hpp +++ b/include/glim/odometry/odometry_estimation_imu.hpp @@ -1,12 +1,12 @@ #pragma once +#include #include #include #include #include - 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& new_stamp, int update_count = 0); + virtual void update_smoother(int update_count = 1); + protected: std::unique_ptr params; @@ -103,6 +108,8 @@ class OdometryEstimationIMU : public OdometryEstimationBase { // Optimizer using FixedLagSmootherExt = gtsam_points::IncrementalFixedLagSmootherExtWithFallback; std::unique_ptr smoother; + + std::shared_ptr tbb_task_arena; }; } // namespace glim diff --git a/include/glim/viewer/interactive/manual_loop_close_modal.hpp b/include/glim/viewer/interactive/manual_loop_close_modal.hpp index ca214fa9..c9646f28 100644 --- a/include/glim/viewer/interactive/manual_loop_close_modal.hpp +++ b/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 tbb_task_arena; }; } // namespace glim \ No newline at end of file diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp index 202e1dbb..2529e116 100644 --- a/src/glim/mapping/global_mapping.cpp +++ b/src/glim/mapping/global_mapping.cpp @@ -30,6 +30,10 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#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(64); #endif + +#ifdef GTSAM_USE_TBB + tbb_task_arena = std::make_shared(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 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.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(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.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(); diff --git a/src/glim/mapping/global_mapping_pose_graph.cpp b/src/glim/mapping/global_mapping_pose_graph.cpp index e834c31c..15bb4229 100644 --- a/src/glim/mapping/global_mapping_pose_graph.cpp +++ b/src/glim/mapping/global_mapping_pose_graph.cpp @@ -21,6 +21,10 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#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.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.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.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.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(); diff --git a/src/glim/mapping/sub_mapping.cpp b/src/glim/mapping/sub_mapping.cpp index 2787fa6d..d097f54b 100644 --- a/src/glim/mapping/sub_mapping.cpp +++ b/src/glim/mapping/sub_mapping.cpp @@ -25,6 +25,10 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#endif + namespace glim { using gtsam::symbol_shorthand::B; @@ -81,6 +85,10 @@ SubMapping::SubMapping(const SubMappingParams& params) : params(params) { stream = std::make_shared(); stream_buffer_roundrobin = std::make_shared(8); #endif + +#ifdef GTSAM_USE_TBB + tbb_task_arena = std::make_shared(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(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(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()); diff --git a/src/glim/odometry/odometry_estimation_cpu.cpp b/src/glim/odometry/odometry_estimation_cpu.cpp index f782f739..2fe353b0 100644 --- a/src/glim/odometry/odometry_estimation_cpu.cpp +++ b/src/glim/odometry/odometry_estimation_cpu.cpp @@ -21,6 +21,10 @@ #include +#ifdef GTSAM_USE_TBB +#include +#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(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(X(current)).matrix()); Eigen::Isometry3d T_last_current = last_T_target_imu.inverse() * T_target_imu; diff --git a/src/glim/odometry/odometry_estimation_ct.cpp b/src/glim/odometry/odometry_estimation_ct.cpp index 89a7c642..4c468cbe 100644 --- a/src/glim/odometry/odometry_estimation_ct.cpp +++ b/src/glim/odometry/odometry_estimation_ct.cpp @@ -20,6 +20,10 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#endif + namespace glim { using gtsam::symbol_shorthand::X; @@ -64,6 +68,10 @@ OdometryEstimationCT::OdometryEstimationCT(const OdometryEstimationCTParams& par isam2_params.relinearizeSkip = params.isam2_relinearize_skip; isam2_params.setRelinearizeThreshold(params.isam2_relinearize_thresh); smoother.reset(new FixedLagSmootherExt(params.smoother_lag, isam2_params)); + +#ifdef GTSAM_USE_TBB + tbb_task_arena = std::make_shared(1); +#endif } OdometryEstimationCT::~OdometryEstimationCT() {} @@ -159,7 +167,15 @@ EstimationFrame::ConstPtr OdometryEstimationCT::insert_frame(const PreprocessedF // lm_params.set_verbose(); try { - values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize(); +#ifdef GTSAM_USE_TBB + auto arena = static_cast(tbb_task_arena.get()); + arena->execute([&] { +#endif + values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize(); +#ifdef GTSAM_USE_TBB + }); +#endif + } catch (std::exception& e) { logger->error("an exception was caught during odometry estimation"); logger->error("{}", e.what()); @@ -212,7 +228,15 @@ EstimationFrame::ConstPtr OdometryEstimationCT::insert_frame(const PreprocessedF // Update smoother Callbacks::on_smoother_update(*smoother, new_factors, new_values, new_stamps); - smoother->update(new_factors, new_values, new_stamps); +#ifdef GTSAM_USE_TBB + auto arena = static_cast(tbb_task_arena.get()); + arena->execute([&] { +#endif + smoother->update(new_factors, new_values, new_stamps); +#ifdef GTSAM_USE_TBB + }); +#endif + Callbacks::on_smoother_update_finish(*smoother); // Find out marginalized frames diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp index 287430f3..57f67d95 100644 --- a/src/glim/odometry/odometry_estimation_imu.cpp +++ b/src/glim/odometry/odometry_estimation_imu.cpp @@ -19,6 +19,10 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#endif + namespace glim { using Callbacks = OdometryEstimationCallbacks; @@ -60,6 +64,7 @@ OdometryEstimationIMUParams::OdometryEstimationIMUParams() { save_imu_rate_trajectory = config.param("odometry_estimation", "save_imu_rate_trajectory", false); num_threads = config.param("odometry_estimation", "num_threads", 4); + num_smoother_update_threads = 1; } OdometryEstimationIMUParams::~OdometryEstimationIMUParams() {} @@ -93,6 +98,10 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptrisam2_relinearize_skip; isam2_params.setRelinearizeThreshold(params->isam2_relinearize_thresh); smoother.reset(new FixedLagSmootherExt(params->smoother_lag, isam2_params)); + +#ifdef GTSAM_USE_TBB + tbb_task_arena = std::make_shared(params->num_smoother_update_threads); +#endif } OdometryEstimationIMU::~OdometryEstimationIMU() {} @@ -119,8 +128,18 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed // The very first frame if (frames.empty()) { - init_estimation->insert_frame(raw_frame); - auto init_state = init_estimation->initial_pose(); + EstimationFrame::ConstPtr init_state; + +#ifdef GTSAM_USE_TBB + auto arena = static_cast(tbb_task_arena.get()); + arena->execute([&] { +#endif + init_estimation->insert_frame(raw_frame); + init_state = init_estimation->initial_pose(); +#ifdef GTSAM_USE_TBB + }); +#endif + if (init_state == nullptr) { logger->debug("waiting for initial IMU state estimation to be finished"); return nullptr; @@ -187,7 +206,7 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed new_factors.emplace_shared(B(0), 6, 1e2); new_factors.add(create_factors(current, nullptr, new_values)); - smoother->update(new_factors, new_values, new_stamps); + update_smoother(new_factors, new_values, new_stamps); update_frames(current, new_factors); return frames.back(); @@ -291,8 +310,7 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed // Update smoother Callbacks::on_smoother_update(*smoother, new_factors, new_values, new_stamps); - smoother->update(new_factors, new_values, new_stamps); - smoother->update(); + update_smoother(new_factors, new_values, new_stamps, 1); Callbacks::on_smoother_update_finish(*smoother); // Find out marginalized frames @@ -363,4 +381,30 @@ void OdometryEstimationIMU::update_frames(int current, const gtsam::NonlinearFac } } +void OdometryEstimationIMU::update_smoother( + const gtsam::NonlinearFactorGraph& new_factors, + const gtsam::Values& new_values, + const std::map& new_stamp, + int update_count) { +#ifdef GTSAM_USE_TBB + auto arena = static_cast(tbb_task_arena.get()); + arena->execute([&] { +#endif + smoother->update(new_factors, new_values, new_stamp); + for (int i = 0; i < update_count; i++) { + smoother->update(); + } +#ifdef GTSAM_USE_TBB + }); +#endif +} + +void OdometryEstimationIMU::update_smoother(int count) { + if (count <= 0) { + return; + } + + update_smoother(gtsam::NonlinearFactorGraph(), gtsam::Values(), std::map(), count - 1); +} + } // namespace glim diff --git a/src/glim/viewer/interactive/manual_loop_close_modal.cpp b/src/glim/viewer/interactive/manual_loop_close_modal.cpp index ce4e4d43..69abf7ea 100644 --- a/src/glim/viewer/interactive/manual_loop_close_modal.cpp +++ b/src/glim/viewer/interactive/manual_loop_close_modal.cpp @@ -16,6 +16,10 @@ #include #include +#ifdef GTSAM_USE_TBB +#include +#endif + namespace glim { ManualLoopCloseModal::ManualLoopCloseModal() : request_to_open(false) { @@ -28,6 +32,10 @@ ManualLoopCloseModal::ManualLoopCloseModal() : request_to_open(false) { canvas.reset(new guik::GLCanvas(Eigen::Vector2i(512, 512))); progress_modal.reset(new guik::ProgressModal("manual_loop_close_progress")); model_control.reset(new guik::ModelControl("model_control")); + +#ifdef GTSAM_USE_TBB + tbb_task_arena = std::make_shared(1); +#endif } ManualLoopCloseModal::~ManualLoopCloseModal() {} @@ -147,7 +155,15 @@ std::shared_ptr ManualLoopCloseModal::align(guik::ProgressInt }; gtsam_points::LevenbergMarquardtOptimizerExt optimizer(graph, values, lm_params); - values = optimizer.optimize(); + +#ifdef GTSAM_USE_TBB + auto arena = static_cast(tbb_task_arena.get()); + arena->execute([&] { +#endif + values = optimizer.optimize(); +#ifdef GTSAM_USE_TBB + }); +#endif const gtsam::Pose3 estimated = values.at(0).inverse() * values.at(1);