ORB-SLAM2 with Realsense D435

#Realsense D435でVisual SLAMがしたい
Realsense D435は２万円くらいでグローバルシャッターで屋外でも使えるKinectの後継としては有力候補な子。ということは、みんなこいつでVisual SLAMしたいよね。僕もしたいです。というわけで動かすことはできたのでメモ。
#ORB-SLAM2を選んだ理由
もちろん有名なVisual SLAM実装であるのもそうだけど、このYoutubeビデオがあったから行けそうな気がした。
https://www.youtube.com/watch?v=yl6MgnYNDE8
#ORB-SLAM2コンパイル上の注意
このコメントみたいにCMakeListいじらないとだめ。ちなみに僕はLXD上のUbuntu 16.04 kineticでコンパイル。(ROSはTCP/IPでデータ送れるからね。ホストは18.04 melodic)
https://github.com/raulmur/ORB_SLAM2/issues/494#issuecomment-356221762
#yamlファイルの用意
yamlを作るにはrealsenseのノード(roslaunch realsense2_camera rs_rgbd.launch)を一回立てて、ファクトリー設定の校正情報を取得する(rostopic echo /camera/color/camera_info )どれがyamlのパラメータのどれに対応してるかはここを見る。
http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html
歪み周りが0.0入ってるんだがこれ校正されてなくないか？
#launch
yamlファイルの位置と名前は自由。あとはgit cloneした位置にも依存。
rosrun ORB_SLAM2 RGBD ~/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/ORB_SLAM2/Examples/RGB-D/D435.yaml /camera/rgb/image_raw:=/camera/color/image_raw /camera/depth_registered/image_raw:=/camera/aligned_depth_to_color/image_raw
やってることはORB SLAM2がSubscribeしているトピックに中身的に合いそうなトピックをリネームして突っ込んでいる。これが対応しそうと考えた理由は見つけた中では唯一公式にD435に対応していたrtabmapのチュートリアル。
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
#おわりに
ちなみにこれだと全然性能は出せていない。なぜかというとまずimage_rawがどうもグレイスケールで読まれているのと、たぶん/camera/depth_registered/image_rawという名前的に、openniが想定されているので、今突っ込んでるやつだと255までしか値が出てない気がするのでuint16じゃない。最悪ココらへんはラッパー挟むべきか。ちなみにYoutubeのもグレイになってる。画像出るとこの下のRGB値を見ると３つ一緒になってるし多分正しいのでは。
2-18/08/31追記:ソースを見ると内部でグレイスケールにしていたのでそれで良いようだ。uint16じゃないのも、ROSのメッセージの仕様でbyte単位で区切っているための模様。つまり、普通に動く。校正はもう少ししてもいいかもしれないが。
カーテンみたいなテクスチャレスな場所だときつかった。気が向いたら情報流出的に問題なさそうなところでテストして動画あげたいところ。
うまいこと動かせるように治ったら、次はStereo DSOやりたいけどこれはなんかROSの対応悪いし行けるかよくわからない。最悪、SDKを直叩きすればいいんだろうが。