はじめに

まずはものから
こちらがそれです

この前にあげたPTAMの改良版
Qiita全部書き直します https://t.co/5DbJhH49vj pic.twitter.com/1tkkSQDsVH
— よだかのもやし (@night_moyashi) January 19, 2021

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

アルゴリズム

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

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

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

ここまでが下準備です。

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

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

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

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

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

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

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

-l\vec{a}=\vec{c}-k\vec{b}\\
k\vec{b}-l\vec{a}=\vec{c}

となります
この式から

\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.

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

\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}

となってしまい、

\begin{pmatrix}
b_{1}&-a_{1}\\
b_{2}&-a_{2}\\
b_{3}&-a_{3}\\
\end{pmatrix}

は正則行列ではないので方程式は解けません。
そこで、連立方程式を変形して

\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
一応、コードの中で核となる部分をば

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}$が少しでもずれると外れ値が出るようです。

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

解を一般化する

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

\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}

ただし

A_1=a_1+a_2,B_1=b_1+b_2\\
A_2=a_2+a_3,B_2=b_2+b_3

とします。
これを解いて、

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}

となります。ただ、ここで下準備をしておきます。

\alpha=-B_1A_2+B_2A_1

として

k=-\frac{C_2A_1-C_1A_2}{\alpha}\\
l=-\frac{C_2B_1-C_1B_2}{\alpha}

としておきます。

x座標と各誤差との関係

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

\vec{d}=(c_1+\Delta_x,c_2+\Delta_y,c_3+\Delta_z)

とします。
これを先ほどの解に当てはめて、

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

となり、方程式に当てはめると、

\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}

となります
ただし、

\Delta_1=\Delta_x+\Delta_y\\
\Delta_2=\Delta_y+\Delta_z

すると

\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}

となります。ここで

\vec{s}=(A_2,B_2)\\
\vec{t_x}=(a_1,b_1)\\
\vec{u}=(A_1,B_1)

とおくと、

\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の仕様上、

\vec{n}\times\vec{m}=m_2n_1-m_1n_2

とします。

y座標、z座標と各誤差との関係

同様の操作をy座標、z座標でも行います。
まずはy座標から

\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}

この時、

\vec{s}=(A_2,B_2)\\
\vec{t_y}=(a_2,b_2)\\
\vec{u}=(A_1,B_1)

と、おくと

\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座標。

\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}

\vec{s}=(A_2,B_2)\\
\vec{t_z}=(a_3,b_3)\\
\vec{u}=(A_1,B_1)

とおいて

\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}

となります。

連立方程式に

これを連立して、行列の掛け算で表すと

\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}

また

\alpha=\vec{s}\times\vec{u}

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

\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}

プログラム

これを実装してこんな感じになります。

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])

ただ、これだと、誤差が元データから大きくなることがあります。これを避けるため、予め範囲を決めて範囲内にある誤差のみで計算させます。

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],
        ])

結果としてはこんな感じで動いてくれます。

PTAMの修正アルゴリズム追加版
結構良くなってる。https://t.co/iH3Axc4ft5 pic.twitter.com/tO3FgQSg2M
— Eagleもやし (@night_moyashi) February 14, 2021

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

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