【ROS】Ubuntu16.04+ROS kinetic環境におけるRealsense D415を使ったSLAM(RTABMAP)

前回の記事に引き続き、環境MAPの作成をRtabmapというアプリで試して見ます｡

ロボットを自律走行させる場合､ロボット自身が環境のどこにいるか把握させる必要があり､カメラで周囲(壁とか床とか)を点群で計測し、移動するときの軌跡を推定するためのソフトです。

今回のテスト環境は､前回に引き続き下記の通りです｡
PC：Microsoft Surface Pro3
OS：Ubuntu16.04LTS　※上記PCにデュアルブートでインストール済み
カメラ：Realsense D415

Step.1 事前準備

冒頭に記載した､前回の記事の手順①から③までを進めます。
※D435iでIMUを使う場合は、③-1にあるデータを追加でインストールします。

Step.2 RTABMAPのインストール

aptコマンドを使ってインストールします。
sudo apt-get install ros-kinetic-rtabmap-ros

Step.3 カメラから深度、RGBデータの取得

D415/D435を使う場合は下記コマンドを実行

roslaunch realsense2_camera rs_camera.launch align_depth:=true enable_pointcloud:=true

D435iでIMUを使う場合は下記コマンドを実行
$ roslaunch realsense2_camera rs_camera.launch align_depth:=true enable_pointcloud:=true unite_imu_method:="linear_interpolation"

さらにターミナルを追加で開いて､下記コマンドを実行
$ rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" /imu/data_raw:=/camera/imu /imu/data:=/rtabmap/imu

※太字の部分は多分必須｡
※動かない場合は下記サイトで確認してみてください。

Step.4 マッピングの開始

下記コマンドを実行します。D415/D435とD435iで異なります。
D415/D435:
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 approx_sync:=false

D435i:
roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start"  Optimizer/GravitySigma 0.3" depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false wait_imu_to_init:=true imu_topic:=/rtabmap/imu

※うまく起動しない場合は上に添付した参考サイトを確認してみてください。

無事に起動すると、左側がRGBカメラと特徴点､右に点群とカメラの軌跡が表示されます｡
使ってみての感想ですが、端末スペックが足りていないのか、カメラを早く動かすと補足エラーが起きてしまいます。それなりの高スペックマシンを用意したほうがよさそうです。