diff --git a/CMakeLists.txt b/CMakeLists.txt index c47426b..3e761ce 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/README.md b/README.md index 15e6614..d1dfda2 100644 --- a/README.md +++ b/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) diff --git a/config/config_ext.json b/config/config_ext.json index e71fd84..396e390 100644 --- a/config/config_ext.json +++ b/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", diff --git a/config/config_imu_prediction.json b/config/config_imu_prediction.json new file mode 100644 index 0000000..de0f1a9 --- /dev/null +++ b/config/config_imu_prediction.json @@ -0,0 +1,5 @@ +{ + "imu_prediction": { + "max_publish_rate": 25.0 + } +} \ No newline at end of file diff --git a/modules/odometry/imu_prediction/CMakeLists.txt b/modules/odometry/imu_prediction/CMakeLists.txt new file mode 100644 index 0000000..5ed4cc7 --- /dev/null +++ b/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 +) diff --git a/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp b/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp new file mode 100644 index 0000000..0fc4dea --- /dev/null +++ b/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +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 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> imu_queue; // (t, ax, ay, az, wx, wy, wz) + std::unique_ptr 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::SharedPtr pred_odom_pub; + rclcpp::Publisher::SharedPtr pred_pose_pub; + + // logging + std::shared_ptr logger; +}; + +} // namespace glim diff --git a/modules/odometry/imu_prediction/package.xml b/modules/odometry/imu_prediction/package.xml new file mode 100644 index 0000000..47fec6c --- /dev/null +++ b/modules/odometry/imu_prediction/package.xml @@ -0,0 +1,24 @@ + + + + imu_prediction + 0.0.0 + Extension modules for GLIM + k.koide + GPLv3 + + catkin + ament_cmake + + roscpp + rclcpp + + glim + sensor_msgs + nav_msgs + geometry_msgs + + + ament_cmake + + diff --git a/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp new file mode 100644 index 0000000..7365814 --- /dev/null +++ b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp @@ -0,0 +1,106 @@ +#include + +#include + +#include +#include +#include +#include + +#include + +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(imu_params); + + const Config config(GlobalConfigExt::get_config_path("config_imu_prediction")); + min_publish_interval = 1.0 / config.param("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("glim_ros", "imu_frame_id", ""); + odom_frame_id = config_ros.param("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 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 diff --git a/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_create.cpp b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_create.cpp new file mode 100644 index 0000000..e7b3002 --- /dev/null +++ b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_create.cpp @@ -0,0 +1,5 @@ +#include + +extern "C" glim::ExtensionModule* create_extension_module() { + return new glim::IMUPredictionModule(); +} \ No newline at end of file diff --git a/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_ros2.cpp b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_ros2.cpp new file mode 100644 index 0000000..6577f48 --- /dev/null +++ b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_ros2.cpp @@ -0,0 +1,61 @@ +#include + +#define GLIM_ROS2 +#include +#include + +namespace glim { + +std::vector IMUPredictionModule::create_subscriptions(rclcpp::Node& node) { + // deskewed_points_imu_pub = node.create_publisher("~/deskewed_points_imu", rclcpp::SystemDefaultsQoS()); + pred_odom_pub = node.create_publisher("~/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("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