RealsenseでSLAM, visual odometry

ロボットの自己位置推定を行う際に必要なSLAMをrealsenseで実現する。
Odometryはロボットの移動量を様々な方法で計算する手法。
SLAMは周囲の環境の地図作成とロボットの自己位置を推定するアルゴリズム。

環境

• Ubuntu 18.04
• ROS melodic
• Intel realsense D415

準備

ROSやRealsenseのインストールの方法は以前に書いたとおり。
https://qiita.com/kei_mo/items/c0387b7d277948451881

Visual odometry, SLAMに必要なrtabmap(http://wiki.ros.org/rtabmap_ros) 等のパッケージをインストールする。
RealsenseのSLAM with D435i(https://github.com/IntelRealSense/realsense-ros/wiki/SLAM-with-D435i) を参考にインストール。
使っているのがROS melodcicであるためその部分を変更した。

sudo apt-get install ros-melodic-imu-filter-madgwick
sudo apt-get install ros-melodic-rtabmap-ros
sudo apt-get install ros-melodic-robot-localization

SLAMの実行

SLAMを実行する。

roslaunch realsense2_camera　opensource_tracking.launch

rtabmapから実行する方法もある
https://qiita.com/k65c1/items/0d9039c021ecbb26b04e

Visual odometryのみ行う。

計算資源が少ない等、SLAMは重すぎる場合は、visual odometryのみ行うことができる。
このためにはlaunch fileを自分で作っていく。
*Launch fileはROSのノードをまとめて立ち上げるためのファイル。

パッケージの作成

まずはパッケージをつくる。
パッケージは以下のコマンドで作成できる。
catkin_create_pkg [package名] [依存package名1]　[依存package名2] …

cd ~/catkin_ws/src
catkin_create_pkt visual_odometry std_msgs rospy roscpp rtabmap

*依存パッケージは上記で十分だが必要条件ではないかも

Launch fileの作成

Launch fileは慣例的にlaunchディレクトリの作成に作る。

cd ~/catkin_ws/src/visual_odomety
mkdir launch
cd launch
touch run_visual_odometry.launch

run_visual_odometry.launchの中身は次のとおりにする。

<launch>
   <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
   <remap from="rgb/image"       to="/camera/color/image_raw"/>
   <remap from="depth/image"     to="/camera/depth/image_rect_raw"/>
   <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
   <param name="frame_id"    type="string" value="camera_link"/>
   <param name="publish_tf"  type="bool"   value="false"/>
   </node>
</launch>

ポイントはpublish_tfをfalseにするところ。

ビルド

cd ~/catkin_ws
catkin_make 

source devel/setup.bash

実行

Terminal 1 realsenseを立ち上げる。

roslaunch realsense2_camera rs_camera.launch

Terminal 2 visual odometryを立ち上げる。

roslaunch visual_odometry run_visual_odometry.launch

Topicの確認

Terminal 3

rostopic list

/rtabmap/odomがpublishされていることがわかる。!

ノードの確認

rosgraph

その他

• 計算資源が少なく処理を軽くしたい場合はパラメータを調整して処理を少し軽くすることもできる http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning#Increase_Speed