Closed Janphr closed 1 year ago
Hi, there is one thing you need to check. That is whether the corners extraction and correspondence of the image and point cloud are correct as the following figure shows.
It's like in the Video in my case, so in 3 of the image/point cloud pairs the corner extraction works and i only have to redraw them in the image. All the others I skip. The same way you do it in the video... Maybe I can record a video tomorrow
OK, I'll wait your video.
There you go :)
*here's a link to a better quality: https://nextcloud.telematik-zentrum.de/index.php/s/dzcHpqn5QJ9Yx5M
Hi, sorry to reply too late. The problem you encountered is very weird. I have two suggestions.
First, check whether your code is the latest.
Second, check your OpenCV version. In this repo, OpenCV is used to obtain the initial guess for the PnP. You can print the initial guess here to check whether it is normal.
Hey,
I do use a newer OpenCV, but it only required me to update a few constant types. Usually not a problem...
Here is the output when i set debug and verbose to true:
fx: 2371.32 fy: 2371.41 cx: 763.882 cy: 576.72 682.978 850.021 670.539 503.497 765.315 987.445 767.092 546.738 783.085 1075.11 769.461 480.395 323.467 454.986 678.332 543.851 258.892 422.995 715.656 549.775 165.88 389.819 782.6 552.737 1 1 1 1 1 1 1 1 1 1 1 1 10.0397 10.0122 9.87967 9.90715 7.88032 7.80647 7.71352 7.78737 5.90076 5.79592 5.74828 5.85312 0.356181 -0.339073 0.408142 1.1034 -0.0100035 -0.728594 -0.0210648 0.697525 -0.0601553 -0.759965 -0.0231655 0.676644 0.882614 0.327666 -0.601901 -0.0469525 0.875364 0.355485 -0.609276 -0.0893971 0.863823 0.324032 -0.621937 -0.0821458 1 1 1 1 1 1 1 1 1 1 1 1 iteration= 0 chi2= 18097501193.787075 time= 4.8605e-05 cumTime= 4.8605e-05 edges= 12 schur= 0 lambda= 834867804.016248 levenbergIter= 1 iteration= 1 chi2= 9473924218.287287 time= 1.0637e-05 cumTime= 5.9242e-05 edges= 12 schur= 0 lambda= 556578536.010832 levenbergIter= 1 iteration= 2 chi2= 5893100293.544987 time= 8.45e-06 cumTime= 6.7692e-05 edges= 12 schur= 0 lambda= 371052357.340555 levenbergIter= 1 iteration= 3 chi2= 3389280756.625223 time= 8.701e-06 cumTime= 7.6393e-05 edges= 12 schur= 0 lambda= 247368238.227036 levenbergIter= 1 iteration= 4 chi2= 1720957763.709346 time= 7.888e-06 cumTime= 8.4281e-05 edges= 12 schur= 0 lambda= 164912158.818024 levenbergIter= 1 iteration= 5 chi2= 1044394831.978458 time= 7.648e-06 cumTime= 9.1929e-05 edges= 12 schur= 0 lambda= 109941439.212016 levenbergIter= 1 iteration= 6 chi2= 753783301.464014 time= 7.506e-06 cumTime= 9.9435e-05 edges= 12 schur= 0 lambda= 73294292.808011 levenbergIter= 1 iteration= 7 chi2= 723767108.947105 time= 7.872e-06 cumTime= 0.000107307 edges= 12 schur= 0 lambda= 48862861.872007 levenbergIter= 1 iteration= 8 chi2= 653362689.061365 time= 7.628e-06 cumTime= 0.000114935 edges= 12 schur= 0 lambda= 32575241.248005 levenbergIter= 1 iteration= 9 chi2= 644610593.730741 time= 1.5361e-05 cumTime= 0.000130296 edges= 12 schur= 0 lambda= 173734619.989359 levenbergIter= 3 iteration= 10 chi2= 624594226.254905 time= 1.1351e-05 cumTime= 0.000141647 edges= 12 schur= 0 lambda= 231646159.985812 levenbergIter= 2 iteration= 11 chi2= 612655026.145761 time= 1.131e-05 cumTime= 0.000152957 edges= 12 schur= 0 lambda= 308861546.647749 levenbergIter= 2 iteration= 12 chi2= 604480816.288880 time= 1.1459e-05 cumTime= 0.000164416 edges= 12 schur= 0 lambda= 411815395.530332 levenbergIter= 2 iteration= 13 chi2= 592056380.117380 time= 1.49e-05 cumTime= 0.000179316 edges= 12 schur= 0 lambda= 2196348776.161771 levenbergIter= 3 iteration= 14 chi2= 590011478.968085 time= 1.1177e-05 cumTime= 0.000190493 edges= 12 schur= 0 lambda= 2928465034.882361 levenbergIter= 2 iteration= 15 chi2= 587805308.612349 time= 1.1624e-05 cumTime= 0.000202117 edges= 12 schur= 0 lambda= 3904620046.509815 levenbergIter= 2 iteration= 16 chi2= 587398511.389534 time= 7.654e-06 cumTime= 0.000209771 edges= 12 schur= 0 lambda= 2603080031.006543 levenbergIter= 1 iteration= 17 chi2= 586753988.727862 time= 1.1105e-05 cumTime= 0.000220876 edges= 12 schur= 0 lambda= 3470773374.675391 levenbergIter= 2 iteration= 18 chi2= 586657050.548368 time= 7.552e-06 cumTime= 0.000228428 edges= 12 schur= 0 lambda= 2313848916.450260 levenbergIter= 1 iteration= 19 chi2= 586388849.139607 time= 1.1309e-05 cumTime= 0.000239737 edges= 12 schur= 0 lambda= 3085131888.600347 levenbergIter= 2 iteration= 20 chi2= 586379322.338856 time= 7.725e-06 cumTime= 0.000247462 edges= 12 schur= 0 lambda= 2056754592.400231 levenbergIter= 1 iteration= 21 chi2= 586237685.878402 time= 1.1135e-05 cumTime= 0.000258597 edges= 12 schur= 0 lambda= 2742339456.533641 levenbergIter= 2 iteration= 22 chi2= 586016211.063787 time= 1.1087e-05 cumTime= 0.000269684 edges= 12 schur= 0 lambda= 3656452608.711521 levenbergIter= 2 iteration= 23 chi2= 585963325.338446 time= 7.648e-06 cumTime= 0.000277332 edges= 12 schur= 0 lambda= 2437635072.474347 levenbergIter= 1 iteration= 24 chi2= 585868947.157292 time= 1.1185e-05 cumTime= 0.000288517 edges= 12 schur= 0 lambda= 3250180096.632463 levenbergIter= 2 iteration= 25 chi2= 585838502.389597 time= 7.584e-06 cumTime= 0.000296101 edges= 12 schur= 0 lambda= 2166786731.088308 levenbergIter= 1 iteration= 26 chi2= 585783219.791033 time= 1.1286e-05 cumTime= 0.000307387 edges= 12 schur= 0 lambda= 2889048974.784411 levenbergIter= 2 iteration= 27 chi2= 585765320.024336 time= 7.524e-06 cumTime= 0.000314911 edges= 12 schur= 0 lambda= 1926032649.856274 levenbergIter= 1 iteration= 28 chi2= 585728245.464567 time= 1.1129e-05 cumTime= 0.00032604 edges= 12 schur= 0 lambda= 2568043533.141698 levenbergIter= 2 iteration= 29 chi2= 585726287.597358 time= 7.497e-06 cumTime= 0.000333537 edges= 12 schur= 0 lambda= 1712029022.094465 levenbergIter= 1 iteration= 30 chi2= 585704962.664324 time= 1.1227e-05 cumTime= 0.000344764 edges= 12 schur= 0 lambda= 2282705362.792621 levenbergIter= 2 iteration= 31 chi2= 585652191.429380 time= 1.1088e-05 cumTime= 0.000355852 edges= 12 schur= 0 lambda= 3043607150.390161 levenbergIter= 2 iteration= 32 chi2= 585642734.480102 time= 7.532e-06 cumTime= 0.000363384 edges= 12 schur= 0 lambda= 2029071433.593440 levenbergIter= 1 iteration= 33 chi2= 585610741.957258 time= 1.1122e-05 cumTime= 0.000374506 edges= 12 schur= 0 lambda= 2705428578.124587 levenbergIter= 2 iteration= 34 chi2= 585564107.006794 time= 1.1096e-05 cumTime= 0.000385602 edges= 12 schur= 0 lambda= 3607238104.166116 levenbergIter= 2 iteration= 35 chi2= 585547482.884104 time= 7.575e-06 cumTime= 0.000393177 edges= 12 schur= 0 lambda= 2404825402.777411 levenbergIter= 1 iteration= 36 chi2= 585542992.701682 time= 7.562e-06 cumTime= 0.000400739 edges= 12 schur= 0 lambda= 1603216935.184940 levenbergIter= 1 iteration= 37 chi2= 585533619.907361 time= 1.1104e-05 cumTime= 0.000411843 edges= 12 schur= 0 lambda= 2137622580.246587 levenbergIter= 2 iteration= 38 chi2= 585510395.710978 time= 1.1094e-05 cumTime= 0.000422937 edges= 12 schur= 0 lambda= 2850163440.328783 levenbergIter= 2 iteration= 39 chi2= 585506326.869826 time= 7.592e-06 cumTime= 0.000430529 edges= 12 schur= 0 lambda= 1900108960.219188 levenbergIter= 1 iteration= 40 chi2= 585491037.222664 time= 1.115e-05 cumTime= 0.000441679 edges= 12 schur= 0 lambda= 2533478613.625584 levenbergIter= 2 iteration= 41 chi2= 585465188.745582 time= 1.1141e-05 cumTime= 0.00045282 edges= 12 schur= 0 lambda= 3377971484.834112 levenbergIter= 2 iteration= 42 chi2= 585456116.446942 time= 7.528e-06 cumTime= 0.000460348 edges= 12 schur= 0 lambda= 2251980989.889408 levenbergIter= 1 iteration= 43 chi2= 585441240.696771 time= 1.1126e-05 cumTime= 0.000471474 edges= 12 schur= 0 lambda= 3002641319.852544 levenbergIter= 2 iteration= 44 chi2= 585434445.267989 time= 7.56e-06 cumTime= 0.000479034 edges= 12 schur= 0 lambda= 2001760879.901696 levenbergIter= 1 iteration= 45 chi2= 585423709.550220 time= 1.1313e-05 cumTime= 0.000490347 edges= 12 schur= 0 lambda= 2669014506.535594 levenbergIter= 2 iteration= 46 chi2= 585419204.851062 time= 7.696e-06 cumTime= 0.000498043 edges= 12 schur= 0 lambda= 1779343004.357063 levenbergIter= 1 iteration= 47 chi2= 585411169.855439 time= 1.108e-05 cumTime= 0.000509123 edges= 12 schur= 0 lambda= 2372457339.142750 levenbergIter= 2 iteration= 48 chi2= 585411073.486814 time= 7.641e-06 cumTime= 0.000516764 edges= 12 schur= 0 lambda= 1581638226.095166 levenbergIter= 1 iteration= 49 chi2= 585407027.395537 time= 1.1057e-05 cumTime= 0.000527821 edges= 12 schur= 0 lambda= 2108850968.126889 levenbergIter= 2 iteration= 50 chi2= 585393514.471661 time= 1.1075e-05 cumTime= 0.000538896 edges= 12 schur= 0 lambda= 2811801290.835851 levenbergIter= 2 iteration= 51 chi2= 585392448.732366 time= 7.54e-06 cumTime= 0.000546436 edges= 12 schur= 0 lambda= 1874534193.890567 levenbergIter= 1 iteration= 52 chi2= 585383490.590204 time= 1.1137e-05 cumTime= 0.000557573 edges= 12 schur= 0 lambda= 2499378925.187423 levenbergIter= 2 iteration= 53 chi2= 585366851.102270 time= 1.1095e-05 cumTime= 0.000568668 edges= 12 schur= 0 lambda= 3332505233.583230 levenbergIter= 2 iteration= 54 chi2= 585361484.281560 time= 7.584e-06 cumTime= 0.000576252 edges= 12 schur= 0 lambda= 2221670155.722153 levenbergIter= 1 iteration= 55 chi2= 585351648.226872 time= 1.1142e-05 cumTime= 0.000587394 edges= 12 schur= 0 lambda= 2962226874.296204 levenbergIter= 2 iteration= 56 chi2= 585347519.810632 time= 7.568e-06 cumTime= 0.000594962 edges= 12 schur= 0 lambda= 1974817916.197469 levenbergIter= 1 iteration= 57 chi2= 585340201.391233 time= 1.095e-05 cumTime= 0.000605912 edges= 12 schur= 0 lambda= 2633090554.929959 levenbergIter= 2 iteration= 58 chi2= 585337464.134367 time= 7.515e-06 cumTime= 0.000613427 edges= 12 schur= 0 lambda= 1755393703.286639 levenbergIter= 1 iteration= 59 chi2= 585331834.862625 time= 1.1117e-05 cumTime= 0.000624544 edges= 12 schur= 0 lambda= 2340524937.715519 levenbergIter= 2 iteration= 60 chi2= 585322075.372998 time= 1.1159e-05 cumTime= 0.000635703 edges= 12 schur= 0 lambda= 3120699916.954025 levenbergIter= 2 iteration= 61 chi2= 585317657.137374 time= 7.495e-06 cumTime= 0.000643198 edges= 12 schur= 0 lambda= 2080466611.302683 levenbergIter= 1 iteration= 62 chi2= 585317112.600467 time= 7.632e-06 cumTime= 0.00065083 edges= 12 schur= 0 lambda= 1386977740.868455 levenbergIter= 1 iteration= 63 chi2= 585316090.320732 time= 1.12e-05 cumTime= 0.00066203 edges= 12 schur= 0 lambda= 1849303654.491274 levenbergIter= 2 iteration= 64 chi2= 585309318.314095 time= 1.1025e-05 cumTime= 0.000673055 edges= 12 schur= 0 lambda= 2465738205.988365 levenbergIter= 2 iteration= 65 chi2= 585296635.798168 time= 1.0237e-05 cumTime= 0.000683292 edges= 12 schur= 0 lambda= 3287650941.317820 levenbergIter= 2 iteration= 66 chi2= 585292210.673859 time= 7.006e-06 cumTime= 0.000690298 edges= 12 schur= 0 lambda= 2191767294.211880 levenbergIter= 1 iteration= 67 chi2= 585284282.284147 time= 1.0254e-05 cumTime= 0.000700552 edges= 12 schur= 0 lambda= 2922356392.282506 levenbergIter= 2 iteration= 68 chi2= 585280652.382073 time= 7.011e-06 cumTime= 0.000707563 edges= 12 schur= 0 lambda= 1948237594.855004 levenbergIter= 1 iteration= 69 chi2= 585274479.965347 time= 1.0266e-05 cumTime= 0.000717829 edges= 12 schur= 0 lambda= 2597650126.473338 levenbergIter= 2 iteration= 70 chi2= 585271894.614969 time= 7.042e-06 cumTime= 0.000724871 edges= 12 schur= 0 lambda= 1731766750.982225 levenbergIter= 1 iteration= 71 chi2= 585266981.849206 time= 1.0564e-05 cumTime= 0.000735435 edges= 12 schur= 0 lambda= 2309022334.642967 levenbergIter= 2 iteration= 72 chi2= 585258531.385371 time= 1.0317e-05 cumTime= 0.000745752 edges= 12 schur= 0 lambda= 3078696446.190623 levenbergIter= 2 iteration= 73 chi2= 585254486.848716 time= 7.072e-06 cumTime= 0.000752824 edges= 12 schur= 0 lambda= 2052464297.460415 levenbergIter= 1 iteration= 74 chi2= 585253772.055091 time= 6.977e-06 cumTime= 0.000759801 edges= 12 schur= 0 lambda= 1368309531.640277 levenbergIter= 1 iteration= 75 chi2= 585252798.863550 time= 1.0375e-05 cumTime= 0.000770176 edges= 12 schur= 0 lambda= 1824412708.853702 levenbergIter= 2 iteration= 76 chi2= 585246738.478973 time= 1.0315e-05 cumTime= 0.000780491 edges= 12 schur= 0 lambda= 2432550278.471602 levenbergIter= 2 iteration= 77 chi2= 585235126.971440 time= 1.034e-05 cumTime= 0.000790831 edges= 12 schur= 0 lambda= 3243400371.295470 levenbergIter= 2 iteration= 78 chi2= 585231013.162974 time= 7.076e-06 cumTime= 0.000797907 edges= 12 schur= 0 lambda= 2162266914.196980 levenbergIter= 1 iteration= 79 chi2= 585223521.663787 time= 1.0322e-05 cumTime= 0.000808229 edges= 12 schur= 0 lambda= 2883022552.262639 levenbergIter= 2 iteration= 80 chi2= 585220132.056293 time= 7.048e-06 cumTime= 0.000815277 edges= 12 schur= 0 lambda= 1922015034.841759 levenbergIter= 1 iteration= 81 chi2= 585214218.079464 time= 1.0365e-05 cumTime= 0.000825642 edges= 12 schur= 0 lambda= 2562686713.122345 levenbergIter= 2 iteration= 82 chi2= 585211932.869874 time= 7.099e-06 cumTime= 0.000832741 edges= 12 schur= 0 lambda= 1708457808.748230 levenbergIter= 1 iteration= 83 chi2= 585207283.713877 time= 1.0326e-05 cumTime= 0.000843067 edges= 12 schur= 0 lambda= 2277943744.997640 levenbergIter= 2 iteration= 84 chi2= 585198851.219331 time= 1.0451e-05 cumTime= 0.000853518 edges= 12 schur= 0 lambda= 3037258326.663520 levenbergIter= 2 iteration= 85 chi2= 585195065.287022 time= 7.027e-06 cumTime= 0.000860545 edges= 12 schur= 0 lambda= 2024838884.442347 levenbergIter= 1 iteration= 86 chi2= 585188742.423687 time= 1.031e-05 cumTime= 0.000870855 edges= 12 schur= 0 lambda= 2699785179.256462 levenbergIter= 2 iteration= 87 chi2= 585185652.905362 time= 7.075e-06 cumTime= 0.00087793 edges= 12 schur= 0 lambda= 1799856786.170975 levenbergIter= 1 iteration= 88 chi2= 585180428.949876 time= 3.727e-06 cumTime= 0.000881657 edges= 12 schur= 0 lambda= 2399809048.227966 levenbergIter= 2 iteration= 89 chi2= 585179037.955416 time= 2.057e-06 cumTime= 0.000883714 edges= 12 schur= 0 lambda= 1599872698.818644 levenbergIter= 1 iteration= 90 chi2= 585175437.286413 time= 2.999e-06 cumTime= 0.000886713 edges= 12 schur= 0 lambda= 2133163598.424859 levenbergIter= 2 iteration= 91 chi2= 585166992.682432 time= 2.98e-06 cumTime= 0.000889693 edges= 12 schur= 0 lambda= 2844218131.233145 levenbergIter= 2 iteration= 92 chi2= 585164541.952635 time= 2.049e-06 cumTime= 0.000891742 edges= 12 schur= 0 lambda= 1896145420.822096 levenbergIter= 1 iteration= 93 chi2= 585158245.016723 time= 3.016e-06 cumTime= 0.000894758 edges= 12 schur= 0 lambda= 2528193894.429461 levenbergIter= 2 iteration= 94 chi2= 585157663.434467 time= 2.042e-06 cumTime= 0.0008968 edges= 12 schur= 0 lambda= 1685462596.286308 levenbergIter= 1 iteration= 95 chi2= 585153342.447567 time= 3.015e-06 cumTime= 0.000899815 edges= 12 schur= 0 lambda= 2247283461.715076 levenbergIter= 2 iteration= 96 chi2= 585143088.660033 time= 3.004e-06 cumTime= 0.000902819 edges= 12 schur= 0 lambda= 2996377948.953435 levenbergIter= 2 iteration= 97 chi2= 585140181.018841 time= 2.049e-06 cumTime= 0.000904868 edges= 12 schur= 0 lambda= 1997585299.302290 levenbergIter= 1 iteration= 98 chi2= 585132980.598147 time= 3.017e-06 cumTime= 0.000907885 edges= 12 schur= 0 lambda= 2663447065.736386 levenbergIter= 2 iteration= 99 chi2= 585131472.675902 time= 2.048e-06 cumTime= 0.000909933 edges= 12 schur= 0 lambda= 1775631377.157591 levenbergIter= 1 initial R= [-0,0093562016, -0,99995476, -0,0017035652; 0,0033768467, 0,0016720343, -0,99999291; 0,99995053, -0,0093618883, 0,0033610503] initial t= [0,1145853; -0,21681133; -0,068069853] SolvePnP: solve time cost = 0.00652295 seconds. SolvePnP: estimated model: -0.986164 -0.158979 -0.04697 7.74159 -0.153535 0.769088 0.620428 1.08856 -0.062511 0.619055 -0.782856 0.489114 0 0 0 1 CalibrationResult: Calibration is finished with 3 pairs of data. CalibrationResult: The calibrated transformation from lidar to camera is: -0.986164 -0.158979 -0.04697 7.74159 -0.153535 0.769088 0.620428 1.08856 -0.062511 0.619055 -0.782856 0.489114 0 0 0 1 CalibrationResult: The calibrated transformation from camera to lidar is: -0.986164 -0.153535 -0.062511 7.83218 -0.158979 0.769088 0.619055 0.0907628 -0.04697 0.620428 -0.782856 0.0711552 0 0 0 1
Hi, this is my verbose output. Our initial guess is almost the same. But the result of g2o optimization is different. So I wonder what version of your g2o. You can try the 20200410_git
version of g2o which I am using.
fx: 2371.32
fy: 2371.41
cx: 763.882
cy: 576.72
765 986.4 765 547.2 784.8 1076.4 766.8 477
259.2 419.4 714.6 549 162 388.8 781.2 547.2
1 1 1 1 1 1 1 1
7.88032 7.80647 7.71352 7.78737 5.90076 5.79592 5.74828 5.85312
-0.0100035 -0.728594 -0.0210648 0.697525 -0.0601553 -0.759965 -0.0231655 0.676644
0.875364 0.355485 -0.609276 -0.0893971 0.863823 0.324032 -0.621937 -0.0821458
1 1 1 1 1 1 1 1
iteration= 0 chi2= 42.709232 time= 8.8849e-05 cumTime= 8.8849e-05 edges= 8 schur= 0 lambda= 152.558831 levenbergIter= 1
iteration= 1 chi2= 42.506952 time= 2.0988e-05 cumTime= 0.000109837 edges= 8 schur= 0 lambda= 50.852944 levenbergIter= 1
iteration= 2 chi2= 42.506948 time= 1.6199e-05 cumTime= 0.000126036 edges= 8 schur= 0 lambda= 33.901963 levenbergIter= 1
iteration= 3 chi2= 42.506948 time= 1.4254e-05 cumTime= 0.00014029 edges= 8 schur= 0 lambda= 22.601308 levenbergIter= 1
iteration= 4 chi2= 42.506948 time= 2.1074e-05 cumTime= 0.000161364 edges= 8 schur= 0 lambda= 30.135078 levenbergIter= 2
iteration= 5 chi2= 42.506948 time= 1.3097e-05 cumTime= 0.000174461 edges= 8 schur= 0 lambda= 20.090052 levenbergIter= 1
iteration= 6 chi2= 42.506948 time= 5.3073e-05 cumTime= 0.000227534 edges= 8 schur= 0 lambda= 5392882234.771203 levenbergIter= 7
initial R=
[-0.007204837079124338, -0.9999037122179297, 0.01185987417582013;
-0.02046688959083909, -0.01171024348210403, -0.9997219496580365;
0.9997645706630832, -0.007445568507028211, -0.02038054848298931]
initial t=
[0.04614209151443596;
-0.008098835615409049;
-0.06495254951535746]
SolvePnP: solve time cost = 0.000935216 seconds.
SolvePnP: estimated model:
0.00222436 -0.999933 0.0113986 -0.0279186
-0.0130307 -0.0114267 -0.99985 -0.0646112
0.999913 0.0020755 -0.0130552 -0.114777
0 0 0 1
CalibrationResult: Calibration is finished with 2 pairs of data.
CalibrationResult: The calibrated transformation from lidar to camera is:
0.00222436 -0.999933 0.0113986 -0.0279186
-0.0130307 -0.0114267 -0.99985 -0.0646112
0.999913 0.0020755 -0.0130552 -0.114777
0 0 0 1
CalibrationResult: The calibrated transformation from camera to lidar is:
0.00222436 -0.0130307 0.999913 0.113987
-0.999933 -0.0114267 0.0020755 -0.0284168
0.0113986 -0.99985 -0.0130552 -0.0657817
0 0 0 1
Hey,
I followed the steps in your video and every step is pretty much the same, but in the end the pose is completely different. Any idea what I could have done wrong?
Best regards!