Publish IMU-based prediction results

CMakeLists.txt

@@ -7,6 +7,7 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
 option(ENABLE_ORBSLAM "Enable ORB-SLAM odometry" OFF)
 option(ENABLE_VELSUPP "Enable Velocity suppressor" ON)
 option(ENABLE_IMUVAL "Enable IMU calibration validator" ON)
+option(ENABLE_IMUPRED "Enable IMU prediction module" ON)
 option(ENABLE_GRAVITY "Enable Gravity estimation and correction module" ON)
 option(ENABLE_DESKEWING "Enable Point Cloud Deskewing module" ON)
 
@@ -64,6 +65,11 @@ if(ENABLE_IMUVAL)
   list(APPEND glim_ext_LIBRARIES imu_validator)
 endif()
 
+if(ENABLE_IMUPRED)
+  add_subdirectory(modules/odometry/imu_prediction)
+  list(APPEND glim_ext_LIBRARIES imu_prediction)
+endif()
+
 if(ENABLE_GRAVITY)
   add_subdirectory(modules/odometry/gravity_estimator)
   list(APPEND glim_ext_LIBRARIES gravity_estimator)

README.md

@@ -1,9 +1,9 @@
-**!!! This repository constains half-baked code that may not be well-maintained and not suitable for practical purposes. !!!**  
+**!!! This repository constains half-baked code that may not be well-maintained and not suitable for practical purposes. !!!**
 **!!! We don't have resource to maintain this extension library. If you find an issue, please fix it by yourself and open a PR to share the solution. !!!**
 
 # glim_ext
 
-glim_ext is a set of extension modules for [GLIM](https://github.com/koide3/glim), 3D LiDAR mapping framework. With this package, we aim to provide reference implementations that demonstrate how GLIM can be extended through the global callback slot mechanism. 
+glim_ext is a set of extension modules for [GLIM](https://github.com/koide3/glim), 3D LiDAR mapping framework. With this package, we aim to provide reference implementations that demonstrate how GLIM can be extended through the global callback slot mechanism.
 
 [![EXT](https://github.com/koide3/glim_ext/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/glim_ext/actions/workflows/build.yml)
 
@@ -13,7 +13,7 @@ Each module in glim_ext uses several external libraries that employ different li
 
 ## Usage
 
-To enable an extension module, add the so filename to `extension_modules` in `glim/config/config_ros.json`. E.g., 
+To enable an extension module, add the so filename to `extension_modules` in `glim/config/config_ros.json`. E.g.,
 
 ```
 "extension_modules": [
@@ -37,15 +37,24 @@ Example (`libflat_earther.so`):
 
 ## Example Modules
 
-### Callback demo
+### Callback demo (libglim_callback_demo.so)
 - This modules subscribes to all available callbacks to demonstrate how mapping states can be retrieved
 
 ## Odometry estimation Modules
 
-### IMU validator
+### Point cloud deskewing (libdeskewer.so)
+- Publishing and saving deskewed point clouds (without downsampling).
+
+### IMU validator (libimu_validator.so)
 - Validator for LiDAR-IMU transformation configurations.
 
-### Velocity supressor
+### IMU prediction (libimu_prediction.so)
+- Publishing IMU-based state estimation at a high frequency for low-latency applications.
+
+### Gravity estimation (libgravity_estimator.so)
+- Estimating the gravity vector in the sensor frame, and forcing the upward direction of the state estimation to be aligned with the gravity direction.
+
+### Velocity supressor (libvelocity_supressor.so)
 - Regulating the velocity range.
 
 ### ORB_SLAM odometry (Not Maintained)
@@ -54,14 +63,14 @@ Example (`libflat_earther.so`):
 
 ## Global Optimization Modules
 
-### Flat earther
+### Flat earther (libflat_earther.so)
 - Forcing the height of proximiate submaps be the same.
 - This is useful in situations only a single floor exists and the height of the sensor from the floor is mostly unchanged.
 
-### GNSS constraints (ROS2 only)
+### GNSS constraints (libgnss_global.so, ROS2 only)
 - GNSS-based constraints for global optimization
 
-### ScanContext Loop Detector
+### ScanContext Loop Detector (libscancontext_loop_detector.so)
 - Explicit loop detection based on ScanContext
 - Dependency: [ScanContext](https://github.com/irapkaist/scancontext) (CC BY-NC-SA 4.0)
 

config/config_ext.json

@@ -6,6 +6,7 @@
     // common
     "config_sensors_ext": "config_sensors_ext.json",
     // odometry
+    "config_imu_prediction": "config_imu_prediction.json",
     "config_velocity_suppressor": "config_velocity_suppressor.json",
     "config_orb_slam": "config_orb_slam.json",
     "config_deskewing": "config_deskewing.json",

config/config_imu_prediction.json

@@ -0,0 +1,5 @@
+{
+  "imu_prediction": {
+    "max_publish_rate": 25.0
+  }
+}

modules/odometry/imu_prediction/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.16)
+project(imu_prediction)
+
+find_package(glim REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Iridescence REQUIRED)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+set(CMAKE_CXX_STANDARD 17)
+
+ament_auto_add_library(imu_prediction SHARED
+  src/glim_ext/imu_prediction_module.cpp
+  src/glim_ext/imu_prediction_module_ros2.cpp
+  src/glim_ext/imu_prediction_module_create.cpp
+)
+target_include_directories(imu_prediction PRIVATE
+  include
+)
+target_link_libraries(imu_prediction
+  glim::glim
+  Iridescence::iridescence
+  glim_ext
+)

modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <deque>
+#include <spdlog/spdlog.h>
+
+#include <glim/util/extension_module_ros2.hpp>
+#include <glim/util/concurrent_vector.hpp>
+#include <glim/odometry/estimation_frame.hpp>
+
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/navigation/ImuFactor.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+namespace glim {
+
+/// @brief A module that predicts the current state of the IMU by integrating the IMU measurements since the last point cloud frame.
+/// While I considered implementing an efficient method to propagate the update of a past state to the latest state (e.g., stocastic cloning),
+/// I found that simply re-integrating the IMU measurements takes only 0.1ms for 100 measurements, which is fast enough for our use case.
+class IMUPredictionModule : public ExtensionModuleROS2 {
+public:
+  IMUPredictionModule();
+  ~IMUPredictionModule();
+
+private:
+  std::vector<GenericTopicSubscription::Ptr> create_subscriptions(rclcpp::Node& node) override;
+  bool needs_wait() const override;
+
+  void on_insert_imu(const double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel);
+  void on_update_new_frame(const EstimationFrame::ConstPtr& frame);
+
+  void publish_pred_frame(const EstimationFrame::ConstPtr& frame);
+
+private:
+  // Input and output queues
+  std::deque<Eigen::Matrix<double, 7, 1>> imu_queue;                 // (t, ax, ay, az, wx, wy, wz)
+  std::unique_ptr<gtsam::PreintegratedImuMeasurements> integration;  // Integration since the last frame
+  EstimationFrame::ConstPtr last_frame;
+  EstimationFrame::Ptr pred_frame;
+
+  // ROS-related
+  std::string imu_frame_id;
+  std::string odom_frame_id;
+
+  double min_publish_interval;
+  double last_publish_time;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pred_odom_pub;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pred_pose_pub;
+
+  // logging
+  std::shared_ptr<spdlog::logger> logger;
+};
+
+}  // namespace glim

modules/odometry/imu_prediction/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>imu_prediction</name>
+  <version>0.0.0</version>
+  <description>Extension modules for GLIM</description>
+  <maintainer email="k.koide@aist.go.jp">k.koide</maintainer>
+  <license>GPLv3</license>
+
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+
+  <depend condition="$ROS_VERSION == 1">roscpp</depend>
+  <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+
+  <depend>glim</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <export>
+    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+  </export>
+</package>

modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp

@@ -0,0 +1,106 @@
+#include <glim_ext/imu_prediction_module.hpp>
+
+#include <filesystem>
+
+#include <glim/util/config.hpp>
+#include <glim/util/logging.hpp>
+#include <glim/odometry/callbacks.hpp>
+#include <glim_ext/util/config_ext.hpp>
+
+#include <guik/viewer/light_viewer.hpp>
+
+namespace glim {
+
+IMUPredictionModule::IMUPredictionModule() : logger(create_module_logger("imupred")) {
+  logger->info("starting IMU prediction module");
+
+  auto imu_params = gtsam::PreintegrationParams::MakeSharedU();
+  imu_params->accelerometerCovariance = gtsam::Matrix3::Identity();
+  imu_params->gyroscopeCovariance = gtsam::Matrix3::Identity();
+  imu_params->integrationCovariance = gtsam::Matrix3::Identity();
+  integration = std::make_unique<gtsam::PreintegratedImuMeasurements>(imu_params);
+
+  const Config config(GlobalConfigExt::get_config_path("config_imu_prediction"));
+  min_publish_interval = 1.0 / config.param<double>("imu_prediction", "max_publish_rate", 25.0);
+  last_publish_time = 0.0;
+
+  const Config config_ros(GlobalConfig::get_config_path("config_ros"));
+  imu_frame_id = config_ros.param<std::string>("glim_ros", "imu_frame_id", "");
+  odom_frame_id = config_ros.param<std::string>("glim_ros", "odom_frame_id", "");
+
+  OdometryEstimationCallbacks::on_insert_imu.add(
+    [this](const double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel) { this->on_insert_imu(stamp, linear_acc, angular_vel); });
+  OdometryEstimationCallbacks::on_update_new_frame.add([this](const EstimationFrame::ConstPtr& frame) { this->on_update_new_frame(frame); });
+
+  logger->info("ready");
+}
+
+IMUPredictionModule::~IMUPredictionModule() {
+  logger->info("stopped");
+}
+
+bool IMUPredictionModule::needs_wait() const {
+  return false;
+}
+
+void IMUPredictionModule::on_insert_imu(const double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel) {
+  if (last_frame && pred_frame) {
+    integration->integrateMeasurement(linear_acc, angular_vel, stamp - pred_frame->stamp);
+
+    const gtsam::NavState nav_world_imu(gtsam::Pose3(last_frame->T_world_imu.matrix()), last_frame->v_world_imu);
+    const gtsam::NavState pred_nav_world_imu = integration->predict(nav_world_imu, gtsam::imuBias::ConstantBias(last_frame->imu_bias));
+
+    pred_frame->stamp = stamp;
+    pred_frame->T_world_imu = Eigen::Isometry3d(pred_nav_world_imu.pose().matrix());
+    pred_frame->v_world_imu = pred_nav_world_imu.velocity();
+
+    publish_pred_frame(pred_frame);
+  }
+
+  Eigen::Matrix<double, 7, 1> imu_data;
+  imu_data << stamp, linear_acc.x(), linear_acc.y(), linear_acc.z(), angular_vel.x(), angular_vel.y(), angular_vel.z();
+  imu_queue.emplace_back(imu_data);
+}
+
+void IMUPredictionModule::on_update_new_frame(const EstimationFrame::ConstPtr& frame) {
+  last_frame = frame;
+  pred_frame = frame->clone_wo_points();
+
+  const gtsam::imuBias::ConstantBias imu_bias(frame->imu_bias);
+  integration->resetIntegrationAndSetBias(imu_bias);
+
+  gtsam::NavState nav_world_imu(gtsam::Pose3(frame->T_world_imu.matrix()), frame->v_world_imu);
+
+  int remove_loc = 0;
+  int num_integrated = 0;
+  double integrated_time = frame->stamp;
+  for (size_t i = 0; i + 1 < imu_queue.size(); i++) {
+    const auto& curr_imu = imu_queue[i];
+    const auto& next_imu = imu_queue[i + 1];
+    if (next_imu(0) <= frame->stamp) {
+      remove_loc = i + 1;
+      continue;
+    }
+
+    const double dt = next_imu(0) - integrated_time;
+    if (dt < 1e-6) {
+      continue;
+    }
+
+    num_integrated++;
+    const auto& a = curr_imu.block<3, 1>(1, 0);
+    const auto& w = curr_imu.block<3, 1>(4, 0);
+    integration->integrateMeasurement(a, w, dt);
+
+    const auto pred_nav_world_imu = integration->predict(nav_world_imu, imu_bias);
+
+    integrated_time = next_imu(0);
+    pred_frame->stamp = next_imu(0);
+    pred_frame->T_world_imu = Eigen::Isometry3d(pred_nav_world_imu.pose().matrix());
+    pred_frame->v_world_imu = pred_nav_world_imu.velocity();
+  }
+
+  imu_queue.erase(imu_queue.begin(), imu_queue.begin() + remove_loc);
+}
+
+}  // namespace glim

modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_create.cpp

@@ -0,0 +1,5 @@
+#include <glim_ext/imu_prediction_module.hpp>
+
+extern "C" glim::ExtensionModule* create_extension_module() {
+  return new glim::IMUPredictionModule();
+}

modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_ros2.cpp

@@ -0,0 +1,61 @@
+#include <glim_ext/imu_prediction_module.hpp>
+
+#define GLIM_ROS2
+#include <glim/util/config.hpp>
+#include <glim/util/ros_cloud_converter.hpp>
+
+namespace glim {
+
+std::vector<GenericTopicSubscription::Ptr> IMUPredictionModule::create_subscriptions(rclcpp::Node& node) {
+  // deskewed_points_imu_pub = node.create_publisher<sensor_msgs::msg::PointCloud2>("~/deskewed_points_imu", rclcpp::SystemDefaultsQoS());
+  pred_odom_pub = node.create_publisher<nav_msgs::msg::Odometry>("~/predicted_odom", rclcpp::SystemDefaultsQoS());
+
+  return {};
+}
+
+void IMUPredictionModule::publish_pred_frame(const EstimationFrame::ConstPtr& frame) {
+  const double dt = frame->stamp - last_publish_time;
+  if (dt < min_publish_interval) {
+    // Avoid too frequent publishing, ROS2 can be slow to handle too many messages
+    return;
+  }
+
+  if (imu_frame_id.empty()) {
+    imu_frame_id = GlobalConfig::instance()->param<std::string>("meta", "imu_frame_id", "");
+
+    if (imu_frame_id.empty()) {
+      logger->warn("IMU frame ID is not set. Using 'imu' as default.");
+      imu_frame_id = "imu";
+    } else {
+      logger->info("auto-detected IMU frame ID: {}", imu_frame_id);
+    }
+  }
+
+  if (pred_odom_pub->get_subscription_count()) {
+    const Eigen::Isometry3d T_odom_imu(frame->T_world_imu);
+    const Eigen::Quaterniond quat_odom_imu(T_odom_imu.linear());
+    const Eigen::Vector3d v_odom_imu = frame->v_world_imu;
+
+    nav_msgs::msg::Odometry odom;
+    odom.header.stamp = from_sec(frame->stamp);
+    odom.header.frame_id = "odom";
+    odom.child_frame_id = "imu";
+    odom.pose.pose.position.x = T_odom_imu.translation().x();
+    odom.pose.pose.position.y = T_odom_imu.translation().y();
+    odom.pose.pose.position.z = T_odom_imu.translation().z();
+    odom.pose.pose.orientation.x = quat_odom_imu.x();
+    odom.pose.pose.orientation.y = quat_odom_imu.y();
+    odom.pose.pose.orientation.z = quat_odom_imu.z();
+    odom.pose.pose.orientation.w = quat_odom_imu.w();
+
+    odom.twist.twist.linear.x = v_odom_imu.x();
+    odom.twist.twist.linear.y = v_odom_imu.y();
+    odom.twist.twist.linear.z = v_odom_imu.z();
+
+    pred_odom_pub->publish(odom);
+
+    logger->debug("published predicted odom (stamp={:.6f})", frame->stamp);
+  }
+}
+
+}  // namespace glim