カメラの位置・姿勢推定２ PNP問題 実験編１
下記のエントリで透視投影モデル，およびその計算式を導入し，
daily-tech.hatenablog.com
下記のエントリでPNP問題とその解法を導入しました．
daily-tech.hatenablog.com
今回のエントリとその次のエントリで実際に PNP 問題を解いてみたいと思います．両方のエントリで同じ問題「１．世界座標が既知の７点を，位置が既知のカメラに投影した時の画像座標を求める．」と「２．世界座標，画像座標が既知の点７点をつかってカメラの位置を求める」を扱いますが，今回のエントリでは OpenCV の関数を用いて，次回のエントリでは自作のプログラムで問題を解きます．
０．問題のイメージ図
※以前アップしていた絵だとすべての点が同一平面上にあるので，自作のPNP関数ではうまくいきませんでした．OpenCVのsolvePNPでは，同一平面上にあっても解は求まりましたが．
１．世界座標が既知の７点を，位置が既知のカメラに投影した時の画像座標を求める．
ここでは，投影画像座標を求めます．問題を簡単にするために，画像にレンズひずみはないものとし，内部パラメータは下記のように定義します．
画像幅 ：640pix
画像高さ：480pix
主点x ：320
主点y ：240
焦点距離：160pix
カメラ高さ：1m
カメラ角度：俯角30度
ここでは，投影点を求めるために，OpenCV の projectPoint 関数を使います．下記， Python のスクリプトです．
ここで注意が必要なのは，R, t は「カメラ座標から見たワールド座標の位置」でなければいけないことと，オイラー角表現を用いているので回転行列の掛け算が Rx * Ry * Rz になってます．
import numpy as np
import cv2
import math
world = np.array(\
[\
( 5.00, 0.00, 0.00), \
( 5.00, 0.00, 0.50), \
( 6.00, -1.00, 0.00), \
( 6.00, -1.00, 0.50), \
( 6.00, 1.00, 0.00), \
( 6.00, 1.00, 0.50), \
( 7.00, 0.00, 0.50), \
])
# Camera Extrinsic Parameter
xRadEuler_C2W = -120 / 180 * math.pi
yRadEuler_C2W = 0 / 180 * math.pi
zRadEuler_C2W = -90 / 180 * math.pi
Rx = np.matrix([[1, 0, 0], [0, math.cos(xRadEuler_C2W), -math.sin(xRadEuler_C2W)], [0, math.sin(xRadEuler_C2W), math.cos(xRadEuler_C2W)]])
Ry = np.matrix([[ math.cos(yRadEuler_C2W), 0, math.sin(yRadEuler_C2W)], [0, 1, 0], [-math.sin(yRadEuler_C2W), 0, math.cos(yRadEuler_C2W)]])
Rz = np.matrix([[ math.cos(zRadEuler_C2W), -math.sin(zRadEuler_C2W), 0], [ math.sin(zRadEuler_C2W), math.cos(zRadEuler_C2W), 0], [0, 0, 1]])
# Notice : Rotation Matrix from Euler Angle.
R = Rx * Ry * Rz
# tvec is expressed wrt camra coord.
tvec = R * np.matrix((0, 0, -1)).T
# Camera Intrinsic Paramter
dist_coeffs = np.zeros((5, 1))
width = 640
height = 480
focal_length = 160
center = (width / 2, height / 2)
camera_matrix = np.array([[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1]], dtype = "double")
if __name__ == "__main__":
print("Calculate Projected Point")
print("\nExtrinsic Paramter")
print("Translation")
print(tvec)
print("Rotation")
print(R)
print("\nCamera Matrix : ")
print(camera_matrix)
print("\nDistortion Coefficient")
print(dist_coeffs)
print("\nGenerate Rotation Vector from Rotation Matrix")
rvec, jacob = cv2.Rodrigues(R)
print(rvec)
print("\nProject Point on Screen")
result = cv2.projectPoints(world, rvec, tvec, camera_matrix, None)
for n in range(len(world)):
print(world[n], '==>', result[0][n])
上記スクリプトを実行すると，下記の結果（画像座標での7点の投影位置）が得られます．
Project Point on Screen [ 5. 0. 0. ] ==> [[ 320. 294.12609945]] [ 5. 0. 0.5 ] ==> [[ 320. 312.2071607]] [ 6. -1. 0. ] ==> [[ 291.91086401 299.94150262]] [ 6. -1. 0.5 ] ==> [[ 290.62146125 315.41433581]] [ 6. 1. 0. ] ==> [[ 348.08913599 299.94150262]] [ 6. 1. 0.5 ] ==> [[ 349.37853875 315.41433581]] [ 7. 0. 0.5 ] ==> [[ 320. 317.74146755]]
２．世界座標，画像座標が既知の点７点をつかってカメラの位置を求める
今度は逆に，上記の結果を使ってカメラ位置を求めます．
import numpy as np
import cv2
import math
world = np.array(\
[\
( 5.00, 0.00, 0.00), \
( 5.00, 0.00, 0.50), \
( 6.00, -1.00, 0.00), \
( 6.00, -1.00, 0.50), \
( 6.00, 1.00, 0.00), \
( 6.00, 1.00, 0.50), \
( 7.00, 0.00, 0.50), \
])
img_pnts = np.array(\
[\
(320. , 294.12609945), \
(320. , 312.2071607), \
(291.91086401, 299.94150262), \
(290.62146125, 315.41433581), \
(348.08913599, 299.94150262), \
(349.37853875, 315.41433581), \
(320. , 317.74146755), \
])
# Camera Intrinsic Paramter
dist_coeffs = np.zeros((5, 1))
width = 640
height = 480
focal_length = 160
center = (width / 2, height / 2)
camera_matrix = np.array(
[[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1]], dtype = "double"
)
if __name__ == "__main__":
print("Solve Perspective N Point Problem")
print("\nCamera Matrix : ")
print(camera_matrix)
print("\nDistortion Coefficient")
print(dist_coeffs)
(success, rot_vec, trans_vec) = cv2.solvePnP(world, img_pnts, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)
print("\nTranslation Vector")
print(trans_vec)
print("\nRotation Vector")
print(rot_vec)
print("\nRotation Matrix")
R, jacob = cv2.Rodrigues(rot_vec)
print(R)
上記スクリプトを実行すると，下記の結果（画像座標での7点の投影位置）が得られます．もともとのカメラ位置が求まりました．
Translation Vector [[ -8.87481507e-11] [ -8.66025403e-01] [ 4.99999999e-01]] Rotation Vector [[-1.58351453] [-1.58351453] [-0.91424254]] Rotation Matrix [[ 1.53020929e-11 1.00000000e+00 -5.93469718e-13] [ 5.00000000e-01 -7.13717974e-12 8.66025404e-01] [ 8.66025404e-01 -1.35487732e-11 -5.00000000e-01]]
次のエントリでは，自作のアルゴリズムでPNP問題を解いてみます．それではまた！
