🗺️

RealsenseでSLAMお試し

2022/02/02に公開約4,700字

SLAMしてみたい

SLAM(Simultaneous Localization and Mapping)というのは、エージェント(ロボットとか)が自分で動きながらセンサ情報をもとに地図を生成する技術です。

改造したルンバでSLAMやって、自由自在に動かしたいなーと思い、確率ロボティクスという本を読んで自力実装しようかと思ったのですが、難しすぎて挫折し続けています。

詳解 確率ロボティクス Pythonによる基礎アルゴリズムの実装(Amazon)

https://karaage.hatenadiary.jp/entry/2017/05/12/073000

ベイズとか分かってないとダメなやつですね、これ…

数式ばかりみてても頭がおかしくなりそうなので、とりあえず手を動かして体感してみることにします。

手頃な3次元センサとしてRealsense D435を使ってみます。といいつつ、最近値上がりしたのでそれほど手軽ではないかもですが…また。D435は画角がそれほど広くないのでSLAMにはそれほど適していないのですが、その話は省略します。

PCはLinux(Ubuntu 20.04)を想定しています。ROSというミドルウェアを使います。Realsense、ROSのセットアップに関しては、以下記事参照ください。

https://zenn.dev/karaage0703/articles/a267aa24a80c44

SLAMを試す

rtabmap

Visual SLAMのrtabmapを試します。以下記事ほぼそのままです。

https://qiita.com/k65c1/items/0d9039c021ecbb26b04e

インストールは以下コマンドです。

$ sudo apt install -y ros-noetic-rtabmap ros-noetic-rtabmap-ros

あとは、2つのターミナルのウィンドウ開いて、それぞれ以下コマンド実行するだけです。

$ roslaunch realsense2_camera rs_aligned_depth.launch
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info

良い感じにSLAMできます。

gmapping

FastSLAMという2003年に発表された手法です。この手法はオドメトリが必要になります。

オドメトリとは、一言でいうとセンサを搭載したエージェント(ロボットとか車とか)の移動情報ですね。

なので、本来はオドメトリ情報のとれるエージェントにセンサをとりつけないとダメなのですが、今回はとりあえず、センサを固定して無理やり動かしてみます。

gmappingは、LiDARの2次元スキャンの情報しかとれないので、まずはRealsenseの出力を変換します。

以下でdepthimage_to_laserscanというパッケージをダウンロードしてビルドします。

$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/depthimage_to_laserscan
$ catkin build
$ source ~/.bashrc

以下コマンドでリアルセンスを起動します。

$ roslaunch realsense2_camera rs_aligned_depth.launch

新たにターミナルを開き、以下コマンドで、RealsenseのDepth情報をレーザースキャン形式に変換します。

$ roslaunch depthimage_to_laserscan launchfile_sample.launch 

一旦動作確認します。

$ rviz

設定のFixed Framecamera_depth_frameLaser scan/scanにすると、以下のように2次元のスキャン情報が見れます。

続いて、gmappingを動かします。一気にセットアップしていきます。

途中、私が作成したセットアップファイルをダウンロードしたりしています。気になる方は中身をみてください。

$ sudo apt update
$ sudo apt install -y ros-noetic-openslam-gmapping
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-perception/slam_gmapping
$ cd slam_gmapping/gmapping
$ wget https://raw.githubusercontent.com/karaage0703/slam_sample/main/slam_gmapping_realsense.launch
$ wget https://raw.githubusercontent.com/karaage0703/slam_sample/main/slam_realsense.rviz
$ cd ~/catkin_ws/src
$ catkin build
$ source ~/.bashrc

あとは、以下コマンドでgmappingでSLAMできます。

$ roslaunch gmapping slam_gmapping_realsense.launch

以下rvizコマンドで可視化しましょう

$ cd ~/catkin_ws/src/slam_gmapping/gmapping
$ rviz -d slam_realsense.rviz

以下のように2次元マップが生成されます。ただし、オドメトリ情報がなくセンサを動かせないので、ほんとうに1部分しか地図が生成できません。

他の手法

CartographerとかHector SLAMとかAutowareを使った方法がありそうですが、力尽きたので一旦ここまでにします。もし試したら追記します。

Autowareに関しては、環境設定だけして力尽きています。

https://zenn.dev/karaage0703/articles/cbc319a2a76fe1

まとめ

SLAMをとりあえず試してみました。

ルンバで地図作って移動したいだけなのですが、かなり大変ですね。

情報もたくさんあるようで、意外にまともに動く新しい情報は少なめな印象です。

参考リンク

SLAM

https://jp.mathworks.com/discovery/slam.html

https://speakerdeck.com/ksakurada/visual-slamru-men-fa-zhan-falseli-shi-toji-chu-falsexi-de

Realsense Laserscan

https://wiki.ros.org/depthimage_to_laserscan

https://github.com/IntelRealSense/realsense-ros/issues/1163

Gmapping

https://sy-base.com/myrobotics/ros/gmapping/

https://qiita.com/srs/items/b07a22425548c41bfd04

https://answers.ros.org/question/270354/slam-mapping-with-rviz-realsense-and-a-turtlebot/

https://roboticsknowledgebase.com/wiki/common-platforms/ros/ros-mapping-localization/

https://qiita.com/motoms/items/4c45f75911e210088ea1

Cartographer

https://qiita.com/MMM-lab/items/14e94f9d41ccca4e1c61

https://daily-tech.hatenablog.com/entry/2019/11/25/062219

https://qiita.com/devemin/items/1723058cf3bac85aaa0b

RTABMAP

https://qiita.com/k65c1/items/0d9039c021ecbb26b04e

http://wiki.ros.org/rtabmap_ros/Tutorials

http://introlab.github.io/rtabmap/

https://qiita.com/MoriKen/items/87be494f048ea03fdab8

https://at-wat.github.io/ROS-quick-start-up/ros-navigation.html

Robot

https://github.com/AutonomyLab/create_robot

https://demura.net/robot/hard/20405.html

https://kanpapa.com/today/2021/07/Roomba-robot-ros-part1-otafab.html

Direct LiDAR Odometry

https://github.com/vectr-ucla/direct_lidar_odometry

NDT Matching

https://docs.google.com/presentation/d/1rWhCQtZv4YSWdedoJWPAF3Q4umwaA8yuDADyWIhrM20/edit?usp=sharing

Discussion

ログインするとコメントできます