From 33e1954f102bdbf2cd227cfae480ea8c8b247886 Mon Sep 17 00:00:00 2001 From: Paul597 <2454344015@qq.com> Date: Wed, 9 Jul 2025 15:55:01 +0800 Subject: [PATCH] 7.9 plus Calibration --- Visual measurement-straight/.idea/.gitignore | 8 + .../.idea/Visual measurement-straight.iml | 8 + .../inspectionProfiles/Project_Default.xml | 10 + .../inspectionProfiles/profiles_settings.xml | 6 + Visual measurement-straight/.idea/misc.xml | 7 + Visual measurement-straight/.idea/modules.xml | 8 + Visual measurement-straight/py/Calibration.py | 143 +++++++++++++ .../calc_slope_line.cpython-312.pyc | Bin 0 -> 4052 bytes .../py/__pycache__/calc_way.cpython-312.pyc | Bin 0 -> 11166 bytes .../py/__pycache__/get_data.cpython-312.pyc | Bin 0 -> 7426 bytes .../py/__pycache__/model.cpython-312.pyc | Bin 0 -> 1373 bytes .../py/calc_slope_line.py | 74 +++++++ Visual measurement-straight/py/calc_way.py | 198 ++++++++++++++++++ Visual measurement-straight/py/get_data.py | 150 +++++++++++++ Visual measurement-straight/py/measure_lib.py | 45 ++++ Visual measurement-straight/py/model.py | 25 +++ 16 files changed, 682 insertions(+) create mode 100644 Visual measurement-straight/.idea/.gitignore create mode 100644 Visual measurement-straight/.idea/Visual measurement-straight.iml create mode 100644 Visual measurement-straight/.idea/inspectionProfiles/Project_Default.xml create mode 100644 Visual measurement-straight/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 Visual measurement-straight/.idea/misc.xml create mode 100644 Visual measurement-straight/.idea/modules.xml create mode 100644 Visual measurement-straight/py/Calibration.py create mode 100644 Visual measurement-straight/py/__pycache__/calc_slope_line.cpython-312.pyc create mode 100644 Visual measurement-straight/py/__pycache__/calc_way.cpython-312.pyc create mode 100644 Visual measurement-straight/py/__pycache__/get_data.cpython-312.pyc create mode 100644 Visual measurement-straight/py/__pycache__/model.cpython-312.pyc create mode 100644 Visual measurement-straight/py/calc_slope_line.py create mode 100644 Visual measurement-straight/py/calc_way.py create mode 100644 Visual measurement-straight/py/get_data.py create mode 100644 Visual measurement-straight/py/measure_lib.py create mode 100644 Visual measurement-straight/py/model.py diff --git a/Visual measurement-straight/.idea/.gitignore b/Visual measurement-straight/.idea/.gitignore new file mode 100644 index 0000000..35410ca --- /dev/null +++ b/Visual measurement-straight/.idea/.gitignore @@ -0,0 +1,8 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml +# 基于编辑器的 HTTP 客户端请求 +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/Visual measurement-straight/.idea/Visual measurement-straight.iml b/Visual measurement-straight/.idea/Visual measurement-straight.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/Visual measurement-straight/.idea/Visual measurement-straight.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Visual measurement-straight/.idea/inspectionProfiles/Project_Default.xml b/Visual measurement-straight/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..72ad4ef --- /dev/null +++ b/Visual measurement-straight/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,10 @@ + + + + \ No newline at end of file diff --git a/Visual measurement-straight/.idea/inspectionProfiles/profiles_settings.xml b/Visual measurement-straight/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Visual measurement-straight/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Visual measurement-straight/.idea/misc.xml b/Visual measurement-straight/.idea/misc.xml new file mode 100644 index 0000000..1314114 --- /dev/null +++ b/Visual measurement-straight/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/Visual measurement-straight/.idea/modules.xml b/Visual measurement-straight/.idea/modules.xml new file mode 100644 index 0000000..02389d2 --- /dev/null +++ b/Visual measurement-straight/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Visual measurement-straight/py/Calibration.py b/Visual measurement-straight/py/Calibration.py new file mode 100644 index 0000000..639982f --- /dev/null +++ b/Visual measurement-straight/py/Calibration.py @@ -0,0 +1,143 @@ +import cv2 +import numpy as np +import glob +from scipy.optimize import minimize +import model +import math +import calc_way + +def needs_correction(dist_coeffs, error_threshold=0.5): + k1, k2, p1, p2, k3 = dist_coeffs.ravel() + if (abs(k1) > 0.1 or abs(k2) > 0.01 or abs(p1) > 0.005 or + abs(p2) > 0.005 or abs(k3) > 0.01): + return True + return False + +def calibrate(image_fold, columns, rows, size): + # 设置棋盘格参数 + chessboard_size = (columns, rows) # 内部角点数量 (columns, rows) + square_size = size # 棋盘格方块实际大小（单位：毫米/厘米/英寸等） + + # 准备对象点 (0,0,0), (1,0,0), (2,0,0) ..., (8,5,0) + objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32) + objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2) * square_size + + # 存储对象点和图像点的数组 + objpoints = [] # 3D点（真实世界坐标） + imgpoints = [] # 2D点（图像坐标） + + # 获取标定图像 + images = glob.glob(image_fold) + # print("找到的图像文件:", images) + + for fname in images: + img = cv2.imread(fname) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + + # 查找棋盘格角点 + ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None) + + # 如果找到，添加对象点和图像点 + if ret: + objpoints.append(objp) + + # 亚像素级精确化 + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + imgpoints.append(corners2) + + # 绘制并显示角点 + cv2.drawChessboardCorners(img, chessboard_size, corners2, ret) + cv2.imshow('Corners', img) + cv2.waitKey(500) + + cv2.destroyAllWindows() + + # 相机标定 + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + objpoints, imgpoints, gray.shape[::-1], None, None) + + # 输出标定结果 + print("相机内参矩阵（K矩阵）:\n", camera_matrix) + print("\n畸变系数（k1, k2, p1, p2, k3）:\n", dist_coeffs) + print("\n重投影误差:", ret) + + if needs_correction(dist_coeffs): + print("需要矫正：畸变系数过大") + else: + print("无需矫正：畸变可忽略") + return camera_matrix, dist_coeffs, rvecs, tvecs + +def find_corners(image_path, columns, rows): + # 读取图像 + image = cv2.imread(image_path) + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + + # 定义棋盘格尺寸 (内角点数量，非方格数) + pattern_size = (columns, rows) # 例如8x8的棋盘有7x7内角点 + + # 查找棋盘格角点 + ret, corners = cv2.findChessboardCorners(gray, pattern_size, None) + + if ret: + # 提高角点检测精度 + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) + + # 绘制检测结果 + cv2.drawChessboardCorners(image, pattern_size, corners, ret) + cv2.imshow('Chessboard Corners', image) + cv2.waitKey(0) + cv2.destroyAllWindows() + else: + print("未检测到棋盘格") + + print(corners) + # 将形状从 (N,1,2) 转换为 (N,2) + corners_reshaped = corners.reshape(-1, 2) + # print("所有角点坐标:\n", corners_reshaped) + fixed_value = 960 # 固定值 + column_index = 1 # 操作第2列（索引从0开始） + + # 方法：固定值 - 列值 + corners_reshaped[:, column_index] = fixed_value - corners_reshaped[:, column_index] + # print(corners_reshaped) + return corners_reshaped + +def fun_test(x,cls,corners): + print("标定开始") + # print(corners) + error = 0 + model = cls() + f = model.f + H = model.H - 9 + gamma = math.atan(1 / (math.tan(x[0]) * math.tan(x[1]))) + seta = math.atan(1 / math.sqrt(pow(math.tan(x[1]), 2) + 1 / pow(math.tan(x[0]), 2))) + column = 0 + for index, value in enumerate(corners): + if index % 11 == 10: + column += 1 + index += 1 + continue + Xw, Yw = calc_way.calc_distance(value[0], value[1], x[0], x[1]) + Xw1, Yw1 = calc_way.calc_distance(corners[index+1][0], corners[index+1][1], x[0], x[1]) + d2 = math.sqrt((Xw1 - Xw) ** 2 + (Yw1 - Yw) ** 2) + error = error + abs(d2 - 60) + return error/80 + +def get_result_test(cls,corners): + params = cls,corners + bounds = [(0.1, 1.7), (0.1, 1.7)] + result = minimize( + fun_test, + x0=[0.7, 0.9], + args=params, + # method='Nelder-Mead', # 或 'trust-constr' + method='L-BFGS-B', bounds=bounds, + tol=1e-12, # 高精度容差 + options={'gtol': 1e-12, 'maxiter': 1000} + ) + return result + + + diff --git a/Visual measurement-straight/py/__pycache__/calc_slope_line.cpython-312.pyc b/Visual measurement-straight/py/__pycache__/calc_slope_line.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b02b4ae8b95ee2bce2e0a39a522f8c837b7a4056 GIT binary patch literal 4052 zcmeHKU1$_n6ux(6c4u~Hwn;Q5rdpHu+iB67*dJ}xnl+(`rHc3$!?t1E8O`eMX1y~p zCd`%<3f;v*HXtOT;EIBV2u3K>M@xkYeQ~-EhS7r1C*Oj3>r>C2{mCp{O$u$Fdf}dX z?wxb*Irp5q-#PnBAmB&PIzoK|>wO6QPA8?t6q)%PFcU~cDvqMQ!if8@GiUl37?~&= zCz=j3oxqK_1%3+QN~2m#r=!O z;gZ~_2cc0BmG>n%*UrT(@VZPZvIc~z&|K$Kv$Sl1%1jsa!0nd0yKL{mZhDJRP+h*6 z`&J$UW`$NQ$R=a5CNlAK(b5oDl=LQf)9boLl}BK5Wo_U`lXpqimh>%*mC~c|7?VPU zhDG;OXjs%9>W!q&^oht6sCQh}Rf*hUZ_6lOgTs zs=P4MU)rA(U6NJa(2^_?-4-P!Q#ypE*!?f)&*TNwF8ggRrWgY@rx;4yW+~zHv&693 z#Ne(QyijB|Pc*ORc zQerV!(&-Y@J8Uz@hiyD$<8!tEheVZ_s@R^wfY0awu%^Y`{z&ZoICMnUh#u-tW05$h zCW5^afTiatcNm=-rSQ?baY0pqpb5+fn!LV9#Cq4h^2 zacxV2nyeZ9XGjZ}QUhK85d4%v&!pP1o=i_xx%&RK?U%Rbx~_bB=kVR$sooiN`YUVI zCsxDGytFIT^-Qc8+mYFkJ$SYLTGQpGo7-+0x6WTbKf~Sldgg3?-H!Vm57~#EKX?Dw z{piR~2Ob|en%{HG+Ih@ccP!u3Ypv+bi^o%)&!qY%(ppPen`_G*%njW>oR``!bftnR zGAsDMJ3Q8#>CI{v`z>MhB)QpntNVKQ?ISl1Sk2okVOy$WNiwx{X_9Tt7?(OHdd7Pu zm7DdqR$gCuXZ_txQ=4X5r{tN=sV(_dFxP6?Wv$tjuitIe>`rx;+%S-rn&Gm7G$v$( zOZ5|toQr+|5K2#m)xBidx|02?xj0-iYB>YMgHd&W+mGoaJ z?O?XILc^kaDl{x=kAvB~$pdD4|541Yk}duQW(zJaRl{tdjM+YyscGt6zT1B>ls9Zcv>ea(N5i%UYbr68n(ejS-}SGjAx%f4J|0i?g@ ze&Fx%_gn9Y_f>0aFu!T`XFAHpwMDNq_+OS<|ze zwg3_*;Syv6EXpjQYHUAbR-&CEnhw>1azdB+h0DR;0h)YmUnFj5B&;P2n8fFf~&U`hIX6jXtS9h^u5qKU>=0Nx7`+1Ej|c!6RnvRyk7kmUUxDQ zSNokn=vvsI&fE#Qc0t$UK;U)NHC))23Z|KK(8bPq(ef2pGQMdtm|J%FGi%wp)aTFn zWm)A{zVUg@^5=n??Apw+ESU_B4_kpXxpysTa~jXSV!5_1+m=3;4mhEZ0}k5Rp4gzO zMF}kr;l+d_iIKw7@_gZ}FW_7YNH`GX{4kuy;9SD-nsa2~;Rrk7#G$CRo9uxdv=Y|8 z1Tx2BjGrR$DUzO|n%_|4b76VfGbU!li-DA9RtQiQ7+(5z^ulP0o8vglz4ZHV;AMk= Z19SDxlmX6P8h7vm5_xE(J~-j6N~Qj1)-!W# zpLoqflqzDRJ!j74Kj%OHxqRP$X8+>xI4O84{~f$ES4L4!P(pt!^Dj@g(G+!^3Q|G3 zlWNPxbely>TidJ_DrCLP>Oy9GXa2R#3gucaW*K^Gvq8RlVV;(6hkTxp?|^(CA)kf( zd_q14`GtghC*&6s@?DT$O2~I}RM7Hks?7tj6=EJ@24XM7Hi&&eJA`?VazLCPWFagV zp#9uesIlW64Av$ZuN8hz--g`llthU};UL&Z)NM`;+9)c!XZuqOp20 zy$CB;Vssf~8LTd8tj3B6S_Wa)O15l_Yk{>u*bfpXIq=&0Y{Z;IeMxp)+yaa`=E8a< z%%Rz~#Qcz>lw1qu$c<@Du(sr!x3X)gm17>sgGsN_*Pt0otmIy7=@>70;1n>|DQSjC za$bp-ycaB|s0r)wuh5qN3awy2uUh(EQZ2!L)$%rq2UMmjAYM|LXs;lu^aYg(h=GV| z)qUFmN$;6j!;iD+}Ryb=^#j41&OOsv|`E^J^u+GdbPbJ8WN)IwZX1% zBpelmfY>dx*N384#O|KMGuk`ya(Y|FGWN>I0fnoA5+^_GPx@1J<9Q>mDbCgLL$hw*@R8(^ zRA79^`@6<=We)%Lz-04>trM+N!8>ostM|yodlmP-_~BWXcX)SlcdBVrygo28aI@fM z^R3n!t+#_8yfxLNRJ?Mp;^U478$PSPU;XgqPhXrq)vWAqkzZ|*D_WG2({j;i#dRiL zKg&6XnupIO&!&WHZ_3=N@!62r{H+S6HePj1z(^SR8 z>8bPgYVTf@x9(G__CF~6tn_~A!wu6-&41pa)V9b~EpplE>9%&cv|Zufj5o}(?qM#; zr7F|($mVoiredr?&aYC~4e{C;qu)^a;z+mR*(7tD!0Jv{B7Z12d?|S;)tA0H@=Jx^ zEOVRDba)a%VrTr=Qyi{u z1+1LVw&>97V)Y30NixBPHo(TRVrdg>4C!UT#)fH=Yg8m{jsCE9NcD_S>9C7?E<=_TO#I=9FiSTq8KZ&CW#^uN;G=_)4JIyVjV<=fLi6KrS*sDNdPJdUmqm! zWAzau3+V3v_U7nwwMrnn61i0;)+%9EF;L}jSOap1M(V<(NptDawHFOig2ohcOL)Je zJz&(I-)HWn`%D9Q(jX)?z!Fm*+;R?tO0{DTHfDlhW(?o=0VI!2N z`~sldCTxXFKN9&p%v+pV=3qBKZM^XPvazz8+i!}u`fv14F(3S5s#mGneXsU`^+Ek- zjrSWLwtV`=bjxYw;2C-E8M*3=Qr0RLwJNT&0BS2l&@Z9h(d5yzB^6EADem?0!;krb zR4CaMZ+Pr21`SHaN@Q<&rfIV9!;*=T8$Xx%nvd%L&~Udw=JxTz1Mb*Zcp|8W@lpmG*c#QhPmo6{7Fa{ z-s!jCqvrHZ?K!gTxSPmaga$s%1=F&GH9IPf+CLF8_ zd_&T4ZTMDLJEVFxy+=)RTmvU2nM(tbl~(Fs8XB1X@O7ilM$Aprmo$@$TY$L)$gt+PiQzH5mK_VGyNZj)p#~QBxWi zA+XBAmIhGekW{T_duqV*?5$B5a3gC}OWV~b>TQv`2+1r&xipBn6hc0r@CsCHfM2vr z!>1Xi7mmg)^3O($B9=M=A=9E}S0RhX&s5!|MbWF0k7SQ*aAQyAe zRD@XiuOc zr1GKOzJM6+jzrH5h}|!#_CQZhC=yiJKxb!1XLmF-;a0702Rc~s%fV1k+(EqeOy?Y;2srjNYu}I^M>C^zLVabVQ$y_!@G56 zO+||Pr>|(vK^1NMhNav^GwwBCxT|D$Rp!v=?&`Tb%2zg*PjQGVvd(zz6YqLxkL8Bh zB%3NoHH{Sioh_Zg)1R}Y(3-D&4qEdh0*M{**Rwl&e%F98FcSGY0Fj&|(wdV*G8h;> z3$QZTi((w)V)*A060kDg0jxPbjpQPcqT#*j`wSZs0h~=3*X!#vMo-6ZYE2l)0*y#k zL*AkGJgX)qJ%m11+RBpgo2@KMGFj74$Mi%zVbNpWmLW@FbxC7iOfs2eNv3fRV0H-? z^LBh)>9YgQhCxX&9SAeLqyzXPgwgG}D;?_7M!Mlb^!ah_51#$RAC+9is_wse(rtx*+ zqZNIfm|XgG;;+xYPPqA0L9#K`o7^|els>1#KWa)W?1oM0w+Y|_P{LjaaFW{(Bv;sv zN)DjlAOtI5o|@E2s5^>;eue_=2JbofGNFI~wa>{HtQ76hA^SP`T14NTlP~J>_fNh& z1o9;uhm|S30l|0koF-HS1@}<3=RD{C{$ChwJ`qj6mTF4wnP!SLjPe(1A3RlCq_3-I zKm$e9*3}&hb*i>wD9TcB?UGr$Ro1SNwToxuQeYRw=-)Wnhequ-SG$PG5@7_4+A5jI z!eaL3hTRLj^mT^z3$0L&E+C^fAegh#H2sKjJ)!cSQ0~WULBcldO1iFj;#!cw0Ej!>?+v~?7-!}z49$Gw^w9Zp>$JBbdez)E?Tx4T axpM8TlJU2GFq7M`)kj_n!Sah#tfB#zUNA43QPNZJI_@)HVdfi`8KiwWzRj0p~oV`j#X zq~l$;>Z(bhjU(E{Y+-ApK7`~-;e7>&`m z4|C#jsdB32l5i4Gs(hLOty7DqFa^#6xlK2qcN)@lq?45EC?}=DJY+wNwfOotUEw65 zy>f9YhB-}8Z_86}rZu$s4cu9%$7tdW%xM|M9oqW{xX*!08kTc&Y_DPrs@A}*hWp`n z@HmfgI27{LpNmS@@nANs>Qh}+f1GX&sdyEQmqR;`k3bzfK_b)Cj5504?de+?@M4#y#MX3i_DAVBqMmq&engSWY4t59{GvUO(*_lC-DX zzMzM75Rx`9==F1~r0@0kd@j~I;*nJJF{yxc4+MN3nk7IP6PZGqvQKG2N@ z<(mV;k{Ma%q6bg=Q(7=3po9@__E+!-V{>|PymG8EaxBJ9Zjbd(>s!LxlEtMFc4B9A zSS)Uw+$~yL!`s3`ukBf?+Eh5+Kh_^@jO`N2*Cb5MGp3EAX=B3FHe=c*nzkiOUE!Tc zvM91WYK?S=q-}<*6UjP(sCx(+V7-|z1%`rK4&i#BmJeYixI#FO=LOL$)z^&oPR38X>bwAQ*(M79KC@K=Wup<)~7ShrLmkDuR&1(^c*5o2b zAb7JaXyI@sMRcCN3r0apkt4tgLp**x>g*Uj+$k$bn%!kfTCc`B@pN${C;HwX_DUQIK!sE8%J4tKew{t7UD|S+?+f;*w60 ziDPG16Bx40jLEjykZozYdYKro@zsisa=x&KYC=!uXk_8_;Qh=;4UR#+sD*na+;wo* z=gl29MK`6)FTbzMAXhGftji$lmO<8MkoC*NKrO#=K@2p68bgj{`qc6D3;L`I ztqwKi#7zIVkbuzzN=9~GI{2Dp8tmW(ZW|>--W9LuL zGHaL2G8ymW$ucef)tKv+95ds+JY%ls*YYiLX4}B871^I=*-anN6}Afcn6AUx(vOISRL?9S)QSCX7~xkdK?=r z2OP2#tU9KF&0od7`zVPxaK-`!11}TIdjmf9?xT%Ga1}h}FMc#1|IPfh^Yiab&Ao2Z_UNe%>VJL@2~%kS?@YJ$k{vXBZelRVG9QIeIyA!@#Jeu5N+M>UA;ON z56@q{oE9Vty*oZ}@0}@o5Pi$r=i(pTz5e>$c>Lb$pLYZa)aUij?IT2oeW-JUXtED? z2GRbb$v*$dnY*8T(mA4TUVkizUXr;Q-i^F7|K7E^FVCe^y*vIo49K|FgI2)}k76^X z9=G3>wXCoyQ@AqbG_3DmSN?Hp@6i{Z z4Wg~n{Q21Yd!wo8a%g1d?1RfuEUEkfslYerrnwGcFq??AxgrzMS>Vcf9g?REQlQ&^YJ>-R_&^mH>W z#?uEIch)=Tm()IwU(y5^5F!!mNrscuY;ZtQyN|;5-Qy1qcwk2k`iyu3j!lvdbaeUA zNdSTN)shan$vbR`lDF6C4L*ZTK^U~>lyEPoE^DjL>Zt}7qCY4>EeGHd%JAb5gPw z)F6y0>oOUUw3wa@Z%yh9;jOdAqR8eM zYn^DVi@8N><2OdfD4wjcf4CvWPi?*Y>#37sLwlmOL#*l;r6S#9<|r=eDrU*ji`Hn* zWX%QFWVcwgHc`G#B-dSgRwTFmW&h1e;o!@EtriYB#odP!J9b&Dkp2}|R&&ap(x*E+`o5;Lv(fdZXT*NEi|=sv46WErros%fUO zQ>^TayKh($mD@$4JiKGHJ<^*5^SZ|lMaxCwli}x*lqtMp(E}#n-3^hG=i1&m0)~^+ zX!qF0NZr`xs6!-cW9~`(GA)v;CcAHu%}L5KzGrMt)DmsEP&`f97VuwFwmZwTKQLmZ z6+f6Tqj`MS*sjQdGcSa9%u=SuVj9|J8g_{dyKa^UFCIuV92AL_;oYO5=&{+QTL`^C zwN0IxIv(%8S$b0^7`ms4mmcUaOHt&x2}5}IoVDcqQxi{JT>nAag|?Vu^5o^_#EO<{ z`{R~B9gMsG;1pMENm#dr_av+8Co9FOR)MIvQ)o@~Ynx~jD%K^eEmLrm*$EgBx_P2H z{Id)(i-vE!TJ&k@mC`9H;ph|^pNabg!`^9P-)vR=hwNm>)JURX+c#C)MP2zQKK9J$ zj!5gQ&KMyg-BHWLfymD2#+W90Iz~^nPBu)H$J=l8h?`y#_WxYmbU-+GNa%11ro&(B zdLF1TTNe&dZ7z&dMvS7#21lbNbB3O6?wo0UL2Q0Oc=2W7&?|{%w@9oGKOd=%IbA78U0{QZaJ++~#kLb`&HW2?mCrtscmhMFrYKYx1S^hM@_Qx{If1|Xl*Kb@#< zzrO#*ibUsL;l*yTvs)=M_FQ7^PN8|%O_N}Fd7AhI%(%uN zP)jw#!n4|h>a~Jxtyr`Ug;&*Ma{migCiRBV&11Ic##_2-$a2(Wf1J2gw;7h-sDD~l zl{7b^TzDe7X_A~eERb6SVhd9WFB=Db-;9bPNdw<=fnf$6=E6T3^zp+ouDW5 z8on7dY_As}=-&f_4zDE3fSU{W2H`X4DEvl26m(6M>VOZv+Xe<{k55t!vXW*mBAFW0 z7kwkkAF%Q}?+PYsl71a-hAu&$=NX0r5Zauu-SBvz#&P^MR&*P)-p0(|Vr93n(r>Zq z+Zc5l)8ElqM>XT*7RkjCR20@<3I!inTA_47(Mz34Hc`X}4rKoMX1;&spJe_#ZfR*E zAS>sT;S5K}TLhu-b)&Whj0#bR!Wbmaz{(n|$>lkQuw*AuxN)NJci4cP=RHOgW)SQZ z85%eBqHgM=>R=EKG*bt)6c`nv`dRa=!i@?-G85lQVVcq_z> zV#n&GkravshrPytW_`_p=I+#Yj1ei?q40n(t1pTGC~TXIkz-i%7Xfd?_z_ZKO96~F z6WApQq5r=!H2jnys{4m(apXd!hMy|^?o^F+UOeO}O|YvmLd}5u0YHzE00bQbqO<@? zL?{WUB|@!$k|FYlxMZ)$X@}<4KDd!QowM=nYTJ&yf3|X9JSTDiKL0r;Wgx1jAzXL* zks16f+S4Z}iJW$7VKz>x=W-(JM|}l0cIvCTe+G+$er!+sI(y;K&8r^19RDa%Hesme~-R!z0&_99K!hTXr$EU#eV z9qF??8kZ!U0-Nor5jM4Yf&ArTKIMF*i=zARY{^vk_4@m4YU)IbOPR25V5ns zy+!Qhu-MB`;nnjM8V(N4IH|GiLVv(PuP9KVST-%s#>uBiXHd7izLMqo|0x@P20@xG ow4&5BzBxF=7P}h)_<@}cQ1cwaFmFg|igbU8rJ4Q(0?Oa^9kuB*O#lD@ literal 0 HcmV?d00001 diff --git a/Visual measurement-straight/py/calc_slope_line.py b/Visual measurement-straight/py/calc_slope_line.py new file mode 100644 index 0000000..e97dad9 --- /dev/null +++ b/Visual measurement-straight/py/calc_slope_line.py @@ -0,0 +1,74 @@ +import math +import model +import calc_way +from scipy import stats + +model = model.Model() +alpha = model.alpha +beta = model.beta + +""" +计算地面点的竖直垂线在图像中的斜率 +参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta +返回值：斜率k +""" +def get_k(alpha,beta,x,y): + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + Xw,Yw = calc_way.calc_distance(x,y,alpha,beta) + u = Xw * math.cos(gamma) - Yw * math.sin(gamma) + v = Xw * math.sin(gamma) + Yw * math.cos(gamma) + tanlamda = -v/u/math.sqrt(1+(math.tan(math.pi/2-seta))**2) + lamda = math.atan(tanlamda) + k = math.tan(math.pi/2-lamda) + return k + +def get_k2(alpha,beta,Xw,Yw): + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + u = Xw * math.cos(gamma) - Yw * math.sin(gamma) + v = Xw * math.sin(gamma) + Yw * math.cos(gamma) + tanlamda = -v / u / math.sqrt(1 + (math.tan(math.pi / 2 - seta)) ** 2) + lamda = math.atan(tanlamda) + k = math.tan(math.pi / 2 - lamda) + return k + +""" +计算地面点的竖直垂线在图像中的截距 +参数：点在图像中的坐标x、y，斜率k +返回值：截距b +""" +def get_b(x,y,k): + b = y - x * k + return b + + +""" +将检测到的路沿上侧数据点拟合为一条直线 +参数：上侧数据点坐标x_top,y_top +返回值：拟合出直线的斜率slope、截距intercept、r方值 +""" +def linear_regression(x, y): + slope, intercept, r_value, p_value, std_err = stats.linregress(x, y) + return slope, intercept, abs(r_value) + +""" +计算图像中两条直线的交点 +参数： line1: 第一条直线的系数 (A1, B1, C1) + line2: 第二条直线的系数 (A2, B2, C2) +返回值：交点在图像中的二维坐标x、y +""" +def find_intersection(line1, line2): + A1, B1, C1 = line1 + A2, B2, C2 = line2 + + # 计算行列式 + denominator = A1 * B2 - A2 * B1 + + if denominator == 0: + # 直线平行或重合 + return None + else: + x = (B1 * C2 - B2 * C1) / denominator + y = (A2 * C1 - A1 * C2) / denominator + return (x, y) diff --git a/Visual measurement-straight/py/calc_way.py b/Visual measurement-straight/py/calc_way.py new file mode 100644 index 0000000..c5d7f4c --- /dev/null +++ b/Visual measurement-straight/py/calc_way.py @@ -0,0 +1,198 @@ +import math + +import numpy as np + +import model + +model = model.Model() +u0 = model.u0 +v0 = model.v0 +f = model.f +H = model.H +dx = model.dx +dy = model.dy +alpha = model.alpha +beta = model.beta +""" +计算图像中一点到车辆坐标的距离 +参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta +返回值：点在车辆坐标系下的二维坐标 +""" +def calc_distance(x, y, alpha, beta): + Xp = (x - u0) * dx + Yp = -(y - v0) * dy + sq = math.sqrt(f * f + Yp * Yp) + eta = math.atan(Yp / f) + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta)) + OM = H / math.tan(seta + eta) + epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM) + d = math.sqrt(MP ** 2 + OM ** 2) + Xw = d * math.cos(gamma + epsilon) + Yw = -d * math.sin(gamma + epsilon) + return Xw, Yw + +""" +计算车辆坐标系下某点在图像中的位置 +参数：点在车辆坐标系下的坐标Xw、Yw，相机的外部偏转角参数alpha、beta +返回值：点在图像中的二维坐标 +""" +def calc_distance2(Xw, Yw, alpha, beta): + d = math.sqrt(Xw ** 2 + Yw ** 2) + seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + # 计算组合角度 + angle = math.atan2(-Yw, Xw) # 使用atan2处理所有象限 + # 解出epsilon + epsilon = angle - gamma + # 将结果调整到[-pi, pi]范围内 + epsilon = (epsilon + math.pi) % (2 * math.pi) - math.pi + OM = math.fabs(d * math.cos(epsilon)) + MP = math.fabs(d * math.sin(epsilon)) + eta = math.atan(H / OM) - seta + Yp = math.tan(eta) * f + Xp = MP * math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta)/H + if epsilon > 0 : + Xp = Xp + else: + Xp = -Xp + x = Xp / dx +u0 + y = -Yp / dy +v0 + return int(x), int(y) + + + +""" +计算图像中一点距离地面的高度 +参数：地面端点在图像中的坐标x_bot、y_bot，垂线上端点在图像中的坐标x_top、y_top，相机的外部偏转角参数alpha、beta +返回值：点距离地面的高度 +""" +def calc_height(x_bot,y_bot,x_top,y_top,alpha,beta): + Xp = (x_bot - u0) * dx + Yp = -(y_bot - v0) * dy + sq = math.sqrt(f * f + Yp * Yp) + eta = math.atan(Yp / f) + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta)) + OM = H/math.tan(seta + eta) + Yp1 = -(y_top - v0) * dy + sq1 = math.sqrt(f * f + Yp1 * Yp1) + eta1 = math.atan(Yp1 / f) + # print(f"Yp1: {Yp1},eta1: {eta1}") + Zw = H - OM*math.tan(seta + eta1) + return Zw + +""" +计算图像中车辆坐标系Xw、Yw所对应的轴线 +参数： +返回值：轴线上点在图像中的二维坐标x、y +""" +def calc_zeros_yto0(val): + # 定义搜索范围和步长 + x_range = np.linspace(0, 1280, 1000) + y_range = np.linspace(0, 960, 1000) + + # 存储解 + solutions = [] + tolerance = 1e-3 # 容忍误差 + + # 网格搜索 + for x in x_range: + for y in y_range: + error = equations_yto0(x, y, val) + if error < tolerance: + solutions.append((x, y)) + + # 去除近似重复的解 + unique_solutions = [] + for sol in solutions: + # 检查是否与已找到的解近似 + is_unique = True + for usol in unique_solutions: + if np.allclose(sol, usol, atol=1): + is_unique = False + break + if is_unique: + unique_solutions.append(sol) + x = [] + y = [] + # print("找到的解:") + for sol in unique_solutions: + # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}") + x.append(sol[0]) + y.append(sol[1]) + return x, y + +def equations_yto0(x, y, val): + Xp = (x - u0) * dx + Yp = -(y - v0) * dy + sq = math.sqrt(f * f + Yp * Yp) + eta = math.atan(Yp / f) + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta)) + OM = H / math.tan(seta + eta) + epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM) + d = math.sqrt(MP ** 2 + OM ** 2) + Xw = d * math.cos(gamma + epsilon) + Yw = -d * math.sin(gamma + epsilon) + return (Yw - val)**2 # 误差平方和 + +""" +计算图像中车辆坐标系Yw=某一固定值所对应的轴线 +参数： +返回值：轴线上点在图像中的二维坐标x、y +""" +def calc_zeros_xto0(val): + # 定义搜索范围和步长 + x_range = np.linspace(0, 1280, 1000) + y_range = np.linspace(0, 960, 1000) + + # 存储解 + solutions = [] + tolerance = 1e-3 # 容忍误差 + + # 网格搜索 + for x in x_range: + for y in y_range: + error = equations_xto0(x, y, val) + if error < tolerance: + solutions.append((x, y)) + + # 去除近似重复的解 + unique_solutions = [] + for sol in solutions: + # 检查是否与已找到的解近似 + is_unique = True + for usol in unique_solutions: + if np.allclose(sol, usol, atol=1): + is_unique = False + break + if is_unique: + unique_solutions.append(sol) + x = [] + y = [] + # print("找到的解:") + for sol in unique_solutions: + # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}") + x.append(sol[0]) + y.append(sol[1]) + return x, y + +def equations_xto0(x, y, val): + Xp = (x - u0) * dx + Yp = -(y - v0) * dy + sq = math.sqrt(f * f + Yp * Yp) + eta = math.atan(Yp / f) + gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta))) + seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2))) + MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta)) + OM = H / math.tan(seta + eta) + epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM) + d = math.sqrt(MP ** 2 + OM ** 2) + Xw = d * math.cos(gamma + epsilon) + Yw = -d * math.sin(gamma + epsilon) + return (Xw-val)**2 # 误差平方和 + diff --git a/Visual measurement-straight/py/get_data.py b/Visual measurement-straight/py/get_data.py new file mode 100644 index 0000000..4f56b25 --- /dev/null +++ b/Visual measurement-straight/py/get_data.py @@ -0,0 +1,150 @@ +import numpy as np +import pandas as pd +import calc_way +from scipy import stats +import calc_slope_line +import matplotlib.pyplot as plt +import model +import os +# 数据截断线 +model = model.Model() +limit_slope = model.limit_slope +limit_intercept = model.limit_intercept +def grid_downsample(points, cell_size=15): + """网格化降采样，保持空间结构""" + df = pd.DataFrame(points, columns=['x', 'y']) + df['x_grid'] = (df['x'] // cell_size) * cell_size + df['y_grid'] = (df['y'] // cell_size) * cell_size + sampled = df.groupby(['x_grid', 'y_grid']).first().reset_index() + return sampled[['x', 'y']].values + +""" +读取yolo网络识别路沿的坐标数据,筛选出目标区域的数据点，并将路沿上下侧数据分离 +参数：保存数据的txt文件路径 +返回值：在目标区域内的下侧数据点坐标x_bot、y_bot，上侧数据点坐标x_top,y_top +""" +def get_data(txt_name): + # 加载数据 + data = np.loadtxt(txt_name) + int_data = data.astype(int) + + # 网格化降采样 + grid_sampled = grid_downsample(int_data, cell_size=20) + + # 数据截断 + x = [] + y = [] + for i in range(grid_sampled.shape[0]): + grid_sampled[i][1] = 960 - int(grid_sampled[i][1]) + if limit_slope * int(grid_sampled[i][0]) + limit_intercept - int(grid_sampled[i][1]) < 0: + continue + x.append(int(grid_sampled[i][0])) + y.append(int(grid_sampled[i][1])) + x = np.array(x) + y = np.array(y) + + # 原始数据粗分类 + slope, intercept, r_2 = calc_slope_line.linear_regression(x, y) + y_pred = slope * x + intercept + x_bot = [] + y_bot = [] + x_top = [] + y_top = [] + for i in range(len(x)): + if x[i] * slope + intercept - y[i] > 0: + x_bot.append(x[i]) + y_bot.append(y[i]) + else: + x_top.append(x[i]) + y_top.append(y[i]) + x_bot = np.array(x_bot) + y_bot = np.array(y_bot) + x_top = np.array(x_top) + y_top = np.array(y_top) + slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot) + slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top) + print(f"未清洗数据拟合上下沿：r2_bot = {r2_bot},r2_top = {r2_top}") + + # 第一次数据清洗，消除误识别点 + # 计算残差 + residuals = y - y_pred + # 计算残差的标准差 (MSE的平方根) + residual_std = np.sqrt(np.sum(residuals ** 2) / (len(x) - 2)) + standardized_residuals = residuals / residual_std + # 设置阈值 (常用 2.5-3.0 个标准差) + threshold = 2.0 + # 标记异常点 + outlier_mask = np.abs(standardized_residuals) > threshold + outliers_x = x[outlier_mask] + outliers_y = y[outlier_mask] + print(f"第一次数据清洗发现 {np.sum(outlier_mask)} 个异常点:") + for i, (x_val, y_val) in enumerate(zip(outliers_x, outliers_y)): + print(f"点 {i + 1}: x={x_val}, y={y_val}, 残差={residuals[outlier_mask][i]:.2f}") + # 剔除异常点 + clean_x = x[~outlier_mask] + clean_y = y[~outlier_mask] + clean_slope, clean_intercept, clean_r_2 = calc_slope_line.linear_regression(clean_x, clean_y) + print(f"清洗数据后整体拟合参数r_2 = {r_2}") + + # 第一次数据清洗后的数据再分类 + x_bot_clean = [] + y_bot_clean = [] + x_top_clean = [] + y_top_clean = [] + for i in range(len(clean_x)): + if clean_x[i] * clean_slope + clean_intercept - clean_y[i] > 0: + x_bot_clean.append(clean_x[i]) + y_bot_clean.append(clean_y[i]) + else: + x_top_clean.append(clean_x[i]) + y_top_clean.append(clean_y[i]) + x_bot_clean = np.array(x_bot_clean) + y_bot_clean = np.array(y_bot_clean) + x_top_clean = np.array(x_top_clean) + y_top_clean = np.array(y_top_clean) + + # 第二次数据清洗，消除误分类点 + clean_slope_bot, clean_intercept_bot, clean_r2_bot = calc_slope_line.linear_regression(x_bot_clean, y_bot_clean) + clean_slope_top, clean_intercept_top, clean_r2_top = calc_slope_line.linear_regression(x_top_clean, y_top_clean) + print(f"清洗数据后上下沿拟合参数clean_r2_bot = {clean_r2_bot},clean_r2_top = {clean_r2_top}") + # 绘制拟合线 + y_bot_pred = clean_slope_bot * x_bot_clean + clean_intercept_bot + y_top_pred = clean_slope_top * x_top_clean + clean_intercept_top + # 计算残差 + residuals_bot = y_bot_clean - y_bot_pred + residuals_top = y_top_clean - y_top_pred + # 计算残差的标准差 (MSE的平方根) + residual_std_bot = np.sqrt(np.sum(residuals_bot ** 2) / (len(x_bot_clean) - 2)) + residual_std_top = np.sqrt(np.sum(residuals_top ** 2) / (len(x_top_clean) - 2)) + # 计算标准化残差 (Z-score) + standardized_residuals_bot = residuals_bot / residual_std_bot + standardized_residuals_top = residuals_top / residual_std_top + # 设置阈值 (常用 2.5-3.0 个标准差) + threshold = 1.0 + # 标记异常点 + outlier_mask_bot = np.abs(standardized_residuals_bot) > threshold + outlier_mask_top = np.abs(standardized_residuals_top) > threshold + outliers_x_bot = x_bot_clean[outlier_mask_bot] + outliers_y_bot = y_bot_clean[outlier_mask_bot] + outliers_x_top = x_top_clean[outlier_mask_top] + outliers_y_top = y_top_clean[outlier_mask_top] + print(f"第二次数据清洗下沿发现 {np.sum(outlier_mask_bot)} 个异常点:") + # for i, (x_val, y_val) in enumerate(zip(outliers_x_bot, outliers_y_bot)): + # print(f"点 {i + 1}: x={x_val}, y={y_val}, 残差={residuals_bot[outlier_mask_bot][i]:.2f}") + print(f"第二次数据清洗上沿发现 {np.sum(outlier_mask_top)} 个异常点:") + # for i, (x_val, y_val) in enumerate(zip(outliers_x_top, outliers_y_top)): + # print(f"点 {i + 1}: x={x_val}, y={y_val}, 残差={residuals_top[outlier_mask_top][i]:.2f}") + # 剔除异常点 + x_bot_clean = x_bot_clean[~outlier_mask_bot] + y_bot_clean = y_bot_clean[~outlier_mask_bot] + x_top_clean = x_top_clean[~outlier_mask_top] + y_top_clean = y_top_clean[~outlier_mask_top] + + # 判断数据的有效性 + clean_slope_bot, clean_intercept_bot, clean_r2_bot = calc_slope_line.linear_regression(x_bot_clean, y_bot_clean) + clean_slope_top, clean_intercept_top, clean_r2_top = calc_slope_line.linear_regression(x_top_clean, y_top_clean) + print(f"清洗数据后上下沿拟合参数clean_r2_bot = {clean_r2_bot},clean_r2_top = {clean_r2_top}") + if ((1-clean_r2_bot) > 1e-3) or ((1-clean_r2_top) > 1e-3): + print("无效数据") + return 0, None, None, None, None + return 1, x_bot_clean, y_bot_clean, x_top_clean, y_top_clean diff --git a/Visual measurement-straight/py/measure_lib.py b/Visual measurement-straight/py/measure_lib.py new file mode 100644 index 0000000..7a9c754 --- /dev/null +++ b/Visual measurement-straight/py/measure_lib.py @@ -0,0 +1,45 @@ +import math +import numpy as np +import matplotlib.pyplot as plt +import calc_way +import get_data +import calc_slope_line +import cv2 +import model +import os +import time + +model = model.Model() +alpha = model.alpha +beta = model.beta + + +def vs_measurement(txt_name, position=900): + # 加载数据 + state, x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name) + if state == 0: + return 0, None, None + # 拟合上下沿 + slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot) + slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top) + Xw_bot = [] + Yw_bot = [] + for i in range(len(x_bot)): + Xw, Yw = calc_way.calc_distance(x_bot[i], y_bot[i], alpha, beta) + Xw_bot.append(Xw) + Yw_bot.append(Yw) + slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot) + # 计算路沿与车身夹角 + angle = math.atan(slope_Xw) + # 位置修正 + position = position + model.x + # 确认目标点位置并计算高度 + Yw_pos = slope_Xw * position + intercept_Xw + x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, alpha, beta) + k_pos = calc_slope_line.get_k(alpha, beta, x_pos, y_pos) + b_pos = calc_slope_line.get_b(x_pos, y_pos, k_pos) + x_pos_top, y_pos_top = calc_slope_line.find_intersection((k_pos, -1, b_pos), (slope_top, -1, intercept_top)) + Zw_pos = calc_way.calc_height(x_pos, y_pos, x_pos_top, y_pos_top, alpha, beta) + distance_pos = -(Yw_pos + model.y) * math.cos(angle) + return 1, Zw_pos, distance_pos + diff --git a/Visual measurement-straight/py/model.py b/Visual measurement-straight/py/model.py new file mode 100644 index 0000000..a7dd362 --- /dev/null +++ b/Visual measurement-straight/py/model.py @@ -0,0 +1,25 @@ +import numpy as np +from scipy.optimize import minimize + +#保存相机内参、外参等参数 +class Model: + def __init__(self): + # 内参 + self.K = np.array([[801.8319, 0, 647.8920], + [0, 801.7619, 532], + [0, 0, 1]]) + self.f = 3.6 + self.H = 1019.0000170167332 + self.dx = self.f / self.K[0, 0] + self.dy = self.f / self.K[1, 1] + self.u0 = self.K[0, 2] + self.v0 = self.K[1, 2] + # 外参 + self.alpha = 0.7072338025822084 + self.beta = 0.9077237961986776 + # 位置修正 + self.y = -70 + self.x = 22 + # 数据截断线 + self.limit_slope = 0.3259949467095897 + self.limit_intercept = 452.86565535382374