下記のエントリで基礎行列計算の理論編を勉強しました.ということで,いつものごとくPythonを使って実験です.
daily-tech.hatenablog.com
問題設定
下記のように二次曲面の形状を設置し,この物体を複数の視点に置いたカメラで撮影する状況を考えます.
対称物体の投影図
OpenCVのProjectPoints関数を用いて,上図の三視点に置かれたカメラで物体を撮影します.
OpenCVのProjectPoint関数の定義は下記のようになっており,物体の座標系で点を指定することができます.物体の座標系とカメラの座標系の関係を tvecs, rvecs に指定してあげれば,入力点群が自動的にカメラ座標系に変換され,そののちに画像に投影されます.
cv2.projectPoints(pntsInObj, rvec2, tvec2, cameraMatrix, distCoeffs)
一点注意点としては,この時の rvecとtvecですが,下記数式の t と R (を Rodrigues形式に変換したもの) を渡してあげる必要があります.つまり,物体座標系からカメラ座標系への変換式の回転要素 R と並進要素 T を渡してあげればOKです.
CAMXP = CAMX t + CAMX R OBJ ・ OBJP
基礎行列の計算
今回の実験では,図の CAM1 と CAM2 の位置関係(既知)を使って基礎行列真値を計算します.そのあとに2画像の投影点ペアを用いて基礎行列を計算し,真値にたいしての誤差が計算方法によってどの程度変動するかを調べます.まず,既知の位置関係を使って,エピポーラ拘束式が満たされていることを確認します.二つの視点を図示したものが下記になります.
最後のスクリプトを実行すると,下記の結果が得られてエピポーラ拘束条件が満たされていることがわかります.
Max Element of m1Fm2 : 2.22044604925e-16 Max Element of m2Fm1 : 1.66533453694e-16
実験結果
ちょっと時間がないので,「最小二乗法 + SVDによるランク補正」と「Taubin法 + SVDによるランク補正」の二つの実行結果を載せます.基礎行列真値に対して,添付のスクリプトを実行すると下記の結果が得られました.一応,最小二乗法のよりも Taubin 法のほうが結果がかなりいいことがわかります.また時間ができたらほかのスキームも作って更新します.
Accuracy check for each numerical scheme. FTrue VS FLSSVD : 0.00273989625189 FTrue VS FTaubinSVD : 0.000641120755582
Python スクリプト
ちょっとコードが長くなってしまったのですが,一応載せておきます.
test_fund_mat.py
# -*- coding: utf-8 -*- import cv2 import numpy as np from geometry_k.rotation import RMatFromEulerAngle from geometry_k.rotation import OuterProductOperator from fund_mat import calc_fund_mat from fund_mat import normalize_mat from fund_mat import FundMatCalcMode from fund_mat import calc_dist_between_2F from data_manipulation.noise import add_gauss_noise # Camera Intrinsic Parameter f = 160 width = 640 height = 480 pp = (width / 2, height / 2) cameraMatrix = np.matrix(np.array([[f, 0, pp[0]], [0, f, pp[1]], [0, 0, 1]], dtype = "double")) distCoeffs = np.zeros((5, 1)) # Camera Extrinsic Parameter 0 xEul_C0_to_OBJ = 0 yEul_C0_to_OBJ = 0 zEul_C0_to_OBJ = 0 ROBJ_to_0 = RMatFromEulerAngle(xEul_C0_to_OBJ, yEul_C0_to_OBJ, zEul_C0_to_OBJ) C0_to_OBJ_in_C0 = (0, 0, 10) # Camera Extrinsic Parameter 1 xEul_C1_to_OBJ = 0 yEul_C1_to_OBJ = -45 zEul_C1_to_OBJ = 0 ROBJ_to_1 = RMatFromEulerAngle(xEul_C1_to_OBJ, yEul_C1_to_OBJ, zEul_C1_to_OBJ) C1_to_OBJ_in_C1 = (0, 0, 10) # Camera Extrinsic Parameter 2 xEul_C2_to_OBJ = 0 yEul_C2_to_OBJ = 45 zEul_C2_to_OBJ = 0 ROBJ_to_2 = RMatFromEulerAngle(xEul_C2_to_OBJ, yEul_C2_to_OBJ, zEul_C2_to_OBJ) C2_to_OBJ_in_C2 = (0, 0, 10) pntRow = 10 pntCol = 10 depth = 0.15 if __name__ == "__main__": print("Fundamental Mat Test") # Create Curve Surface in 3D World pntsInObj = np.zeros((0, 3)) for i in range(pntCol + 1): for j in range(pntRow + 1): x = (j - pntCol / 2) y = (i - pntRow / 2) z = depth * x**2 a = np.array([[x, y, z]]) pntsInObj = np.append(pntsInObj, a, axis=0) # Calculate Translation, Rotation for Each Camera. rvec0, jac = cv2.Rodrigues(ROBJ_to_0) tvec0 = np.eye(3) * np.matrix(C0_to_OBJ_in_C0).T rvec1, jac = cv2.Rodrigues(ROBJ_to_1) tvec1 = np.eye(3) * np.matrix(C1_to_OBJ_in_C1).T rvec2, jac = cv2.Rodrigues(ROBJ_to_2) tvec2 = np.eye(3) * np.matrix(C2_to_OBJ_in_C2).T # Convert Points on 3D Curved Surface for Each Cam Image Points. imgPnts0, jac = cv2.projectPoints(pntsInObj, rvec0, tvec0, cameraMatrix, distCoeffs) imgPnts1, jac = cv2.projectPoints(pntsInObj, rvec1, tvec1, cameraMatrix, distCoeffs) imgPnts2, jac = cv2.projectPoints(pntsInObj, rvec2, tvec2, cameraMatrix, distCoeffs) # Draw Points img0 = np.full((height, width, 3), (255, 255, 255), np.uint8) img1 = np.full((height, width, 3), (255, 255, 255), np.uint8) img2 = np.full((height, width, 3), (255, 255, 255), np.uint8) for pnt in imgPnts0: cv2.circle(img0, (int(pnt[0][0]), int(pnt[0][1])), 3, (0, 0, 0), -1) for pnt in imgPnts1: cv2.circle(img1, (int(pnt[0][0]), int(pnt[0][1])), 3, (255, 0, 0), -1) for pnt in imgPnts2: cv2.circle(img2, (int(pnt[0][0]), int(pnt[0][1])), 3, (0, 0, 255), -1) # Calculate True Fundamental Matrix R1_to_2 = ROBJ_to_2 * ROBJ_to_1.T T1_in_C2 = np.matrix(C2_to_OBJ_in_C2).T - R1_to_2 * np.matrix(C1_to_OBJ_in_C1).T tMat_1_in_C2 = OuterProductOperator(T1_in_C2) Ainv = np.linalg.inv(cameraMatrix) Ftrue21 = normalize_mat(Ainv.T * tMat_1_in_C2 * R1_to_2 * Ainv) R2_to_1 = ROBJ_to_1 * ROBJ_to_2.T T2_in_C1 = np.matrix(C1_to_OBJ_in_C1).T - R2_to_1 * np.matrix(C2_to_OBJ_in_C2).T tMat_2_in_C1 = OuterProductOperator(T2_in_C1) Ainv = np.linalg.inv(cameraMatrix) Ftrue12 = normalize_mat(Ainv.T * tMat_2_in_C1 * R2_to_1 * Ainv) # Denoise Img Points noisedImgPnts1 = add_gauss_noise(imgPnts1, 0, 2) noisedImgPnts2 = add_gauss_noise(imgPnts2, 0, 2) #noisedImgPnts1 = imgPnts1 #noisedImgPnts2 = imgPnts2 # Calculate Fundamental Matrix Fls = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.LS) FlsSVD = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.LSSVD) Ftaubin = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.Taubin) FtaubinSVD = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.TaubinSVD) m1Fm2 = [] m2Fm1 = [] for i in range(imgPnts1.shape[0]): m1 = (np.matrix([imgPnts1[i][0][0], imgPnts1[i][0][1], 1.0])).T m2 = (np.matrix([imgPnts2[i][0][0], imgPnts2[i][0][1], 1.0])).T m1Fm2.append(abs((m1.T * Ftrue12 * m2)[0, 0])) m2Fm1.append(abs((m2.T * Ftrue21 * m1)[0, 0])) print() print("Confirm if true F mat really satisfies epipolar equation.") print("Max Element of m1Fm2 : ", max(m1Fm2)) print("Max Element of m2Fm1 : ", max(m2Fm1)) print() print("Accuracy check for each numerical scheme.") print("FTrue VS FLS : ", calc_dist_between_2F(Ftrue12, Fls)) print("FTrue VS FLSSVD : ", calc_dist_between_2F(Ftrue12, FlsSVD)) print("FTrue VS FTaubin : ", calc_dist_between_2F(Ftrue12, Ftaubin)) print("FTrue VS FTaubinSVD : ", calc_dist_between_2F(Ftrue12, FtaubinSVD)) # Display cv2.imshow("CAM0", cv2.resize(img0, None, fx = 0.5, fy = 0.5)) cv2.imshow("CAM1", cv2.resize(img1, None, fx = 0.5, fy = 0.5)) cv2.imshow("CAM2", cv2.resize(img2, None, fx = 0.5, fy = 0.5)) cv2.waitKey(0)
fund_mat.py
# -*- coding: utf-8 -*- import numpy as np import scipy.linalg as splinalg from enum import Enum def normalize_mat(F): sumMat = 0 for j in range(F.shape[0]): for i in range(F.shape[1]): sumMat = sumMat + F[j, i]**2 return F / sumMat**0.5 def create_cov_mat(dataVec1, dataVec2, f0): x1 = dataVec1[0, 0] y1 = dataVec1[1, 0] x2 = dataVec2[0, 0] y2 = dataVec2[1, 0] cov = np.matrix(np.zeros((9, 9))) cov[0, 0] = x1**2 + x2**2 cov[0, 1] = x2 * y2 cov[0, 2] = f0 * x2 cov[0, 3] = x1 * y1 cov[0, 6] = f0 * x1 cov[1, 0] = x2 * y2 cov[1, 1] = x1**2 + y2**2 cov[1, 2] = f0 * y2 cov[1, 4] = x1 * y1 cov[1, 7] = f0 * x1 cov[2, 0] = f0 * x2 cov[2, 1] = f0 * y2 cov[2, 2] = f0**2 cov[3, 0] = x1 * y1 cov[3, 3] = y1**2 + x2**2 cov[3, 4] = x2 * y2 cov[3, 5] = f0 * x2 cov[3, 6] = f0 * y1 cov[4, 1] = x1 * y1 cov[4, 3] = x2 * y2 cov[4, 4] = y1**2 + y2**2 cov[4, 5] = f0 * y2 cov[4, 7] = f0 * y1 cov[5, 3] = f0 * x2 cov[5, 4] = f0 * y2 cov[5, 5] = f0**2 cov[6, 0] = f0 * x1 cov[6, 3] = f0 * y1 cov[6, 6] = f0**2 cov[7, 1] = f0 * x1 cov[7, 4] = f0 * y1 cov[7, 7] = f0**2 return cov def create_zeta_vec(dataVec1, dataVec2, f0): x1 = dataVec1[0, 0] y1 = dataVec1[1, 0] x2 = dataVec2[0, 0] y2 = dataVec2[1, 0] zeta = np.matrix(np.zeros(9)).T zeta[0, 0] = x1*x2 zeta[1, 0] = x1*y2 zeta[2, 0] = f0*x1 zeta[3, 0] = y1*x2 zeta[4, 0] = y1*y2 zeta[5, 0] = f0*y1 zeta[6, 0] = f0*x2 zeta[7, 0] = f0*y2 zeta[8, 0] = f0**2 return zeta class FundMatCalcMode(Enum): LS = 0 Taubin = 1 LSSVD = 2 TaubinSVD = 3 def calc_fund_mat(imgPnts1, imgPnts2, calcMode): # For numeeical stability. f1 = max(max(imgPnts1[:, 0, 0]), max(imgPnts1[:, 0, 1])) f2 = max(max(imgPnts2[:, 0, 0]), max(imgPnts2[:, 0, 1])) f0 = max(f1, f2) f0 = 1 # Calculation via LS. if (calcMode == FundMatCalcMode.LS): F = calc_fmat_by_LS(imgPnts1[:, 0], imgPnts2[:, 0], f0) elif (calcMode == FundMatCalcMode.Taubin): F = calc_fmat_by_Taubin(imgPnts1[:, 0], imgPnts2[:, 0], f0) elif (calcMode == FundMatCalcMode.LSSVD): Finit = calc_fmat_by_LS(imgPnts1[:, 0], imgPnts2[:, 0], f0) F = calc_fmat_by_SVD(Finit) elif (calcMode == FundMatCalcMode.TaubinSVD): Finit = calc_fmat_by_Taubin(imgPnts1[:, 0], imgPnts2[:, 0], f0) F = calc_fmat_by_SVD(Finit) else: raise Exception("Un-Supported Calc Mode!") return F def create_M_mat(imgPnts1, imgPnts2, f0): M = np.matrix(np.zeros((9, 9))) for i in range(imgPnts1.shape[0]): zeta = create_zeta_vec(np.matrix(imgPnts1[i, :]).T, np.matrix(imgPnts2[i, :]).T, f0) M = M + zeta * zeta.T return M / imgPnts1.shape[0] def create_N_mat(imgPnts1, imgPnts2, f0): N = np.matrix(np.zeros((9, 9))) for i in range(imgPnts1.shape[0]): cov = create_cov_mat(np.matrix(imgPnts1[i, :]).T, np.matrix(imgPnts2[i, :]).T, f0) N = N + cov return N / imgPnts1.shape[0] def calc_fmat_by_LS(imgPnts1, imgPnts2, f0): M = create_M_mat(imgPnts1, imgPnts2, f0) lamdas, v = np.linalg.eigh(M) #print(v[:, np.argmin(lamdas)]) return normalize_mat(np.reshape(v[:, np.argmin(lamdas)], (3, 3))) def calc_fmat_by_Taubin(imgPnts1, imgPnts2, f0): M = create_M_mat(imgPnts1, imgPnts2, f0) N = create_N_mat(imgPnts1, imgPnts2, f0) lamdas, v = splinalg.eig(N, M) return normalize_mat(np.reshape(v[:, np.argmax(np.absolute(lamdas))], (3, 3))) def calc_fmat_by_SVD(F): u, s, v = np.linalg.svd(F) ss = (s[0]**2 + s[1]**2)**0.5 Fsvd = u * np.matrix([[s[0]/ss, 0, 0], [0, s[1]/ss, 0], [0, 0, 0]]) * v return normalize_mat(Fsvd) def calc_dist_between_2F(F1, F2): dist1 = 0 dist2 = 0 for j in range(F1.shape[0]): for i in range(F1.shape[1]): dist1 = dist1 + (F1[j, i] - F2[j, i])**2 dist2 = dist2 + (F1[j, i] + F2[j, i])**2 dist1 = dist1**0.5 dist2 = dist2**0.5 return dist1 if dist1 < dist2 else dist2
geometry_k.rotation.py
# Python System Package import numpy as np import math def importCheck(): print("Geometry Imported!") def RMatFromFixedAngle(xDeg, yDeg, zDeg): xRad = xDeg / 180 * math.pi yRad = yDeg / 180 * math.pi zRad = zDeg / 180 * math.pi Rx = np.matrix([[1, 0, 0], \ [0, math.cos(xRad), -math.sin(xRad)], \ [0, math.sin(xRad), math.cos(xRad)]]) Ry = np.matrix([[ math.cos(yRad), 0, math.sin(yRad)], \ [0, 1, 0], \ [-math.sin(yRad), 0, math.cos(yRad)]]) Rz = np.matrix([[ math.cos(zRad), -math.sin(zRad), 0], \ [ math.sin(zRad), math.cos(zRad), 0], \ [0, 0, 1]]) return Rz * Ry * Rx def RMatFromEulerAngle(xDeg, yDeg, zDeg): xRad = xDeg / 180 * math.pi yRad = yDeg / 180 * math.pi zRad = zDeg / 180 * math.pi Rx = np.matrix([[1, 0, 0], \ [0, math.cos(xRad), -math.sin(xRad)], \ [0, math.sin(xRad), math.cos(xRad)]]) Ry = np.matrix([[ math.cos(yRad), 0, math.sin(yRad)], \ [0, 1, 0], \ [-math.sin(yRad), 0, math.cos(yRad)]]) Rz = np.matrix([[ math.cos(zRad), -math.sin(zRad), 0], \ [ math.sin(zRad), math.cos(zRad), 0], \ [0, 0, 1]]) return Rx * Ry * Rz def OuterProductOperator(t): T = np.matrix([[0, -t[2, 0], t[1, 0]], [t[2, 0], 0, -t[0, 0]], [-t[1, 0], t[0, 0], 0]]) return T def TMatFromRt(R, t): T = np.zeros((4, 4)) T[0:3, 0:3] = R.A T[0:3, 3:4] = t.A T[3, :] = np.array([0, 0, 0, 1]) return np.matrix(T) def ExtParamsFromRt(R, t): E = np.zeros((3, 4)) E[0:3, 0:3] = R.A E[0:3, 3:4] = t.A return np.matrix(E) def InnerParamMat(f, Cu, Cv, skewU, skewV): A = np.matrix([[f/skewU, 0, Cu], [0, f/skewV, Cv], [0, 0, 1]]) return A
data_manipulation.noise.py
# -*- coding: utf-8 -*- import numpy as np def add_gauss_noise(data, avg, stdv, absmax=0): noise = np.random.normal(avg, stdv, data.shape) if absmax != 0: noise = np.clip(noise, -(absmax + avg), absmax + avg) dataWError = data + noise return dataWError