はじめに
現在、息子の遊び相手になってくれる自律移動ロボット 及び 息子の手動車いすを電動化かつ半自動運転化する仕組みを個人で検討中。
その中で、今更ながら Intel Realsenseカメラを入手することができたので、導入に向けて早速立ち上げてみました。
Realsenseの様な深度計測カメラは2年ほど前に、Realsense R200 / SR300 を使ったことはあり、これをROS上に組み込んで、ペットボトルを高速で追従させたり、
しまいには子供の顔位置を認識させてその口元に正確にスプーンを運ぶ「自動ご飯食べさせアームロボット」を本気で制作したりしていました（遠い目）
しかし、R200もSR300 も Intel提供のドライバが不安定だったのか、一度開発環境が崩れてしまって、その後に復旧できないという悲劇に襲われてしまい、それきりになってしまいました。
（ファームウェアエンジニアの友人に解析協力を依頼しましたが、結局ハマってしまい、復活不可という自体になってました）
今回のD435 / T265のドライバはだいぶ安定しているという噂を聞いたので、以前の悪夢を忘れられたらよい、ですが。。。
Realsense D435
普通のカメラ同様のRGB映像の取得に加えて、ステレオビジョンによる距離計測が可能なカメラです。最小11cm〜最大10mまでの各映像の奥行き値を計測可能とのことです。
そのため、主な用途は
・物体検出時に物体との距離が分かる
なので、ものを掴むための位置を把握したり、障害物との距離を検知したり、かなり広い用途での活躍が期待できます。
近距離用だったSR300と比べても圧倒的にコンパクトなので、幅広いロボットに取り付け可能なのも利点。
ちなみに、IMU(加速度/ジャイロ)搭載のD435i という品番もあります。今から買うならこちらの方が良いかも。
（単独でVisual SLAMができます）
Realsense T265
2対の魚眼カメラと、IMU(加速度/ジャイロ)搭載しており、カメラだけで完結してVisual SLAMが実行できます。
つまり、主な用途は、
・カメラが移動した際の現在位置情報を把握できる
なので、移動ロボットに搭載しておくだけで簡単に現在位置が把握できるということですね。Visual SLAMなので、これだけだと絶対的な位置が分からない(あくまで実行開始場所からの相対位置）のと、特徴の少ない場所(例えば、真っ白な部屋）では使えないなどのデメリットはあるものの、日常的な場所で使いモノになるのかは、じっくり見ていきたいと思います。
初期確認 @ D435
Windows上でまずは動作確認をしてみました。
ツールは以下を参照。
Releases · IntelRealSense/librealsense · GitHub
RealSense SDK もしくはViewerをダウンロード して確認します。普通のRGBカメラ映像 と 距離画像(グレイスケール)の映像が見えれば、カメラ自体は正しく動いているのがわかりました。
D435のファームアップ
D435購入時期が古かったため、念の為もカメラのファームウェアバーションを上げておきます。
以下のサイトを参照にして、バージョンを5.9.2→5.10.13(当時では最新)に変更しました。
①Download Latest Firmware for Intel® RealSense™ D400 Product Family
からZIPファイルをダウンロード。Signed_Image_UVC_5_10_13_0.binファイルがあったので、
②Download Windows* Device Firmware Update Tool for Intel® RealSense™ D400 Product Family
にあるIntel RealSense D400 Series DFU Tool for Windows.zipツールもダウンロードして、これを起動する。
③ ②でダウンロードしたintel-realsense-dfu.exeを「管理者として実行」を行う
④ Realsense D435をPCのUSB3.0に刺した状態で、「2」を選択。カメラバージョンが読める事を確認する
⑤ 「１」を選択してバージョンアップ作業開始。「１」
Ubuntu上でのカメラ動作確認 (D４３５/T265)
インストール手順
上記サイトを参考にしながら設定を進めました。
まずは関連パッケージのインストール
sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE sudo apt-get update
そして、ここからrealsenseドライバのインストールになります。D435だけで使用していた際には、aptから realsense2関連パッケージを入れていたのですが、
今回はT265で SDK1.22.0 以降が要求されております。 (なぜかaptから入れると 1.18.1から更新されない…）
なので、私は一旦、公式gitサイトからSDK一式を落としてきて、buildしておりますのでご注意を！
github.com
にしたがってインストール。
git clone https://github.com/IntelRealSense/librealsense.git sudo apt-get install git libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev sudo apt-get install libglfw3-dev ./scripts/setup_udev_rules.sh ./scripts/patch-realsense-ubuntu-lts.sh sudo make install
カメラ画像確認方法
realsense-viewer で SDK起動。
まず、D435を接続して… 普通のRGBカメラ映像 と 距離画像の映像が見えることを確認します。
左側が距離画像、右側がRGBカメラ画像になります。左側の距離画像を見ると…前方のおもちゃの土台までの距離は32cmとなっています。
実際にものさしで測ってみると…33cm程度なので誤差は1cm程度、とても優秀です！！
RGB映像と距離画像は画角が違う様で近距離になる程、ずれ量がある点には注意。おそらく画素位置の補正は必要だろうなぁ〜
次に T265 を接続。普通に魚眼カメラ映像×2が見えます。同時に画面右側にはソフト起動時を原点とした座標を表示しています。
こちらは座標の軌跡を3D表示しています。座った状態でカメラを起動した後、立ち上がってぐるっとリビングを移動したので、その軌跡が残っています。
しっかりとloop closeできているので、それなりに自己位置認識の精度はありそうですね。
D435確認方法
RGB映像の確認方法は以下
roslaunch realsense2_camera rs_camera.launch rosrun image_view image_view image:=/camera/color/image_raw
上記に加えて、深度映像を確認します。
rosrun image_view image_view image:=/camera/depth/image_rect_raw
で深度映像が見えます。
次に、D435の特徴である画像をPointCloud で取得できます。これをRviz上に表示してみます。
roslaunch realsense2_camera rs_rgbd.launch enable_pointcloud:=true align_depth:=false depth_registered_processing:=true align_depth:=true
別ターミナルにて
cd ~/catkin_ws/src/realsense2_camera/rviz rviz -d pointcloud.rviz
おぉ、各画素位置が深度情報に応じて立体的にマッピングされている！ なんだか面白い（月並みなセリフ）
[RGB画像]
[PointCloud画像]
これを使えば Visual SLAMが綺麗に可視化される訳ですね!
T265確認方法
asukiaaa.blogspot.com
を参照しています。
roslaunch realsense2_camera rs_t265.launch
で起動して、その後に
rostopic echo /tf
でカメラの現在位置（
カメラを動かすと x,y位置が変わって、自己位置（相対位置）が取得できることが分かります。
また、
roslaunch realsense2_camera demo_t265.launch
を使えば、 起動時の場所を原点として、カメラの現在の位置と角度がrviz上に表示されます。おぉ、素晴らしい！
今後の参考文献 (都度更新）
本ブログをアップする直前のタイミングで、からあげ先生のサイトで「D435＋Jetson Nano」の設定方法がアップされてました（光栄です！）
今後のJetson移植時に参考にします。
qiita.com
D435でVisual SLAMをするなら参考になりそうなサイトです。
qiita.com
まとめ
周囲の障害物との距離を算出できる距離計測カメラD435 と 周囲の画像を元に自己位置を認識するカメラT265 をROS上にて動かすための情報を整理してみました。
今回は一旦はノートPC上で実装しましたが、本当はロボットにのせるエッジデバイスでカメラは使用するべきなので、最終的にはそちらで再度環境を立ち上げようと考えています。
ちょうどいま、Jetson Nanoを注文しているので、入手次第 そちらでの環境セットアップを進めて、簡単な自律移動ロボットで遊んでみたいと思います！
