Я извлекаю совпадающие характерные точки из пары камер.У меня чуть более 200 характерных точек, на глаз ~ 90% из них действительно совпадают ... Я пытаюсь восстановить относительное положение и ориентацию двух камер, используя функцию findessentialmat в opencv.
Aпример минимального выполнения здесь:
minimal.py
import numpy as np
import cv2
DIM=(1280, 960)
K0 =np.array([[741.33782999, 0., 682.12142279],
[ 0., 742.80374714, 464.20413311],
[ 0., 0., 1. ]])
def readfile(filename):
q = []
p = []
with open(filename) as fp:
for cnt, line in enumerate(fp):
px, py, qx, qy = line.split()
p = np.append(p, [float(px), float(py)])
q = np.append(q, [float(qx), float(qy)])
n = int(len(p)/2)
p = np.reshape(p, (n, 2))
q = np.reshape(q, (n, 2))
return p, q
p12, q12 = readfile("d_temp")
retval, mask = cv2.findEssentialMat(p12, q12, K0, method=cv2.RANSAC)
NN, R, t, _ = cv2.recoverPose(retval, p12, q12, K0, mask=mask)
dst, jacobian = cv2.Rodrigues(R)
print("rotation vector")
print(np.degrees(dst))
print("translation vector")
print(t)
пример файла данных здесь ... (извините за длинный файл, но в порядке воспроизводимости, я думаю,это хорошая идея ...)
# d.dat
447.7982482910156 783.8470458984375 844.46875 478.64984130859375
472.2575988769531 783.9226684570312 861.17919921875 463.9976806640625
471.056884765625 785.6572265625 861.0101928710938 463.9947814941406
679.7252197265625 613.1166381835938 835.0584716796875 162.0465087890625
444.4239196777344 773.3471069335938 834.8787231445312 474.52978515625
476.6434631347656 548.7996215820312 695.3291625976562 295.1576232910156
316.2449035644531 876.6978149414062 767.7656860351562 608.9740600585938
28.56636619567871 835.704833984375 449.8716125488281 713.1991577148438
482.4017028808594 755.1663208007812 850.0111083984375 430.626220703125
471.2223205566406 795.663818359375 868.9219360351562 471.4686279296875
585.5891723632812 928.5010986328125 1024.892333984375 481.9182434082031
134.27027893066406 438.8541564941406 1023.7425537109375 448.80987548828125
560.4802856445312 782.7987060546875 934.5108642578125 399.1592102050781
437.55364990234375 783.9776611328125 835.0953979492188 485.783203125
558.8355102539062 783.1337280273438 934.3226928710938 401.5900573730469
493.6907958984375 811.2030639648438 899.3700561523438 465.0192565917969
333.5005187988281 474.5987548828125 514.41748046875 354.7676696777344
281.6152648925781 854.6431274414062 725.10009765625 614.118896484375
489.6645812988281 819.7667846679688 901.4575805664062 474.939697265625
567.0088500976562 793.2717895507812 946.3422241210938 409.54779052734375
473.9570007324219 741.4544677734375 835.43359375 427.68084716796875
263.5504455566406 593.8671875 308.0599670410156 864.5149536132812
329.52447509765625 759.8671875 982.2100219726562 759.9042358398438
223.50721740722656 877.8092041015625 697.9076538085938 710.441162109375
448.5132751464844 783.9352416992188 845.5458984375 478.6185302734375
166.29356384277344 703.2822875976562 575.96337890625 587.17431640625
195.16452026367188 645.3009033203125 548.0439453125 573.9312133789062
679.7011108398438 613.3897094726562 835.2294311523438 162.2067413330078
560.1014404296875 783.4053344726562 934.7252807617188 400.22491455078125
260.0428466796875 682.58203125 988.1619262695312 737.9358520507812
173.69033813476562 726.2459716796875 244.25367736816406 609.8120727539062
193.5665283203125 495.71881103515625 493.33575439453125 457.4891052246094
318.1491394042969 455.9480895996094 518.5922241210938 354.5550537109375
245.27215576171875 839.358642578125 686.4036865234375 623.212646484375
743.0471801757812 889.39306640625 1060.811767578125 363.8262023925781
99.52837371826172 455.26544189453125 503.113525390625 550.7799682617188
276.1446838378906 699.6495971679688 751.2548217773438 852.3948974609375
190.50099182128906 745.3849487304688 614.2245483398438 600.6458740234375
189.45762634277344 399.43841552734375 471.45098876953125 468.6031799316406
324.149169921875 851.31103515625 753.8226928710938 590.1079711914062
477.1978759765625 545.6710205078125 691.1409301757812 295.8692626953125
399.811767578125 455.1125793457031 349.0351257324219 853.0255126953125
150.71499633789062 637.7068481445312 127.8241958618164 617.8019409179688
174.24513244628906 443.05633544921875 460.3077697753906 445.8348388671875
290.4751892089844 563.4282836914062 1177.7535400390625 647.7413330078125
35.504730224609375 375.5752868652344 1132.147216796875 732.7445678710938
570.9328002929688 891.2274780273438 430.9983825683594 598.0726928710938
129.2855224609375 447.6126403808594 269.48297119140625 518.7491455078125
916.583984375 654.3966674804688 131.51922607421875 544.0574340820312
453.64923095703125 909.1268920898438 310.0630798339844 873.9638061523438
272.41595458984375 779.4100952148438 242.86692810058594 574.124267578125
145.89622497558594 426.38616943359375 839.9185791015625 446.7758483886719
353.2640075683594 649.1328125 568.7075805664062 853.6813354492188
336.7197570800781 477.26580810546875 522.921630859375 354.66552734375
465.9938659667969 766.7280883789062 1245.819091796875 328.0860595703125
603.0092163085938 918.9746704101562 1035.0767822265625 462.84014892578125
177.19642639160156 786.0979614257812 1025.5509033203125 709.3068237304688
743.3534545898438 889.3507080078125 1060.8709716796875 362.4633483886719
424.6748962402344 745.2859497070312 805.2852172851562 469.1532897949219
666.6295776367188 846.3238525390625 1186.088134765625 331.71063232421875
265.49560546875 617.9306640625 1212.5106201171875 742.3143920898438
235.90284729003906 899.5637817382812 237.75428771972656 630.4393920898438
309.32379150390625 528.5013427734375 557.26025390625 405.9731140136719
365.8267822265625 794.9527587890625 1054.2913818359375 461.01287841796875
479.682373046875 811.927978515625 887.924560546875 476.56243896484375
546.8660888671875 825.3713989257812 954.0406494140625 419.4182434082031
263.32550048828125 719.295166015625 938.8235473632812 842.3818969726562
410.2393798828125 752.4403076171875 802.4618530273438 483.38238525390625
322.66204833984375 894.9562377929688 781.970458984375 615.1595458984375
84.81143188476562 472.9810485839844 1218.040771484375 608.5279541015625
198.11459350585938 504.37188720703125 500.67315673828125 641.6554565429688
148.35653686523438 638.7088623046875 186.9049835205078 736.7140502929688
303.63336181640625 844.1828002929688 730.8097534179688 596.8998413085938
187.0508575439453 509.2354431152344 490.9916076660156 470.39593505859375
973.512939453125 734.4038696289062 730.7944946289062 682.7640991210938
317.968994140625 850.9186401367188 748.3575439453125 593.0419311523438
547.7904663085938 864.0103149414062 959.0444946289062 599.9322509765625
264.11163330078125 719.9434814453125 939.9754638671875 844.283203125
667.170166015625 850.118408203125 1074.2694091796875 365.3814392089844
480.75238037109375 556.4450073242188 699.0028686523438 298.0946960449219
264.4339294433594 696.676513671875 619.2904052734375 537.121826171875
239.04403686523438 301.0240478515625 189.99940490722656 249.11346435546875
398.0354309082031 789.0571899414062 994.8922729492188 768.0885009765625
311.0710754394531 763.4730834960938 568.8160400390625 637.556884765625
87.1875 889.8743286132812 775.7293090820312 556.4700927734375
190.0430145263672 742.0386352539062 602.095703125 597.9042358398438
227.24232482910156 776.474609375 115.55091094970703 617.3444213867188
265.21966552734375 707.8191528320312 639.8690795898438 541.1021118164062
162.44972229003906 723.4267578125 245.604736328125 615.490478515625
437.36431884765625 785.8866577148438 836.6463623046875 487.3155517578125
793.1463623046875 923.9131469726562 1126.0709228515625 637.1099243164062
206.6471405029297 476.3389892578125 264.1603698730469 445.1524353027344
903.2003173828125 625.0552978515625 897.1513061523438 67.21099853515625
256.93646240234375 801.38671875 672.7518310546875 598.4820556640625
88.26525115966797 470.6452331542969 666.8927612304688 731.1099853515625
282.2829284667969 762.2633056640625 693.4093017578125 528.4248046875
121.02236938476562 770.4916381835938 698.5695190429688 358.8071594238281
486.1945495605469 880.182861328125 650.6167602539062 584.8759765625
442.5867614746094 771.2802124023438 831.2958374023438 474.41204833984375
352.16180419921875 797.5498657226562 578.2386474609375 631.1714477539062
183.8833465576172 510.9300231933594 280.1349182128906 506.5831604003906
380.51165771484375 810.257080078125 1074.3548583984375 473.5334777832031
231.75296020507812 884.1380615234375 837.761474609375 427.0298767089844
80.79380798339844 894.5822143554688 1132.9962158203125 519.955078125
211.58810424804688 484.9515075683594 500.9017639160156 450.52032470703125
264.9154968261719 618.31689453125 130.1840362548828 479.39434814453125
118.52188110351562 681.4915771484375 1222.2967529296875 677.895751953125
481.3524475097656 817.30810546875 900.7264404296875 140.29383850097656
243.51730346679688 306.3136291503906 493.9825439453125 470.5849609375
474.4856872558594 373.6757507324219 576.774658203125 452.58154296875
419.7844543457031 747.1199951171875 555.3731689453125 639.4868774414062
245.72634887695312 793.8189697265625 583.1657104492188 758.9775390625
182.6488494873047 582.034423828125 515.431884765625 759.8704223632812
162.671875 815.442138671875 733.5533447265625 537.5822143554688
450.0556335449219 843.4663696289062 885.4121704101562 515.9638061523438
148.79116821289062 442.63946533203125 981.3590087890625 775.7106323242188
126.88723754882812 545.5296630859375 1071.9697265625 753.0889282226562
199.48191833496094 498.1930236816406 489.1456298828125 450.1411437988281
275.2535705566406 578.6431884765625 579.5006103515625 462.88641357421875
481.34197998046875 871.9025268554688 1218.254150390625 642.8507080078125
76.59978485107422 468.64898681640625 245.7015838623047 645.285400390625
459.75018310546875 854.2384033203125 603.11865234375 587.4470825195312
471.3988037109375 914.074462890625 164.49220275878906 722.4299926757812
214.27532958984375 494.5464172363281 950.4194946289062 667.5511474609375
198.86412048339844 502.3232116699219 914.5925903320312 797.0887451171875
126.89524841308594 660.0762939453125 1235.8087158203125 671.393310546875
431.3277893066406 752.6722412109375 815.523681640625 468.5042724609375
319.27752685546875 758.914794921875 622.181884765625 631.2896728515625
272.63970947265625 778.7651977539062 530.503662109375 608.778564453125
324.4476013183594 658.4978637695312 881.9677124023438 485.70660400390625
257.4388732910156 852.4768676757812 703.123046875 623.9972534179688
212.24095153808594 485.63641357421875 953.366943359375 512.15478515625
232.307861328125 600.2705078125 1039.3995361328125 467.665283203125
255.8918914794922 752.2881469726562 255.54869079589844 561.2832641601562
282.0978088378906 789.4028930664062 371.9212951660156 511.0465087890625
578.9715576171875 206.7810516357422 612.0197143554688 628.6561889648438
226.20420837402344 776.3167114257812 119.6030044555664 427.47601318359375
342.3455505371094 885.6651000976562 384.51922607421875 433.4626770019531
397.6732482910156 863.8599243164062 172.97869873046875 619.8243408203125
360.3802185058594 652.3429565429688 456.40509033203125 599.084716796875
650.9277954101562 856.9739379882812 1062.89306640625 382.1123352050781
661.31396484375 865.3259887695312 946.0584716796875 516.408447265625
224.0924835205078 734.0776977539062 139.72430419921875 593.4146118164062
650.9525146484375 868.1492919921875 1142.61474609375 520.051513671875
673.8118896484375 306.71563720703125 141.61410522460938 578.706298828125
333.21051025390625 853.7525634765625 1175.6201171875 748.06884765625
679.0411376953125 341.63543701171875 227.8588104248047 713.6207885742188
426.1820068359375 745.2628173828125 257.1972351074219 396.6258544921875
662.1176147460938 876.2039794921875 1041.9356689453125 518.449462890625
301.7644348144531 518.9301147460938 564.2955932617188 411.1322326660156
237.16954040527344 689.663818359375 802.1461181640625 716.6635131835938
626.2874755859375 825.3125610351562 604.45947265625 545.74755859375
134.82618713378906 685.4896850585938 1221.602783203125 682.282958984375
315.6167297363281 736.1107177734375 143.47314453125 591.7796020507812
223.23167419433594 776.7254638671875 646.8034057617188 599.654052734375
96.94023895263672 334.7595520019531 1205.7244873046875 444.9001159667969
1074.5599365234375 744.3084106445312 442.6503601074219 719.4102172851562
751.9534301757812 897.6613159179688 605.0469360351562 553.6414184570312
344.93878173828125 656.6336669921875 275.20733642578125 510.4526062011719
493.5873718261719 904.2405395507812 662.9373168945312 674.652099609375
247.3878936767578 783.7380981445312 654.9146118164062 592.4580688476562
237.5862274169922 804.0629272460938 662.11279296875 608.0201416015625
490.70050048828125 820.9711303710938 902.7584228515625 475.5335388183594
323.59918212890625 826.238525390625 731.7511596679688 577.90283203125
268.9367370605469 832.5117797851562 708.8573608398438 608.2982177734375
459.1673889160156 853.7330932617188 832.7071533203125 640.3046875
185.94142150878906 716.7340087890625 952.4401245117188 713.91796875
169.84353637695312 792.5083618164062 463.1822814941406 554.0010986328125
376.000244140625 917.5015258789062 568.4920654296875 854.6214599609375
294.2245178222656 751.673095703125 194.80746459960938 735.2972412109375
409.14154052734375 775.4378662109375 897.9807739257812 853.47705078125
346.73614501953125 863.1689453125 777.9972534179688 586.1721801757812
775.0552978515625 913.116455078125 1099.552001953125 519.9706420898438
75.65567779541016 468.3360900878906 247.00537109375 563.03955078125
365.0856018066406 509.4056091308594 446.0355529785156 584.685546875
261.2919616699219 607.8782348632812 983.4957275390625 599.6174926757812
258.0390625 850.2388305664062 701.110107421875 621.8951416015625
158.16346740722656 721.5205688476562 566.798828125 601.413330078125
398.6167907714844 461.20989990234375 943.6886596679688 454.4562683105469
398.7389831542969 453.9648132324219 492.7894592285156 294.4828186035156
228.9871826171875 677.8126831054688 683.387451171875 613.234375
211.7838592529297 701.2118530273438 264.15472412109375 510.5503845214844
433.7532043457031 776.8671875 828.3287963867188 482.2206115722656
302.5193786621094 836.0662231445312 727.1840209960938 592.915283203125
260.25701904296875 837.8589477539062 696.3265991210938 615.0262451171875
680.8944091796875 899.5816040039062 760.4712524414062 587.1380615234375
696.654296875 347.95928955078125 1158.426513671875 883.6439208984375
187.51544189453125 631.2157592773438 1091.4049072265625 518.4441528320312
260.2186279296875 683.5731811523438 989.5481567382812 736.9423828125
310.6062927246094 763.869140625 686.1119384765625 547.693359375
232.9293212890625 884.1865234375 640.79736328125 634.771484375
131.82093811035156 584.59716796875 141.35507202148438 880.2279052734375
513.0730590820312 798.75 906.6400146484375 447.0837097167969
208.50296020507812 499.74163818359375 1221.7332763671875 703.2756958007812
151.7079315185547 538.6076049804688 1171.26611328125 744.8568115234375
455.8720397949219 923.6216430664062 293.6156311035156 520.2632446289062
755.9539184570312 425.693359375 721.1444702148438 557.8267822265625
236.97491455078125 803.6630249023438 345.28497314453125 573.0845336914062
256.1427917480469 751.1841430664062 815.1627807617188 575.2886352539062
150.6051483154297 808.0634765625 1100.4906005859375 376.1284484863281
451.0987548828125 783.4356689453125 845.8777465820312 475.60662841796875
190.6558074951172 726.7108764648438 198.06886291503906 806.1127319335938
253.4427947998047 811.1024169921875 680.5772094726562 602.2901611328125
474.2422180175781 889.94970703125 1053.8233642578125 782.5803833007812
255.00839233398438 917.341552734375 513.7374877929688 640.0439453125
1222.5206298828125 622.4488525390625 134.43798828125 701.2150268554688
159.8446807861328 635.2228393554688 117.02838134765625 618.8228759765625
292.8087158203125 783.5065307617188 885.8173217773438 486.5264587402344
270.91424560546875 630.530029296875 1082.656494140625 474.8466491699219
169.22506713867188 690.652587890625 534.94384765625 663.4573364257812
184.35617065429688 718.0018920898438 575.8900756835938 588.4228515625
145.36634826660156 793.1689453125 241.65538024902344 558.2052612304688
335.16741943359375 476.04388427734375 700.72900390625 300.7030334472656
338.5652160644531 602.8555908203125 545.0261840820312 405.92352294921875
195.44569396972656 486.26318359375 1114.947998046875 454.03131103515625
444.54022216796875 505.8288879394531 1046.0294189453125 507.0823059082031
Я запускаю код несколько раз так:
for i in `seq 10`; do sort -R d.dat > d_temp; python minimal.py; done
Это означает, что яиспользовать тот же набор пар точечных точек соответствия, но я их тасую.В результате я получил восстановленные повороты (в данном случае) вокруг оси X между 14 и 23 градусами, вокруг оси Y от 11 до 33 градусов и вокруг оси Z от -45 до -50 градусов.Вектор перевода также сильно различается (x -> [-.99: 0.49]. Y -> [-0.11: 0.15], z -> [-0.9: 0] (у меня нет абсолютного измерения для вращения, ноперевод должен быть в X ~ 42см, Y и Z ~ 0см)).Насколько я понимаю, это связано с Ransac и 5-точечным алгоритмом.Они не являются детерминированными (если имеется более 5 действительных соответствий характерных точек (можете ли вы исправить меня?)).
Но в этом случае как можно узнать, какой «порядок» соответствующих характерных точек дает реальноенеобходимая матрица?Или есть лучший алгоритм для решения этой проблемы?