はじめに
最小二乗法は様々なパラメータフィッティングのファーストチョイスとして使われがちではある。カメラキャリブレーションでも同様で、内部パラメータなどDLT法で推定されることが多く、この手法の中で線形最小二乗法が用いられる場合もある。また、バンドル調整などではガウスニュートン法などの非線形最小二乗法が用いられることもある。
直接計測された計測値の誤差を検討するときには単純に計測値の分散等を計算すればいい。一方で最小二乗法などを使用して間接的に計測された数値(推定されたパラメータ)は、直接計測した計測値の誤差伝播を考慮する必要がある。これらをpythonをつかって計算した。
したごしらえ(点群と画像生成)
点群を生成し、これを既知の内部パラメータを持つカメラで画像平面上に投影したときのピクセル位置を計算する。gainは-1.0~1.0までの乱数に対するゲインで、この乱数をピクセル位置に加算することで計測誤差を再現した。とりあえず、以下プログラムでは画像の特徴点位置に2pixelの誤差を含ませてみた。投影画像gen_img.bmpと共に点群とそれに対応するpixel位置を記録したobjimgpoints.csvが出力される。
import numpy as np import random import cv2 #カメラ内部パラメータ fx = 1500.0 fy = 1500.0 cx = 1920/2 cy = 1080/2 #カメラ姿勢(ZYXオイラー角) roll = 30.0*np.pi/180.0 pitch = 7.0*np.pi/180.0 yaw = 0.0*np.pi/180.0 #カメラ位置 tx = 0.0 ty = 0.0 tz = 25.0 #ターゲット姿勢(ZYXオイラー角) roll_t = 10.0*np.pi/180.0 pitch_t = 40.0*np.pi/180.0 yaw_t = 0.0*np.pi/180.0 w_num = 10 h_num = 8 len_point = h_num*w_num len_point = len_point + h_num*w_num len_point = len_point + h_num*h_num cnt = 0 np_objpoints = np.zeros([len_point,3]) for i in range(w_num): for j in range(h_num): np_objpoints[cnt,0] = i - 0.0 - (w_num-1)/2 np_objpoints[cnt,1] = j - 0.0 - (h_num-1)/2 np_objpoints[cnt,2] = -(h_num-1)/2 - 0.0 cnt = cnt + 1 for i in range(w_num): for j in range(h_num): np_objpoints[cnt,0] = i - 0.0 - (w_num-1)/2 np_objpoints[cnt,1] = -(h_num-1)/2 - 0.0 np_objpoints[cnt,2] = j - 0.0 - (h_num-1)/2 cnt = cnt + 1 for i in range(h_num): for j in range(h_num): np_objpoints[cnt,0] = (w_num-1)/2 + 0.0 np_objpoints[cnt,1] = i - 0.0 - (h_num-1)/2 np_objpoints[cnt,2] = j - 0.0 - (h_num-1)/2 cnt = cnt + 1 Rx = np.array([[1,0,0],[0,np.cos(roll_t),-np.sin(roll_t)],[0,np.sin(roll_t),np.cos(roll_t)]]) Ry = np.array([[np.cos(pitch_t),0,np.sin(pitch_t)],[0,1,0],[-np.sin(pitch_t),0,np.cos(pitch_t)]]) Rz = np.array([[np.cos(yaw_t),-np.sin(yaw_t),0],[np.sin(yaw_t),np.cos(yaw_t),0],[0,0,1]]) Rzyx = Rx@Ry@Rz for i in range(len(np_objpoints)): np_objpoints[i,:] = Rzyx@np_objpoints[i,:] np_imgpoints = np.zeros([len_point,2]) np_imgpoints_true = np.zeros([len_point,2]) 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]]) for i in range(len(np_objpoints)): gain = 0.0 XYZ = np.array([np_objpoints[i,0] + gain*(random.random()*2-1),np_objpoints[i,1] + gain*(random.random()*2-1),np_objpoints[i,2] + gain*(random.random()*2-1),1]) uv = np_mtx@RT@XYZ.T gain = 2.0 np_imgpoints[i,0] = uv[0]/uv[2] + gain*(random.random()*2-1)+0.0 np_imgpoints[i,1] = uv[1]/uv[2] + gain*(random.random()*2-1)+0.0 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[i,0]), int(np_imgpoints[i,1])),radius=5,color=(255, 255, 255),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法による外部・内部パラメータ推定結果---- ----並進位置---- X : 0.278± 0.079 m Y : 0.102± 0.081 m Z : 25.229± 0.102 m ----姿勢---- ROLL : 29.771± 0.172 deg. PITCH: 7.639± 0.165 deg. YAW : -0.031± 0.035 deg. ----内部パラメータ---- fx : 1515.146± 6.544 pixel fy : 1515.383± 6.516 pixel cx : 943.035± 4.846 pixel cy : 533.659± 4.971 pixel s : 0.284± 0.976 pixel ----ガウスニュートン法で調整した内部パラメータ推定結果---- ----並進位置---- X : 0.092± 0.075 m Y : -0.106± 0.077 m Z : 24.969± 0.097 m ----姿勢---- ROLL : 30.225± 0.166 deg. PITCH: 7.221± 0.159 deg. YAW : 0.024± 0.034 deg. ----内部パラメータ---- fx : 1498.502± 6.259 pixel fy : 1499.060± 6.231 pixel cx : 954.436± 4.605 pixel cy : 546.377± 4.754 pixel s : -0.276± 0.941 pixel
プログラム
ヤコビ行列計算式の長さがえぐいことになっているが単純に偏微分してるだけ。sympyなどのシンボリック計算ツールを使えばコピペするだけでよい。昔(10年くらい前)は便利ツールの情報が少なくて渋々手計算したが便利な世の中になったものである。
# # 内部パラメータ推定 # import os import numpy as np DIST_PATH = "dist.csv" BASE_PATH = "objimgpoints.csv" # メイン関数 def main(): calcCamera() # カメラの歪みを計算 # カメラの内部パラメータを計算する関数 def calcCamera(): #作業領域変更 os.chdir(os.path.dirname(os.path.abspath(__file__))) base_cam = loadMFile(BASE_PATH)#三次元位置と画像特徴点位置のペア情報読み込み cam0 = base_cam[:,3:5]#画像特徴点位置の切り出し base = base_cam[:,0:3]#三次元位置の切り出し # dist = loadDistortionFile(DIST_PATH)#レンズ歪がある場合は歪係数ファイル読み込み # cam0 = calc_distortion(loadMFile(cam0_PATH),dist)#歪係数ファイルを画像特徴点位置に適用 M = np.zeros([len(cam0)*3,12+len(cam0)])#特異値分解分解される側行列 v = np.zeros([12+len(cam0),1])#特異値分解パラメータ行列 P = np.zeros([3,4])#透視投影行列 for i in range(len(cam0)): M[i*3+0,0] = base[i,0] M[i*3+0,1] = base[i,1] M[i*3+0,2] = base[i,2] M[i*3+0,3] = 1 M[i*3+1,4+0] = base[i,0] M[i*3+1,4+1] = base[i,1] M[i*3+1,4+2] = base[i,2] M[i*3+1,4+3] = 1 M[i*3+2,8+0] = base[i,0] M[i*3+2,8+1] = base[i,1] M[i*3+2,8+2] = base[i,2] M[i*3+2,8+3] = 1 M[i*3+0,12+i] = -cam0[i,0] M[i*3+1,12+i] = -cam0[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[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 #RQ分解 A = P[:,0:3] B = P[:,3] R = np.zeros([3,3])#回転行列 t = np.zeros(3)#並進ベクトル 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#fx mtx[0,1] = b#s mtx[0,2] = c#cx mtx[1,1] = d#fy mtx[1,2] = e#cy 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 #カメラ姿勢角(ZYXオイラー表現) rpy = rot2rpy(R) yaw = rpy[2] pitch = rpy[1] roll = rpy[0] #再投影誤差を最小化する目的関数周りで内部パラメータ分散を計算する #初期値入力 new_x = np.zeros(11) new_x[0] = t[0] new_x[1] = t[1] new_x[2] = t[2] new_x[3] = rpy[0] new_x[4] = rpy[1] new_x[5] = rpy[2] new_x[6] = mtx[0,0] new_x[7] = mtx[1,1] new_x[8] = mtx[0,2] new_x[9] = mtx[1,2] new_x[10] = mtx[0,1] J = [] E = [] Ex = [] Ey = [] for i in range(len(base)): #観測値入力 X = base[i,0] Y = base[i,1] Z = base[i,2] #計測値入力 x = cam0[i,0] y = cam0[i,1] #推定内部パラメータ入力 fx = new_x[6] fy = new_x[7] cx = new_x[8] cy = new_x[9] s = new_x[10] tx = new_x[0] ty = new_x[1] tz = new_x[2] roll = new_x[3] pitch = new_x[4] yaw = new_x[5] #ヤコビ行列計算 j00 = -fx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j01 = -s/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j02 = -cx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j03 = -(Y*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cx*np.sin(roll)*np.cos(pitch) + fx*(-np.sin(pitch)*np.sin(roll)*np.cos(yaw) + np.sin(yaw)*np.cos(roll)) + s*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j04 = -(X*(-cx*np.cos(pitch) - fx*np.sin(pitch)*np.cos(yaw) - s*np.sin(pitch)*np.sin(yaw)) + Y*(-cx*np.sin(pitch)*np.sin(roll) + fx*np.sin(roll)*np.cos(pitch)*np.cos(yaw) + s*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cx*np.sin(pitch)*np.cos(roll) + fx*np.cos(pitch)*np.cos(roll)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j05 = -(X*(-fx*np.sin(yaw)*np.cos(pitch) + s*np.cos(pitch)*np.cos(yaw)) + Y*(fx*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll))) + Z*(fx*(-np.sin(pitch)*np.sin(yaw)*np.cos(roll) + np.sin(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j06 = -(X*np.cos(pitch)*np.cos(yaw) + Y*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + tx)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j07 = 0 j08 = -1 j09 = 0 j0a = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j10 = 0 j11 = -fy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j12 = -cy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j13 = -(Y*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cy*np.sin(roll)*np.cos(pitch) + fy*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j14 = -(X*(-cy*np.cos(pitch) - fy*np.sin(pitch)*np.sin(yaw)) + Y*(-cy*np.sin(pitch)*np.sin(roll) + fy*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cy*np.sin(pitch)*np.cos(roll) + fy*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j15 = -(X*fy*np.cos(pitch)*np.cos(yaw) + Y*fy*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*fy*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j16 = 0 j17 = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j18 = 0 j19 = -1 j1a = 0 J.append([j00,j01,j02,j03,j04,j05,j06,j07,j08,j09,j0a]) J.append([j10,j11,j12,j13,j14,j15,j16,j17,j18,j19,j1a]) #再投影誤差算出 Rzyx = rpy2rot(np.array([roll,pitch,yaw])) K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]]) 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]]) XYZ = np.array([X,Y,Z,1]) PXYZ = K@RT@XYZ.T ex = x - PXYZ[0]/PXYZ[2] ey = y - PXYZ[1]/PXYZ[2] Ex.append(ex) Ey.append(ey) J = np.array(J) Ex = np.array(Ex) Ey = np.array(Ey) #再投影誤差から不偏分散算出 xe = np.zeros(2) for i in range(len(Ex)): b = np.zeros(2) b[0] = Ex[i]**2 b[1] = Ey[i]**2 xe = xe + b xe = xe/(len(Ex) - 11)#11個の未知数があるので計測数から11自由度分減算する Ssig = np.zeros([len(Ex)*2,len(Ex)*2])#誤差行列 for i in range(len(Ex)): Ssig[i*2,i*2] = xe[0] Ssig[i*2+1,i*2+1] = xe[1] errM = np.linalg.inv(J.T@np.linalg.inv(Ssig)@J)#共分散行列 err = np.zeros(len(errM)) for i in range(len(errM)): err[i] = np.sqrt(errM[i,i])#対角成分の平方根が標準偏差 print("----DLT法による外部・内部パラメータ推定結果----") print("----並進位置----") print("X :{:13.3f}±{:13.3f} m".format(tx,err[0])) print("Y :{:13.3f}±{:13.3f} m".format(ty,err[1])) print("Z :{:13.3f}±{:13.3f} m".format(tz,err[2])) print("----姿勢----") print("ROLL :{:13.3f}±{:13.3f} deg.".format(roll*180/np.pi,err[3]*180/np.pi)) print("PITCH:{:13.3f}±{:13.3f} deg.".format(pitch*180/np.pi,err[4]*180/np.pi)) print("YAW :{:13.3f}±{:13.3f} deg.".format(yaw*180/np.pi,err[5]*180/np.pi)) print("----内部パラメータ----") print("fx :{:13.3f}±{:13.3f} pixel".format(fx,err[6])) print("fy :{:13.3f}±{:13.3f} pixel".format(fy,err[7])) print("cx :{:13.3f}±{:13.3f} pixel".format(cx,err[8])) print("cy :{:13.3f}±{:13.3f} pixel".format(cy,err[9])) print("s :{:13.3f}±{:13.3f} pixel".format(s,err[10])) #DLT法で推定した外部・内部パラメータを初期値としてガウスニュートン法で調整する #初期値入力 new_x[0] = t[0] new_x[1] = t[1] new_x[2] = t[2] new_x[3] = rpy[0] new_x[4] = rpy[1] new_x[5] = rpy[2] new_x[6] = mtx[0,0] new_x[7] = mtx[1,1] new_x[8] = mtx[0,2] new_x[9] = mtx[1,2] new_x[10] = mtx[0,1] #最大繰り返し回数 ita_num = 100 for j in range(ita_num): J = []#ヤコビ行列初期化 E = []#再投影誤差初期化 Ex = []#x軸方向のみ再投影誤差初期化 Ey = []#y軸方向のみ再投影誤差初期化 for i in range(len(base)): #観測値入力 X = base[i,0] Y = base[i,1] Z = base[i,2] #計測値入力 x = cam0[i,0] y = cam0[i,1] #内部パラメータ入力 fx = new_x[6] fy = new_x[7] cx = new_x[8] cy = new_x[9] s = new_x[10] #外部パラメータ入力 tx = new_x[0] ty = new_x[1] tz = new_x[2] roll = new_x[3] pitch = new_x[4] yaw = new_x[5] #ヤコビ行列算出 j00 = -fx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j01 = -s/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j02 = -cx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j03 = -(Y*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cx*np.sin(roll)*np.cos(pitch) + fx*(-np.sin(pitch)*np.sin(roll)*np.cos(yaw) + np.sin(yaw)*np.cos(roll)) + s*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j04 = -(X*(-cx*np.cos(pitch) - fx*np.sin(pitch)*np.cos(yaw) - s*np.sin(pitch)*np.sin(yaw)) + Y*(-cx*np.sin(pitch)*np.sin(roll) + fx*np.sin(roll)*np.cos(pitch)*np.cos(yaw) + s*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cx*np.sin(pitch)*np.cos(roll) + fx*np.cos(pitch)*np.cos(roll)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j05 = -(X*(-fx*np.sin(yaw)*np.cos(pitch) + s*np.cos(pitch)*np.cos(yaw)) + Y*(fx*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll))) + Z*(fx*(-np.sin(pitch)*np.sin(yaw)*np.cos(roll) + np.sin(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j06 = -(X*np.cos(pitch)*np.cos(yaw) + Y*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + tx)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j07 = 0 j08 = -1 j09 = 0 j0a = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j10 = 0 j11 = -fy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j12 = -cy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j13 = -(Y*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cy*np.sin(roll)*np.cos(pitch) + fy*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j14 = -(X*(-cy*np.cos(pitch) - fy*np.sin(pitch)*np.sin(yaw)) + Y*(-cy*np.sin(pitch)*np.sin(roll) + fy*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cy*np.sin(pitch)*np.cos(roll) + fy*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2 j15 = -(X*fy*np.cos(pitch)*np.cos(yaw) + Y*fy*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*fy*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j16 = 0 j17 = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) j18 = 0 j19 = -1 j1a = 0 J.append([j00,j01,j02,j03,j04,j05,j06,j07,j08,j09,j0a]) J.append([j10,j11,j12,j13,j14,j15,j16,j17,j18,j19,j1a]) #再投影誤差算出 Rzyx = rpy2rot(np.array([roll,pitch,yaw])) K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]]) 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]]) XYZ = np.array([X,Y,Z,1]) PXYZ = K@RT@XYZ.T ex = x - PXYZ[0]/PXYZ[2] ey = y - PXYZ[1]/PXYZ[2] E.append(ex) E.append(ey) Ex.append(ex) Ey.append(ey) J = np.array(J) E = np.array(E) Ex = np.array(Ex) Ey = np.array(Ey) #更新誤差算出 delta = (np.linalg.inv(J.T@J)@J.T)@E.T #各測定値との再投影誤差最大値が規定値を下回ったら繰り返し計算終了 if np.abs(np.max(E)) < 0.001: break #パラメータ更新 new_x = new_x - delta #再投影誤差から不偏分散算出 xe = np.zeros(2) for i in range(len(Ex)): b = np.zeros(2) b[0] = Ex[i]**2 b[1] = Ey[i]**2 xe = xe + b xe = xe/(len(Ex) - 11)#11個の未知数があるので計測数から11自由度分減算する Ssig = np.zeros([len(Ex)*2,len(Ex)*2])#誤差行列 for i in range(len(Ex)): Ssig[i*2,i*2] = xe[0] Ssig[i*2+1,i*2+1] = xe[1] errM = np.linalg.inv(J.T@np.linalg.inv(Ssig)@J)#共分散行列 err = np.zeros(len(errM)) for i in range(len(errM)): err[i] = np.sqrt(errM[i,i])#対角成分の平方根が標準偏差 print("----ガウスニュートン法で調整した内部パラメータ推定結果----") print("----並進位置----") print("X :{:13.3f}±{:13.3f} m".format(tx,err[0])) print("Y :{:13.3f}±{:13.3f} m".format(ty,err[1])) print("Z :{:13.3f}±{:13.3f} m".format(tz,err[2])) print("----姿勢----") print("ROLL :{:13.3f}±{:13.3f} deg.".format(roll*180/np.pi,err[3]*180/np.pi)) print("PITCH:{:13.3f}±{:13.3f} deg.".format(pitch*180/np.pi,err[4]*180/np.pi)) print("YAW :{:13.3f}±{:13.3f} deg.".format(yaw*180/np.pi,err[5]*180/np.pi)) print("----内部パラメータ----") print("fx :{:13.3f}±{:13.3f} pixel".format(fx,err[6])) print("fy :{:13.3f}±{:13.3f} pixel".format(fy,err[7])) print("cx :{:13.3f}±{:13.3f} pixel".format(cx,err[8])) print("cy :{:13.3f}±{:13.3f} pixel".format(cy,err[9])) print("s :{:13.3f}±{:13.3f} pixel".format(s,err[10])) def rot2rpy(R): 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 return np.array([roll,pitch,yaw]) def rpy2rot(rpy): yaw = rpy[2] pitch = rpy[1] roll = rpy[0] 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]]) return Rz@Ry@Rx def calc_distortion(point_data,params): cx = params[0] cy = params[1] k1 = params[2] k2 = params[3] k3 = params[4] k4 = params[5] point_data_ans = point_data.copy() for i in range(len(point_data)): x = point_data[i,0] - cx y = point_data[i,1] - cy r2 = x**2 + y**2 f = (1+k1*r2+k2*r2**2)/(1+k3*r2+k4*r2**2) point_data_ans[i,0] = x*f + cx point_data_ans[i,1] = y*f + cy return point_data_ans # 歪曲収差係数CSVファイルを読み込む関数 def loadDistortionFile(dist_path): try: dist = np.loadtxt(dist_path, delimiter=',') except Exception as e: raise e return dist # 測量CSVファイルを読み込む関数 def loadMFile(m_path): try: r = np.loadtxt(m_path, delimiter=',') except Exception as e: raise e return r if __name__ == '__main__': main()