雙目標定

流程圖

直接上代碼

說明：博主Python剛學，對內置函數使用不習慣，代碼很多冗余，參考參考得了

TXT文件保存的是給出的左右相機的坐標點以及真實的三維點（用來計算誤差）

import numpy as np import math ''' x33, y33, z33為所有實際的點 ''' x3, y3, z3 = [], [], [] with open("data_3.txt") as f:for line in f:tmp3 = line.split()if tmp3: # 防止文件空行x3.append(float(tmp3[0]))y3.append(float(tmp3[1]))z3.append(float(tmp3[2]))x33, y33, z33 = [], [], [] with open("data_actual.txt") as f:for line in f:tmp33 = line.split()if tmp33: # 防止文件空行x33.append(float(tmp33[0]))y33.append(float(tmp33[1]))z33.append(float(tmp33[2])) #二維 xl2, yl2 = [], [] with open("data_2_left.txt") as f:for line in f:tmp2 = line.split()if tmp2:xl2.append(float(tmp2[0]))yl2.append(float(tmp2[1]))xr2, yr2 = [], [] with open("data_2_right.txt") as f:for line in f:tmp2 = line.split()if tmp2:xr2.append(float(tmp2[0]))yr2.append(float(tmp2[1]))#表示矩陣K k = np.zeros((len(x3)*2, 11)) for i in range(len(x3)):k[2 * i][0], k[2 * i][1], k[2 * i][2], k[2 * i][3] = x3[i], y3[i], z3[i], 1k[2 * i][4], k[2 * i][5], k[2 * i][6], k[2 * i][7] = 0, 0, 0, 0k[2 * i][8], k[2 * i][9], k[2 * i][10] = -xl2[i]*x3[i], -xl2[i]*y3[i], -xl2[i]*z3[i]k[2 * i + 1][0], k[2 * i + 1][1], k[2 * i + 1][2], k[2 * i + 1][3] = 0, 0, 0, 0k[2 * i + 1][4], k[2 * i + 1][5], k[2 * i + 1][6], k[2 * i + 1][7] = x3[i], y3[i], z3[i], 1k[2 * i + 1][8], k[2 * i + 1][9], k[2 * i + 1][10] = -yl2[i] * x3[i], -yl2[i] * y3[i], -yl2[i] * z3[i]U = np.zeros((len(x3)*2, 1)) for i in range(len(x3)):U[2 * i] = xl2[i]U[2 * i + 1] = yl2[i]m = np.matmul(np.matmul(np.linalg.inv(np.matmul(k.T, k)), k.T), U)list_m = [] for i in range(11):list_m.append(m[i][0])list_m.append(1) m_1 = np.array(list_m).reshape(3, 4) print("左射影矩陣") print(m_1) print("\n")k = np.zeros((len(x3) * 2, 11)) for i in range(len(x3)):k[2 * i][0], k[2 * i][1], k[2 * i][2], k[2 * i][3] = x3[i], y3[i], z3[i], 1k[2 * i][4], k[2 * i][5], k[2 * i][6], k[2 * i][7] = 0, 0, 0, 0k[2 * i][8], k[2 * i][9], k[2 * i][10] = -xr2[i]*x3[i], -xr2[i]*y3[i], -xr2[i]*z3[i]k[2 * i + 1][0], k[2 * i + 1][1], k[2 * i + 1][2], k[2 * i + 1][3] = 0, 0, 0, 0k[2 * i + 1][4], k[2 * i + 1][5], k[2 * i + 1][6], k[2 * i + 1][7] = x3[i], y3[i], z3[i], 1k[2 * i + 1][8], k[2 * i + 1][9], k[2 * i + 1][10] = -yr2[i] * x3[i], -yr2[i] * y3[i], -yr2[i] * z3[i]U = np.zeros((len(x3)*2, 1)) for i in range(len(x3)):U[2 * i] = xr2[i]U[2 * i + 1] = yr2[i]m = np.matmul(np.matmul(np.linalg.inv(np.matmul(k.T, k)), k.T), U)list_m = [] for i in range(11):list_m.append(m[i][0])list_m.append(1) m_2 = np.array(list_m).reshape(3, 4) print("右射影矩陣") print(m_2) print("\n")xtl, ytl = [], [] with open("test_left.txt") as f:for line in f:tmp2 = line.split()if tmp2:xtl.append(float(tmp2[0]))ytl.append(float(tmp2[1]))xtr, ytr = [], [] with open("test_right.txt") as f:for line in f:tmp2 = line.split()if tmp2:xtr.append(float(tmp2[0]))ytr.append(float(tmp2[1]))x_sub=0.0 y_sub=0.0 z_sub=0.0 r_sub=0.0 x_sub_max=0.0 y_sub_max=0.0 z_sub_max=0.0 r_sub_max=0.0 re_x,re_y,re_z,R=[], [], [],[] for i in range(28):A=np.array([[xtl[i]*m_1[2][0]-m_1[0][0],xtl[i]*m_1[2][1]-m_1[0][1],xtl[i]*m_1[2][2]-m_1[0][2]],[ytl[i]*m_1[2][0]-m_1[1][0],ytl[i]*m_1[2][1]-m_1[1][1],ytl[i]*m_1[2][2]-m_1[1][2]],[xtr[i] * m_2[2][0] - m_2[0][0], xtr[i] * m_2[2][1] - m_2[0][1], xtr[i] * m_2[2][2] - m_2[0][2]],[ytr[i] * m_2[2][0] - m_2[1][0], ytr[i] * m_2[2][1] - m_2[1][1], ytr[i] * m_2[2][2] - m_2[1][2]],])A_inv=np.linalg.pinv(A)B=np.array([[m_1[0][3]-xtl[i]*m_1[2][3]],[m_1[1][3]-ytl[i]*m_1[2][3]],[m_2[0][3]-xtr[i]*m_2[2][3]],[m_2[1][3]-ytr[i]*m_2[2][3]],])result=np.dot(A_inv,B)print(i+1,end=' ')print("預測x:{:.3f}".format(result[0][0]),end=' ')print("預測y:{:.3f}".format(result[1][0]),end=' ')print("預測z:{:.3f}".format(result[2][0]))#print(result[0][0],result[1][0],result[2][0])print("\n")re_x.append(float(result[0][0]-x33[i]))re_y.append(float(result[1][0]-y33[i]))re_z.append(float(result[2][0]-z33[i]))R.append(float(math.sqrt(result[0][0]*result[0][0]+result[1][0]*result[1][0]+result[2][0]*result[2][0]))-float(math.sqrt(x33[i]*x33[i]+y33[i]*y33[i]+z33[i]*z33[i])))if x_sub_max<abs(result[0][0]-x33[i]):x_sub_max=abs(result[0][0]-x33[i])if y_sub_max<abs(result[1][0]-y33[i]):y_sub_max=abs(result[1][0]-y33[i])if z_sub_max<abs(result[2][0]-z33[i]):z_sub_max=abs(result[2][0]-z33[i])if r_sub_max<abs(R[i]):r_sub_max=abs(R[i])x_sub=abs(result[0][0]-x33[i])+x_suby_sub=abs(result[1][0]-y33[i])+y_subz_sub=abs(result[2][0]-z33[i])+z_subr_sub=abs(R[i])+r_sub x_sub=x_sub/28 y_sub=y_sub/28 z_sub=z_sub/28 r_sub=r_sub/28 print("x的平均誤差為：",x_sub) print("y的平均誤差為：",y_sub) print("z的平均誤差為：",z_sub) print("r的平均誤差為：",r_sub) print("x的最大誤差為：",x_sub_max) print("y的最大誤差為：",y_sub_max) print("z的最大誤差為：",z_sub_max) print("r的最大誤差為：",r_sub_max)x_dou,y_dou,z_dou,r_dou=0,0,0,0 for i in range(28):x_dou=(re_x[i])*(re_x[i])+x_douy_dou=(re_y[i])*(re_y[i])+y_douz_dou=(re_z[i])*(re_z[i])+z_dour_dou=(R[i])*(R[i])+r_dou x_dou=x_dou/28 y_dou=y_dou/28 z_dou=z_dou/28 r_dou=r_dou/28 print("x的均方誤差為：",x_dou) print("y的均方誤差為：",y_dou) print("z的均方誤差為：",z_dou) print("r的均方誤差為：",r_dou)

總結

以上是生活随笔為你收集整理的由左右相机图像点重建三维点的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。