Auto detecting IMU/Points frame IDs and acc_scale by koide3 · Pull Request #67 · koide3/glim_ros2

koide3 · 2026-02-14T09:54:57Z

No description provided.

Copilot

Pull request overview

This PR aims to reduce required manual configuration by auto-detecting IMU/LiDAR frame IDs at runtime and auto-detecting IMU acceleration unit scaling, and extends RViz outputs with scan-end pose/odometry topics.

Changes:

Default imu_frame_id / lidar_frame_id to empty and auto-detect them via GlobalConfig meta fields.
Add ~/odom_scanend, ~/pose_scanend (and corrected variants) publishers in the RViz viewer.
Change acc_scale default to 0.0 and infer scale from IMU acceleration norm on first use.

Reviewed changes

Copilot reviewed 3 out of 3 changed files in this pull request and generated 7 comments.

File	Description
src/glim_ros/rviz_viewer.cpp	Adds scan-end pose/odom publishing and runtime frame-id fallback/auto-detection.
src/glim_ros/glim_ros.cpp	Auto-detects IMU/LiDAR frame IDs into `GlobalConfig` meta and auto-detects `acc_scale`.
include/glim_ros/rviz_viewer.hpp	Declares new scan-end publishers.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-02-14T10:58:20Z

+
+    odom_scan_end_pub->publish(odom);
+
+    logger->debug("published odom (stamp={})", new_frame->stamp);


This debug log says published odom and prints new_frame->stamp, but this message publishes the scan-end odometry using imu_end_stamp. Logging the published stamp/topic (e.g., imu_end_time / odom_scanend) would avoid confusion when debugging timing issues.

Suggested change

logger->debug("published odom (stamp={})", new_frame->stamp);

logger->debug("published odom_scanend (stamp={})", imu_end_stamp);

Copilot · 2026-02-14T10:58:20Z

+    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();


odom.header.stamp is set to the scan-end time, but the twist is copied from v_odom_imu (the begin/frame velocity). That makes the Odometry message internally inconsistent in time. Consider leaving twist unset/zero for scan-end messages or computing an end-of-scan velocity if available.

Suggested change

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();

// Twist at scan end is not available; publish zero twist to avoid time inconsistency.

odom.twist.twist.linear.x = 0.0;

odom.twist.twist.linear.y = 0.0;

odom.twist.twist.linear.z = 0.0;

Copilot · 2026-02-14T10:58:20Z

+    if (std::abs(imu_end_time - new_frame->stamp) < 1e-3) {
+      logger->warn("Scan end time is too close to the frame time (imu_end_time={}, frame_time={})", imu_end_time, new_frame->stamp);
+    }


Same as the odom scan-end block: this warning will also trigger whenever imu_rate_trajectory is missing (end time defaults to frame time), and it can be emitted twice per frame if both scan-end topics are subscribed. Consider gating it on trajectory availability and/or warning once per condition.

Copilot · 2026-02-14T10:58:20Z

+    pose.pose.orientation.w = quat_world_imuend.w();
+    pose_scan_end_pub->publish(pose);
+
+    logger->debug("published pose (stamp={})", new_frame->stamp);


This debug log prints new_frame->stamp, but this pose is published at imu_end_stamp. Logging the published stamp/topic (e.g., imu_end_time / pose_scanend) would make timing debugging clearer.

Suggested change

logger->debug("published pose (stamp={})", new_frame->stamp);

logger->debug("published pose_scanend (stamp={})", imu_end_time);

Copilot · 2026-02-14T10:58:21Z

  imu_time_offset = config_ros.param<double>("glim_ros", "imu_time_offset", 0.0);
  points_time_offset = config_ros.param<double>("glim_ros", "points_time_offset", 0.0);
-  acc_scale = config_ros.param<double>("glim_ros", "acc_scale", 1.0);
+  acc_scale = config_ros.param<double>("glim_ros", "acc_scale", 0.0);


Changing the default acc_scale from 1.0 to 0.0 changes runtime behavior for users who rely on defaults (it now enables auto-detection). If auto-detection is intended to be opt-in, consider keeping the default at 1.0 and treating 0.0 as a sentinel value (or add an explicit auto_detect_acc_scale param).

Suggested change

acc_scale = config_ros.param<double>("glim_ros", "acc_scale", 0.0);

acc_scale = config_ros.param<double>("glim_ros", "acc_scale", 1.0);

Copilot · 2026-02-14T10:58:21Z

+  if (std::abs(acc_scale) < 1e-6) {
+    const double norm = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z).norm();
+    if (norm > 7.0 && norm < 12.0) {
+      acc_scale = 1.0;
+      spdlog::debug("assuming [m/s^2] for acceleration unit (acc_scale={}, norm={})", acc_scale, norm);
+    } else if (norm > 0.8 && norm < 1.2) {
+      acc_scale = 9.80665;
+      spdlog::debug("assuming [g] for acceleration unit (acc_scale={}, norm={})", acc_scale, norm);
+    } else {
+      acc_scale = 1.0;
+      spdlog::warn("unexpected acceleration norm {}. assuming [m/s^2] for acceleration unit (acc_scale={})", norm, acc_scale);
+    }
+  }


The auto-detection locks acc_scale based on a single IMU message. If the first sample is during motion/vibration, the acceleration norm may fall outside the expected ranges and permanently select the wrong scale. Consider estimating over a short window (e.g., median norm over N samples) and/or only finalizing when the norm is stable and near gravity.

Copilot · 2026-02-14T10:58:21Z

+    if (std::abs(imu_end_time - new_frame->stamp) < 1e-3) {
+      logger->warn("Scan end time is too close to the frame time (imu_end_time={}, frame_time={})", imu_end_time, new_frame->stamp);


The scan-end warning condition will trigger every frame when imu_rate_trajectory is empty because imu_end_time defaults to new_frame->stamp (diff=0 < 1e-3). This can spam logs whenever there are subscribers. Consider only warning when imu_rate_trajectory is available (e.g., cols() > 0) and the end stamp is unexpectedly close, or throttle/warn-once when scan-end info is unavailable.

Suggested change

if (std::abs(imu_end_time - new_frame->stamp) < 1e-3) {

logger->warn("Scan end time is too close to the frame time (imu_end_time={}, frame_time={})", imu_end_time, new_frame->stamp);

static bool scan_end_time_warned = false;

if (!scan_end_time_warned && std::abs(imu_end_time - new_frame->stamp) < 1e-3) {

logger->warn("Scan end time is too close to the frame time (imu_end_time={}, frame_time={})", imu_end_time, new_frame->stamp);

scan_end_time_warned = true;

koide3 added 2 commits February 14, 2026 18:06

publish poses at the end of scan

b6ae5e1

auto-detect imu/points topics and acc_norm

13a6bcb

koide3 requested a review from Copilot February 14, 2026 10:54

Copilot started reviewing on behalf of koide3 February 14, 2026 10:54 View session

Copilot AI reviewed Feb 14, 2026

View reviewed changes

koide3 added 2 commits February 14, 2026 21:20

Merge branch 'master' of github.com:koide3/glim_ros2 into auto_frame

5bf4a10

fix debug msgs

25dee1a

koide3 merged commit b077e8e into master Feb 14, 2026
7 checks passed

koide3 deleted the auto_frame branch February 14, 2026 14:26

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Auto detecting IMU/Points frame IDs and acc_scale#67

Auto detecting IMU/Points frame IDs and acc_scale#67
koide3 merged 4 commits intomasterfrom
auto_frame

koide3 commented Feb 14, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Copilot AI Feb 14, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants


		odom_scan_end_pub->publish(odom);

		logger->debug("published odom (stamp={})", new_frame->stamp);

	logger->debug("published odom (stamp={})", new_frame->stamp);
	logger->debug("published odom_scanend (stamp={})", imu_end_stamp);

-    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();
+    // Twist at scan end is not available; publish zero twist to avoid time inconsistency.
+    odom.twist.twist.linear.x = 0.0;
+    odom.twist.twist.linear.y = 0.0;
+    odom.twist.twist.linear.z = 0.0;

	logger->debug("published pose (stamp={})", new_frame->stamp);
	logger->debug("published pose_scanend (stamp={})", imu_end_time);

	acc_scale = config_ros.param<double>("glim_ros", "acc_scale", 0.0);
	acc_scale = config_ros.param<double>("glim_ros", "acc_scale", 1.0);

		if (std::abs(imu_end_time - new_frame->stamp) < 1e-3) {
		logger->warn("Scan end time is too close to the frame time (imu_end_time={}, frame_time={})", imu_end_time, new_frame->stamp);

Conversation

koide3 commented Feb 14, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 14, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants