基礎行列計算のPython実装

## About fundamental matrix
![pair1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/663246/877664fa-c9c2-82b3-bd5c-7f296aef523c.png)![pair2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/663246/b4f7aed0-5709-1f7e-5c5c-9fb7f1b96b23.png)

2台のカメラで同じシーンを撮影し、1台目のカメラで撮影された画像では点(x, y)に、2台目のカメラで撮影された画像では点(x', y')にシーンのある点が映っているとすると、両者の間には次の関係が成り立ちます。

```math
\begin{align*}
\begin{pmatrix}
\begin{pmatrix}
x/f_0 \\
y/f_0 \\
1 \\
\end{pmatrix},
F\begin{pmatrix}
x\prime/f_0 \\
y\prime/f_0 \\
1 \\
\end{pmatrix}
\end{pmatrix}=0 \tag{1}
\end{align*}
```

f0は[楕円当てはめ](https://qiita.com/Hiroaki-K4/items/e29cc422d7205cf19d05)と同様にスケールを調整する定数です。F=(Fij)は2台のカメラの相対位置とパラメータ（焦点距離など）から決まる3x3の行列で、基礎行列と呼ばれます。[こちら](https://www.morikita.co.jp/books/mid/081791)の本をもとに、基礎行列の計算を実装してみたので、その時のコードや結果を共有します。
式(1)はエピ極線拘束条件またはエピ極線方程式と呼ばれます。式(1)はFを何倍しても同じ式を表すので、これを定数倍して次のように正規化します。

```math
\begin{align*}
||F||=\sqrt{\sum_{i,j=1}^3F_{ij}^2}=1 \tag{2}
\end{align*}
```

9次元ベクトルを次のように定義すると、

```math
\begin{align*}
\xi=\begin{pmatrix}
xx\prime \\
xy\prime \\
f_0x \\
yx\prime \\
yy\prime \\
f_0y \\
f_0x\prime \\
f_0y\prime \\
f_0^2 \\
\end{pmatrix},
\theta=\begin{pmatrix}
F_{11} \\
F_{12} \\
F_{13} \\
F_{21} \\
F_{22} \\
F_{23} \\
F_{31} \\
F_{32} \\
F_{33} \\
\end{pmatrix} \tag{3}
\end{align*}
```

式(1)の左辺は(ξ,θ)/f0²であることがわかります。したがって、式(1)のエピ極線方程式は次のように書くこともできます。

```math
\begin{align*}
(\xi,\theta)=0 \tag{4}
\end{align*}
```

式(4)は[楕円当てはめ](https://qiita.com/Hiroaki-K4/items/e29cc422d7205cf19d05)と同じ要領です。

## Covariance matrix and algebraic methods
2つの画像の対応点から基礎行列Fを計算することは、様々な画像系のアプリケーションの基礎となっています。誤差のある対応点(xα,yα),(xα′,yα′),(α=1,...,N)から式(1)を満たす基礎行列Fを計算することは、次のような単位ベクトルθを計算することになります。

```math
\begin{align*}
(\xi_\alpha,\theta)\approx0, \alpha=1,...,N \tag{5}
\end{align*}
```

ξαは式(3)のx=xα,y=yα,x′=xα′,y′=yα′に対するξの値です。データxα,yα,xα′,yα′は、その真値xαˉ,yαˉ,xα′に誤差△xα​,△yα​,△xα​′,△yα​′を加えたものとして、次のように書くことができます。

```math
\begin{align*}
x_\alpha=\bar{x_\alpha}+\triangle{x_\alpha},y_\alpha=\bar{y_\alpha}+\triangle{y_\alpha},x_\alpha\prime=\bar{x_\alpha\prime}+\triangle{x_\alpha\prime},y_\alpha\prime=\bar{y_\alpha\prime}+\triangle{y_\alpha\prime} \tag{6}
\end{align*}
```

これらを式(3)から得られるξαに代入すると、次のようになります。

```math
\begin{align*}
\xi_\alpha=\bar{\xi_\alpha}+\triangle_1\xi_\alpha+\triangle_2\xi_\alpha \tag{7}
\end{align*}
```

ここでξαˉは真値、△1ξαと△2ξαはそれぞれ1次と2次の誤差項です。これらは以下の様になります。

```math
\begin{align*}
\triangle_1\xi_\alpha=\begin{pmatrix}
\bar{x_\alpha}\prime\triangle x_\alpha+\bar{x_\alpha}\triangle x_\alpha\prime \\
\bar{y_\alpha}\prime\triangle x_\alpha+\bar{x_\alpha}\triangle y_\alpha\prime \\
f_0\triangle x_\alpha \\
\bar{x_\alpha}\prime\triangle y_\alpha+\bar{y_\alpha}\triangle x_\alpha\prime \\
\bar{y_\alpha}\prime\triangle y_\alpha+\bar{y_\alpha}\triangle y_\alpha\prime \\
f_0\triangle y_\alpha \\
f_0\triangle x_\alpha\prime \\
f_0\triangle y_\alpha\prime \\
0 \\
\end{pmatrix},
\triangle_2\xi_\alpha=\begin{pmatrix}
\triangle x_\alpha\triangle x_\alpha\prime \\
\triangle x_\alpha\triangle y_\alpha\prime \\
0 \\
\triangle y_\alpha\triangle x_\alpha\prime \\
\triangle y_\alpha\triangle y_\alpha\prime \\
0 \\
0 \\
0 \\
0 \\
\end{pmatrix} \tag{8}
\end{align*}
```

誤差△xαと△yαを確率変数と考えると、ξαの共分散行列は次のように定義されます。

```math
\begin{align*}
V[\xi_\alpha]=E[\triangle_1\xi_\alpha\triangle_1\xi_\alpha^T] \tag{9}
\end{align*}
```

Eは分布の期待値を表します。△xαと△yαが期待値0、標準偏差σの正規分布に従うなら、互いに独立で次の式が成り立ちます。

```math
\begin{align*}
E[\triangle x_\alpha]&=E[\triangle y_\alpha]=E[\triangle x_\alpha\prime]=E[\triangle y_\alpha\prime]=0, \\ \tag{10}
E[\triangle x_\alpha^2]&=E[\triangle y_\alpha^2]=E[\triangle x_\alpha\prime^2]=E[\triangle y_\alpha\prime^2]=\sigma^2, \\
E[\triangle x_\alpha\triangle y_\alpha]&=E[\triangle x_\alpha\prime\triangle y_\alpha\prime]=E[\triangle x_\alpha\triangle y_\alpha\prime]=E[\triangle x_\alpha\prime\triangle y_\alpha]=0
\end{align*}
```

式(8)を用いると、式(9)は次のように書けます。

```math
\begin{align*}
V[\xi_\alpha]=\sigma^2V_0[\xi_\alpha] \tag{11}
\end{align*}
```

すべての要素にσ²が掛けられるため、それが抜き出された行列はV0[ξα]と書かれ、正規化共分散行列と呼ばれます。

```math
\begin{align*}
V_0[\xi_\alpha]=\begin{pmatrix}
\bar{x_\alpha}^2+\bar{x_\alpha\prime}^2 & \bar{x_\alpha\prime}\bar{y_\alpha\prime} & f_0\bar{x_\alpha\prime} & \bar{x_\alpha}\bar{y_\alpha} & 0 & 0 & f_0\bar{x_\alpha} & 0 & 0 \\
\bar{x_\alpha\prime}\bar{y_\alpha\prime} & \bar{x_\alpha}^2+\bar{y_\alpha\prime}^2 & f_0\bar{y_\alpha\prime} & 0 & \bar{x_\alpha}\bar{y_\alpha} & 0 & 0 & f_0\bar{x_\alpha}  & 0 \\
f_0\bar{x_\alpha\prime} & f_0\bar{y_\alpha\prime} & f_0^2 & 0 & 0 & 0 & 0 & 0 & 0 \\
\bar{x_\alpha}\bar{y_\alpha} & 0 & 0 & \bar{y_\alpha}^2+\bar{x_\alpha\prime}^2 & \bar{x_\alpha\prime}\bar{y_\alpha\prime} & f_0\bar{x_\alpha\prime} & f_0\bar{y_\alpha\prime} & 0 & 0 \\
0 & \bar{x_\alpha}\bar{y_\alpha} & 0 & \bar{x_\alpha\prime}\bar{y_\alpha\prime} & \bar{y_\alpha}^2+\bar{y_\alpha\prime}^2 & f_0\bar{y_\alpha\prime} & 0 & f_0\bar{y_\alpha} & 0 \\
0 & 0 & 0 & f_0\bar{x_\alpha\prime} & f_0\bar{y_\alpha\prime} & f_0^2 & 0 & 0 & 0 \\
f_0\bar{x_\alpha} & 0 & 0 & f_0\bar{y_\alpha} & 0 & 0 & f_0^2 & 0 & 0 \\
0 & f_0\bar{x_\alpha} & 0 & 0 & f_0\bar{y_\alpha} & 0 & 0 & f_0^2 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0
\end{pmatrix} \tag{12}
\end{align*}
```

楕円当てはめと同様に、式(9)の共分散行列において△2ξαを考慮する必要はありません。また、式(12)のxαˉ,yαˉ,xα′ˉは、実際の計算ではxα,yα,xα′,yα′に置き換えられます。
誤差のある対応点から基礎行列Fを計算するには、誤差の性質を考慮して式(5)を満たすθを計算することになります。これは楕円当てはめと同じです。

## Solution1. Least squares method
### 1. 次の9x9行列Mを計算する

```math
\begin{align*}
M=\frac{1}{N}\sum_{\alpha=1}^N\xi_\alpha\xi_\alpha^\intercal \tag{13}
\end{align*}
```

### 2. 固有値問題を解き、最小固有値λに対する単位固有ベクトルθを求める

```math
\begin{align*}
M\theta=\lambda\theta \tag{14}
\end{align*}
```

[楕円当てはめの記事](https://qiita.com/Hiroaki-K4/items/e29cc422d7205cf19d05)で述べていますが、これは2乗和を最小化するθを求めています。

## Solution2. Taubin method
### 1. 次の9x9行列M,Nを計算する

```math
\begin{align*}
M=\frac{1}{N}\sum_{\alpha=1}^N\xi_\alpha\xi_\alpha^\intercal,
N=\frac{1}{N}\sum_{\alpha=1}^NV_0[\xi_\alpha] \tag{15}
\end{align*}
```

### 2. 固有値問題を解き、最小固有値λに対する単位固有ベクトルθを求める

```math
\begin{align*}
M\theta=\lambda N\theta \tag{16}
\end{align*}
```

## Code
一連の処理を試せるpythonコードは[こちら](https://github.com/Hiroaki-K4/3d_computer_vision/blob/main/fundamental_matrix/calculate_f_matrix.py)です。

## Reference
- [3D Computer Vision Computation Handbook](https://www.morikita.co.jp/books/mid/081791)