IsaacSimで読み込んだLIMO（無人走行UGV）でRTAB-MAPにより走行用地図を作ろうとしたが、失敗したことについてのまとめ

# 1. はじめに
[IsaacSimで読み込んだLIMO（無人走行UGV）でRTAB-MAPを試してみる（Visual Odometryの表示編）](https://qiita.com/bebebefu/items/18abde0a7bf5180720ba)[[1]](#参考記事やサイト)では、Isaac SimとROS 2 RTAB-Mapを連携させ、LIMOロボットに搭載した単眼カメラからの情報を使ってVisual OdometryによりLIMOの移動軌跡をrtabmap vizで表示することには一応成功しました。

そしてVisual SLAMによるロボット自律走行に向けて、本記事ではIsaacSim上で構築したシミュレーション環境から得た情報を基にRTAB-Mapを用いてロボット走行用地図を作ろうとしました。しかしながら、色々と試してはみたのですが結果的に地図は生成できませんでした（地図の基となるdbファイルは生成できますが、何もデータがない状態を解消できませんでした）。

**本記事は失敗例であり、私の勉強不足を晒すようなものです。しかしながら、失敗例の紹介も今後の研究や開発の役に立つかと思いましたので、本記事を投稿しました**

# 2. 実行環境
- CPU: CORE i7 7th Gen
- メモリ: 32GB
- GPU: GeForce RTX 2070
- OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
- ROS2: Humble
- Nvidia Driver: 535 

# 3. やろうとしたこと
[IsaacSimで読み込んだLIMO（無人走行UGV）でRTAB-MAPを試してみる（Visual Odometryの表示編）](https://qiita.com/bebebefu/items/18abde0a7bf5180720ba)[[1]](#参考記事やサイト)の記事では、以下のコマンドでVisual Odometryの表示に成功しましたので、"/camera/color/image_raw", "/camera/depth/image_raw", "/camera/camera_info",ROS2 topicからの情報は適正に得られているとの認識で進めました(なお、ROS2 topic echoの結果を見ても適正に取得できていました。)。

```:terminal
ros2 launch rtabmap_launch rtabmap.launch.py \
    rgb_topic:=/camera/color/image_raw \
    depth_topic:=/camera/depth/image_raw \
    camera_info_topic:=/camera/camera_info \
    frame_id:=cam_link \
    map_frame_id:=map \
    odom_frame_id:=odom \
    approx_sync:=true \
    rtabmap_args:="--Odom/Strategy 0 --Vis/MinInliers 1 --Vis/EpipolarGeometry true"
```

そして作戦としては得られた情報をdbファイルに一旦保存し、そこから2D地図や3D地図をエクスポートするつもりでした。。。

実行したコマンドは以下になります。まず[[1]](#参考記事やサイト)の記事で紹介したIsaac Simにおけるシミュレーション空間を▶で実行した後に

```:terminal(1)
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link cam_link
```

```:terminal(2)
ros2 launch rtabmap_launch rtabmap.launch.py \
  use_sim_time:=true \
  stereo:=false \
  visual_odometry:=true \
  rgb_topic:=/camera/color/image_raw \
  depth_topic:=/camera/depth/image_raw \
  camera_info_topic:=/camera/camera_info \
  frame_id:=cam_link \
  map_frame_id:=map \
  odom_frame_id:=odom \
  publish_tf_map:=true \
  publish_tf_odom:=true \
  approx_sync:=true \
  wait_for_transform:=1.0 \
  database_path:=~/rtabmap_data/map.db \
  rtabmap_args:="--Odom/Strategy 0 \
                 --OdomF2M/BundleAdjustment 0 \
                 --Reg/BundleAdjustment 0 \
                 --Reg/MinInliers 3 \
                 --Vis/MinInliers 1 \
                 --Grid/FromDepth true \
                 --Grid/3D true \
                 --RGBD/OptimizeFromGraphEnd true \
                 --Grid/RangeMax 10.0" \
  odom_args:="--Odom/Strategy 0 \
              --OdomF2M/BundleAdjustment 0"
```
- 「ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link cam_link」のコマンドについて：
ロボットのbase_linkとカメラのcam_linkをつなげるための静的変換をしていなかったので、このコマンドを使いました。カメラがロボットに固定されているのでこのようなコマンドにしました。

- use_sim_time:=true　について：
最初はこの記載は入れていなかったのですが、時間がずれているといった旨のログが出ていました。そのためROSノードはシミュレーション時間を使用するということを明確に指示するため追加しました。


- wait_for_transform:=1.0について:
👆に関連してある程度のずれは許容できるように、TFのタイミングを最大1秒まで待てるようにしました。

- Reg/MinInliers 3, --Vis/MinInliers 1について：
最低インライア数を緩和し、少数の特徴点でも一応マッチングが成立するよう試みています。

# 4. 発生した主な問題・エラーメッセージ
- マッチング失敗 「Not enough inliers, Too low inliers」というログ：
rgbd_odometry がフレーム間の特徴点を検出してもその数が少ないため弾かれていました。

- 「Requested time XX.XX but the latest data is at time YY.YY...」というログ：
これはカメラのヘッダ時刻がROSの現在時刻より先に進んでいるなど意味で、時刻同期の問題が解決していなかったようです。

これらのほかにも色々と警告はあり結果的にdbファイルの中身は空でした。そして、RTAB-MapのGUIや rtabmap-databaseViewer で map.db を開いてもNodeテーブルやLinkテーブルにまったくエントリがなく、結果として地図生成に必要なデータが一切保存されないまま終了しました。

# 5. 考えられる原因
- 時刻同期が不十分：
IsaacSimの起動順や/clock の初期値などで大きなズレが生じているものと考えられます。use_sim_time:=true だけでなく、シミュレータのPlay開始タイミングやカメラのヘッダ時刻の設定などを合わせる必要がありそうです。

- シミュレーションのFPS低下・特徴点不足：
IsaacSimの実行自体が重い環境で動作し、フレームレートが低かったり、シーンにテクスチャが少なかったりするとマッチングに失敗しやすいという指摘があります。特に今回の環境は以下のように単純な壁や床だけでしたので特徴点が取りずらかったのかもしれません。

![https___qiita-image-store.s3.ap-northeast-1.amazonaws.com_0_3581920_2928759a-8b60-9554-b80c-96e9c18c419a.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/a8e5f8ba-9d3c-ac13-2b7e-609cdfa5b35f.png)

# 6. まとめ
以上で、Isaac Sim上でRTAB-Mapを用いた自律走行用地図生成をしようとしましたが、失敗した事例のまとめとなります。今後、研究や開発を進めるうえで、少しでも参考になれば幸いです。

まだ私もこの事象について詳細なところまで詰め切れていませんので、時間のある時に再挑戦したいと思っています。

# 参考記事やサイト
[1][IsaacSimで読み込んだLIMO（無人走行UGV）でRTAB-MAPを試してみる（Visual Odometryの表示編）](https://qiita.com/bebebefu/items/18abde0a7bf5180720ba)