Support GTSAM built with TBB

CMakeLists.txt

@@ -49,6 +49,7 @@ add_library(glim SHARED
   src/glim/util/logging.cpp
   src/glim/util/export_factors.cpp
   src/glim/util/time_keeper.cpp
+  src/glim/util/trajectory_manager.cpp
   src/glim/util/extension_module.cpp
   src/glim/util/load_module.cpp
   src/glim/util/data_validator.cpp

config/config_ros.json

@@ -37,6 +37,7 @@
     "keep_raw_points": false,
     // IMU config
     "imu_time_offset": 0.0,
+    "points_time_offset": 0.0,
     "acc_scale": 1.0,
     // TF config
     "imu_frame_id": "imu",

config/config_sensors.json

@@ -12,8 +12,8 @@
   //
   // --- LiDAR per-point time settings ---
   // autoconf_perpoint_times     : If true, automatically configure per-point timestamps. If false, use the following settings.
+  // autoconf_prefer_frame_time  : If true, frame timestamp will never be overridden by anything. Otherwise, absolute per-point timestamps will be used if available.
   // perpoint_relative_time      : If true, recognize per-point timestamps as relative to the frame timestamp. If false, recognize as absolute timestamps.
-  // perpoint_prefer_frame_time  : If true, frame timestamp will never be overridden by anything. Otherwise, absolute per-point timestamps will be used if available.
   // perpoint_time_scale         : Scale factor for per-point timestamps. If 1.0, timestamps are in seconds. If 1e-9, timestamps are in nanoseconds.
   //
   // --- Camera config (only used in extension modules) ---
@@ -59,8 +59,8 @@
     ],
     // LiDAR per-point time settings
     "autoconf_perpoint_times": true,
+    "autoconf_prefer_frame_time": false,
     "perpoint_relative_time": true,
-    "perpoint_prefer_frame_time": false,
     "perpoint_time_scale": 1.0,
     // Camera config (required for only extension modules)
     "global_shutter_camera": true,

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_map.hpp

@@ -31,6 +31,36 @@ struct SubMap {
   /// @brief Get the origin odometry frame
   EstimationFrame::ConstPtr origin_odom_frame() const { return odom_frames[frames.size() / 2]; }
 
+  /**
+   * @brief Get the custom data and cast it to the specified type.
+   * @note  This method does not check the type of the custom data.
+   * @param key  Key of the custom data
+   * @return T*  Pointer to the custom data. nullptr if not found.
+   */
+  template <typename T>
+  T* get_custom_data(const std::string& key) {
+    const auto found = custom_data.find(key);
+    if (found == custom_data.end()) {
+      return nullptr;
+    }
+    return reinterpret_cast<T*>(found->second.get());
+  }
+
+  /**
+   * @brief Get the custom data and cast it to the specified type.
+   * @note  This method does not check the type of the custom data.
+   * @param key  Key of the custom data
+   * @return T*  Pointer to the custom data. nullptr if not found.
+   */
+  template <typename T>
+  const T* get_custom_data(const std::string& key) const {
+    const auto found = custom_data.find(key);
+    if (found == custom_data.end()) {
+      return nullptr;
+    }
+    return reinterpret_cast<const T*>(found->second.get());
+  }
+
   /**
    * @brief Save the submap
    * @param path  Save path
@@ -57,6 +87,8 @@ struct SubMap {
 
   std::vector<EstimationFrame::ConstPtr> frames;       ///< Optimized odometry frames
   std::vector<EstimationFrame::ConstPtr> odom_frames;  ///< Original odometry frames
+
+  std::unordered_map<std::string, std::shared_ptr<void>> custom_data;  ///< User-defined custom data
 };
 
 }  // 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/estimation_frame.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <memory>
+#include <unordered_map>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
@@ -47,6 +48,36 @@ struct EstimationFrame {
    */
   void set_T_world_sensor(FrameID frame_id, const Eigen::Isometry3d& T);
 
+  /**
+   * @brief Get the custom data and cast it to the specified type.
+   * @note  This method does not check the type of the custom data.
+   * @param key  Key of the custom data
+   * @return T*  Pointer to the custom data. nullptr if not found.
+   */
+  template <typename T>
+  T* get_custom_data(const std::string& key) {
+    const auto found = custom_data.find(key);
+    if (found == custom_data.end()) {
+      return nullptr;
+    }
+    return reinterpret_cast<T*>(found->second.get());
+  }
+
+  /**
+   * @brief Get the custom data and cast it to the specified type.
+   * @note  This method does not check the type of the custom data.
+   * @param key  Key of the custom data
+   * @return T*  Pointer to the custom data. nullptr if not found.
+   */
+  template <typename T>
+  const T* get_custom_data(const std::string& key) const {
+    const auto found = custom_data.find(key);
+    if (found == custom_data.end()) {
+      return nullptr;
+    }
+    return reinterpret_cast<const T*>(found->second.get());
+  }
+
 public:
   long id;       ///< Frame ID
   double stamp;  ///< Timestamp
@@ -64,5 +95,7 @@ struct EstimationFrame {
   FrameID frame_id;                                            ///< Coordinate frame of $frame
   gtsam_points::PointCloud::ConstPtr frame;                    ///< Deskewed points for state estimation
   std::vector<gtsam_points::GaussianVoxelMap::Ptr> voxelmaps;  ///< Multi-resolution voxelmaps
+
+  std::unordered_map<std::string, std::shared_ptr<void>> custom_data;  ///< User-defined custom data
 };
 }  // namespace glim

include/glim/odometry/initial_state_estimation.hpp

@@ -63,6 +63,10 @@ class NaiveInitialStateEstimation : public InitialStateEstimation {
   void set_init_state(const Eigen::Isometry3d& init_T_world_imu, const Eigen::Vector3d& init_v_world_imu);
 
 private:
+  double window_size;
+
+  bool ready;
+  double init_stamp;
   double stamp;
   Eigen::Vector3d sum_acc;
 

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/util/trajectory_manager.hpp

@@ -10,39 +10,19 @@ namespace glim {
 
 class TrajectoryManager {
 public:
-  TrajectoryManager() {
-    odom_stamps.push_back(0.0);
-    T_odom_sensor.push_back(Eigen::Isometry3d::Identity());
+  TrajectoryManager();
+  ~TrajectoryManager();
 
-    T_world_odom.setIdentity();
-  }
-  ~TrajectoryManager() {}
+  void add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor, int priority = 1);
+  void update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor);
 
-  void add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor) {
-    this->odom_stamps.push_back(stamp);
-    this->T_odom_sensor.push_back(T_odom_sensor);
-  }
-
-  void update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor) {
-    auto found = std::lower_bound(odom_stamps.begin(), odom_stamps.end(), stamp);
-    int idx = std::distance(odom_stamps.begin(), found);
-    T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse();
-
-    if (idx > 1) {
-      odom_stamps.erase(odom_stamps.begin(), odom_stamps.begin() + idx - 1);
-      T_odom_sensor.erase(T_odom_sensor.begin(), T_odom_sensor.begin() + idx - 1);
-    }
-  }
-
-  Eigen::Isometry3d current_pose() const { return T_world_odom * T_odom_sensor.back(); }
-
-  Eigen::Isometry3d odom2world(const Eigen::Isometry3d& pose) const { return T_world_odom * pose; }
-
-  Eigen::Vector3d odom2world(const Eigen::Vector3d& point) const { return T_world_odom * point; }
-
-  const Eigen::Isometry3d get_T_world_odom() const { return T_world_odom; }
+  Eigen::Isometry3d current_pose() const;
+  Eigen::Isometry3d odom2world(const Eigen::Isometry3d& pose) const;
+  Eigen::Vector3d odom2world(const Eigen::Vector3d& point) const;
+  const Eigen::Isometry3d get_T_world_odom() const;
 
 private:
+  int odom_priority;
   std::vector<double> odom_stamps;
   std::vector<Eigen::Isometry3d> T_odom_sensor;
 

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

include/glim/viewer/standard_viewer.hpp

@@ -5,6 +5,9 @@
 #include <thread>
 #include <memory>
 #include <vector>
+#include <unordered_map>
+#include <optional>
+#include <boost/weak_ptr.hpp>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -15,6 +18,10 @@ namespace spdlog {
 class logger;
 }
 
+namespace gtsam {
+class NonlinearFactor;
+}
+
 namespace glim {
 
 class TrajectoryManager;
@@ -55,6 +62,8 @@ class StandardViewer : public ExtensionModule {
 
   bool show_odometry_scans;
   bool show_odometry_keyframes;
+  bool show_odometry_factors;
+
   bool show_submaps;
   bool show_factors;
 
@@ -65,6 +74,11 @@ class StandardViewer : public ExtensionModule {
   Eigen::Vector3d last_imu_vel;
   Eigen::Matrix<double, 6, 1> last_imu_bias;
 
+  using FactorLine = std::tuple<Eigen::Vector3f, Eigen::Vector3f, Eigen::Vector4f, Eigen::Vector4f>;
+  using FactorLineGetter = std::function<std::optional<FactorLine>(const gtsam::NonlinearFactor*)>;
+  std::vector<std::pair<boost::weak_ptr<gtsam::NonlinearFactor>, FactorLineGetter>> odometry_factor_lines;
+  std::unordered_map<std::uint64_t, Eigen::Isometry3f> odometry_poses;
+
   Eigen::Vector2f z_range;
   Eigen::Vector2f auto_z_range;
 
@@ -75,8 +89,8 @@ class StandardViewer : public ExtensionModule {
 
   std::mutex invoke_queue_mutex;
   std::vector<std::function<void()>> invoke_queue;
-  
+
   // Logging
   std::shared_ptr<spdlog::logger> logger;
 };
-}
+}  // namespace glim