『A Fast Resection-Intersection Method for the Known Rotation Problem』を読む

adventar.org

これはプロラボアドベントカレンダー7日目の記事です。

皆さん卒研の進捗どうですか。僕は進捗が生えません。

特徴点ベースのVisual SLAMについての論文『A Fast Resection-Intersection Method for the Known Rotation Problem』*1を読んだので記事を書きます。

SLAMとはRGBカメラ(普通のカメラ)やレーザーなどのセンサーをロボットや車などに載せて、自己位置の推定と周りの地図の作成を行うことです。Visual SLAMはその中でも画像を入力とするものを指します。
下の動画はORB-SLAM2というOSSのSLAMを動かしている様子です。

www.youtube.com

論文の内容

『A Fast Resection-Intersection Method for the Known Rotation Problem』では画像と既知のカメラの回転から、シーンポイントの3次元座標とカメラの3次元座標を求めるKnown Rotation Problemという問題を解きます。Known Rotation Problemは次のように定式化されます。

\begin{equation}
\min_{\{\boldsymbol{X}_i\}, \{\boldsymbol{t}_j\}}
\max_{i,j}
\|
\boldsymbol{u}_{i,j} - f(\boldsymbol{X}_i | \boldsymbol{R}_j, \boldsymbol{t}_j)
\|_2 \\
s.t. \qquad \boldsymbol{R}^3_j \boldsymbol{s}_k + \boldsymbol{t}_j > 0 \ \forall{j, k}
\end{equation}
と はそれぞれj番目のフレームにおけるカメラの回転と位置、を番目のシーンポイントの3次元座標、を番目の画像から見た番目のシーンポイントの2次元座標です。
はシーンポイント を回転と位置が とであるカメラに投影する関数です。カメラの焦点距離とかで変わります。
制約不等式は、シーンポイントがカメラの前にあることを表しています(カメラの前にないとカメラに写りません)。

は固定してを最適化するRes、は固定してを最適化するIntの2つの問題に分けて解きます。
どちらの問題も、
\begin{eqnarray}
r_i(\boldsymbol{x}) = \frac
{
\|
\boldsymbol{A}_i \boldsymbol{x} + \boldsymbol{b}_i
\|_2
}
{
\boldsymbol{c}^T_i \boldsymbol{x} + d_i
}
\end{eqnarray}
を使って
\begin{eqnarray}
\min_{\boldsymbol{x} \in \mathbb{R}^3 }
\max_{i}
r_i(\boldsymbol{x}) \\
s.t. \qquad \boldsymbol{c}^T_i \boldsymbol{x} + d_i > 0 \ \forall{i}
\end{eqnarray}
と表せます。

この問題を解くために目的関数
\begin{eqnarray}
\max_{i}
r_i(\boldsymbol{\hat{x}})
\end{eqnarray}
の値が減少するように
\begin{eqnarray}
\boldsymbol{\hat{x}} \leftarrow \boldsymbol{\hat{x}} + \alpha \boldsymbol{\lambda}
\end{eqnarray}
とを繰り返し更新します。目的関数を減少させるベクトルを求めてそれに適当な正の実数を掛けています

まず、を求めます。
の最大値を最小化するので、の中でも最大値を取るだけを考えます。各について
\begin{eqnarray}
\boldsymbol{g}_l = - \frac
{
\nabla r_l(\boldsymbol{\hat{x}})
}
{
\|
\nabla r_l(\boldsymbol{\hat{x}})
\|_2
}
\end{eqnarray}
を計算し、を包含する最小の球の中心が目的関数を最も減少させる方向なので*2、これをとします。

次にを求めます。
をの関数と考えると

\begin{eqnarray}
r_i(\alpha) = \frac
{
\|
\boldsymbol{A}_i (\boldsymbol{\hat{x}} + \alpha \boldsymbol{\lambda}) + \boldsymbol{b}_i
\|_2
}
{
\boldsymbol{c}^T_i (\boldsymbol{\hat{x}} + \alpha \boldsymbol{\lambda}) + d_i
}
\end{eqnarray}
となってを求める問題は
\begin{eqnarray}
\min_{\alpha \in \mathbb{R}_+ }
\max_{i}
r_i(\alpha) \\
s.t. \qquad \boldsymbol{c}^T_i (\boldsymbol{\hat{x}} + \alpha \boldsymbol{\lambda}) + d_i > 0 \ \forall{i}
\end{eqnarray}
と表せます。
目的関数は最小値の両側でそれぞれ単調増加か単調減少なので、注目している点からちょっとずらした点を見て減少しているか増加しているか考え、を二分探索します。

実験

FDMがこの論文の手法です。速くなった。

最後に

間違ったことを書いていても僕を筑後川に沈めたりせずに教えてもらえるとうれしいです。

明日(12/8)の記事の担当ははと君です。楽しみですね。

*1:Q. Zhang, T.-J. Chin, and H. M. Le, “A fast resection-intersection method for the known rotation problem,” in CVPR, 2018, https://arxiv.org/pdf/1902.03747.pdf

*2:証明は論文を読んでほしいです