IMUと3D-LiDARを使用したGLIMによる3次元地図の作成について

[Shuma-s58]

IMUと3D-LiDARを使用してGLIMによる3次元地図の作成を行おうとしたときに生じた問題についてここに記す

DockerでのGLIMの使用 (With GPU)

公式ドキュメント
Dockerを使用した起動準備
https://koide3.github.io/glim/docker.html
起動方法
https://koide3.github.io/glim/quickstart.html

以下のコードでrosbagから起動 (公式ドキュメントと少し異なる)
xhost +local:root

docker run   -it   --rm   --net=host   --ipc=host   --pid=host   --gpus all   -e=DISPLAY   -e=ROS_DOMAIN_ID   -v /home/shuma/glim_rosbag/20250906_glim_ros2_retake:/data/rosbag   -v ./config:/glim/config   koide3/glim_ros2:humble_cuda12.2   ros2 run glim_ros glim_rosnode --ros-args -p config_path:=/glim/config

/home/shuma/glim_rosbag/20250906_glim_ros2_retake :ホストPC上のrosbagの保存場所(20250906_glim_ros2_retakeはrosbagの名前)
/data/rosbag :Dockerコンテナ上のrosbagの保存場所(ホストPCからrosbagをコピーされる)

./config :ホストPC上のGLIMパラメータの保管場所(この中でパラメータの編集を行う)
/glim/config :Dockerコンテナ上のGLIMパラメータの保管場所(./configの内容がコピーされる)

---

GLIMパラメータの編集

• T_lidar_imuの値の確認方法
  ros2 topic echo /tf_static ### 3D-LiDARとIMUのframe_idを確認
ros2 run tf2_ros tf2_echo imu surestar　### imu -> surestarの関係性を確認
  ー出力例ー

At time 0.0
- Translation: [-0.350, -0.000, 0.368]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
- Rotation: in RPY (radian) [0.000, -0.000, 0.000]
- Rotation: in RPY (degree) [0.000, -0.000, 0.000]
- Matrix:
  1.000  0.000  0.000 -0.350
  0.000  1.000  0.000 -0.000
  0.000  0.000  1.000  0.368
  0.000  0.000  0.000  1.000

ー出力に基づく値の決定ー

• ./config/config_sensors.json:

  "T_lidar_imu": [-0.350, -0.000, 0.368, 0.000, 0.000, 0.000, 1.000],

---

ーその他の変更するパラメーター

• ./config/config_ros.json:

 "imu_topic": "/imu/data_raw",
 "points_topic": "/surestar_points",

• ./config/config_ros.json:

    // IMU config
    "imu_time_offset": 0.0,
    "points_time_offset": 0.0,
    "acc_scale": 0.25,  ### 変更するパラメータ (IMUの出力による変更)
    // TF config
    "imu_frame_id": "imu",   ### 変更するパラメータ
    "lidar_frame_id": "surestar",  ### 変更するパラメータ
    "base_frame_id": "base_link",  ### 変更するパラメータ
    "odom_frame_id": "odom",
    "map_frame_id": "map",
    "publish_imu2lidar": true,
    "tf_time_offset": 1e-6,

---

• 上記の変更によって最初の挙動は以下の画像のようになったが, 現状は最後までは行かずに途中で止まってしまう

パラメータ変更前

パラメータ変更後

[Shuma-s58]

IMUの問題点

IMUのtopicの出力は現状以下のようになっている

linear_acceleration:
  x: -1.3150301727294922
  y: 0.13739619140625
  z: 39.00380679931641

z軸方向の加速度は39となり、9.8m/s^2の約4倍の値となってる
GLIM上では"acc_scale"を1.0 -> 0.25にすることによってこの約4倍の値でも扱うことができるようになる
ただし、robot_localizationではIMUの重力方向の値は9.8m/s^2を入力として扱うため、この約4倍の値が入力として入っていることは問題である
ChatGPTによれば、この約4倍の値が入力として使用されると自己位置推定などに大きな悪影響を引き起こす可能性があるということであった
この問題を修正するためにIMUのパラメータの確認を行ったが、これに関連したパラメータを見つけることができなかった
https://github.com/open-rdc/orne-box/blob/humble-devel/orne_box_bringup/config/imu/adis16465.param.yaml
なので、IMUの生データを後から修正したものをrobot_localizationで使用することを検討している

ーーー

3D-LiDARの問題点

3D-LiDARのtopicのhzを確認したところ, 不安定な周期でpublishされていることがわかった
全体的に4.5~5.0周辺で揺れていることがわかる

average rate: 4.861
	min: 0.156s max: 0.578s std dev: 0.04573s window: 72
average rate: 4.875
	min: 0.156s max: 0.578s std dev: 0.04430s window: 78
average rate: 4.876
	min: 0.156s max: 0.578s std dev: 0.04295s window: 83
average rate: 4.834
	min: 0.156s max: 0.578s std dev: 0.04630s window: 89
average rate: 4.847
	min: 0.156s max: 0.578s std dev: 0.04503s window: 95
average rate: 4.704
	min: 0.156s max: 0.815s std dev: 0.07592s window: 97
average rate: 4.677
	min: 0.156s max: 0.815s std dev: 0.07571s window: 102
average rate: 4.523
	min: 0.156s max: 0.987s std dev: 0.10635s window: 104
average rate: 4.497
	min: 0.156s max: 0.987s std dev: 0.10593s window: 108

これにより実際にIMUとの周期の整合が取れずにGLIM上でwarningが出ている

[2025-09-13 09:36:53.121] [glim] [warning] large time gap between consecutive LiDAR frames!!
[2025-09-13 09:36:53.121] [glim] [warning] current=2080.177402 last=2079.360631 diff=0.816771

この問題に対しても現在対処法を検討中である

[Ryusei-Baba]

最後屋内用に調整してしまったので、参考になるかわかりませんが、昨年使用していたパラメータです。

https://github.com/Ryusei-Baba/glim