* 起始相机 标定板像素坐标 startPixX:[188.96553379402616,189.50900407654672,190.05578107632854,190.6117310540232,191.1606548954415,191.70532595025972,192.24914831176892,192.79332203174974,193.33612959632538,\ 193.88559716277,\ 194.42853055494513,\ 194.9680161349787,\ 256.0512239267404,\ 256.59993515751245,\ 257.15553706070494,\ 257.70503642081394,\ 258.25283048993134,\ 258.80481561536516,\ 259.35953823168416,\ 259.9045161863789,\ 260.4445397233523,\ 260.9875681927779,\ 261.53800047133706,\ 262.08284191922394,\ 323.1487932063029,\ 323.7013794963903,\ 324.2552402628347,\ 324.81059009161004,\ 325.3618360917567,\ 325.9197028042165,\ 326.4571609643691,\ 327.0099210684873,\ 327.56116147912496,\ 328.10546355510076,\ 328.6649598304255,\ 329.19966559817453,\ 390.2408569846252,\ 390.8012186557068,\ 391.35832757335186,\ 391.91447170056546,\ 392.4652753050608,\ 393.01422402692265,\ 393.5733590418279,\ 394.1106754174031,\ 394.6803019282964,\ 395.22277621009954,\ 395.7946679373018,\ 396.33019707107803,\ 457.34285149638777,\ 457.8965358478284,\ 458.46093480427356,\ 459.008990797655,\ 459.5753737864782,\ 460.1192004871258,\ 460.678991823595,\ 461.2276013474107,\ 461.79059404178065,\ 462.3534516087302,\ 462.90968584326066,\ 463.4670396064918,\ 524.4319195009859,\ 525.0301981858412,\ 525.5516122458861,\ 526.0983102334608,\ 526.662000120021,\ 527.2229543107616,\ 527.787065123165,\ 528.3413123378735,\ 528.9100452260038,\ 529.45789343442,\ 530.0223586064997,\ 530.5793911044741,\ 591.5379145487788,\ 592.0740426385996,\ 592.6312180461191,\ 593.1838696402498,\ 593.7543970478752,\ 594.3185106482149,\ 594.8833152136657,\ 595.446300849569,\ 596.0174992318649,\ 596.5823106504987,\ 597.1411758059193,\ 597.7124360353479,\ 658.630054785966,\ 659.4342490783355,\ 659.7323040897917,\ 660.2978089499567,\ 660.8882798319728,\ 661.4483220733039,\ 661.9837713885979,\ 662.5260590372075,\ 663.1338452250144,\ 663.7089989848998,\ 664.2727487624198,\ 664.8177075161483] startPixY:[ 920.3377739112948 , \ 987.4310982093862 , \ 1054.5492910287796 , \ 1121.6417950557047 , \ 1188.7605817620683 , \ 1255.8577494240274 , \ 1322.9850703189272 , \ 1390.0831326976083 , \ 1457.2219424286438 , \ 1524.3359669522433 , \ 1591.4702773052575 , \ 1658.590164587394 , \ 919.7494010849366 , \ 986.852127169112 , \ 1053.9675935221462 , \ 1121.053356165078 , \ 1188.1750465261907 , \ 1255.2818792160213 , \ 1322.4044771305394 , \ 1389.4981714317832 , \ 1456.635463392758 , \ 1523.74846825093 , \ 1590.9000518649457 , \ 1658.0001395494496 , \ 919.1636581187013 , \ 986.252400068027 , \ 1053.3789678534083 , \ 1120.4662793678995 , \ 1187.5913487700916 , \ 1254.684080182309 , \ 1321.8238513142517 , \ 1388.912987196053 , \ 1456.04962757027 , \ 1523.1657236863618 , \ 1590.3014308496586 , \ 1657.4163195610265 , \ 918.5717859867166 , \ 985.679816123276 , \ 1052.7850550507044 , \ 1119.8803307174746 , \ 1186.999187143107 , \ 1254.0899559991506 , \ 1321.228923699219 , \ 1388.3336390730449 , \ 1455.4662950240909 , \ 1522.5764175001204 , \ 1589.7244000917344 , \ 1656.841200997501 , \ 917.9835462364267 , \ 985.0813160464932 , \ 1052.2117024019685 , \ 1119.293225619103 , \ 1186.4075743248277 , \ 1253.5072657230494 , \ 1320.6464939298921 , \ 1387.7520772687847 , \ 1454.8854612678274 , \ 1521.9960369252206 , \ 1589.145423096063 , \ 1656.2371610347716 , \ 917.4077074829939 , \ 984.5080632191153 , \ 1051.6046516881254 , \ 1118.703462428718 , \ 1185.824998725002 , \ 1252.9239534104959 , \ 1320.0620149011927 , \ 1387.1545590462724 , \ 1454.3040343387984 , \ 1521.4080273232066 , \ 1588.5702393106535 , \ 1655.6750173130986 , \ 916.840748514932 , \ 983.8982116159334 , \ 1050.972503424963 , \ 1118.1155244613833 , \ 1185.2213057292877 , \ 1252.326183495791 , \ 1319.477823261829 , \ 1386.6038029743918 , \ 1453.7190082163115 , \ 1520.8431324203652 , \ 1587.979709375507 , \ 1655.0829572277557 , \ 916.0401123201925 , \ 983.2977412022778 , \ 1050.4379462831357 , \ 1117.5247345714843 , \ 1184.6437853193488 , \ 1251.7500503547014 , \ 1318.8655947597179 , \ 1386.0230303098385 , \ 1453.1442001392056 , \ 1520.2511726078335 , \ 1587.4070005086714 , \ 1654.5335173342903 ] startWorldX:[0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33,\ 0,\ 3,\ 6,\ 9,\ 12,\ 15,\ 18,\ 21,\ 24,\ 27,\ 30,\ 33] startWorldY:[ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 66 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 69 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 72 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 75 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 78 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 81 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 84 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ,\ 87 ] * 九点起始相机像素坐标 NinePixSX:[589.2530222885299,\ 701.4502440989799,\ 702.5143716550485,\ 590.3387006288543,\ 478.1765302600638,\ 477.1400298296101,\ 476.0433733297309,\ 588.2307166897236,\ 700.3436192332101] NinePixSY:[981.6132147715933 ,\ 980.5517080276952 ,\ 1092.556832898525 ,\ 1093.7953887686174 ,\ 1094.9335455943483 ,\ 982.9788075260918 ,\ 870.8287323085045 ,\ 869.5735863146681 ,\ 868.4767890272936 ] NinePixEX:[578.3058554687458,\ 690.4163782900973,\ 691.9610938601982,\ 579.8580232841458,\ 467.73147574465094,\ 466.1853252379038,\ 464.7252365948582,\ 576.8375972532203,\ 688.8680197941364] NinePixEY:[ 1224.5425114218197 ,\ 1222.974871576902 ,\ 1334.8853147809225 ,\ 1336.5274400287349 ,\ 1338.1056204463955 ,\ 1226.2753164516682 ,\ 1114.2803461014073 ,\ 1112.5744175453785 ,\ 1110.9896617390411 ] * 九点起始相机机械坐标 NineBotSX:[0.00034332275390625,\ -5.0006103515625,\ -4.999847412109375,\ 0.00072479248046875,\ 5.000190734863281,\ 4.9999237060546875,\ 5,\ -0.0003814697265625,\ -5.000648498535156] NineBotSY:[ 0.00057220458984375 ,\ 0.00057220458984375 ,\ 5 ,\ 5 ,\ 4.999961853027344 ,\ 7.62939453125E-05 ,\ -5 ,\ -5 ,\ -4.999961853027344 ] NineBotEX:[0.00034332275390625,\ -5.0006103515625,\ -4.999847412109375,\ 0.00072479248046875,\ 5.000190734863281,\ 4.9999237060546875,\ 5,\ -0.0003814697265625,\ -5.000648498535156] NineBotEY:[ 0.00057220458984375 ,\ 0.00057220458984375 ,\ 5 ,\ 5 ,\ 4.999961853027344 ,\ 7.62939453125E-05 ,\ -5 ,\ -5 ,\ -4.999961853027344 ] RotePixSX:[619.7238122445144,\ 613.160915771966,\ 605.8054632449707,\ 597.8550536755898,\ 589.3540923625183,\ 579.9971433571602,\ 570.0608733424086,\ 559.6535002633227,\ 548.465728894034] RotePixSY:[ 686.9372022826761 ,\ 760.4561437652419 ,\ 834.0087055222949 ,\ 907.5470977466263 ,\ 981.4633390927919 ,\ 1055.053194166951 ,\ 1128.2562528374413 ,\ 1201.8221369522248 ,\ 1275.363661468136 ] RotePixEX:[742.2246513137378,\ 702.410022187826,\ 661.7314648271098,\ 620.4276965323969,\ 578.455819287899,\ 535.7704822016517,\ 492.52070368147383,\ 448.48502264195474,\ 403.90440121779386 ] RotePixEY:[ 925.8910841081664 ,\ 1000.827655680299 ,\ 1075.541286927812 ,\ 1149.8640322622996 ,\ 1224.3434474167652 ,\ 1298.1869499594388 ,\ 1371.362051152623 ,\ 1444.625275234559 ,\ 1517.5118236375802 ] RoteBotSX:[-10.27332878112793,\ -6.986742973327637,\ -3.6995046138763428,\ -0.4136563837528229,\ 2.888383150100708,\ 6.175053119659424,\ 9.44379997253418,\ 12.728231430053711,\ 16.01095199584961] RoteBotSY:[ 85.34555053710938 ,\ 85.02522277832031 ,\ 84.66937255859375 ,\ 84.28684997558594 ,\ 83.87947845458984 ,\ 83.43386840820312 ,\ 82.96239471435547 ,\ 82.46963500976562 ,\ 81.94190216064453 ] tuple_gen_const (96, 0, startWorldZ) CamparmS:[area_scan_polynomial,0.05,49.32421434596083,-2271776.2105572904,37169147235.66686,0.0012414760666998228,-0.025139874205344942,3.449577676339493E-06,3.45E-06,1096.4894972362567,1101.4396135570917,2448,2048] CamparmE:[area_scan_polynomial,0.05,11.203108109765045,-755834.9806875421,17940141171.680748,-0.024303425424073852,-0.0011094734603724226,3.449751607272305E-06,3.45E-06,1239.9027328789548,1099.180494038572,2448,2048] *标定板物理坐标是MM为单位 vector_to_pose (startWorldX, startWorldY, startWorldZ, startPixX, startPixY, CamparmS, iterative, error, Pose, Quality) * PoseS:[-7.30929, -106.795, 647.003, 0.612034, 0.645847, 0.492762, 0] * PoseE:[-174.55422123903658,-109.4434320533541,648.4054608136798,0.3934448523535419,0.47671714151467265,0.7125930331427324,0] PoseS:[-0.007309293025263901,-0.10679487514765092,0.6470025347559445,0.6120335395027948,0.6458469089050162,0.49276164293390323,0] PoseE:[-0.17455422123903658,-0.1094434320533541,0.6484054608136798,0.3934448523535419,0.47671714151467265,0.7125930331427324,0] *先将九点的像素坐标转化到标定板上 image_points_to_world_plane (CamparmS, PoseS, NinePixSX, NinePixSY, mm, NineWorldX, NineWorldY) image_points_to_world_plane (CamparmE, PoseE, NinePixEX, NinePixEY, mm, NineWorldEX, NineWorldEY) * NineWorldX:[2895.04, 2890.62, 7898.8, 7911.06, 7918.83, 2913.08, -2102.42, -2115.59, -2121.74] * NineWorldY:[83875.0, 88892.3, 88898.0, 83882.0, 78866.6, 78861.3, 78853.1, 83870.5, 88884.5] * NineWorldEX:[174931.1, 174923.7, 179924.3, 179934.8, 179942.3, 174945.7, 169940.9, 169927.2, 169918.8] * NineWorldEY:[83992.4, 89001.6, 89009.6, 84001.1, 78992.1, 78983.1, 78978.0, 83987.3, 88993.4] NineWorldX:[2.89505, 2.89062, 7.8988, 7.91106, 7.91883, 2.91308, -2.10241, -2.11558, -2.12173] NineWorldY:[83.8749, 88.8922, 88.8978, 83.8819, 78.8665, 78.8612, 78.853, 83.8704, 88.8844] NineWorldEX:[174.931, 174.924, 179.924, 179.935, 179.942, 174.946, 169.941, 169.927, 169.919] NineWorldEY:[83.9924, 89.0016, 89.0096, 84.0011, 78.9921, 78.9831, 78.978, 83.9873, 88.9934] vector_to_hom_mat2d (NineWorldX, NineWorldY, NineBotSX, NineBotSY, HomMat2DNineS) vector_to_hom_mat2d (NineWorldEX, NineWorldEY, NineBotEX, NineBotEY, HomMat2DNineE) * HomMat2DNineS:[1.34627e-06, -0.000996921, 83.6137, 0.000997718, 2.0682e-06, -3.06535] * HomMat2DNineE:[0.0000015353493,-0.00099832549,83.583915,0.000999516323,0.0000020882,-175.02273] HomMat2DNineS:[0.00134296, -0.996924, 83.6139, 0.997719, 0.00206821, -3.06536] HomMat2DNineE:[0.0015367, -0.998327, 83.5838, 0.999528, 0.00208125, -175.024] * Sx方向的缩放因子。 * Sy方向的缩放因子。 * Phi 旋转角度弧度。 * Thetax方向的倾斜角度弧度。 * Txx方向的平移。 * Tyy方向的平移 hom_mat2d_to_affine_par (HomMat2DNineS, SSx, SSy, SPhi, STheta, STx, STy) hom_mat2d_to_affine_par (HomMat2DNineE, ESx, ESy, EPhi, ETheta, ETx, ETy) *先将旋转的像素坐标转化到标定板上 image_points_to_world_plane (CamparmS, PoseS, RotePixSX, RotePixSY, mm, RoteWorldX, RoteWorldY) image_points_to_world_plane (CamparmE, PoseE, RotePixEX, RotePixEY, mm, RoteWorldEX, RoteWorldEY) * RoteWorldX:[-10273.3, -6986.76, -3699.51, -413.664, 2888.38, 6175.05, 9443.8, 12728.2, 16011.0] * RoteWorldY:[85345.7, 85025.3, 84669.5, 84287.0, 83879.6, 83434.0, 82962.5, 82469.7, 81942.0] * RoteWorldEX:[-372638.9, -384796.3, -397149.7, -409675.4, -422473.3, -435405.2, -448465.8, -461795.7, -475314.6] * RoteWorldEY:[252621.8, 254209.9, 255739.6, 257218.1, 258677.9, 260058.3, 261367.6, 262637.6, 263855.2] RoteWorldX:[-10.2733, -6.98674, -3.6995, -0.413656, 2.88838, 6.17505, 9.4438, 12.7282, 16.011] RoteWorldY:[85.3456, 85.0252, 84.6694, 84.2868, 83.8795, 83.4339, 82.9624, 82.4696, 81.9419] RoteWorldEX:[161.675, 165.003, 168.32, 171.618, 174.922, 178.197, 181.442, 184.689, 187.919] RoteWorldEY:[91.4784, 89.6584, 87.8002, 85.9146, 83.9992, 82.0526, 80.0813, 78.0752, 76.0453] gen_contour_polygon_xld (ContourS, RoteWorldX, RoteWorldY) fit_circle_contour_xld (ContourS, algebraic, -1, 0, 0, 3, 2, RowS, ColumnS, Radius, StartPhi, EndPhi, PointOrder) gen_contour_polygon_xld (ContourE, RoteWorldEX, RoteWorldEY) fit_circle_contour_xld (ContourE, algebraic, -1, 0, 0, 3, 2, RowE, ColumnE, Radius, StartPhi, EndPhi, PointOrder) CenterROWS:-45.7867 CenterCOLS:-291.78 CenterROWE:-46.7529 CenterCOLE:-292.914 CenterROW:(CenterROWSCenterROWE)/2 CenterCOL:(CenterCOLSCenterCOLE)/2 angle_lx (CenterROWS, CenterCOLS, RoteWorldX[0], RoteWorldY[0], AngleS1) tuple_deg (AngleS1, AngleS1) angle_lx (CenterROWS, CenterCOLS, RoteWorldX[1], RoteWorldY[1], AngleS2) tuple_deg (AngleS2, AngleS2) * 标定板坐标系算出来的角度 0.499457 AngleO:AngleS1-AngleS2 * 计算机械臂角度和标定板角度做对比 gen_contour_polygon_xld (ContourB, RoteBotSX, RoteBotSY) fit_circle_contour_xld (ContourB, algebraic, -1, 0, 0, 3, 2, RowB, ColumnB, Radius, StartPhi, EndPhi, PointOrder) CenterBotxS:-45.7835 CenterBotyS:-291.758 angle_lx (CenterBotxS, CenterBotyS, RoteBotSX[0], RoteBotSY[0], AngleSB1) tuple_deg (AngleSB1, AngleSB1) angle_lx (CenterBotxS, CenterBotyS, RoteBotSX[1], RoteBotSY[1], AngleSB2) tuple_deg (AngleSB2, AngleSB2) * 标定板坐标系算出来的角度 0.499508 AngleBO:AngleSB1-AngleSB2 * 角度比值 1.00006 Anglek:AngleBO/AngleO * 基准值图像坐标 rowbaseS:859.429 colbaseS:844.294 rowbaseE:841.572 colbaseE:1314.529 * 实际图像坐标 rownowS:746.128 colnowS:882.218 rownowE:739.828 colnowE:1357.37 * 实际纠偏值 x: -5.199 y:-2.814 r:-0.161 * 基准值的标定板坐标 image_points_to_world_plane (CamparmS, PoseS, rowbaseS, colbaseS, mm,rowbaseX, colbaseY) image_points_to_world_plane (CamparmE, PoseE, rowbaseE, colbaseE, mm,rowbaseEX, colbaseEY) rowbaseX:-3.14262 colbaseY:96.0078 rowbaseEX:179.099 colbaseEY:95.7045 * 真实值的标定板坐标 image_points_to_world_plane (CamparmS, PoseS, rownowS, colnowS, mm,rownowX, rownowY) image_points_to_world_plane (CamparmE, PoseE, rownowE, colnowE, mm,rownowEX, rownowEY) rownowX:-1.48966 rownowY:90.9268 rownowEX:180.956 rownowEY:91.1357 * 第一步 计算角度偏移量 angle_lx (rowbaseX, colbaseY, rowbaseEX, colbaseEY, AngleR) tuple_deg (AngleR, AngleR) angle_lx (rownowX, rownowY, rownowEX, rownowEY, AngleR1) tuple_deg (AngleR1, AngleR1) vector_to_rigid ([rownowX,rownowEX],[rownowY,rownowEY],[rowbaseX,rowbaseEX],[colbaseY,colbaseEY],HomMat2D) hom_mat2d_to_affine_par(HomMat2D, Sx, Sy, Phi, Theta, Tx, Ty) tuple_deg (Phi, Phix) * 计算出来 -0.1609 方向不对就取反 Angle:AngleR-AngleR1 * 计算九点XY偏移量 首先将真实的标定板坐标转换成机械坐标 affine_trans_point_2d (HomMat2DNineS, rownowX, rownowY, QSx, QSy) affine_trans_point_2d (HomMat2DNineE, rownowEX, rownowEY, QEx, QEy) QSx:-7.03521 QSy:-4.36357 QEx:-7.12135 QEy:6.03626 * 然后将基准的标定板坐标转换成机械坐标 affine_trans_point_2d (HomMat2DNineS, rowbaseX, colbaseY, QBSx, QBSy) affine_trans_point_2d (HomMat2DNineE, rowbaseEX, colbaseEY, QBEx, QBEy) QBSx:-12.1028 QBSy:-6.00225 QBEx:-11.6854 QBEy:4.18965 * 旋转完角度后的新坐标 hom_mat2d_identity(HomMat2DIdentity) hom_mat2d_rotate(HomMat2DIdentity, Phi, CenterROW, CenterCOL, HomMat2DRotate) affine_trans_point_2d(HomMat2DRotate, [rownowX,rownowEX], [rownowY,rownowEY], Qx, Qy) Qx:[-0.412846, 182.033] Qy:[90.7995, 90.4957] affine_trans_point_2d (HomMat2DNineS, Qx[0], Qy[0], QSRx, QSRy) affine_trans_point_2d (HomMat2DNineE, Qx[1], Qy[1], QERx, QERy) QSRx:-6.90686 QSRy:-3.28947 QERx:-6.48077 QERy:7.11142 CentreBaseX:(QBSxQBEx)*0.5 CentreBaseY:(QBSyQBEy)*0.5 CentreNowX:(QSRxQERx)*0.5 CentreNowY:(QSRyQERy)*0.5 detx:CentreBaseX-CentreNowX dety:CentreBaseY-CentreNowY detx:-5.20029 dety:-2.81727 * 注 计算角度要 真实 → 基准 计算XY 要基准减去实际 cameraparms:[area_scan_polynomial,0.05,-20.958144417451823,165614.36296792259,-980889674.4570378,-0.04246652479402499,0.02985846591449899,3.4465721102019377E-06,3.45E-06,1767.4398397528914,890.5765236310382,2048,2448] cameraparme:[area_scan_polynomial,0.05,9.902894531866593,-2357267.360641208,58115246715.28305,0.01718242185464444,0.012567049226993532,3.450149788135826E-06,3.45E-06,1004.3700284933637,1245.0035981568567,2048,2448] poses:[-0.04867768589189727,-0.09483282186562732,0.647432885267122,359.24543071583935,0.7838173714426028,0.24089030783017581,0] posee:[-0.18240019972403027,-0.1108603229520475,0.6505081931568573,358.86160518184124,0.4872553315484634,0.14242516792872204,0] hom2dnines:[0.008694792815196261,0.996871625852825,-87.96378651028326,-0.998804675609422,0.008723368175144654,4.80124409438667] hom2dninee:[0.00786658238658428,0.999091889849506,-86.39020257293696,-1.0012031998637414,0.008935591934384608,177.0472532686243] rowbases:505.959 colbases:662.337 rowbasee:459.171 colbasee:986.749 rownows:635.242 colnows:650.163 rownowe:469.45 colnowe:971.731 image_points_to_world_plane (cameraparms, poses, rowbases, colbases, mm, SX, SY) image_points_to_world_plane (cameraparme, posee, rowbasee, colbasee, mm, EX, EY) image_points_to_world_plane (cameraparms, poses, 635.242, 650.163, mm, sSX, sSY) image_points_to_world_plane (cameraparme, posee, 469.45, colnowe, mm, eEX, eEY) affine_trans_point_2d (hom2dnines, SX, SY, QQSx, QQSy) affine_trans_point_2d (hom2dninee, EX, EY, EESx, EESy) angle_lx (SX, SY,EX, EY, Angle1) tuple_deg (Angle1, Deg1) angle_lx (sSX, sSY, eEX, eEY, Angle2) tuple_deg (Angle2, Deg2) a:Deg1-Deg2 hom_mat2d_identity(HomMat2DIdentity) hom_mat2d_rotate(HomMat2DIdentity, rad(a), -44.1645, -286.075, HomMat2DRotate) affine_trans_point_2d(HomMat2DRotate, [sSX,eEX], [sSY,eEY], zQx, zQy) affine_trans_point_2d (hom2dnines, zQx[0], zQy[0], zQSRx, zQSRy) affine_trans_point_2d (hom2dninee, zQx[1], zQy[1], zQERx, zQERy) * -3.325 * 12.0838 b:(zQSRxzQERx)*0.5 c:(zQSRyzQERy)*0.5 g:(QQSxEESx)*0.5 h:(QQSyEESy)*0.5 detx4:g-b dety4:h-c