🗺️

RealsenseでSLAMお試し

2022/02/02に公開

ROS

robot

slam

tech

SLAMしてみたい

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

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

詳解　確率ロボティクス　Ｐｙｔｈｏｎによる基礎アルゴリズムの実装（Amazon）

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

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

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

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

SLAMを試す

rtabmap

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

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

$ 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 Frameをcamera_depth_frame、Laser 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に関しては、環境設定だけして力尽きています。

まとめ

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

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

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

参考リンク

SLAM

Realsense Laserscan

Gmapping

Cartographer

RTABMAP

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