vSLAMの動作原理の概要 + 最近の研究事例DROID-SLAM紹介

# 1. SLAMとは
SLAMはSimultaneous Localization and Mappingの略で、自己位置の推定(Localization)と、環境のマップ構築(Mapping)という2つの問題を同時に解くタスクです。元々はロボット制御の文脈で提案された技術ですが、最近では拡張現実や自動運転などへの応用も展開されています。一般にSLAMにはLiDAR、Wi-Fi、カメラなど種々のセンサーの測定結果が利用されますが、特にカメラで撮影した画像の情報を手掛かりとしたSLAMはVisual SLAM(vSLAM)と呼ばれます。ここでは特にvSLAMについて、その手法の概要と最近の研究の紹介をしたいと思います。

# 2. vSLAMの動作原理
近年のvSLAMの研究は、他の画像処理タスクと同様に深層学習を用いたものが活発ですが、基本的な考え方は深層学習の登場以前の古典的なvSLAMである程度確立しています。そこで、まずは古典的なvSLAMがどのような原理で動作しているのかを簡単に説明します。
古典的なvSLAMは、カメラで撮影された動画のフレーム画像群に対して、各フレーム画像に映っているオブジェクトの位置関係の幾何学的な制約が満たされるようにカメラポーズ(カメラの位置と向き)と環境の三次元構造を決定する最適化問題の形で定式化されます。このような幾何学的な制約からカメラポーズと環境の三次元構造を決定する処理は、バンドル調整と呼ばれます。この最適化問題の目的関数として何を用いるかによって、vSLAMの多くは非直接法(特徴点ベースの方法)または直接法と呼ばれるものに分類されます。非直接法では、フレーム画像から検出されたコーナーやエッジなどの特徴点のフレーム画像面上および世界座標上での位置の辻褄が合うようにカメラポーズと特徴点の三次元位置が決定されます。歴史的には最初のSLAMは非直接法によるもので、代表的なものとしてはPTAM(2007)やORB-SLAM(2015)などが知られています。非直接法は特徴点だけを利用するため計算負荷が軽く、このことはリアルタイム処理を志向する(ロボット制御や自動運転では特に)vSLAMにとっては有利な点ですが、テクスチャレスなオブジェクトの多い環境では十分な数の特徴点の検出ができず、原理的に解けない問題となってしまうことが欠点です。一方、直接法では、特徴点か否かに関わらずフレーム画像面上での全てのピクセルの輝度値が利用されます。代表的なものとしてはDTAM(2011)やLSD-SLAM(2014)が知られています。特徴点の検出をベースとしていないのでテクスチャレスなオブジェクトでも問題なく動作することは利点ですが、最適化の計算自体が難しくなることが欠点です。またローリングシャッターカメラで撮影された動画に対しては、直接法は適切に動作しないことが知られています。(非直接法・直接法の動作原理の説明を図2.1-2.2に示しました。)

![図2-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2724841/2a91e943-0149-2492-b617-435c9c9bda64.png "fig-2.1")
&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;図2.1: 非直接法（特徴点ベースの手法）の動作原理

図2.1について説明します。

1. N個のカメラポーズで撮影したフレーム群が存在する（図では$N=3$）。予めカメラ行列は特定しておく。
1. エッジやコーナーなどの特徴点を検出し、そのうち$j$番目の特徴点の画像面$i$上での座標を$z^{(i,j)}(i=1,2,\cdots,N)$とする。
1. 各フレームのカメラポーズ（カメラの世界座標での位置と向き）に適当な初期値$p^{(i)}(i=1,2,\cdots,N)$を設定する。
1. 手順2で検出した特徴点の世界座標での位置$X^{(j)}$に適当な初期値を設定する。
1. カメラ行列とカメラポーズ$p^{(i)}(i=1,2,\cdots,N)$、特徴点の位置$X^{(j)}$から、各画像面に特徴点を投影したときの画像面上での座標$z(p^{(i)},X^{(j)})$が一意に定まる。
1. geometric errorを最小化するようにカメラボーズ、特徴点位置を決定する：
```math
p, X = \mathop{\rm argmin}\limits_{p,X} \sum_{i,j}L(e_g^{(i,j)})
```
ここで$e_g^{i,j}=z^{(i,j)} - z(p^{(i)}, X^{(j)}) $で、$L(・)$はHuberノルムなど

以上が図2.1の説明になります。

![図2-2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2724841/8656b3df-0389-87e6-487f-f28e7ce42a86.png "fig-2.2")
&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;図2.2: 直接法の動作原理

図2.2について説明します。

1. N個のカメラポーズで撮影したフレーム群が存在する（図では$N=3$）。予めカメラ行列は特定しておく。
1. 特定のフレーム$i$（図では$i=2$に着目）の特定の画素$j$に着目し、その画像面上での座標$z^{(i,j)}$を定める。
1. 各フレームのカメラポーズ（カメラの世界座標での位置と向き）に適当な初期値$p^{(i)}(i=1,2,\cdots,N)$を設定する。
1. 手順2で着目した画素のデプスに適当な初期値$d^{(2,j)}$を設定する。
1. カメラポーズ$p^{(i)}(i=1,2,\cdots,N)$、着目画素の着目フレームにおけるデプス$d^{(2,j)}$から、着目画素の世界座標での位置$X(p^{(2)}, d^{(2,j)})$が一意に定まる。さらにカメラ行列から、これを各フレームに投影したときの各画像面上での座標$z^{(2,j,i)}(i=1,2,\cdots,N)$が定まる。
1. Photometric errorを最小化するようにカメラポーズ、デプスを決定する：
```math
p, d = \mathop{\rm argmin}\limits_{p,d} \sum_{i,j,k}L(e_p^{(i,j,k)})
```
ここで$e_p^{(i,j,k)} = l^{(k)}(z^{(i,j,k)}) - l^{(i)}(z^{(i,j)})$で、$L(・)$はHuberノルムなど
$l^{(i)}(z)$は画像面$i$上の座標$z$における輝度値

以上が図2.2の説明になります。

# 3. 最近の発展
近年のvSLAM研究は、深層学習を用いたものが活発に行われています。ここでは、最近のvSLAM研究の一例として、NeurIPS2021で発表されたDROID-SLAMを紹介します。このアルゴリズムは、vSLAMの一連の処理をEnd-to-Endで実行できるものとなっていて、単眼カメラだけでなく、ステレオカメラ・RGB-Dカメラに対応可能なもので、これまでのvSLAMのstate of the artを大きく更新しました。
DROID-SLAMは、直接法と非直接法の折衷的な方法で、バンドル調整(カメラポーズとデプスの推定)をアップデートオペレータと呼称されるニューラルネットワークで実現するのが特徴です。アップデートオペレータは図3.1のような構造のリカレントニューラルネットワーク(RNN)で、カメラポーズ$G$とデプス$d$を逐次的に更新する推論モデルです。アップデートオペレータには、共通するオブジェクトが映ったフレーム対(図ではフレーム$i,j$)が入力されます。一般的にRNNは時系列データやテキストなどの系列データを処理するために利用されることが多いですが、ここでは少し変わった使われ方をされています。ニュートンラフソン法などの一般的な最適化アルゴリズムでは値の更新を繰り返すことで最適な値に収束していきますが、アップデートオペレータではRNNの繰り返し構造でこのような振る舞いを模倣しています。つまり、図3.1で推論が右の方向に進むにしたがってカメラポーズ$G$とデプス$d$が少しずつ更新されていき、適当な回数の更新を繰り返したあとに正確な値に収束することを狙った構造になっています。

![図3-1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2724841/5a9a688f-827c-8735-71cb-75609c1d1728.png)
&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;図3.1: アップデートオペレータ

図3.1について説明します。

アップデートオペレータには、共通するオブジェクトが映るフレーム対が入力されます。
(上図はフレーム$i,j$が入力された場合を示しています。)
図では奥行きがあるように描かれていて、右端には”$\times|E|$”と書かれていることが確認できますが、これはフレーム対の数($|E|$)の分だけ同じ処理がおこなわれることを示しています。

図の上部の緑の矩形で囲まれた部分は、フレーム対の対応するピクセルを特定する処理です。
DROID-SLAMでは、geometric errorを用いた非直接法的なアプローチをとりながら、画像全体の情報を用いる直接法的な側面も持っています。これにより、最適化が比較的容易な非直接法のメリットと、豊富な情報を扱える直接法のメリットを享受しています。

図の下部にあるDBAは、フレーム対の対応するピクセル情報から、カメラポーズとデプスの推定をおこないます。繰り返し更新されることで最適な値に収束するようなアーキテクチャになっています。

以上が図3.1の説明になります。

DROID-SLAMは、大規模なCGデータセットであるTartanAirを用いて訓練されます。TartanAirはvSLAMのために構築されたデータセットで、1037シーケンス、合計100万フレーム以上から成り、カメラポーズやデプスなどの正解データとともに公開されています。DROID-SLAMの訓練では、その正解データを用いた教師あり学習によってvSLAMとしての能力が獲得されます。

論文中では実データでの性能を他手法と比較した結果が記載されています。ここでは、EuRoCとTUM-RGBDの２つのデータセットに対する単眼カメラ推定の精度評価の結果をご紹介します。これらのデータセットのうち、TUM-RGBDはモーションブラーや激しい回転運動が含まれていて、単眼カメラによる位置推定が難しいデータセットです。評価に用いている指標は絶対軌道誤差(ATE)で、正解の軌道とSLAMで推定した軌道との誤差をメートル単位で算出しています。それぞれのデータセットでの性能を他手法と比較したテーブルが図3.2-3.3（引用元：[DROID-SLAM: Deep Visual SLAM for Monocular, Stereo, and RGB-D Cameras](https://arxiv.org/abs/2108.10869)）です。

![図3-2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2724841/b6a1158b-f257-c16d-de08-4e818d35962a.png)
&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;図3.2: 他手法との性能比較（単眼SLAM）

図3.2について説明します。

EuRoCの各シーケンス(MH01, MH02, ..., V203)に対して、絶対軌道誤差(ATE)をメートル単位で記載されています。
D3VO+DSOで値が”$-$“となっている箇所は、当該論文中で記載がなく値が不明であるものです。
ORB-SLAM、SVO、ORB-SLAM3で値がXとなっているものは、算出に失敗しているものです。

深層学習ベースの手法(DeepFactors ~ D3VO+DSO)は比較的ATEが大きく精度は今一つですが、失敗しているシーケンスはなく、頑健性が高いことが分かります。
一方で最適化ベースの手法(ORB-SLAM ~ ORB-SLAM3)は比較的ATEが小さく精度は高いですが、失敗しているシーケンス(Xのシーケンス)が散見され、頑健性に課題があることが分かります。

それと比べると最下段のDROID-SLAMでは、精度と頑健性の両面で優れていることが確認できます。

以上が図3.2の説明になります。

![図3-3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2724841/bf8a65d1-0c2d-08e1-02f0-a84d0007ba70.png)
&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;&emsp;図3.3: 他手法との性能比較（単眼SLAM）

図3.3について説明します。

TUM-RGBDの各シーケンス(360, desk, ..., xyz)に対して、絶対軌道誤差(ATE)をメートル単位で記載されています。
ORB-SLAM2、ORB-SLAM3で値がXとなっているものは、算出に失敗しているものです。

最適化ベースの手法(ORB-SLAM2、ORB-SLAM3)は、それぞれ半分以上のシーケンスで算出に失敗しています。
一方で深層学習ベースの手法(DeepTAM ~ DeepFactors)は全シーケンスで算出に成功していますが、精度は今一つ(最も精度の高いDeepTAMでも平均11.6 cmのATE)です。

それと比べると最下段のDROID-SLAMでは、ATE＝3.8 cmと高い精度で、なおかつ頑健であることが確認できます。

以上が図3.3の説明になります。

最適化ベースのvSLAMでは、幾何学的制約から導出される最適化問題が不良設定問題となって原理的に解けなくなり、自己位置の推定が不能となる問題があります。一方で深層学習ベースのvSLAMでは、原理的に幾何学的制約から自己位置が決定できない場合でも、学習データから得られた知識を用いて推論自体は実行することが可能です。この意味で、一般に深層学習ベースの手法は頑健性の点で最適化ベースの手法よりも優れています。ところが、深層学習ベースの手法では学習したデータに対する過剰適合が課題となっていて、精度の面では最適化ベースの手法の方が優れていることが多いです。この傾向は図3.2-3.3の結果からも確認できますが、DROID-SLAMは頑健性と精度の両面で多手法より優れていることが確認できます。
その他、論文中ではステレオカメラやRGB-Dカメラを用いたパフォーマンス評価も記載されています。詳細は論文をご確認ください。


# 4. まとめ
今回はvSLAMの動作原理と、最近のvSLAMの研究事例としてDROID-SLAMを紹介しました。従来の深層学習ベースのvSLAMでは、学習していないデータに対する汎化性能が課題となっていましたが、DROID-SLAMはこれを克服したものとなっています。DROID-SLAMである程度vSLAMは成熟した感がありますが、今後どのような発展を辿るか注目です。

# 参考文献
- 代表的なｖSLAMのサーベイ論文
[T. Taketomi et al., Visual SLAM algorithms: a survey from 2010 to 2016, 2017](https://ipsjcva.springeropen.com/articles/10.1186/s41074-017-0027-2)
- vSLAMとviSLAMのサーベイ論文
[M. Servieres et al., Visual and Visual-Inertial SLAM: State of the Art, Classification, and Experimental Benchmarking, 2021](https://www.hindawi.com/journals/js/2021/2054828/)
- ORB-SLAM3の論文
[C. Campos et al., ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM, 2020](https://arxiv.org/abs/2007.11898)
- EuRoCの論文
[M. Burri et al., The EuRoC micro aerial vehicle Datasets, 2016](https://www.researchgate.net/publication/291954561_The_EuRoC_micro_aerial_vehicle_datasets)
- TUM-RGBDの論文
[J. Sturm et al., A benchmark for the evaluation of RGB-D SLAM systems, 2012](https://www.researchgate.net/publication/261353760_A_benchmark_for_the_evaluation_of_RGB-D_SLAM_systems)
- TartanAir(DROID-SLAMの学習データセット)の論文
[W. Wang et al., TartanAir: A Dataset to Push the Limits of Visual SLAM, 2020](https://arxiv.org/abs/2003.14338)
- RAFT(DROID-SLAMと同著者のオプティカルフロー推定NN)の論文
[Z. Teed et al., RAFT: Recurrent All-Pairs Field Transforms for Optical Flow, 2020](https://arxiv.org/abs/2003.12039)
- DROID-SLAMの論文
[Z. Teed et al., DROID-SLAM: Deep Visual SLAM for Monocular, Stereo, and RGB-D Cameras, 2021](https://arxiv.org/abs/2108.10869)