注意：本稿は検証途中のものです。それっぽい結果にはなったけど著者本人も諸々不安なところがあり検証を続けているところです。留意してお読みください。
はじめに
ARでよく使われるVPSですが、多くのサービスが最初は無料で利用できていてもM&Aなどを経てクローズドになり使えなくなったりと、継続的に使っていくのに不安があります。
また、サービスにより様々な「クセ」があり、そのブラックボックスな中身が分からないことで試行錯誤が多く発生します。
そこで、よりよい活用のための仕組みの理解と、いざとなれば自分で作れるものなのかの確認のために、実際に現実世界で動くVPSを作成してみることを目標に、OSSなどを活用してオープンなものを自力で実装してみることをしてみようと思います。
計画
三次元点群と特徴点特徴量のデータが作れれば、それと撮影画像の特徴点とのマッチングで対応点をとり、自己位置を推定することができそうです。
三次元点群については、オープン実装のVSLAMを調査して使ってみたいと思います。過去にOpenVSLAM(現在は公開停止中ですが)を動かしてみたことがあるので、この辺りはいけそうな気がします！（ほんとかな？？）
特徴点の計算はどのVSLAMを使うかにもよりますが、OpenCVに用意されている実装で問題ないでしょう。
マッチングもとりあえず性能を考えなければOpenCVのBluteForceなどでやってみればよさそうです。
ということで、その1では個人的にもっとも未知な、対応付けされた三次元点と二次元点からカメラの情報を復元する部分を取り上げたいと思います。
参考文献
こちらの本を参考にします。今回は最低限の原理と計算の確認ということで、「5.2.2 3Dの点群からカメラ行列を計算する」に絞ってやってみたいと思います。
データの作成
まずは、データの作成です。参考文献の通りにコードを書けばサンプルデータに関しては本の通りの出力がでます。では、実際に自分で作ったデータでうまくいくのかを確認していきたいと思います。
まず、Unityで適当にシーンを作ります。
今回はこのように、キューブを適当に配置したシーンを作りました。キューブの座標及び各頂点の座標はあとから使うので、考えやすいキリの良い配置にするとよいと思います。
カメラ位置を適当に配置して、画面の大きさを適当に（今回は1024x768）設定して実行しレンダリング画像を得ます。このあたりRenderTextureを書き出したりでもいいとは思いますが、適当に。
以上のような画像が得られます。
この画像の各頂点を、三次元の座標との対応を取りながらデータ化します。
Unity上でレイキャストしたりして求まるとは思いますが、今回はノイズ付加のつもりで手動でやってみました。
そして以下のようなデータを作りました。
| x | y | X | Y | Z |
|---|---|---|---|---|
| 843 | 235 | 2.5 | 2.5 | 2.5 |
| 780 | 227 | 1.5 | 2.5 | 2.5 |
| 626 | 210 | -1.5 | 2.5 | 2.5 |
| 584 | 205 | -2.5 | 2.5 | 2.5 |
| ・ | ・ | ・ | ・ | ・ |
| ・ | ・ | ・ | ・ | ・ |
| ・ | ・ | ・ | ・ | ・ |
実際に計算してみる
Google Colaboratory上で進めます。
実践コンピュータビジョンのp.114の関数を実装します。numpyのインポートの仕方だけ少し変えています。
import numpy as np
def compute_P(x, X):
n = x.shape[1]
if X.shape[1] != n:
raise ValueError("Number of Points don't match.")
M = np.zeros((3*n, 12+n))
for i in range(n):
M[3*i,0:4] = X[:,i]
M[3*i+1,4:8] = X[:,i]
M[3*i+2,8:12] = X[:,i]
M[3*i:3*i+3,i+12] = -x[:,i]
U, S, V = np.linalg.svd(M)
return V[-1,:12].reshape((3,4))
次に、用意したデータに合わせて修正したp.115のプログラムを実行します。
データを直接コードに書くなんて悪い子ですね。
x = np.array([[843,780,626,584,825,759,598,554,752,674,493,446,717,634,446,397,830,621,812,749,594,551,738,490,703,625,445,398,794,740,568,776,720,580,542,701,637,483,442,668,601,443,401,785,602,766,712,577,539,692,481,660,595,441,402],[235,227,210,205,240,232,212,207,259,248,221,214,269,256,226,217,296,261,305,294,266,260,342,287,359,340,296,285,459,442,387,477,459,412,399,551,524,459,441,585,554,479,459,506,442,528,508,456,442,613,510,649,614,532,511]])
x = np.vstack((x, np.ones(x.shape[1])))
X = np.array([[2.5,1.5,-1.5,-2.5,2.5,1.5,-1.5,-2.5,2.5,1.5,-1.5,-2.5,2.5,1.5,-1.5,-2.5,2.5,-1.5,2.5,1.5,-1.5,-2.5,2.5,-1.5,2.5,1.5,-1.5,-2.5,2.5,1.5,-2.5,2.5,1.5,-1.5,-2.5,2.5,1.5,-1.5,-2.5,2.5,1.5,-1.5,-2.5,2.5,-1.5,2.5,1.5,-1.5,-2.5,2.5,-1.5,2.5,1.5,-1.5,-2.5],[2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-1.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5,-2.5],[2.5,2.5,2.5,2.5,1.5,1.5,1.5,1.5,-1.5,-1.5,-1.5,-1.5,-2.5,-2.5,-2.5,-2.5,2.5,2.5,1.5,1.5,1.5,1.5,-1.5,-1.5,-2.5,-2.5,-2.5,-2.5,2.5,2.5,2.5,1.5,1.5,1.5,1.5,-1.5,-1.5,-1.5,-1.5,-2.5,-2.5,-2.5,-2.5,2.5,2.5,1.5,1.5,1.5,1.5,-1.5,-1.5,-2.5,-2.5,-2.5,-2.5]])
X = np.vstack((X,np.ones(X.shape[1])))
Pest = compute_P(x,X)
Pest/Pest[2,3]
ここまででカメラ行列が求まりました。
[[ 1.94738769e+01 -1.33146084e+01 7.16340383e+01 6.12813908e+02]
[-4.88241370e+00 -6.79224712e+01 1.13374183e+01 3.78637121e+02]
[-5.19094658e-02 -3.14393781e-02 6.63663212e-02 1.00000000e+00]]
次に、p.85のプログラムを利用してカメラ行列を分解して、カメラ位置と姿勢を求めます。
from scipy import linalg
def factor(P):
K,R = linalg.rq(P[:,:3])
T = np.diag(np.sign(np.diag(K)))
if np.linalg.det(T) < 0:
T[1,1] *= -1
K = np.dot(K,T)
R = np.dot(T,R)
t = np.dot(np.linalg.inv(K),P[:,3])
c = -np.dot(R.T,t)
return K,R,t,c
これで分解。
from scipy.spatial.transform import Rotation as RT
def euler2(R):
r = RT.from_matrix(R)
return -r.as_euler('yxz', degrees=True)
さらにカメラ行列からオイラー角表現に。
yxzの並びは探索的手法（笑）により導き出しました。
K,R,t,c = factor(Pest)
print(c)
print(euler2(R))
[ 5.1818497 3.6523029 -9.28463122]
[-38.03130133 20.46260288 3.02192096]
上がカメラ位置
下がカメラ姿勢のオイラー角表現です。
そして答え合わせ。
うーん、位置はあってそうだけど回転の符号が反対なのとXYZの対応が違うみたいですね。
本当に合っているのだろうか...
というところで、遅い夏休みの時間切れです。