概要

動かしたときの流れをメモとして残す。

※~~2024年2月5日時点ではROS 2版が動作しないため調査中~~
※2024年2月10日時点でROS 2版が動作しない問題は解決

今回試す手法

簡単な概要

この論文は、移動ロボットの状態推定とマッピングの精度と計算効率を大幅に向上させる軽量LIOアルゴリズムである Direct LiDAR-Inertial Odometry (DLIO) を紹介している。 DLIOは、連続時間軌道を構築するために新しい粗密アプローチを採用し、正確な動き補正とロバストな状態推定を保証する。

この論文の貢献

連続時間軌道を構築するための新しい粗から密への手法を提案。一定のジャークと角加速度の運動モデルを使用した一連の解析方程式が、高速かつ並列化可能な点ごとの運動補正を実現する
動き補正と事前構築を1つのステップに組み合わせ、スキャンからマップへの位置合わせを直接実行し、アルゴリズムの全体的な計算オーバーヘッドを大幅に削減する、新しい凝縮されたアーキテクチャを提案。
上記2つの貢献を達成するために不可欠な強力なパフォーマンス保証を持つ新しい非線形幾何学的オブザーバーをパイプラインで活用し、最小限の計算複雑さでロボットの完全な状態の正確な推定値を堅牢に生成する
最先端のデータセットに対して複数のデータセットを使用した広範な実験結果を通じて検証した

実行

このサイトを参考にDockerで環境を構築する

ROS1 Noetic

コンテナの構築と必要なパッケージのインストール、パスの設定

# noeticのコンテナを起動
rocker --nvidia --x11 --user --home osrf/ros:noetic-desktop-full
 
sudo apt update
sudo apt install git python3-catkin-tools libomp-dev libpcl-dev libeigen3-dev
export PYTHONPATH=$PYTHONPATH:/usr/lib/python3/dist-packages

※paythonパスを通さないとビルドに失敗する

後は公式サイトの手順通り進めて、catkin buildがエラーなく終了すれば準備完了。

公式が用意している2つのrosbagファイルも事前にダウンロードしておく。

launchファイルのパスを通したら、roslaunchを実行

source devel/setup.bash

roslaunch direct_lidar_inertial_odometry dlio.launch \
  rviz:=true \
  pointcloud_topic:=/robot/lidar \
  imu_topic:=/robot/imu

別のターミナルでダウンロードしておいたrosbagファイルを実行

source /opt/ros/noetic/setup.bash
rosbag play trees.bag

以下のように表示されたら正常動作

rosbagの再生が終わったら地図の保存

# 0.1は地図に保存する点群を間引くボクセルのサイズ
rosservice call /robot/dlio_map/save_pcd 0.1 ~/Downloads

ROS 2 humble

ROS 2版のbagファイル準備

公式のサンプルrosbagを以下の手順でROS 2版に変換

コードの修正

ソースコードをローカルにcloneしてブランチをmasterからfeature/ros2に変更し、dlio.yamlとparams.yamlを以下のように変更

※これまでROS 2版が動かなかったのは、外部パラメータがサンプルrosbagのセンサーと一致していなかったため。ROS 1のdlio.yamlファイル記載の内容に戻した。ついでにuse_sim_timeがtrueのままだと、CPU処理時間の表示が0のままになるため今回はfalseに変更

dlio.yaml

-extrinsics/baselink2imu/t: [ 0., 0., 0. ]
-extrinsics/baselink2imu/R: [ -1.,  0.,  0.,
-            0.,  1.,  0.,
-            0.,  0.,  -1. ]
+extrinsics/baselink2imu/t: [ 0.006253, -0.011775, 0.007645 ]
+extrinsics/baselink2imu/R: [ 1.,  0.,  0.,
+            0.,  1.,  0.,
+            0.,  0.,  1. ]

params.yaml

-use_sim_time: true
+use_sim_time: false

プログラムの実行

# コンテナ起動
rocker --nvidia --x11 --user --home osrf/ros:humble-desktop-full
# conda 無効
conda deactivate

# 依存ライブラリのインストール
sudo apt update
sudo apt install libomp-dev libpcl-dev libeigen3-dev

# wsに移動

# build
colcon build
source install/setup.bash
# ノード立ち上げ
ros2 launch direct_lidar_inertial_odometry dlio.launch.py \
  rviz:=true \
  pointcloud_topic:=/robot/lidar \
  imu_topic:=/robot/imu

# 別のターミナルでbagファイルを再生
source /opt/ros/humble/setup.bash
ros2 bag play aggressive_spinning

以下のように表示されたら正常動作

ROS 1と同様に地図の保存も可能

ros2 service call /save_pcd direct_lidar_inertial_odometry/srv/SavePCD "{leaf_size: 0.1, save_path: '/home/user/ros/rosbag'}"

参考情報

# conda有効のままだと以下のようなwarningが出る
(base) user@ed5411677632:~/ros/humble$ colcon build
Starting >>> direct_lidar_inertial_odometry
[Processing: direct_lidar_inertial_odometry]
--- stderr: direct_lidar_inertial_odometry
CMake Warning at CMakeLists.txt:67 (add_executable):
  Cannot generate a safe runtime search path for target dlio_odom_node
  because files in some directories may conflict with libraries in implicit
  directories:

    runtime library [libz.so.1] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /home/user/miniconda3/lib

  Some of these libraries may not be found correctly.

CMake Warning at CMakeLists.txt:72 (add_executable):
  Cannot generate a safe runtime search path for target dlio_map_node because
  files in some directories may conflict with libraries in implicit
  directories:

    runtime library [libz.so.1] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /home/user/miniconda3/lib

  Some of these libraries may not be found correctly.

---
Finished <<< direct_lidar_inertial_odometry [42.4s]

Summary: 1 package finished [42.8s]
  1 package had stderr output: direct_lidar_inertial_odometry