LoginSignup
44
48
@scomup(Liu Yang)inパナソニック アドバンストテクノロジー株式会社

画像から3次元復元しよう！バンドル調整をpythonで実装してみる

Last updated at Posted at 2024-02-12

はじめに

バンドル調整（Bundle Adjustment）は、複数のカメラからの画像データを使用して、カメラの位置と姿勢と三次元点の位置を同時に最適化する手法です。最初の論文は、1958年にD. C. Brownによって提案された1、かなり長い歴史を持つ技術です。

当時はアメリカ空軍が航空写真からの環境復元するための研究でしたが、近年では、visual-SLAMやSfMの普及とともに、より身近なところで使われるようになりました。

有名なvisual-SLAM（例：orb-slam2やVINS-Mono）は、ceresやgtsam、g2oなどのグラフ最適化ライブラリを利用してバンドル調整問題を解いています。しかし、内部の原理をちゃんと理解しないと、課題の改善ができない、独自の研究や発展につながらない可能性が高いです。

この記事では、初心者に向けバンドル調整の理論の紹介と式の導出を行いながら、ライブラリに頼らずに理論を理解し、ゼロからPythonで実装してみます。バンドル調整をしっかりと理解したい人におすすめです。

バンドル調整とは？

以下の図は、ある物体を複数のカメラ（または同じカメラを複数回移動させる）から撮影するイメージを示します。この物体には特徴的な点※（角点など）があり、それぞれのカメラ画像に映っています（赤い点）。

一方、これらの特徴点の3D空間における位置(3D points)とカメラの姿勢が既知であれば、計算によって3D空間点をカメラの2D画像平面上に投影することができます（水色の点）。

しかし、初期状態では3D点の位置とカメラの姿勢のおおよその値しかわかりません。そのため、投影された水色の点と実際に映った赤い点は一致しません。この誤差は再投影誤差と呼ばれ、赤い点線で示されます。バンドル調整は、この再投影誤差を最小化することで、3D点群の位置とそれぞれのカメラの姿勢をまとめて計算する技術です。
BA.png

上記の図には、3つの3D点がそれぞれ3つのカメラに映っているため、合計9つの再投影誤差があります。式(1)は、バンドル調整の目的を示し、これらすべての誤差の合計値を最小化します。

$$
\begin{aligned}
\underset{x}{\textrm{argmin}} \quad \mathrm{F}(x)
&=\sum_{ｋ} \Vert r_{k} \Vert^2_{\Sigma_k^{-1}}
\end{aligned}
\tag{1}
$$

ここで、$r_{k}$はk番目の再投影誤差を表します。式(1)は典型的な最適化問題であり、再投影誤差値とそのヤコビ行列の計算ができれば、グラフ最適化の手法で解くことができます。以下では再投影誤差の計算方法を詳細に説明します。

※注意：実際のSLAM/SFMでは、SIFT、SURF、ORBなどの特徴点マッチング技術を使用して、3D点と2D点を関連付ける必要がありますが、この文書ではその議論は行いません。すでに特徴点のマッチングが完了していると仮定します。

再投影誤差

式(2)は、再投影誤差の定式化を示しています。

$$
r = u^\prime - u
\tag{2}
$$

式(2)の$u$は実際のカメラ画像に映った2D点、$u^\prime$は3D点をカメラ画像に投影した点です。$u$は既知のため、$u^\prime$の計算を以下で説明します。

カメラは、物体を自身の座標系で観察するので、まず、世界座標系に存在する点をカメラ座標系に変換する必要があります。この変換を関数$\mathrm{p_{c}}(T_{wc}, p_w)$と表します。

$$
\begin{aligned}
\mathrm{p_{c}}(T_{wc}, p_w)
&= T_{wc}^{-1} p_w
\end{aligned}
\tag{3}
$$

ここで、

  • ローマ字体は関数名、e.g. $\mathrm{p_{c}}$
  • イタリック体は変数(パラメータ)名：　e.g. 関数$\mathrm{p_{c}}$の計算結果は$ p_{c}$

関数$\mathrm{p_{c}}(T_{wc}, p_w)$は2つのパラメータを持ちます。それぞれは、世界座標系におけるカメラの姿勢($T_{wc}$、$T_{wc} \in SE(3)$)と世界座標系における3D点($p_w$)です。計算結果はカメラ座標系の3D点($p_c$)となります。

次に、カメラ座標系の3D点($p_c$)を画像の2D点($u^\prime$)として、2Dの画像平面に投影します。

この投影にはいくつかのカメラモデルにより定義できますが、この記事は最も一般的に使われているピンホールモデル（Pinhole camera model）を使います。

ピンホールモデルでは、カメラは非常に小さな穴（ピンホール）を通って光を受けるという仮定をします。このとき、焦点距離($f_x$、$f_y$)と画像センサーの中心位置($c_x$、$c_y$)を考慮し、ピクセル単位で2D画像に投影することができます。

以下は、ピンホールモデルを用いた投影計算を$\mathrm{u^\prime}( p_c)$と表します。

詳細な計算は式(4)に示します。

$$
\begin{aligned}
\mathrm{u^\prime}( p_c)
&= \begin{bmatrix} u_0^\prime \\ u_1^\prime \end{bmatrix} \\
&=
\begin{bmatrix} x f_x /z + c_x \\ y f_y /z + c_y \end{bmatrix}
\end{aligned}
\tag{4}
$$

$p_c$の各成分をそれぞれx,y,zと示します。

$\mathrm{p_{c}}(T_{wc}, p_w)$と$\mathrm{u^\prime}( p_c) $の2つのステップを組み合わせることで、世界座標系における点($p_w$)をカメラ画像に投影した2D点($u^\prime$)を計算することができます。$u^\prime$の計算を式(2)に代入すると、最終的な再投影誤差は式(5)となります。

$$
\begin{aligned}
r
&= u^\prime - u \\
&= \mathrm{u^\prime}(\mathrm{p_{c}}(T_{wc}, p_w)) - u
\end{aligned}
\tag{5}
$$

再投影誤差のヤコビ行列

以前「グラフ最適化」の記事で説明したように、最適化するためには、ヤコビ行列の計算が必要です。ここで、再投影誤差(式(5))のヤコビ行列を導出してみましょう。

再投影誤差は関数$\mathrm{p_{c}}$と関数$\mathrm{u^\prime}$の合成ですので、連鎖律によって、$T_{wc}$ と $p_w$ に関するヤコビ行列を以下のようにそれぞれ計算できます。

$$
J_{T_{wc}} = \frac{\partial \mathrm{u^\prime}}{\partial p_c}
\frac{\partial \mathrm{p_{c}}}{\partial T_{wc}}
\tag{6}
$$

$$
J_{p_w} = \frac{\partial \mathrm{u^\prime}}{\partial p_c}
\frac{\partial \mathrm{p_{c}}}{\partial p_w}
\tag{7}
$$
次は、 $\mathrm{p_{c}}$ と$\mathrm{u^\prime}$ それぞれの微分を議論します。

まず、関数の微分の定義を簡単に説明します。関数の微分は、関数の各パラメータの微小変化を無限小に近づける場合、その変化に対する関数の各成分の変化率を表します。
$\mathrm{u^\prime}$の微分($\frac{\partial \mathrm{u^\prime}}{\partial p_c}$)については、式(8)のようにヤコビ行列の定義を用いて直接計算できます。この式は、$u$ の各成分を各パラメータで偏微分した結果を行列としてまとめたものです。

$$
\begin{aligned}
\frac{\partial \mathrm{u^\prime}}{\partial p_c}
&=
\begin{bmatrix}
\frac{\partial \mathrm{u^\prime}_0}{\partial x} & \frac{\partial \mathrm{u^\prime}_0}{\partial y} & \frac{\partial \mathrm{u^\prime}_0}{\partial z} \\
\frac{\partial \mathrm{u^\prime}_1}{\partial x} & \frac{\partial \mathrm{u^\prime}_1}{\partial y} & \frac{\partial \mathrm{u^\prime}_1}{\partial z}
\end{bmatrix} \\
&=
\begin{bmatrix}
f_x/z & 0 & -f_x x/z^2 \\
0 & f_y/z & -f_y y/z^2
\end{bmatrix} \\
\end{aligned}
\tag{8}
$$

$$
\newcommand{\skew}[1]{[{#1}]_{\times}} %skew matrix
\newcommand{\se}[1]{ \mathfrak{se}{(#1)} } %lie algebra se3
$$

次に、$\mathrm{p_{c}}$の微分について議論します。
$\mathrm{p_{c}}$ は2つのパラメータを持ち、それぞれの偏微分を計算する必要があります。パラメータ$T_{wc}$は$SE(3)$※に属するため、微小変化は$\mathfrak{se}(3)$の指数展開によって定義されます。

したがって、$T_{wc}$に関する$\mathrm{p_{c}}$の微分は、式(9)のように導出することができます。$\delta$($ \in \se{3}$)は微小の変化量です。

$$
\begin{aligned}
\frac{\partial \mathrm{p_{c}}}{\partial T_{wc}}
&= \lim_{\delta \to \mathbb{0}} %line1
\frac{ (T_{wc}\exp(\delta))^{-1}p_w- T_{wc}^{-1}p_w }{\delta} \\
&= %line2
\frac{(I-\hat{\delta})T_{wc}^{-1}p_w- T_{wc}^{-1}p_w}{\delta} \\
&= %line3
-\frac{\hat{\delta}T_{wc}^{-1}p_w}{\delta} \\
&=　%line4
\frac{
\begin{bmatrix}
\skew{\omega} & \rho \\
0 & 0
\end{bmatrix}
\\T_{wc}^{-1} p_w}{[\rho, \omega]^{T}} \\
&=
-\frac{
\begin{bmatrix}
I & -\skew{T_{wc}^{-1}p_w} \\
0 & 0
\end{bmatrix}
\begin{bmatrix}
\rho \\
\omega
\end{bmatrix}
}{[\rho, \omega]^{T}} \\
&=
\begin{bmatrix}
-I & \skew{T_{wc}^{-1} p_w} \\
0 & 0
\end{bmatrix}
\\
\end{aligned}
\tag{9}
$$

※：$SE(3)$群について、詳細説明は以前の記事「3次元剛体変換」を参考ください。

そして、$p_{w}$に関する$\mathrm{p_{c}}$の偏微分は、式(10)のように導出することができます。

$$
\begin{aligned}
\frac{\partial \mathrm{p_{c}}}{\partial p_w}
&= \lim_{\delta \to \mathbb{0}} %line1
\frac{ T_{wc}^{-1} (p_w + \begin{bmatrix} \delta \\ 0\end{bmatrix} )- T_{wc}^{-1}p_w }{\delta} \\
&= \lim_{\delta \to \mathbb{0}} %line1
\frac{ T_{wc}^{-1} (p_w + \begin{bmatrix} \delta \\ 0\end{bmatrix} )- T_{wc}^{-1}p_w }{\delta} \\
&= \lim_{\delta \to \mathbb{0}} %line1
\frac{ T_{wc}^{-1}\begin{bmatrix} \delta \\ 0\end{bmatrix} }{\delta} \\
&= R_{wc}^{-1}
\end{aligned}
\tag{10}
$$

ここで、$\delta$($ \in \mathbb{R}^{3}$)は$p_w$の微小の変化量です。

最後に、式(8)、(9)、(10)を式(6)、(7)に代入することで、再投影誤差に関するすべてのヤコビ行列を計算することができます。

バンドル調整のpython実装

理論の説明と式の導出が完了したので、次は、バンドル調整のpython実装を挑戦しましょう。以前「グラフ最適化」の記事で紹介した独自グラフ最適化ライブラリを使用します。

バンドル調整では、計算対象である3D点とカメラの姿勢をそれぞれグラフの頂点として定義します。

#　カメラの頂点
class CameraVertex(BaseVertex):
    def __init__(self, x):
        super().__init__(x, 6)

    def update(self, dx):
        # x in SE(3)のため、指数展開より更新
        self.x = self.x @ expSE3(dx)

#　3D点の頂点
class PointVertex(BaseVertex):
    def __init__(self, x):
        super().__init__(x, 3)

    def update(self, dx):
        # x in R3 のため、ユークリッド空間における更新
        self.x = self.x + dx

再投影誤差はグラフのエッジとして、定義します。
式(6)、(7)、(8)、(9)、(10)をそのまま実装します。

# 再投影誤差のエッジ
class ReprojEdge(BaseEdge):
    def __init__(self, link, z, omega=np.eye(2), kernel=None):
        super().__init__(link, z, omega, kernel)

    def residual(self, vertices):
        Twc = vertices[self.link[0]].x
        pw = vertices[self.link[1]].x
        u, K = self.z
        pc, dpcdTwc, dpcdpw = transform_inv(Twc, pw, True)
        u_reproj, dudpc = reproject(pc, K, True)
        JTwc = dudpc @ dpcdTwc  # 式(6)
        Jpw = dudpc @ dpcdpw  # 式(7)
        return u_reproj-u, [JTwc, Jpw]  # 式(2)再投影誤差

# 関数T、世界座標系の3d点をカメラの座標系の3d点に変換
def transform_inv(x, p, calcJ=False):
    if x.shape[0] == 6:
        T = expSE3(x)
    else:
        T = x
    Tinv = np.linalg.inv(T)
    Rinv, tinv = makeRt(Tinv)
    r = Rinv @ p + tinv  # 式(3)
    if (calcJ is True):  #ヤコビ行列計算
        M1 = -np.eye(3)  #式(9)
        M2 = skew(r)  #式(9)
        dTdx = np.hstack([M1, M2])
        dTdp = Rinv  #式(10)
        return r, dTdx, dTdp
    else:
        return r

# 関数P、カメラの座標系の3d点を、画像の2d点に変換
def reproject(pc, K, calcJ=False):
    fx = K[0, 0]
    fy = K[1, 1]
    cx = K[0, 2]
    cy = K[1, 2]
    x, y, z = pc
    z_2 = z * z
    r = np.array([(x * fx / z + cx),  # 式(4)
                  (y * fy / z + cy)])
    if (calcJ is True):  #ヤコビ行列計算　#式(8)
        J = np.array([[fx / z,    0, -fx * x / z_2],
                      [0, fy / z, -fy * y / z_2]])
        return r, J
    else:
        return r

実問題のデモ

せっかく作りましたので、難しいバンドル調整の実問題に挑戦しましょう。今回は、Bundle Adjustment in the Large2というデータセットを使います。

このデータセットは、バンドル調整の研究を行うためにFlickrから有名な建物/街全体の写真を収集し、特徴点のマッチングが済んだデータを提供しています。データによりますが、数千から数百万の点がありますので、大変難しい問題が多いです。

最適化する前は、3D点にノイズを加えたため、ぼんやりしたように見えますが、最適化後は、3D点がはっきりになりました。

データ1：トラファルガー広場（ロンドン）

カメラ数：170　3D点数：49267　再投影誤差の数：185815

最適化する前:
青：カメラ、黒3D点数：
data1_before.png

最適化した後：
data1_after.png

実際環境の参考(google earth)：

data1_google.png

データ2：サン・マルコ寺院（ヴェネツィア）

カメラ数：427　3D点数：310384　再投影誤差の数：1699145

最適化する前:
data2_before.png

最適化した後：
data2_after.png

実際環境の参考(google earth)：
data2_google.png

次は、サン・マルコ寺院のデータセットを使用して、再投影誤差を最小化するイメージを可視化します。このデータセットには427台のカメラがあり、全てを表示することはできませんので、6台のカメラの画像のみ表示します。

  • 水色：3D点を画像に再投影した2D点
  • 赤色：実際に映った2D点

最適化する前の再投影誤差:

最適化前では、画像に再投影した2D点と実際に映った2D点が一致していませんが、
1.png

最適化した後の再投影誤差:

最適化後はほぼ一致していることが確認できます。
2.png

まとめ：

この記事では、バンドル調整の理論を解説し、リー群ベースの計算式の導出もすべて示しました。有名な最適化ライブラリに頼らずにPythonでゼロから実装してみました。実装は本文で導出した式をそのまま使用していますので、初心者でもこの記事を読むことでバンドル調整の仕組みを簡単に理解することができるでしょう。バンドル調整に興味を持っている方には、ぜひコードも参考にしてください。

ただし、残課題として、数百万の点群データを処理する場合、処理時間がかなり長くなることがあります。その原因は2つあります。一つは、ヤコビ行列の計算時間であり、もう一つは大規模な線形方程式を解く時間です。

前者については、Pythonの限界によるものであり、numbaの使用やC++/cuda3への移植を検討する必要があるかもしれません（ただし、本文の目的ではないため、実施しません）。後者については、高速な疎行列ソルバーの使用や、g2oの論文4で紹介された「シューア補行列」の使用によって改善できます。これらの議論については、今後の記事で紹介する予定です。

実装はgithubに公開していますので、ご自由に参考にしてください。

  • バンドル調整: slam/demo_bundle_adjustment.py

高速な疎行列ソルバー[SuiteSparse]を使っているため、以下のコマンドでインストールする必要があります。

sudo apt-get install libsuitesparse-dev
pip3 install scikit-sparse

  1. D. C. Brown. "A solution to the general problem of multiple station analytical stereotriangulation" AFMTC-TR-58-8, ASTIA Document No. 134278, (1958) ↩

  2. S. Agarwal, et al. "Bundle Adjustment in the Large" European Conference on Computer Vision, pp.28-42, (2010) ↩

  3. フィックスターズ社より、[バンドル調整のCUDA実装](https://proc-cpuinfo.fixstars.com/2020/10/cuda- bundle-adjustment/) ↩

  4. R Kümmerle, et al. "g2o: A General Framework for Graph Optimization" IEEE International Conference on Robotics and Automation, pp. 3607-3613, (2011) ↩

44
48
2

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
44
48