RealSense D435を使ってElasticFusion

１週間ほど前3D Sensorアドベントカレンダーなるものを見かけたものの，まだ書くのは難しいかなと思い購読ボタンだけ押して静観していましたが，埋まっていなそうだったのと，数ヶ月前にRealSense D435を買ってviewerを立ち上げたっきり放置していたのを思い出して，3Dセンサ初心者ながら，勢いで登録しました．

それで，何をするかですが，何となくSLAMとかしたいなということで，D435が役に立つRGB-D SLAMのの手法の一つ，ElasticFusionを動かしてみることにしました．大した理由はなく，RealSenseを使って動かしやすそうだった（かつ動かした日本語ブログがぱっと見つからなかった）からです．

筆者の環境

デスクトップPCにGPUが刺さっており，RealSense D435を接続して，Docker上で動かす想定です．ちなみにElasticFusionはGPU必須です．

• RealSense D435 (FW 05.12.01.00)
• RealSense SDK 2.31.0
• GTX 1080
• CUDA 10.2

$ uname -a
Linux ubuntu18 4.18.0-25-generic #26~18.04.1-Ubuntu SMP Thu Jun 27 07:28:31 UTC 2019 x86_64 x86_64 x86_64 GNU/Linux

Fig. 1 RealSense D435と珍しく立った[^1]付属の三脚

今回作成したコード

少なくとも筆者と同様の環境であれば，自分が作った以下のリポジトリを使えば再現できるはずです……．

• https://github.com/eduidl/ElasticFusion-Dockerfile

ElasticFusionとは

以下の論文で提案されているDenseなRGB-D SLAMの手法です．

• ElasticFusion: Dense SLAM Without A Pose Graph, T. Whelan, S. Leutenegger, R. F. Salas-Moreno, B. Glocker and A. J. Davison, RSS '15
• ElasticFusion: Real-Time Dense SLAM and Light Source Estimation, T. Whelan, R. F. Salas-Moreno, B. Glocker, A. J. Davison and S. Leutenegger, IJRR '16

動かすのに結構手間取ってしまって論文を読む時間はなかったので，解説はできません．
気になる方は，takminさんのVisual SLAM概観というスライドの55枚目から少し紹介があるので，そちらを読むか，元論文を読んでみてください．

リポジトリについて

元のリポジトリは https://github.com/mp3guy/ElasticFusion で，こちらはRealSense SDK 2.0に対応していません．
ですが，2.0に対応するPRが上がっており，そのPR元である https://github.com/Daichou/ElasticFusion を使うことにしています．

環境構築

環境を汚したくないので，Dockerfileを作りました．
まあ，上手くいかない原因の切り分け時に一回ローカルにインストールしたので結局汚れたんですが……．Pangolin消せないという．実際Pangolin uninstallというタイトルのissueが上がっています．

ARG CUDA_VERSION=10.2
FROM nvidia/cudagl:${CUDA_VERSION}-devel-ubuntu18.04

RUN apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE \
 || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
RUN apt-get update && apt-get install -y --no-install-recommends \
    software-properties-common \
 && add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u \
 && apt-get update && apt-get install -y --no-install-recommends \
    cmake \
    freeglut3-dev \
    git \
    libeigen3-dev \
    libjpeg-dev \
    libgl1-mesa-dev \
    libglew-dev \
    librealsense2-dev \
    librealsense2-dkms \
    librealsense2-utils \
    libsuitesparse-dev \
    libudev-dev \
    libusb-1.0-0-dev \
    libxkbcommon-x11-dev \
    openjdk-8-jdk \
    zlib1g-dev \
 && rm -rf /var/lib/apt/lists/*

WORKDIR /opt

RUN git clone https://github.com/stevenlovegrove/Pangolin.git \
 && cd Pangolin \
 && mkdir build && cd build \
 && cmake .. -DAVFORMAT_INCLUDE_DIR="" \
 && cmake --build .

RUN git clone https://github.com/occipital/OpenNI2.git \
 && cd OpenNI2 \
 && make -j8

RUN ln -sf /usr/include/eigen3/Eigen /usr/include/Eigen \
 && ln -sf /usr/include/eigen3/unsupported /usr/include/unsupported

RUN git clone https://github.com/Daichou/ElasticFusion.git \
 && cd ElasticFusion/Core \
 && mkdir build && cd build \
 && cmake ../src \
 && make -j8 \
 && cd ../../GUI \
 && mkdir build && cd build \
 && cmake -DWITH_REALSENSE=ON ../src \
 && make -j8

ENV PATH $PATH:/opt/ElasticFusion/GUI/build/

WORKDIR /workspace

サンプルデータ

ElasticFusionが提供しているdyson_lab.klg というサンプルデータがあるのでまずはそれで実行．

root@ubuntu18:/workspace# ElasticFusion -l dyson_lab.klg

以下は実行中の様子です．左のカラムではオプションが変えられたり，一時停止・ステップ実行・保存といった操作もできます．見ての通り，大きな画面は，復元した3次元の結果等を表示しています．

dyson_lab.klgのデータをすべて使って復元した結果をPythonとOpen3Dを用いて可視化したものが以下になります．少なくともこのデータに関してはあまり破綻なく復元できていることが解ると思います．

RealSense D435を使う

先程は，-lでデータを指定していましたが，-lを使わないことで，接続しているデバイスを用いて処理を行うようになります．ということで早速実行したところ以下のエラーが起きました．

root@ubuntu18:/workspace# ElasticFusion 
Creating live capture... failed!
DeviceOpen using default: no devices found
Creating live capture... start
Intel RealSense D435 834412070784
Auto Exposure enable
This camera does not support Auto White Balance option.
Auto White Balance disable
terminate called after throwing an instance of 'rs2::backend_error'
  what():  set_xu(...). xioctl(UVCIOC_CTRL_QUERY) failed Last Error: Device or resource busy
Aborted (core dumped)

こちらの原因は単純で，https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md に

The Realsense DKMS kernel drivers package (librealsense2-dkms) supports Ubuntu LTS kernels 4.4, 4.8, 4.10, 4.13, 4.15 and 4.18.

とあるように，自分が使っていた5.0はサポート外だったからでした．その下に，

Configuring and building from the source code
While we strongly recommend to use DKMS package whenever possible, there are certain cases where installing and patching the system manually is necessary:

• Using SDK with non-LTS Ubuntu kernel versions: 4.16 and 5.0

とあったり，https://github.com/IntelRealSense/librealsense/issues/4586 にもあるように，scripts/patch-realsense-ubuntu-lts.sh のパッチを当てれば直るとのことですが，自分のやり方が悪かったのか結局上手く行きませんでした．
で結局Linux Kernelのバージョンをパッチが不要な4.18に下げて実行したところ動きました．

自室の復元結果は少し差し障りがあるので¹，こたつ机（散らかっている）の3次元復元結果を載せます．2枚貼っているのは3D感を出したかったからです．gifにしたかったが，差し障りがあるものを映らないようにするのが大変でやめました．

やり残し

大量にあります．
こたつの結果はまだマシで，実際に部屋をグルグル回ったりすると結構姿勢が崩れたりして，かなりの確率で崩壊した部屋が得られます．
これに関しては，キャリブレーションがおそらく上手くできていないのでより精度良く行うだったり，ハイパーパラメータの調整を行うなどしたりすることで改善するかもしれません．

雑記

• 今回，Linuxのカーネルバージョンが問題になるケースに人生で初めて出会って，（話としては聞いたことがあるが，）実際そういうのもあるのかと身を持って感じた．
• そもそもこんな狭い部屋で3DのRGB-D SLAMをやるものなのかという疑問がある．こんな狭い部屋のデータセットみたことないし．一人でセンサをもって狭い部屋をグルグルするのも悲しいので，もう少し広い部屋がほしい．
• これ書いているときに，CVPR 2019のBAD SLAM（リポジトリ）の方が動かしやすかったのでは？と思った．

1. 正直，別に問題ないのでは？という気持ちもあるが，まあ公開することではないか． ↩