はじめに
カメラの内部パラメータ、外部パラメータ知りたいよね。DLT法で求まるんだわ。
下ごしらえ（点群と画像生成）
点群を生成し、これを既知の内部パラメータを持つカメラで画像平面上に投影したときのピクセル位置を計算する。gainは-1.0~1.0までの乱数に対するゲインで、この乱数をピクセル位置に加算することで計測誤差を再現した。投影画像gen_img.bmpと共に点群とそれに対応するpixel位置を記録したobjimgpoints.csvが出力される。
画像はこんな感じ。
csvは以下のような感じ。
| 三次元位置X[m] | 三次元位置Y[m] | 三次元位置Z[m] | ピクセル位置X[pixel] | ピクセル位置Y[pixel] |
|---|---|---|---|---|
| -4.5 | -3.0 | 14.1 | 629.2 | 142.1 |
| ... | ... | ... | ... | ... |
import numpy as np import random import cv2 #カメラ内部パラメータ fx = 1200.0 fy = 1200.0 cx = 1920/2 cy = 1080/2 #カメラ姿勢(ZYXオイラー角) roll = 5.0*np.pi/180.0 pitch = -5.0*np.pi/180.0 yaw = 2.0*np.pi/180.0 #カメラ位置 tx = 1.0 ty = -1.0 tz = 3.0 #pixel位置ノイズゲイン[-] gain = 3.0 #計測点の個数 w_n = 10#横 h_n = 7#縦 np_objpoints = np.zeros([w_n*h_n,3]) cnt = 0 for i in range(w_n): for j in range(h_n): gain = 3 np_objpoints[cnt,0] = i - (w_n-1)/2 np_objpoints[cnt,1] = j - (h_n-1)/2 np_objpoints[cnt,2] = gain*(random.random()*2-1) + 15 cnt = cnt + 1 np_mtx = np.zeros([3,3]) np_mtx[0,0] = fx np_mtx[0,1] = 0 np_mtx[0,2] = cx np_mtx[1,1] = fy np_mtx[1,2] = cy np_mtx[2,2] = 1 Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]]) Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]]) Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]]) Rzyx = Rz@Ry@Rx RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]]) np_imgpoints = np.zeros([w_n*h_n,2]) np_imgpoints_true = np.zeros([w_n*h_n,2]) for i in range(len(np_objpoints)): XYZ = np.array([np_objpoints[i,0],np_objpoints[i,1],np_objpoints[i,2],1]) uv = np_mtx@RT@XYZ.T np_imgpoints[i,0] = uv[0]/uv[2] + gain*(random.random()*2-1) np_imgpoints[i,1] = uv[1]/uv[2] + gain*(random.random()*2-1) np_imgpoints_true[i,0] = uv[0]/uv[2] np_imgpoints_true[i,1] = uv[1]/uv[2] height = int(cy*2) width = int(cx*2) img = np.zeros((height, width, 3)) for i in range(len(np_imgpoints)): cv2.circle(img,center=(int(np_imgpoints_true[i,0]), int(np_imgpoints_true[i,1])),radius=5,color=(0, 255, 0),thickness=-1,lineType=cv2.LINE_4,shift=0) cv2.imwrite('gen_img.bmp',img) np.savetxt('objimgpoints.csv', np.concatenate((np_objpoints, np_imgpoints), axis=1),delimiter=",")
こんな感じ。
DLT法本体
生成したobjimgpoints.csvを読み込んで内部パラメータ、外部パラメータを推定する。設定した数値と近い値になるはず。
import numpy as np import cv2 #ファイル入力 filename = "objimgpoints.csv" csv_input = np.loadtxt(filename,delimiter = ",",encoding = "shift-JIS",dtype = "unicode_",skiprows = 0) imgpos = np.zeros([len(csv_input),2])#ピクセル位置[pixel](x,y) objpos = np.zeros([len(csv_input),3])#三次元位置[m](x,y,z) for i in range(len(csv_input)): objpos[i,:] = np.array([float(csv_input[i,0]),float(csv_input[i,1]),float(csv_input[i,2])]) imgpos[i,:] = np.array([float(csv_input[i,3]),float(csv_input[i,4])]) #同次連立方程式Mv=0 M = np.zeros([len(imgpos)*3,12+len(imgpos)]) for i in range(len(imgpos)): M[i*3+0,0] = objpos[i,0] M[i*3+0,1] = objpos[i,1] M[i*3+0,2] = objpos[i,2] M[i*3+0,3] = 1 M[i*3+1,4+0] = objpos[i,0] M[i*3+1,4+1] = objpos[i,1] M[i*3+1,4+2] = objpos[i,2] M[i*3+1,4+3] = 1 M[i*3+2,8+0] = objpos[i,0] M[i*3+2,8+1] = objpos[i,1] M[i*3+2,8+2] = objpos[i,2] M[i*3+2,8+3] = 1 M[i*3+0,12+i] = -imgpos[i,0] M[i*3+1,12+i] = -imgpos[i,1] M[i*3+2,12+i] = -1 #特異値分解 U,S,Vt = np.linalg.svd(M,full_matrices=True) #最小特異値 v = Vt[-1] #スケール係数の平均 ramda = np.mean(v[12:-1]) #透視投影変換行列 P = np.zeros([3,4]) P[0,0] = v[0]/ramda P[0,1] = v[1]/ramda P[0,2] = v[2]/ramda P[0,3] = v[3]/ramda P[1,0] = v[4]/ramda P[1,1] = v[5]/ramda P[1,2] = v[6]/ramda P[1,3] = v[7]/ramda P[2,0] = v[8]/ramda P[2,1] = v[9]/ramda P[2,2] = v[10]/ramda P[2,3] = v[11]/ramda A = P[:,0:3]#[KR] B = P[:,3]#[Kt] R = np.zeros([3,3])#回転行列 t = np.zeros(3)#併進位置 #RQ分解 R[2,:] = A[2,:]/np.linalg.norm(A[2,:]) f = np.linalg.norm(A[2,:]) e = A[1,:].T@R[2,:] d = np.linalg.norm(A[1,:]-e*R[2,:]) R[1,:] = (A[1,:]-e*R[2,:])/np.linalg.norm(A[1,:]-e*R[2,:]) c = A[0,:].T@R[2,:] b = A[0,:].T@R[1,:] a = np.linalg.norm(A[0,:]-b*R[1,:]-c*R[2,:]) R[0,:] = (A[0,:]-b*R[1,:]-c*R[2,:])/np.linalg.norm(A[0,:]-b*R[1,:]-c*R[2,:]) mtx = np.zeros([3,3])#内部パラメータ mtx[0,0] = a mtx[0,1] = b mtx[0,2] = c mtx[1,1] = d mtx[1,2] = e mtx[2,2] = f mtx = mtx/mtx[2,2] t[2] = B[2]/f t[1] = (B[1]-e*t[2])/d t[0] = (B[0]-b*t[1]-c*t[2])/a yaw = 0 pitch = np.arcsin(-R[2,0]) roll = 0 if np.abs(R[2,0]) != 1: yaw = np.arctan2(R[1,0],R[0,0]) roll = np.arctan2(R[2,1],R[2,2]) if R[2,0] == 1: roll = 0 yaw = np.arctan2(R[0,1],R[1,1]) if R[2,0] == -1: roll = 0 yaw = np.arctan2(R[0,1],R[1,1]) if roll > np.pi: roll = roll - 2*np.pi if roll < -np.pi: roll = roll + 2*np.pi if pitch > np.pi: pitch = pitch - 2*np.pi if pitch < -np.pi: pitch = pitch + 2*np.pi if yaw > np.pi: yaw = yaw - 2*np.pi if yaw < -np.pi: yaw = yaw + 2*np.pi print("内部パラメータ推定結果") print(mtx) print("opencv decomposeProjectionMatrix関数によるRQ分解結果") d = cv2.decomposeProjectionMatrix(P) print(d[0]/d[0][2,2]) print("外部パラメータ推定結果") print("カメラ位置[m]") print(t) print("カメラ姿勢(ZYXオイラー)[deg.]") print([roll*180/np.pi,pitch*180/np.pi,yaw*180/np.pi])
