Pythonで3次元空間認識を自作した話

# はじめに

まずはものから
こちらがそれです
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">この前にあげたPTAMの改良版<br>Qiita全部書き直します<a href="https://t.co/5DbJhH49vj">https://t.co/5DbJhH49vj</a> <a href="https://t.co/1tkkSQDsVH">pic.twitter.com/1tkkSQDsVH</a></p>&mdash; よだかのもやし (@night_moyashi) <a href="https://twitter.com/night_moyashi/status/1351463166920716288?ref_src=twsrc%5Etfw">January 19, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

見ての通り、画像から3次元空間を認識しています

# アルゴリズム

簡単に言えば**ベクトルの差**を利用します
どういうことかというと、

まず、画像上の特徴点を見つけ二枚の画像でマッチングを行います
その後、画像平面が実空間にあると仮定し、画像平面の中心とカメラ間の距離を求めます

そして、画像の中央を原点とした特徴点の座標と先ほど求めた原点とカメラ間距離からカメラからの距離で二枚の画像それぞれから、カメラを原点としたベクトルが導かれます。

ここまでが下準備です。

ここまでの過程で導かれたベクトルのうち、一枚目の画像から導かれたベクトルを

```math
\vec{a}=(a_1,a_2,a_3)
```

二枚目の画像から導かれたベクトルを

```math
\vec{b}=(b_1,b_2,b_3)
```

この二枚の画像の間にカメラが動いたベクトルを

```math
\vec{c}=(c_1,c_2,c_3)
```

とすると、$\vec{a}$と$\vec{b}$の交点$O$とそれぞれの撮影地点$A(\vec{a}),B(\vec{b})$は同一平面上にあるから

```math
-l\vec{a}=\vec{c}-k\vec{b}\\
k\vec{b}-l\vec{a}=\vec{c}
```
となります
この式から

```math
\left\{
\begin{array}{l}
c_1=kb_1-la_1\\
c_2=kb_2-la_2\\
c_3=kb_3-la_3
\end{array}
\right.
```

とういう連立方程式が成り立ちますが、このままでは行列を使って解こうとすると

```math
\begin{pmatrix}
b_{1}&-a_{1}\\
b_{2}&-a_{2}\\
b_{3}&-a_{3}\\
\end{pmatrix}
\begin{pmatrix}
k\\
l\\
\end{pmatrix}
=
\begin{pmatrix}
c_{1}\\
c_{2}\\
c_{3}
\end{pmatrix}\\
\begin{pmatrix}
k\\
l\\
\end{pmatrix}
=
\begin{pmatrix}
c_{1}\\
c_{2}\\
c_{3}
\end{pmatrix}
\begin{pmatrix}
b_{1}&-a_{1}\\
b_{2}&-a_{2}\\
b_{3}&-a_{3}\\
\end{pmatrix}^{-1}
```
となってしまい、

```math
\begin{pmatrix}
b_{1}&-a_{1}\\
b_{2}&-a_{2}\\
b_{3}&-a_{3}\\
\end{pmatrix}
```
は正則行列ではないので方程式は解けません。
そこで、連立方程式を変形して

```math
\left\{
\begin{array}{l}
(c_1+c_2)=k(b_1+b_2)-l(a_1+a_2)\\
(c_2+c_3)=k(b_2+b_3)-l(a_2+a_3)\\

\end{array}
\right.
```
として、これを行列を用いて$k,l$を導き出します。

そして$k\vec{b}$が$B$からの距離となります。

#プログラム
プログラムはこちらで公開しています
[GitHub](https://github.com/acsses/PTAM_test)
一応、コードの中で核となる部分をば

```python:match.py
def convert(data):
    data_converted=[]
    for loc in data:
        data_converted.append(convert_3d(loc))
    A=np.matrix([
        [-1*sum(data_converted[1][:2]),sum(data_converted[0][:2])],
        [-1*sum(data_converted[1][1:]),sum(data_converted[0][1:])],
    ])
    Y=np.matrix([
        [-0.6],
        [0],
    ])
    coe = np.linalg.solve(A,Y).reshape(-1,).tolist()
    
    return data_converted[1][0]*coe[0][0],data_converted[1][1]*coe[0][0],data_converted[1][2]*coe[0][0]
```
#問題点
作った本人である以上、問題点を上げておかなければなりません。
冒頭のtweetでも見受けられたようにどうしても外れ値が出てきます
どうにもこの外れ値は$\vec{c}$の精度が強く影響しているようです

なぜ、このような問題が発生するのかというと**同一平面上にあると仮定しているから**です。
つまり、$\vec{c}$が少しでもずれると外れ値が出るようです。

ここからはその対策です。

## 解を一般化する

先ほどの連立方程式の解を一般化します。

```math
\begin{pmatrix}
A_1&-B_1\\
A_2&-B_2\\
\end{pmatrix}
\begin{pmatrix}
k\\
l\\
\end{pmatrix}
=
\begin{pmatrix}
C_1\\
C_2\\
\end{pmatrix}
```
ただし

```math
A_1=a_1+a_2,B_1=b_1+b_2\\
A_2=a_2+a_3,B_2=b_2+b_3
```
とします。
これを解いて、

```math
k=-\frac{C_2A_1-C_1A_2}{-B_1A_2+B_2A_1}\\
l=-\frac{C_2B_1-C_1B_2}{-B_1A_2+B_2A_1}
```
となります。ただ、ここで下準備をしておきます。

```math
\alpha=-B_1A_2+B_2A_1
```
として

```math
k=-\frac{C_2A_1-C_1A_2}{\alpha}\\
l=-\frac{C_2B_1-C_1B_2}{\alpha}
```
としておきます。

## x座標と各誤差との関係

次に先ほど、一般化した解を使って、x座標と各誤差との関係を導きます。
誤差ありの$\vec{c}$を

```math
\vec{d}=(c_1+\Delta_x,c_2+\Delta_y,c_3+\Delta_z)
```
とします。
これを先ほどの解に当てはめて、

```math
k=-\frac{(C_2+\Delta_2)A_1-(C_1+\Delta_1)A_2}{\alpha}b_1\\
l=-\frac{(C_2+\Delta_2)B_1-(C_1+\Delta_1)B_2}{\alpha}a_1
```
となり、方程式に当てはめると、

```math
\begin{align}
c_1+\Delta_x&=-\frac{(C_2+\Delta_2)A_1-(C_1+\Delta_1)A_2}{\alpha}b_1+\frac{(C_2+\Delta_2)B_1-(C_1+\Delta_1)B_2}{\alpha}a_1\\
c_1\alpha+\alpha\Delta_x&=(C_2+\Delta_2)A_1b_2-(C_1+\Delta_1)A_2b_2+(C_2+\Delta_2)B_1a_1-(C_1+\Delta_1)B_2a_1
\end{align}
```
となります
ただし、

```math
\Delta_1=\Delta_x+\Delta_y\\
\Delta_2=\Delta_y+\Delta_z
```
すると

```math
\begin{align}
c_1\alpha+\alpha\Delta_x&=(C_2+\Delta_2)A_1b_2-(C_1+\Delta_1)A_2b_1+(C_2+\Delta_2)B_1a_1-(C_1+\Delta_1)B_2a_1\\
&=(C_1+\Delta_1)(A_2b_1-B_2a_1)+(D_2+\Delta_2)(B_1a_1-A_1b_1)\\
&=(\Delta_x+\Delta_y)(A_2b_1-B_2a_1)+(\Delta_y+\Delta_z)(B_1a_1-A_1b_1)+C_1(A_2b_1-B_2a_1)+C_2(B_1a_1-A_1b_1)
\end{align}
```
となります。ここで

```math
\vec{s}=(A_2,B_2)\\
\vec{t_x}=(a_1,b_1)\\
\vec{u}=(A_1,B_1)
```
とおくと、

```math
\begin{align}
c_1\alpha+\alpha\Delta_x&=(\Delta_x+\Delta_y)(A_2b_1-B_2a_1)+(\Delta_y+\Delta_z)(B_1a_1-A_1b_1)+C_1(A_2b_1-B_2a_1)+C_2(B_1a_1-A_1b_1)\\
&=(\Delta_x+\Delta_y)(\vec{s}\times\vec{t_x})+(\Delta_y+\Delta_z)(\vec{u}\times\vec{t_x})+C_1(\vec{s}\times\vec{t_x})+C_2(\vec{u}\times\vec{t_x})\\
&=\Delta_x(\vec{s}\times\vec{t_x})+\Delta_y(\vec{s}\times\vec{t_x}+\vec{t_x}\times\vec{u})+\Delta_z(\vec{t_x}\times\vec{u})+C_1(\vec{s}\times\vec{t_x})+C_2(\vec{t_x}\times\vec{t_u})\\
0&=\Delta_x(\vec{s}\times\vec{t_x}-\alpha)+\Delta_y(\vec{s}\times\vec{t_x}+\vec{t_x}\times\vec{u})+\Delta_z(\vec{t_x}\times\vec{u})+C_1(\vec{s}\times\vec{t_x})+C_2(\vec{t_x}\times\vec{u})-c_1\alpha\\
\end{align}
```
となります。ただし、プログラム中で使っているnumpyの仕様上、

```math
\vec{n}\times\vec{m}=m_2n_1-m_1n_2
```
 とします。

## y座標、z座標と各誤差との関係
同様の操作をy座標、z座標でも行います。
まずはy座標から

```math
\begin{align}
c_1+\Delta_y&=-\frac{(C_2+\Delta_2)A_1-(C_1+\Delta_1)A_2}{\alpha}b_2＋\frac{(C_2+\Delta_2)B_1-(C_1+\Delta_1)B_2}{\alpha}a_2\\
c_1\alpha+\alpha\Delta_y&=(C_1+\Delta_1)A_2b_2-(C_2+\Delta_2)A_1b_2+(C_2+\Delta_2)B_1a_2-(C_1+\Delta_1)B_2a_2\\
&=(C_1+\Delta_1)(A_2b_2-B_2a_2)+(C_2+\Delta_2)(B_1a_2-A_1b_2)\\
&=(\Delta_x+\Delta_y)(A_2b_2-B_2a_2)+(\Delta_y+\Delta_z)(B_1a_2-A_1b_2)+C_1(A_2b_2-B_2a_2)+C_2(B_1a_2-A_1b_2)
\end{align}
```
この時、

```math
\vec{s}=(A_2,B_2)\\
\vec{t_y}=(a_2,b_2)\\
\vec{u}=(A_1,B_1)
```
と、おくと

```math
\begin{align}
c_2\alpha+\alpha\Delta_y&=(\Delta_x+\Delta_y)(A_2b_2-B_2a_2)+(\Delta_y+\Delta_z)(B_1a_2-A_1b_2)+C_1(A_2b_2-B_2a_2)+C_2(B_1a_2-A_1b_2)\\
&=(\Delta_x+\Delta_y)(\vec{s}\times\vec{t_y})+(\Delta_y+\Delta_z)(\vec{u}\times\vec{t_y})+C_1(\vec{s}\times\vec{t_y})+C_2(\vec{u}\times\vec{t_y})\\
&=\Delta_x(\vec{s}\times\vec{t_y})+\Delta_y(\vec{t_y}\times\vec{s}+\vec{u}\times\vec{t_y})+\Delta_z(\vec{t_y}\times\vec{u})+C_1(\vec{s}\times\vec{t_y})+C_2(\vec{s}\times\vec{t_y})\\
0&=\Delta_x(\vec{s}\times\vec{t_y})+\Delta_y(\vec{t_y}\times\vec{s}+\vec{u}\times\vec{t_y}-\alpha)\Delta_z(\vec{t_y}\times\vec{u})+C_1(\vec{s}\times\vec{t_y})+C_2(\vec{u}\times\vec{t_y})-c_2\alpha
\end{align}
```
となります。

次にz座標。

```math
\begin{align}
c_3+\Delta_z&=-\frac{(C_2+\Delta_2)A_1-(C_1+\Delta_1)A_2}{\alpha}b_3+\frac{(C_2+\Delta_2)B_1-(C_1+\Delta_1)B_2}{\alpha}a_3\\
c_3\alpha+\alpha\Delta_z&=(C_1+\Delta_1)A_2b_3-(C_2+\Delta_2)A_1b_3+(C_2+\Delta_2)B_1a_3-(C_1+\Delta_1)B_1a_3\\
&=(C_1+\Delta_1)(A_2b_3-B_2a_3)+(C_2+\Delta_2)(B_1a_3-A_1b_3)\\
&=(\Delta_x+\Delta_y)(A_2b_3-B_2a_3)+(\Delta_y+\Delta_z)(B_1a_3-A_1b_3)+C_1(A_2b_3-B_2a_3)+C_2(B_1a_3-A_1b_3)
\end{align}
```

```math
\vec{s}=(A_2,B_2)\\
\vec{t_z}=(a_3,b_3)\\
\vec{u}=(A_1,B_1)
```
とおいて

```math
\begin{align}
c_3\alpha+\alpha\Delta_z&=(\Delta_x+\Delta_y)(A_2b_3-B_2a_3)+(\Delta_y+\Delta_z)(B_1a_3-A_1b_3)+C_1(A_2b_3-B_2a_3)+C_2(B_1a_3-A_1b_3)\\
&=(\Delta_x+\Delta_v)(\vec{t_z}\times\vec{s})+(\Delta_y+\Delta_z)(\vec{u}\times\vec{t_z})+C_1(\vec{t_z}\times\vec{s})+C_2(\vec{u}\times\vec{t_z})\\
&=\Delta_x(\vec{t_z}\times\vec{s})+\Delta_y(\vec{s}\times\vec{t_z}+\vec{u}\times\vec{t_z})+\Delta_z(\vec{u}\times\vec{t_z})+C_1(\vec{t_z}\times\vec{s})+C_2(\vec{u}\times\vec{t_z})\\
0&=\Delta_x(\vec{t_z}\times\vec{s})+\Delta_y(\vec{s}\times\vec{t_z}+\vec{u}\times\vec{t_z})+\Delta_z(\vec{u}\times\vec{t_z}-\alpha)+C_1(\vec{t_z}\times\vec{s})+C_2(\vec{u}\times\vec{t_z})-c_3\alpha
\end{align}
```
となります。
## 連立方程式に
これを連立して、行列の掛け算で表すと

```math
\begin{pmatrix}
\vec{s}\times\vec{t_x}-\alpha&(\vec{s}-\vec{u})\times\vec{t_x}&\vec{u}\times\vec{t_x}\\
\vec{s}\times\vec{t_y}&(\vec{s}-\vec{u})\times\vec{t_y}-\alpha&\vec{u}\times\vec{t_y}\\
\vec{s}\times\vec{t_z}&(\vec{s}-\vec{u})\times\vec{t_z}&\vec{u}\times\vec{t_z}-\alpha\\
\end{pmatrix}
\begin{pmatrix}
\Delta_x\\
\Delta_y\\
\Delta_z\\
\end{pmatrix}
=-
\begin{pmatrix}
C_1(\vec{s}\times\vec{t_x})+C_2(\vec{u}\times\vec{t_x})-c_1\alpha\\
C_1(\vec{s}\times\vec{t_y})+C_2(\vec{u}\times\vec{t_y})-c_2\alpha\\
C_1(\vec{s}\times\vec{t_z})+C_2(\vec{u}\times\vec{t_z})-c_3\alpha\\
\end{pmatrix}
```
また

```math
\alpha=\vec{s}\times\vec{u}
```
ちなみに、この方程式中の$\vec{s},\vec{t_a},\vec{u}$は以下の行列から参照できます。

```math
\begin{pmatrix}
A_1&A_2\\
B_1&B_2\\
\end{pmatrix}\\
\begin{pmatrix}
a_1&a_2&a_3\\
b_1&b_2&b_3\\
\end{pmatrix}
```

## プログラム
これを実装してこんな感じになります。

```python
def each_delta(a,b):
    A=np.array([
        [sum(a[:2]),sum(a[1:])],
        [sum(b[:2]),sum(b[1:])]
    ])
    R=np.array([
        [a[0],a[1],a[2]],
        [b[0],b[1],b[2]],
    ])
    alufa=np.cross(A[:,0],A[:,1])
    Right=np.matrix([
        [np.cross(A[:,1],R[:,0])-alufa,np.cross(A[:,1],R[:,0])+np.cross(A[:,0],R[:,0]),np.cross(A[:,0],R[:,0])],
        [np.cross(A[:,1],R[:,1]),np.cross(A[:,1],R[:,1])+np.cross(A[:,0],R[:,1])-alufa,np.cross(A[:,0],R[:,1])],
        [np.cross(A[:,1],R[:,2]),np.cross(A[:,1],R[:,2])+np.cross(A[:,0],R[:,2]),np.cross(A[:,0],R[:,2])-alufa]
    ])
    Left=np.matrix([
        [-1*(-0.6*np.cross(R[:,0],A[:,1])+0*np.cross(A[:,0],R[:,0])+0.6*alufa)],
        [-1*(-0.6*np.cross(R[:,1],A[:,1])+0*np.cross(A[:,0],R[:,1])-0*alufa)],
        [-1*(-0.6*np.cross(R[:,2],A[:,1])+0*np.cross(A[:,0],R[:,2])-0*alufa)],
    ])
    coe_delta = (np.linalg.pinv(Right)*Left).reshape(-1,).tolist()
    return (coe_delta[0][0],coe_delta[0][1],coe_delta[0][2])
```
ただ、これだと、誤差が元データから大きくなることがあります。これを避けるため、予め範囲を決めて範囲内にある誤差のみで計算させます。

```python
if abs(x**2+y**2+z)<range:
        Y=np.matrix([
            [-0.6+x+y],
            [0+y+z],
        ])
    else:
        Y=np.matrix([
            [-0.6],
            [0],
        ])
```

結果としてはこんな感じで動いてくれます。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">PTAMの修正アルゴリズム追加版<br>結構良くなってる。<a href="https://t.co/iH3Axc4ft5">https://t.co/iH3Axc4ft5</a> <a href="https://t.co/tO3FgQSg2M">pic.twitter.com/tO3FgQSg2M</a></p>&mdash; Eagleもやし (@night_moyashi) <a href="https://twitter.com/night_moyashi/status/1360772485654802433?ref_src=twsrc%5Etfw">February 14, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

ただし、計算量が多く、ARなどのリアルタム処理には向かない状態です。高速化に関してはまた追記します。

とまあ、こんな感じのお話でした。
それでは