From 69da3fd87b1b4fb9446f0bc7dba1326daab56be9 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Sat, 14 Feb 2026 22:42:45 +0900 Subject: [PATCH 1/3] IMU-based prediction --- CMakeLists.txt | 6 + .../odometry/imu_prediction/CMakeLists.txt | 25 +++ .../glim_ext/imu_prediction_module.hpp | 52 +++++++ modules/odometry/imu_prediction/package.xml | 24 +++ .../src/glim_ext/imu_prediction_module.cpp | 145 ++++++++++++++++++ .../glim_ext/imu_prediction_module_create.cpp | 5 + .../glim_ext/imu_prediction_module_ros2.cpp | 14 ++ 7 files changed, 271 insertions(+) create mode 100644 modules/odometry/imu_prediction/CMakeLists.txt create mode 100644 modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp create mode 100644 modules/odometry/imu_prediction/package.xml create mode 100644 modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp create mode 100644 modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_create.cpp create mode 100644 modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_ros2.cpp 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/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..78e4121 --- /dev/null +++ b/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace glim { + +class IMUPredictionModule : public ExtensionModuleROS2 { +public: + IMUPredictionModule(); + ~IMUPredictionModule(); + +private: + std::vector create_subscriptions(rclcpp::Node& node) override; + bool needs_wait() const override; + void task(); + + 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); + +private: + std::atomic_bool kill_switch; + std::thread thread; + + // 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 + 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..f036586 --- /dev/null +++ b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp @@ -0,0 +1,145 @@ +#include + +#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); + + 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("stopping..."); + + // kill_switch = true; + // // input_frame_queue.submit_end_of_data(); + // if (thread.joinable()) { + // thread.join(); + // } + + logger->info("stopped"); +} + +bool IMUPredictionModule::needs_wait() const { + return false; +} + +void IMUPredictionModule::task() { + // while (!kill_switch || !input_frame_queue.empty()) { + // // const auto popped = input_frame_queue.pop_wait(); + // // if (!popped) { + // // break; + // // } + + // // const auto frame = *popped; + // // if (frame->imu_rate_trajectory.size() == 0) { + // // logger->warn("IMU rate trajectory is empty. skip deskewing."); + // // continue; + // // } + + // // if (frame->raw_frame == nullptr) { + // // logger->warn("preprocessed_points is nullptr. skip deskewing."); + // // continue; + // // } + + // // if (frame->raw_frame->raw_points == nullptr) { + // // logger->warn("raw_points is empty. Set keep_raw_points=true in config_ros.json"); + // // continue; + // // } + + // // logger->debug("deskewing frame at time {}", frame->stamp); + // // auto result = deskew_frame(frame); + + // // save_deskewed_frame(result); + // // publish_deskewed_frame(result); + // } +} + +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(); + + guik::viewer()->invoke([pred_frame = pred_frame] { guik::viewer()->update_coord(guik::anon(), guik::VertexColor(pred_frame->T_world_imu)); }); + } + + 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) { + gtsam_points::EasyProfiler prof("IMU prediction"); + prof.push("remove old imu data"); + 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); + + prof.push("integrate imu data"); + int remove_loc = 0; + int num_integrated = 0; + for (size_t i = 0; i < imu_queue.size() - 1; i++) { + const auto& curr_imu = imu_queue[i]; + const auto& next_imu = imu_queue[i + 1]; + if (next_imu(0) < pred_frame->stamp) { + remove_loc = i + 1; + continue; + } + + const double dt = next_imu(0) - pred_frame->stamp; + if (dt < 1e-3) { + continue; + } + + std::cout << "frame_stamp=" << pred_frame->stamp << ", curr_imu_stamp=" << curr_imu(0) << ", next_imu_stamp=" << next_imu(0) << ", dt=" << dt << std::endl; + + 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); + + 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(); + } + std::cout << "num_integrated=" << num_integrated << std::endl; + + // guik::viewer()->invoke([pred_frame = pred_frame] { guik::viewer()->update_coord(guik::anon(), guik::VertexColor(pred_frame->T_world_imu)); }); + + prof.push("done"); + 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..1dc1704 --- /dev/null +++ b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module_ros2.cpp @@ -0,0 +1,14 @@ +#include + +#define GLIM_ROS2 +#include + +namespace glim { + +std::vector IMUPredictionModule::create_subscriptions(rclcpp::Node& node) { + // deskewed_points_imu_pub = node.create_publisher("~/deskewed_points_imu", rclcpp::SystemDefaultsQoS()); + + return {}; +} + +} // namespace glim From 0e1c3c94b924c7717cb10cc386336b719a78c6e6 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Sat, 14 Feb 2026 23:25:57 +0900 Subject: [PATCH 2/3] prediction --- config/config_ext.json | 1 + config/config_imu_prediction.json | 5 ++ .../glim_ext/imu_prediction_module.hpp | 20 +++--- .../src/glim_ext/imu_prediction_module.cpp | 71 +++++-------------- .../glim_ext/imu_prediction_module_ros2.cpp | 47 ++++++++++++ 5 files changed, 81 insertions(+), 63 deletions(-) create mode 100644 config/config_imu_prediction.json 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/include/glim_ext/imu_prediction_module.hpp b/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp index 78e4121..0fc4dea 100644 --- a/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp +++ b/modules/odometry/imu_prediction/include/glim_ext/imu_prediction_module.hpp @@ -1,8 +1,6 @@ #pragma once #include -#include -#include #include #include @@ -18,6 +16,9 @@ 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(); @@ -26,22 +27,25 @@ class IMUPredictionModule : public ExtensionModuleROS2 { private: std::vector create_subscriptions(rclcpp::Node& node) override; bool needs_wait() const override; - void task(); 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); -private: - std::atomic_bool kill_switch; - std::thread thread; + 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 + 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; 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 index f036586..7365814 100644 --- a/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp +++ b/modules/odometry/imu_prediction/src/glim_ext/imu_prediction_module.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include @@ -21,6 +20,14 @@ IMUPredictionModule::IMUPredictionModule() : logger(create_module_logger("imupre 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); }); @@ -29,14 +36,6 @@ IMUPredictionModule::IMUPredictionModule() : logger(create_module_logger("imupre } IMUPredictionModule::~IMUPredictionModule() { - logger->info("stopping..."); - - // kill_switch = true; - // // input_frame_queue.submit_end_of_data(); - // if (thread.joinable()) { - // thread.join(); - // } - logger->info("stopped"); } @@ -44,37 +43,6 @@ bool IMUPredictionModule::needs_wait() const { return false; } -void IMUPredictionModule::task() { - // while (!kill_switch || !input_frame_queue.empty()) { - // // const auto popped = input_frame_queue.pop_wait(); - // // if (!popped) { - // // break; - // // } - - // // const auto frame = *popped; - // // if (frame->imu_rate_trajectory.size() == 0) { - // // logger->warn("IMU rate trajectory is empty. skip deskewing."); - // // continue; - // // } - - // // if (frame->raw_frame == nullptr) { - // // logger->warn("preprocessed_points is nullptr. skip deskewing."); - // // continue; - // // } - - // // if (frame->raw_frame->raw_points == nullptr) { - // // logger->warn("raw_points is empty. Set keep_raw_points=true in config_ros.json"); - // // continue; - // // } - - // // logger->debug("deskewing frame at time {}", frame->stamp); - // // auto result = deskew_frame(frame); - - // // save_deskewed_frame(result); - // // publish_deskewed_frame(result); - // } -} - 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); @@ -86,17 +54,15 @@ void IMUPredictionModule::on_insert_imu(const double stamp, const Eigen::Vector3 pred_frame->T_world_imu = Eigen::Isometry3d(pred_nav_world_imu.pose().matrix()); pred_frame->v_world_imu = pred_nav_world_imu.velocity(); - guik::viewer()->invoke([pred_frame = pred_frame] { guik::viewer()->update_coord(guik::anon(), guik::VertexColor(pred_frame->T_world_imu)); }); + publish_pred_frame(pred_frame); } - Eigen::Matrix imu_data; + 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) { - gtsam_points::EasyProfiler prof("IMU prediction"); - prof.push("remove old imu data"); last_frame = frame; pred_frame = frame->clone_wo_points(); @@ -105,24 +71,22 @@ void IMUPredictionModule::on_update_new_frame(const EstimationFrame::ConstPtr& f gtsam::NavState nav_world_imu(gtsam::Pose3(frame->T_world_imu.matrix()), frame->v_world_imu); - prof.push("integrate imu data"); int remove_loc = 0; int num_integrated = 0; - for (size_t i = 0; i < imu_queue.size() - 1; i++) { + 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) < pred_frame->stamp) { + if (next_imu(0) <= frame->stamp) { remove_loc = i + 1; continue; } - const double dt = next_imu(0) - pred_frame->stamp; - if (dt < 1e-3) { + const double dt = next_imu(0) - integrated_time; + if (dt < 1e-6) { continue; } - std::cout << "frame_stamp=" << pred_frame->stamp << ", curr_imu_stamp=" << curr_imu(0) << ", next_imu_stamp=" << next_imu(0) << ", dt=" << dt << std::endl; - num_integrated++; const auto& a = curr_imu.block<3, 1>(1, 0); const auto& w = curr_imu.block<3, 1>(4, 0); @@ -130,15 +94,12 @@ void IMUPredictionModule::on_update_new_frame(const EstimationFrame::ConstPtr& f 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(); } - std::cout << "num_integrated=" << num_integrated << std::endl; - - // guik::viewer()->invoke([pred_frame = pred_frame] { guik::viewer()->update_coord(guik::anon(), guik::VertexColor(pred_frame->T_world_imu)); }); - prof.push("done"); imu_queue.erase(imu_queue.begin(), imu_queue.begin() + remove_loc); } 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 index 1dc1704..6577f48 100644 --- 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 @@ -1,14 +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 From d6841c22c65d05ef5e1cbc14d3ee289c50188b71 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Sun, 15 Feb 2026 22:46:43 +0900 Subject: [PATCH 3/3] update README.md --- README.md | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) 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)