カメラ位置・姿勢推定０ PNP問題 外部パラメータ推定実験
PNP問題の総まとめとして，そろそろちょっと手を動かしてみようと思い，実験室（＝リビング）で実験をすることにしました．
実験室の設定
まずは実験室の設定です．なんだか怪しい占いをしている部屋みたいな感じになってしまいましたが，一応キャリブレーションルームのつもりです．床面にグリーンの養生テープが貼られていますが，これは一マス50cmの間隔で貼っています．で，これだけだと一つの平面を構成する点群だけになってしまうので，高さをのある点をつくるために「南アルプスの天然水」を導入しました（笑）．
南アルプスの天然水は，高さ31cmです．
カメラの搭載場所
カメラの搭載場所ですが，設定を簡単にするために世界座標の原点に設置しました．ただし，高さ（51cm）と俯角（12度）がつけられています．
キャリブルーム（実験室）の校正用写真
この画像を撮影したカメラの世界座標系での位置（（0, 0.51, 0），俯角12度）が不明であるとして，計算から求めるのが最終目的です．
キャリブルーム写真 校正後写真
PNP問題のエントリでも書いたように，「内部パラメータは既知である」必要があるので，OpenCVのキャリブレーションツールを使って事前にキャリブレーションを完了させます．OpenCVでキャリブレーションした結果，カメラ行列は下記のようになりました．
カメラ行列
A = [[602.214, 0, 685.149], [0, 602.214, 527.226], [0, 0, 1]]
歪み係数
dist = [-0.2563, 0.0694, -0.00007838, -0.0005486, -0.008451]
上記の歪み係数を使って歪み補正した結果が下記の写真です．写真の端の歪みが取れていると思います．
キャリブルームの碁盤の目と該当箇所の画像座標
キャリブルーム各点の世界座標と歪み補正後の画像座標の関係を下記出してみました．青で書かれている座標が世界座標系からみた各点の座標，赤で書かれている点が画像座標系で見た各点の座標です．
上記の図より，19個の点の対応関係が求まります．
計算に使用する点
で，上記の図からわかった対応点を列挙すると，下記のようになります．ここで，正規化画像座標系でも座標を求めていますが，画像座標と正規化画像座標系でv軸とy軸の向きが逆であることには要注意です．
PNP問題を解く！
DLT法で解く方程式
以前の下記エントリでDLT法で解く方程式を導出しましたが，復習です．
daily-tech.hatenablog.com
Pythonスクリプト
import numpy as np
world = np.array(\
[\
[-0.50, 0.00, 0.50], \
[ 0.00, 0.00, 0.50], \
[ 0.50, 0.00, 0.50], \
[-0.50, 0.00, 1.00], \
[ 0.00, 0.00, 1.00], \
[ 0.50, 0.00, 1.00], \
[-1.00, 0.00, 1.50], \
[-1.00, 0.31, 1.50], \
[-0.50, 0.00, 1.50], \
[ 0.00, 0.00, 1.50], \
[ 0.50, 0.00, 1.50], \
[ 1.00, 0.00, 1.50], \
[ 1.00, 0.31, 1.50], \
[-0.50, 0.00, 2.00], \
[-0.50, 0.31, 2.00], \
[ 0.00, 0.00, 2.00], \
[ 0.00, 0.31, 2.00], \
[ 0.50, 0.00, 2.00], \
[ 0.50, 0.31, 2.00], \
])
img = np.array(\
[\
[ 175, 946], \
[ 715, 940], \
[ 1279, 939], \
[ 417, 686], \
[ 705, 683], \
[ 999, 676], \
[ 311, 594], \
[ 288, 478], \
[ 504, 591], \
[ 703, 591], \
[ 903, 585], \
[ 1104, 581], \
[ 1115, 460], \
[ 549, 541], \
[ 540, 451], \
[ 702, 539], \
[ 698, 447], \
[ 852, 535], \
[ 854, 443], \
])
normalized_img = np.array(\
[\
[-0.85, -0.85], \
[ 0.05, -0.84], \
[ 0.99, -0.84], \
[-0.45, -0.42], \
[ 0.03, -0.42], \
[ 0.52, -0.40], \
[-0.62, -0.27], \
[-0.66, -0.08], \
[-0.30, -0.26], \
[ 0.03, -0.26], \
[ 0.36, -0.25], \
[ 0.70, -0.25], \
[ 0.71, -0.05], \
[-0.23, -0.18], \
[-0.24, -0.03], \
[ 0.03, -0.18], \
[ 0.02, -0.02], \
[ 0.28, -0.17], \
[ 0.28, -0.02], \
])
## Create M Matrix that will be solved by SVD.
def createMMatrix(imgPnt, worldPnt):
pntNum = imgPnt.shape[0]
M = np.empty((0, 12 + pntNum))
for i in range(imgPnt.shape[0]):
Xw = worldPnt[i][0]
Yw = worldPnt[i][1]
Zw = worldPnt[i][2]
x = imgPnt[i][0]
y = imgPnt[i][1]
vec = np.zeros((3, 12 + pntNum))
# For X
vec[0, 0] = Xw
vec[0, 1] = Yw
vec[0, 2] = Zw
vec[0, 3] = 1.0
vec[0, 12 + i] = -x
# For Y
vec[1, 4] = Xw
vec[1, 5] = Yw
vec[1, 6] = Zw
vec[1, 7] = 1.0
vec[1, 12 + i] = -y
# For Z
vec[2, 8] = Xw
vec[2, 9] = Yw
vec[2, 10] = Zw
vec[2, 11] = 1.0
vec[2, 12 + i] = -1
M = np.append(M, vec, axis=0)
return M
def calculateProjectionMatrix(M):
U, S, V = np.linalg.svd(M, full_matrices=True)
num = 1
v = V[V.shape[0] - num]
lamda = v[V.shape[0] - num]
sign = 1
if (lamda < 0):
sign = -1
P = sign * np.array([[v[0], v[1], v[2], v[3]], [v[4], v[5], v[6], v[7]], [v[8], v[9], v[10], v[11]]])
return P
def calculateRotationMatrixViaQRFactorization(P):
A1 = np.matrix(P[0, 0:3])
A2 = np.matrix(P[1, 0:3])
A3 = np.matrix(P[2, 0:3])
f = np.linalg.norm(A3)
R3 = A3 / f
e = A2 * R3.T
d = np.linalg.norm(A2 - e * R3)
R2 = (A2 - e * R3) / d
c = A1 * R3.T
b = A1 * R2.T
a = np.linalg.norm(A1 - b * R2 - c * R3)
R1 = (A1 - b * R2 - c * R3) / a
#print()
#print("Rotation Matrix R : ")
R = np.zeros((0, 3))
R = np.append(R, R1, axis=0)
R = np.append(R, R2, axis=0)
R = np.append(R, R3, axis=0)
#print(R)
K = np.matrix([[a, b, c], [0, d, e], [0, 0, f]])
return R, K
if __name__ == "__main__":
print("Solve PNP Problem!")
# 1. Create M Matrix.
M = createMMatrix(normalized_img, world)
# 2. Calculate Projection Matrix
P = calculateProjectionMatrix(M)
# 3. Calculate R and K matrix via QR factorization.
R, K = calculateRotationMatrixViaQRFactorization(P)
print("Camera Intrisic Matrix")
print(K / K[2, 2])
print()
# 5. Translation found by P = [KR KT] -> T = inv(K) * P[:, 3]
print("Camera Translation : ")
print(np.linalg.inv(K) * np.matrix(P[:, 3]).T)
print()
print("Camera Rotation : ")
print(R)
上記スクリプトはPNP問題実験編２で用いたものと同じですが，実行すると下記の結果が出てきます．
Translation に関しても，Rotation に関しても，問題設定のもの（高さ 51 cm, 俯角 12deg (cos(12deg) = 0.978) ）と同じ結果が出ています．
また，正規化画像座標を用いたので，カメラの内部パラメータ行列はほぼ単位行列になっていることがわかります．
Camera Intrisic Matrix [[ 1.02943989 -0.02945167 0.00755392] [ 0. 0.99584322 -0.09206592] [ 0. 0. 1. ]] Camera Translation : [[ 0.00254687] [-0.51267888] [ 0.07332725]] Camera Rotation : [[ 0.99960549 -0.02382944 0.01486711] [ 0.02095918 0.98523074 0.16994441] [-0.01869722 -0.16956576 0.9853415 ]]
