public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug some bugs and recalibrate

Browse Source
This commit is contained in:
gfuse
2021-10-07 16:25:27 +02:00
parent cf15e3fa15
commit 45ed85c0e9
12 changed files with 95 additions and 7632 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-636
View File
@@ -1,636 +0,0 @@
[22:29:46.549] New key frame
[22:29:46.550] BA loop run
[22:29:46.552] init error: -nan
[22:29:46.557] end error: -nan
[22:29:46.660] New key frame
[22:29:46.660] BA loop run
[22:29:46.662] init error: -nan
[22:29:46.667] end error: -nan
[22:29:46.763] New key frame
[22:29:46.763] BA loop run
[22:29:46.766] init error: -nan
[22:29:46.771] end error: -nan
[22:29:46.865] New key frame
[22:29:46.865] BA loop run
[22:29:46.867] init error: -nan
[22:29:46.872] end error: -nan
[22:29:46.962] New key frame
[22:29:46.962] BA loop run
[22:29:46.964] init error: -nan
[22:29:46.969] end error: -nan
[22:29:47.066] New key frame
[22:29:47.066] BA loop run
[22:29:47.068] init error: -nan
[22:29:47.072] end error: -nan
[22:29:47.159] New key frame
[22:29:47.159] BA loop run
[22:29:47.161] init error: -nan
[22:29:47.165] end error: -nan
[22:29:47.254] New key frame
[22:29:47.254] BA loop run
[22:29:47.256] init error: -nan
[22:29:47.259] end error: -nan
[22:29:47.352] New key frame
[22:29:47.353] BA loop run
[22:29:47.355] init error: -nan
[22:29:47.367] end error: -nan
[22:29:47.517] New key frame
[22:29:47.517] BA loop run
[22:29:47.518] init error: -nan
[22:29:47.527] end error: -nan
[22:29:47.693] New key frame
[22:29:47.693] BA loop run
[22:29:47.695] init error: -nan
[22:29:47.720] end error: -nan
[22:29:47.868] New key frame
[22:29:47.868] BA loop run
[22:29:47.869] init error: -nan
[22:29:47.903] end error: -nan
[22:29:48.044] New key frame
[22:29:48.044] BA loop run
[22:29:48.046] init error: -nan
[22:29:48.081] end error: -nan
[22:29:48.309] New key frame
[22:29:48.309] BA loop run
[22:29:48.310] init error: -nan
[22:29:48.611] end error: -nan
[22:29:48.612] New key frame
[22:29:48.612] BA loop run
[22:29:48.613] init error: 15.914324
[22:29:49.243] end error: 0.029411
[22:29:49.245] New key frame
[22:29:49.245] BA loop run
[22:29:49.246] init error: 11.262837
[22:29:49.655] end error: 0.003359
[22:29:50.503] New key frame
[22:29:50.504] BA loop run
[22:29:50.504] init error: 3.479064
[22:29:50.505] end error: 0.000004
[22:31:18.793] New key frame
[22:31:18.793] BA loop run
[22:31:18.794] init error: 11696.693108
[22:31:18.794] end error: 2.302663
[22:31:18.888] New key frame
[22:31:18.888] BA loop run
[22:31:18.888] init error: 1239.227021
[22:31:18.889] end error: 0.000000
[22:31:19.000] New key frame
[22:31:19.000] BA loop run
[22:31:19.000] init error: 4602.244432
[22:31:19.001] end error: 0.000004
[22:31:19.097] New key frame
[22:31:19.097] BA loop run
[22:31:19.097] init error: 753.876451
[22:31:19.097] end error: 0.364979
[22:31:19.187] New key frame
[22:31:19.188] BA loop run
[22:31:19.188] init error: 489.018266
[22:31:19.188] end error: 0.000001
[22:31:19.385] New key frame
[22:31:19.385] BA loop run
[22:31:19.385] init error: 4919.827797
[22:31:19.386] end error: 1.670606
[22:31:19.504] New key frame
[22:31:19.504] BA loop run
[22:31:19.504] init error: 1674659.415647
[22:31:19.505] end error: 89.302075
[22:31:19.590] New key frame
[22:31:19.590] BA loop run
[22:31:19.591] init error: 3310.529731
[22:31:19.591] end error: 10.137941
[22:31:19.694] New key frame
[22:31:19.694] BA loop run
[22:31:19.694] init error: 1774.557852
[22:31:19.694] end error: 0.001622
[22:31:19.879] New key frame
[22:31:19.879] BA loop run
[22:31:19.879] init error: 11939.450031
[22:31:19.880] end error: 3.465171
[22:31:19.988] New key frame
[22:31:19.988] BA loop run
[22:31:19.989] init error: 16959.020042
[22:31:19.989] end error: 7.031975
[22:31:20.087] New key frame
[22:31:20.087] BA loop run
[22:31:20.087] init error: 1291.617951
[22:31:20.088] end error: 0.535589
[22:31:20.174] New key frame
[22:31:20.174] BA loop run
[22:31:20.174] init error: 1090.877340
[22:31:20.175] end error: 0.000000
[22:31:20.308] New key frame
[22:31:20.308] BA loop run
[22:31:20.308] init error: 514.202902
[22:31:20.308] end error: 0.000000
[22:31:20.375] New key frame
[22:31:20.375] BA loop run
[22:31:20.375] init error: 55999.137403
[22:31:20.376] end error: 0.292111
[22:31:20.577] New key frame
[22:31:20.577] BA loop run
[22:31:20.578] init error: 457.175767
[22:31:20.578] end error: 0.000000
[22:31:20.806] New key frame
[22:31:20.806] BA loop run
[22:31:20.806] init error: 70.463540
[22:31:20.806] end error: 0.000000
[22:31:20.869] New key frame
[22:31:20.869] BA loop run
[22:31:20.870] init error: 96.801227
[22:31:20.870] end error: 0.000000
[22:31:21.020] New key frame
[22:31:21.020] BA loop run
[22:31:21.021] init error: 79.855318
[22:31:21.022] end error: 0.000000
[22:31:21.140] New key frame
[22:31:21.140] BA loop run
[22:31:21.141] init error: 66.740565
[22:31:21.142] end error: 0.000000
[22:31:21.242] New key frame
[22:31:21.242] BA loop run
[22:31:21.242] init error: 506.780274
[22:31:21.243] end error: 0.000000
[22:31:21.341] New key frame
[22:31:21.341] BA loop run
[22:31:21.341] init error: 54.567235
[22:31:21.342] end error: 0.000000
[22:31:21.447] New key frame
[22:31:21.447] BA loop run
[22:31:21.448] init error: 1.458657
[22:31:21.448] end error: 0.000000
[22:31:21.697] New key frame
[22:31:21.697] BA loop run
[22:31:21.697] init error: 11.303527
[22:31:21.698] end error: 0.000000
[22:31:21.804] New key frame
[22:31:21.804] BA loop run
[22:31:21.805] init error: 2.341826
[22:31:21.805] end error: 0.000000
[22:31:22.017] New key frame
[22:31:22.017] BA loop run
[22:31:22.017] init error: 0.283735
[22:31:22.017] end error: 0.000000
[22:31:22.106] New key frame
[22:31:22.106] BA loop run
[22:31:22.106] init error: 0.335039
[22:31:22.106] end error: 0.000000
[22:31:22.205] New key frame
[22:31:22.205] BA loop run
[22:31:22.205] init error: 1.167879
[22:31:22.206] end error: 0.000000
[22:31:22.332] New key frame
[22:31:22.332] BA loop run
[22:31:22.332] init error: 4.548102
[22:31:22.333] end error: 0.000000
[22:31:22.438] New key frame
[22:31:22.438] BA loop run
[22:31:22.439] init error: 81.310648
[22:31:22.440] end error: 0.000000
[22:31:22.557] New key frame
[22:31:22.557] BA loop run
[22:31:22.558] init error: 68.456043
[22:31:22.559] end error: 0.000000
[22:31:22.658] New key frame
[22:31:22.658] BA loop run
[22:31:22.659] init error: 153.170832
[22:31:22.660] end error: 0.000000
[22:31:22.753] New key frame
[22:31:22.753] BA loop run
[22:31:22.754] init error: 332.802449
[22:31:22.755] end error: 0.000000
[22:31:22.846] New key frame
[22:31:22.846] BA loop run
[22:31:22.847] init error: 75.240345
[22:31:22.848] end error: 0.000000
[22:31:22.934] New key frame
[22:31:22.934] BA loop run
[22:31:22.935] init error: 104.424590
[22:31:22.935] end error: 0.000000
[22:31:23.027] New key frame
[22:31:23.027] BA loop run
[22:31:23.028] init error: 284.629228
[22:31:23.029] end error: 0.000000
[22:31:23.133] New key frame
[22:31:23.133] BA loop run
[22:31:23.134] init error: 13.221286
[22:31:23.134] end error: 0.000000
[22:31:23.221] New key frame
[22:31:23.221] BA loop run
[22:31:23.222] init error: 15.573307
[22:31:23.222] end error: 0.000000
[22:31:23.313] New key frame
[22:31:23.313] BA loop run
[22:31:23.313] init error: 11.081408
[22:31:23.314] end error: 0.000000
[22:31:23.426] New key frame
[22:31:23.426] BA loop run
[22:31:23.427] init error: 155.448072
[22:31:23.428] end error: 0.000000
[22:31:23.520] New key frame
[22:31:23.520] BA loop run
[22:31:23.520] init error: 312.553095
[22:31:23.521] end error: 0.000000
[22:31:23.709] New key frame
[22:31:23.709] BA loop run
[22:31:23.709] init error: 65.996668
[22:31:23.710] end error: 0.000000
[22:31:23.809] New key frame
[22:31:23.809] BA loop run
[22:31:23.809] init error: 4739.644774
[22:31:23.810] end error: 0.000000
[22:31:23.911] New key frame
[22:31:23.911] BA loop run
[22:31:23.912] init error: 140.378159
[22:31:23.912] end error: 0.000000
[22:31:24.016] New key frame
[22:31:24.016] BA loop run
[22:31:24.017] init error: 56.065433
[22:31:24.018] end error: 0.000000
[22:31:24.111] New key frame
[22:31:24.111] BA loop run
[22:31:24.111] init error: 115.625364
[22:31:24.112] end error: 0.000000
[22:31:24.208] New key frame
[22:31:24.208] BA loop run
[22:31:24.208] init error: 501.796825
[22:31:24.209] end error: 0.000000
[22:31:24.311] New key frame
[22:31:24.311] BA loop run
[22:31:24.312] init error: 491.890705
[22:31:24.312] end error: 0.000000
[22:31:24.425] New key frame
[22:31:24.425] BA loop run
[22:31:24.426] init error: 212.894163
[22:31:24.426] end error: 0.000000
[22:31:24.517] New key frame
[22:31:24.517] BA loop run
[22:31:24.518] init error: 1748.696713
[22:31:24.519] end error: 0.000000
[22:31:24.609] New key frame
[22:31:24.609] BA loop run
[22:31:24.609] init error: 38.487453
[22:31:24.610] end error: 0.000000
[22:31:24.715] New key frame
[22:31:24.715] BA loop run
[22:31:24.716] init error: 117.109507
[22:31:24.717] end error: 0.000000
[22:31:24.834] New key frame
[22:31:24.835] BA loop run
[22:31:24.835] init error: 126.315516
[22:31:24.836] end error: 0.000000
[22:31:24.940] New key frame
[22:31:24.941] BA loop run
[22:31:24.941] init error: 2278.071738
[22:31:24.942] end error: 0.000000
[22:31:25.032] New key frame
[22:31:25.032] BA loop run
[22:31:25.033] init error: 292.094638
[22:31:25.034] end error: 0.000000
[22:31:25.131] New key frame
[22:31:25.131] BA loop run
[22:31:25.131] init error: 18.046504
[22:31:25.132] end error: 0.000000
[22:31:25.221] New key frame
[22:31:25.222] BA loop run
[22:31:25.222] init error: 15.136323
[22:31:25.223] end error: 0.000000
[22:31:25.320] New key frame
[22:31:25.320] BA loop run
[22:31:25.320] init error: 29.173001
[22:31:25.321] end error: 0.000000
[22:31:25.600] New key frame
[22:31:25.600] BA loop run
[22:31:25.601] init error: 310.140433
[22:31:25.602] end error: 0.000000
[22:31:25.721] New key frame
[22:31:25.721] BA loop run
[22:31:25.721] init error: 333.454276
[22:31:25.723] end error: 0.000000
[22:31:25.804] New key frame
[22:31:25.804] BA loop run
[22:31:25.805] init error: 35.690842
[22:31:25.805] end error: 0.000000
[22:31:26.052] New key frame
[22:31:26.052] BA loop run
[22:31:26.053] init error: 908.831586
[22:31:26.054] end error: 0.000000
[22:31:26.240] New key frame
[22:31:26.241] BA loop run
[22:31:26.241] init error: 3100.573611
[22:31:26.242] end error: 0.000000
[22:31:26.337] New key frame
[22:31:26.337] BA loop run
[22:31:26.338] init error: 7.552827
[22:31:26.339] end error: 0.000000
[22:31:26.423] New key frame
[22:31:26.423] BA loop run
[22:31:26.424] init error: 49.923934
[22:31:26.424] end error: 0.000000
[22:31:26.532] New key frame
[22:31:26.532] BA loop run
[22:31:26.533] init error: 56.848491
[22:31:26.533] end error: 0.000000
[22:31:26.656] New key frame
[22:31:26.657] BA loop run
[22:31:26.657] init error: 79.941534
[22:31:26.658] end error: 0.000000
[22:31:26.767] New key frame
[22:31:26.767] BA loop run
[22:31:26.768] init error: 122.897459
[22:31:26.769] end error: 0.000000
[22:31:26.875] New key frame
[22:31:26.875] BA loop run
[22:31:26.875] init error: 156.469338
[22:31:26.876] end error: 0.000000
[22:31:27.006] New key frame
[22:31:27.006] BA loop run
[22:31:27.007] init error: 156.802705
[22:31:27.008] end error: 0.000000
[22:31:27.109] New key frame
[22:31:27.109] BA loop run
[22:31:27.110] init error: 164.251649
[22:31:27.111] end error: 0.000000
[22:31:27.216] New key frame
[22:31:27.216] BA loop run
[22:31:27.217] init error: 131.059494
[22:31:27.217] end error: 0.000000
[22:31:27.325] New key frame
[22:31:27.325] BA loop run
[22:31:27.326] init error: 23.877881
[22:31:27.326] end error: 0.000000
[22:31:27.437] New key frame
[22:31:27.437] BA loop run
[22:31:27.437] init error: 4.858615
[22:31:27.438] end error: 0.000000
[22:31:27.565] New key frame
[22:31:27.565] BA loop run
[22:31:27.566] init error: 9.295936
[22:31:27.566] end error: 0.000000
[22:31:27.717] New key frame
[22:31:27.717] BA loop run
[22:31:27.718] init error: 18.324338
[22:31:27.719] end error: 0.000000
[22:31:27.877] New key frame
[22:31:27.877] BA loop run
[22:31:27.878] init error: 65.885126
[22:31:27.879] end error: 0.000000
[22:31:28.029] New key frame
[22:31:28.029] BA loop run
[22:31:28.030] init error: 1293.317479
[22:31:28.031] end error: 0.000000
[22:31:28.185] New key frame
[22:31:28.185] BA loop run
[22:31:28.185] init error: 99.517714
[22:31:28.186] end error: 0.000000
[22:31:28.334] New key frame
[22:31:28.334] BA loop run
[22:31:28.335] init error: 93.715354
[22:31:28.336] end error: 0.000000
[22:31:28.453] New key frame
[22:31:28.453] BA loop run
[22:31:28.453] init error: 110.644783
[22:31:28.455] end error: 0.000000
[22:31:28.569] New key frame
[22:31:28.569] BA loop run
[22:31:28.570] init error: 226.273434
[22:31:28.571] end error: 0.000000
[22:31:28.694] New key frame
[22:31:28.694] BA loop run
[22:31:28.695] init error: 103.323064
[22:31:28.696] end error: 0.000000
[22:31:28.813] New key frame
[22:31:28.814] BA loop run
[22:31:28.814] init error: 96.069307
[22:31:28.815] end error: 0.000000
[22:31:28.938] New key frame
[22:31:28.938] BA loop run
[22:31:28.939] init error: 121.023193
[22:31:28.940] end error: 0.000000
[22:31:29.050] New key frame
[22:31:29.050] BA loop run
[22:31:29.050] init error: 158.295558
[22:31:29.051] end error: 0.000000
[22:31:29.179] New key frame
[22:31:29.179] BA loop run
[22:31:29.180] init error: 277.136099
[22:31:29.181] end error: 0.000000
[22:31:29.300] New key frame
[22:31:29.300] BA loop run
[22:31:29.301] init error: 607.852927
[22:31:29.302] end error: 0.000000
[22:31:29.458] New key frame
[22:31:29.458] BA loop run
[22:31:29.458] init error: 63.952124
[22:31:29.459] end error: 0.000000
[22:31:29.594] New key frame
[22:31:29.594] BA loop run
[22:31:29.595] init error: 37.939308
[22:31:29.596] end error: 0.000000
[22:31:29.765] New key frame
[22:31:29.765] BA loop run
[22:31:29.766] init error: 24.319269
[22:31:29.767] end error: 0.000000
[22:31:30.086] New key frame
[22:31:30.086] BA loop run
[22:31:30.086] init error: 22.954792
[22:31:30.088] end error: 0.000000
[22:31:30.217] New key frame
[22:31:30.217] BA loop run
[22:31:30.217] init error: 39.346060
[22:31:30.218] end error: 0.000000
[22:31:30.368] New key frame
[22:31:30.368] BA loop run
[22:31:30.368] init error: 23.493694
[22:31:30.370] end error: 0.000000
[22:31:30.523] New key frame
[22:31:30.523] BA loop run
[22:31:30.523] init error: 42.977544
[22:31:30.524] end error: 0.000000
[22:31:30.846] New key frame
[22:31:30.846] BA loop run
[22:31:30.847] init error: 2383.585825
[22:31:30.848] end error: 0.000000
[22:31:31.004] New key frame
[22:31:31.004] BA loop run
[22:31:31.004] init error: 244.533693
[22:31:31.005] end error: 0.000000
[22:31:31.151] New key frame
[22:31:31.151] BA loop run
[22:31:31.151] init error: 131.799398
[22:31:31.152] end error: 0.000000
[22:31:31.314] New key frame
[22:31:31.314] BA loop run
[22:31:31.314] init error: 106.001946
[22:31:31.315] end error: 0.000000
[22:31:31.454] New key frame
[22:31:31.454] BA loop run
[22:31:31.455] init error: 254.228714
[22:31:31.456] end error: 0.000000
[22:31:31.610] New key frame
[22:31:31.610] BA loop run
[22:31:31.610] init error: 219.203953
[22:31:31.611] end error: 0.000000
[22:31:31.776] New key frame
[22:31:31.776] BA loop run
[22:31:31.777] init error: 332.295279
[22:31:31.778] end error: 0.000000
[22:31:31.915] New key frame
[22:31:31.915] BA loop run
[22:31:31.915] init error: 388.566147
[22:31:31.916] end error: 0.000000
[22:31:32.033] New key frame
[22:31:32.033] BA loop run
[22:31:32.033] init error: 226.490780
[22:31:32.034] end error: 0.000000
[22:31:32.168] New key frame
[22:31:32.168] BA loop run
[22:31:32.168] init error: 497.479135
[22:31:32.169] end error: 0.000000
[22:31:32.318] New key frame
[22:31:32.318] BA loop run
[22:31:32.319] init error: 181.703451
[22:31:32.320] end error: 0.000000
[22:31:32.454] New key frame
[22:31:32.454] BA loop run
[22:31:32.455] init error: 225.129429
[22:31:32.456] end error: 0.000000
[22:31:32.592] New key frame
[22:31:32.592] BA loop run
[22:31:32.593] init error: 309.249447
[22:31:32.594] end error: 0.000000
[22:31:32.725] New key frame
[22:31:32.725] BA loop run
[22:31:32.725] init error: 251.550786
[22:31:32.726] end error: 0.000000
[22:31:32.847] New key frame
[22:31:32.847] BA loop run
[22:31:32.847] init error: 263.837745
[22:31:32.848] end error: 0.000000
[22:31:32.968] New key frame
[22:31:32.968] BA loop run
[22:31:32.968] init error: 315.848235
[22:31:32.969] end error: 0.000000
[22:31:33.113] New key frame
[22:31:33.113] BA loop run
[22:31:33.114] init error: 240.650402
[22:31:33.115] end error: 0.000000
[22:31:33.252] New key frame
[22:31:33.252] BA loop run
[22:31:33.252] init error: 362.639353
[22:31:33.254] end error: 0.000000
[22:31:33.396] New key frame
[22:31:33.396] BA loop run
[22:31:33.396] init error: 317.858584
[22:31:33.397] end error: 0.000000
[22:31:33.528] New key frame
[22:31:33.528] BA loop run
[22:31:33.528] init error: 379.217499
[22:31:33.529] end error: 0.000000
[22:31:33.669] New key frame
[22:31:33.669] BA loop run
[22:31:33.669] init error: 159.891671
[22:31:33.670] end error: 0.000000
[22:31:33.791] New key frame
[22:31:33.791] BA loop run
[22:31:33.791] init error: 152.547726
[22:31:33.792] end error: 0.000000
[22:31:33.924] New key frame
[22:31:33.924] BA loop run
[22:31:33.925] init error: 470.502243
[22:31:33.925] end error: 0.000000
[22:31:34.374] New key frame
[22:31:34.374] BA loop run
[22:31:34.375] init error: 836.765949
[22:31:34.376] end error: 0.000000
[22:31:34.612] New key frame
[22:31:34.612] BA loop run
[22:31:34.612] init error: 618.659793
[22:31:34.613] end error: 0.000000
[22:31:34.858] New key frame
[22:31:34.858] BA loop run
[22:31:34.858] init error: 14268.603933
[22:31:34.859] end error: 0.000000
[22:31:35.073] New key frame
[22:31:35.073] BA loop run
[22:31:35.073] init error: 759.093617
[22:31:35.074] end error: 0.000000
[22:31:35.196] New key frame
[22:31:35.196] BA loop run
[22:31:35.197] init error: 399.800171
[22:31:35.197] end error: 0.000000
[22:31:35.316] New key frame
[22:31:35.316] BA loop run
[22:31:35.316] init error: 380.208476
[22:31:35.317] end error: 0.000000
[22:31:35.574] New key frame
[22:31:35.574] BA loop run
[22:31:35.574] init error: 359.733781
[22:31:35.575] end error: 0.000000
[22:31:35.682] New key frame
[22:31:35.682] BA loop run
[22:31:35.683] init error: 413.198884
[22:31:35.684] end error: 0.000000
[22:31:35.805] New key frame
[22:31:35.806] BA loop run
[22:31:35.806] init error: 476.467141
[22:31:35.807] end error: 0.000000
[22:31:35.930] New key frame
[22:31:35.930] BA loop run
[22:31:35.931] init error: 376.075532
[22:31:35.932] end error: 0.000000
[22:31:36.046] New key frame
[22:31:36.046] BA loop run
[22:31:36.046] init error: 490.398950
[22:31:36.047] end error: 0.000000
[22:31:36.337] New key frame
[22:31:36.337] BA loop run
[22:31:36.337] init error: 270.851463
[22:31:36.338] end error: 0.000000
[22:31:36.465] New key frame
[22:31:36.465] BA loop run
[22:31:36.466] init error: 265.473990
[22:31:36.467] end error: 0.000000
[22:31:36.566] New key frame
[22:31:36.566] BA loop run
[22:31:36.566] init error: 1090.064832
[22:31:36.567] end error: 0.000000
[22:31:36.764] New key frame
[22:31:36.764] BA loop run
[22:31:36.765] init error: 225.202629
[22:31:36.765] end error: 0.000000
[22:31:36.907] New key frame
[22:31:36.907] BA loop run
[22:31:36.907] init error: 18.103024
[22:31:36.908] end error: 0.000000
[22:31:37.038] New key frame
[22:31:37.039] BA loop run
[22:31:37.039] init error: 31.446433
[22:31:37.040] end error: 0.000000
[22:31:37.203] New key frame
[22:31:37.203] BA loop run
[22:31:37.204] init error: 22.914176
[22:31:37.205] end error: 0.000000
[22:31:37.345] New key frame
[22:31:37.345] BA loop run
[22:31:37.345] init error: 8.200829
[22:31:37.346] end error: 0.000000
[22:31:37.513] New key frame
[22:31:37.513] BA loop run
[22:31:37.514] init error: 19.818339
[22:31:37.515] end error: 0.002712
[22:31:37.651] New key frame
[22:31:37.651] BA loop run
[22:31:37.652] init error: 44.120608
[22:31:37.652] end error: 0.000000
[22:31:37.766] New key frame
[22:31:37.766] BA loop run
[22:31:37.766] init error: 25.558753
[22:31:37.767] end error: 0.000000
[22:31:37.884] New key frame
[22:31:37.884] BA loop run
[22:31:37.884] init error: 4.360590
[22:31:37.885] end error: 0.000000
[22:32:21.678] New key frame
[22:32:21.678] BA loop run
[22:32:21.678] init error: 0.650481
[22:32:21.679] end error: 0.000000
GPU_version/vio/frame_handler_log.txt
-6895
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/config.h
+5 -16
View File
@@ -124,21 +124,10 @@ public:
/// Gro white noise in continuous. /// Gro white noise in continuous.
static double& GYO_Noise() { return getInstance().GYO_noise; } static double& GYO_Noise() { return getInstance().GYO_noise; }
/// integration uncertainty continuous static double& VO_ekf_t() { return getInstance().vo_ekf_t; }
static double& IUC() { return getInstance().iuc; } static double& VO_ekf_o() { return getInstance().vo_ekf_o; }
/// acc bias in continuous static double& Cmd_ekf_t() { return getInstance().cmd_ekf_t; }
static double& ABC() { return getInstance().abc; } static double& Cmd_ekf_o() { return getInstance().cmd_ekf_o; }
/// gyro bias in continuous
static double& GBC() { return getInstance().gbc; }
/// error in the bias used for preintegration
static double& EBP() { return getInstance().ebp; }
/// the noise used for projection
static double& NRP() { return getInstance().nrp; }
static double& Svo_Ekf() { return getInstance().svo_ekf; }
static double& X_off() { return getInstance().acc_x_off; }
static double& Y_off() { return getInstance().acc_y_off; }
static double& Teta_off() { return getInstance().gyr_z_off; }
static double& Cmd_Cov() { return getInstance().cmd_cov; }
private: private:
@@ -174,7 +163,7 @@ private:
size_t max_fts; size_t max_fts;
size_t quality_min_fts; size_t quality_min_fts;
int quality_max_drop_fts; int quality_max_drop_fts;
double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise,svo_ekf,acc_x_off,acc_y_off,gyr_z_off,cmd_cov; double ACC_noise,GYO_noise,vo_ekf_t,vo_ekf_o,cmd_ekf_t,cmd_ekf_o;
}; };
} // namespace vio } // namespace vio
GPU_version/vio/include/vio/initialization.h
+1 -1
View File
@@ -118,7 +118,7 @@ protected:
xyz_in_cur=xyz_in_hom; xyz_in_cur=xyz_in_hom;
}else if((Homography.T_c2_from_c1.translation()-res.second.se3().translation()).norm()<0.1 && inlier_ekf.size()<inlier_hom.size()){ }else if((Homography.T_c2_from_c1.translation()-res.second.se3().translation()).norm()<0.1 && inlier_ekf.size()<inlier_hom.size()){
auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2); auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
res=ukf_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1)); res=ukf_->UpdateVO(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
T_cur_from_ref=res.second; T_cur_from_ref=res.second;
inliers=inlier_hom; inliers=inlier_hom;
xyz_in_cur=xyz_in_ekf; xyz_in_cur=xyz_in_ekf;
GPU_version/vio/include/vio/ukf.h
+27 -27
View File
@@ -10,6 +10,8 @@
#include <Eigen/Dense> #include <Eigen/Dense>
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <boost/function.hpp> #include <boost/function.hpp>
#include <chrono>
#include <ctime>
class Base{ class Base{
public: public:
Base(const Eigen::Matrix<double, 3, 1> &init) { Base(const Eigen::Matrix<double, 3, 1> &init) {
@@ -24,38 +26,32 @@ public:
}; };
void predict(double dx,double dy,double dpitch,double time){ void predict(double dx,double dy,double dpitch,double time){
double dt=0.2; double dt=0.2;
/*if(t_cmd==0.0){
t_cmd=time;
return;
}*/
t_cmd=time;
state_h_(2)=state_(2)+dt*dpitch; state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2))); state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2)));
state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2))); state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
state_h_(3)=dpitch; state_h_(3)=dpitch;
Eigen::Matrix<double,4,4> R; Eigen::Matrix<double,4,4> R;
R<<vio::Config::Cmd_Cov(),0,0,0, R<<vio::Config::Cmd_ekf_t(),0,0,0,
0,vio::Config::Cmd_Cov(),0,0, 0,vio::Config::Cmd_ekf_t(),0,0,
0,0,vio::Config::Cmd_Cov(),0, 0,0,vio::Config::Cmd_ekf_o(),0,
0,0,0,vio::Config::Cmd_Cov(); 0,0,0,vio::Config::Cmd_ekf_o();
Eigen::Matrix<double,4,4> G; Eigen::Matrix<double,4,4> G;
G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0, G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0,
0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0, 0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
0,0,1.0,dt, 0,0,1.0,dt,
0,0,0,1.0; 0,0,0,1.0;
cov_h_=G*cov_*G.transpose()+R; cov_h_=G*cov_*G.transpose()+R;
setTime();
} }
void correct(double ddx, double ddy, double dpitch, double time){ void correct(double ddx, double ddy, double dpitch, double time){
double dt=0.05; auto imu_end = std::chrono::steady_clock::now();
/*if(t_imu==0.0){ double dt=1e-9*(imu_end-vio_t).count();
t_imu=time; vio_t=std::chrono::steady_clock::now();
return;
}*/
t_imu=time;
Eigen::Matrix<double,4,4> H; Eigen::Matrix<double,4,4> H;
Eigen::Matrix<double,4,4> Q; Eigen::Matrix<double,4,4> Q;
Eigen::Matrix<double,4,1> E; Eigen::Matrix<double,4,1> E;
Eigen::Matrix<double,4,4> k; Eigen::Matrix<double,4,4> k;
//update
E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0); E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1); E(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(2)=state_(2)+dt*state_(3)-state_h_(2); E(2)=state_(2)+dt*state_(3)-state_h_(2);
@@ -71,6 +67,7 @@ public:
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+k*E;
cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
//predict
state_h_(2)=state_(2)+dt*state_(3); state_h_(2)=state_(2)+dt*state_(3);
state_h_(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))); state_h_(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))); state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)));
@@ -96,21 +93,23 @@ public:
H<<1.0,0,0,0, H<<1.0,0,0,0,
0,1.0,0,0, 0,1.0,0,0,
0,0,1.0,0; 0,0,1.0,0;
Q<<vio::Config::Svo_Ekf(),0,0, Q<<vio::Config::VO_ekf_t(),0,0,
0,vio::Config::Svo_Ekf(),0, 0,vio::Config::VO_ekf_t(),0,
0,0,10.0*vio::Config::Svo_Ekf(); 0,0,vio::Config::VO_ekf_o();
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2); E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+(k*E); state_=state_h_+(k*E);
cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
} }
void setTime(){
vio_t=std::chrono::steady_clock::now();
}
Eigen::Matrix<double,4,4> cov_; Eigen::Matrix<double,4,4> cov_;
Eigen::Matrix<double, 4, 1> state_; Eigen::Matrix<double, 4, 1> state_;
private: private:
double t_cmd=0.0; std::chrono::time_point<std::chrono::steady_clock,std::chrono::duration<double>> vio_t;
Eigen::Matrix<double,4,4> cov_h_; Eigen::Matrix<double,4,4> cov_h_;
Eigen::Matrix<double, 4, 1> state_h_;//state{x,y,pitch( rotation around z),dyaw} in world frame Eigen::Matrix<double, 4, 1> state_h_;//state{x,y,pitch( rotation around z),dyaw} in world frame
double t_imu=0.0;
}; };
@@ -125,8 +124,8 @@ public:
/* if(abs(x)<1.0)x=pow(x,3);//picked up from your code /* if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(y)<1.0)y=pow(y,3);//picked up from your code*/ if(abs(y)<1.0)y=pow(y,3);//picked up from your code*/
++imu_syn_count_; ++imu_syn_count_;
if(imu_syn_[imu_syn_count_ % 4]){ if(imu_syn_[imu_syn_count_ % 10]){
imu_syn_[imu_syn_count_ % 4]=false; imu_syn_[imu_syn_count_ % 10]=false;
}else{ }else{
return; return;
} }
@@ -136,8 +135,7 @@ public:
}; };
//IMU frame y front, x right, z up -> left hands (theta counts from y) //IMU frame y front, x right, z up -> left hands (theta counts from y)
void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) { void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
cam_syn_[cmd_syn_count_ % 4]=true; imu_syn_[cmd_syn_count_ % 10]=true;
imu_syn_[cmd_syn_count_ % 4]=true;
++cmd_syn_count_; ++cmd_syn_count_;
if(cmd_syn_count_>50)cmd_syn_count_=0; if(cmd_syn_count_>50)cmd_syn_count_=0;
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
@@ -145,7 +143,7 @@ public:
}; };
//Camera frame z back, x right, y down -> left hands (pitch counts from x)correct //Camera frame z back, x right, y down -> left hands (pitch counts from x)correct
//Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) { std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateVO(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(x,z,-1.0*pitch); filter_->correct(x,z,-1.0*pitch);
Eigen::Matrix<double,3,3> cov; Eigen::Matrix<double,3,3> cov;
@@ -162,10 +160,12 @@ public:
filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2); filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
} }
void setImuTime(){
filter_->setTime();
}
size_t cmd_syn_count_=0; size_t cmd_syn_count_=0;
size_t imu_syn_count_=2; size_t imu_syn_count_=2;
bool cam_syn_[4]={false,false,false,false}; bool imu_syn_[10]={false,false,false,false,false,false,false,false,false,false};
bool imu_syn_[4]={false,false,false,false};
private: private:
Base* filter_= nullptr; Base* filter_= nullptr;
boost::mutex ekf_mut_; boost::mutex ekf_mut_;
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch> <launch>
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" /> <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
<!-- Camera topic to subscribe to --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+8 -6
View File
@@ -1,6 +1,6 @@
svo: vio:
grid_size: 8 #Feature grid size of a cell in [px]. grid_size: 8 #Feature grid size of a cell in [px].
max_n_kfs: 15 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. max_n_kfs: 20 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3.
loba_num_iter: 10 #Number of iterations in the local bundle adjustment. loba_num_iter: 10 #Number of iterations in the local bundle adjustment.
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
@@ -10,7 +10,9 @@ svo:
kfselect_mindist: 0.01 kfselect_mindist: 0.01
poseoptim_thresh: 8.0 poseoptim_thresh: 8.0
init_min_inliers: 50 init_min_inliers: 50
ACC_noise: 0.1 #acc white noise in continuous in EKF ACC_ekf: 0.2 #acc white noise in continuous in EKF
GYO_noise: 0.05 #gyro white noise in continuous in EKF GYO_ekf: 0.05 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #VO noise in EKF VO_ekf_translation: 0.5 #VO noise in EKF
Cmd_Cov: 0.1 #control vector noise in EKF VO_ekf_orientation: 1.5 #VO noise in EKF
CMD_ekf_translation: 0.1 #control vector noise in EKF
CMD_ekf_orientation: 0.01 #control vector noise in EKF
GPU_version/vio/src/config.cpp
+35 -41
View File
@@ -20,47 +20,41 @@
namespace vio { namespace vio {
Config::Config() : Config::Config() :
trace_name(vk::getParam<string>("svo/trace_name", "svo")), trace_name(vk::getParam<string>("vio/trace_name", "svo")),
trace_dir(vk::getParam<string>("svo/trace_dir", "/tmp")), trace_dir(vk::getParam<string>("vio/trace_dir", "/tmp")),
n_pyr_levels(vk::getParam<int>("svo/n_pyr_levels", 3)), n_pyr_levels(vk::getParam<int>("vio/n_pyr_levels", 3)),
use_imu(vk::getParam<bool>("svo/use_imu", false)), use_imu(vk::getParam<bool>("vio/use_imu", false)),
core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)), core_n_kfs(vk::getParam<int>("vio/core_n_kfs", 3)),
map_scale(vk::getParam<double>("svo/map_scale", 1.0)), map_scale(vk::getParam<double>("vio/map_scale", 1.0)),
grid_size(vk::getParam<int>("svo/grid_size", 10)), grid_size(vk::getParam<int>("vio/grid_size", 10)),
init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)), init_min_disparity(vk::getParam<double>("vio/init_min_disparity", 50.0)),
init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)), init_min_tracked(vk::getParam<int>("vio/init_min_tracked", 50)),
init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)), init_min_inliers(vk::getParam<int>("vio/init_min_inliers", 40)),
klt_max_level(vk::getParam<int>("svo/klt_max_level", 4)), klt_max_level(vk::getParam<int>("vio/klt_max_level", 4)),
klt_min_level(vk::getParam<int>("svo/klt_min_level", 2)), klt_min_level(vk::getParam<int>("vio/klt_min_level", 2)),
reproj_thresh(vk::getParam<double>("svo/reproj_thresh", 2.0)), reproj_thresh(vk::getParam<double>("vio/reproj_thresh", 2.0)),
poseoptim_thresh(vk::getParam<double>("svo/poseoptim_thresh", 2.0)), poseoptim_thresh(vk::getParam<double>("vio/poseoptim_thresh", 2.0)),
poseoptim_num_iter(vk::getParam<int>("svo/poseoptim_num_iter", 10)), poseoptim_num_iter(vk::getParam<int>("vio/poseoptim_num_iter", 10)),
structureoptim_max_pts(vk::getParam<int>("svo/structureoptim_max_pts", 20)), structureoptim_max_pts(vk::getParam<int>("vio/structureoptim_max_pts", 20)),
structureoptim_num_iter(vk::getParam<int>("svo/structureoptim_num_iter", 5)), structureoptim_num_iter(vk::getParam<int>("vio/structureoptim_num_iter", 5)),
loba_thresh(vk::getParam<double>("svo/loba_thresh", 2.0)), loba_thresh(vk::getParam<double>("vio/loba_thresh", 2.0)),
loba_robust_huber_width(vk::getParam<double>("svo/loba_robust_huber_width", 1.0)), loba_robust_huber_width(vk::getParam<double>("vio/loba_robust_huber_width", 1.0)),
loba_num_iter(vk::getParam<int>("svo/loba_num_iter", 0)), loba_num_iter(vk::getParam<int>("vio/loba_num_iter", 0)),
kfselect_mindist(vk::getParam<double>("svo/kfselect_mindist", 0.12)), kfselect_mindist(vk::getParam<double>("vio/kfselect_mindist", 0.12)),
triang_min_corner_score(vk::getParam<double>("svo/triang_min_corner_score", 20.0)), triang_min_corner_score(vk::getParam<double>("vio/triang_min_corner_score", 20.0)),
triang_half_patch_size(vk::getParam<int>("svo/triang_half_patch_size", 4)), triang_half_patch_size(vk::getParam<int>("vio/triang_half_patch_size", 4)),
subpix_n_iter(vk::getParam<int>("svo/subpix_n_iter", 10)), subpix_n_iter(vk::getParam<int>("vio/subpix_n_iter", 10)),
max_n_kfs(vk::getParam<int>("svo/max_n_kfs", 10)), max_n_kfs(vk::getParam<int>("vio/max_n_kfs", 10)),
img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)), img_imu_delay(vk::getParam<double>("vio/img_imu_delay", 0.0)),
max_fts(vk::getParam<int>("svo/max_fts", 120)), max_fts(vk::getParam<int>("vio/max_fts", 120)),
quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)), quality_min_fts(vk::getParam<int>("vio/quality_min_fts", 50)),
quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)), quality_max_drop_fts(vk::getParam<int>("vio/quality_max_drop_fts", 40)),
ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)), ACC_noise(vk::getParam<double>("vio/ACC_ekf", 1e-4)),
GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)), GYO_noise(vk::getParam<double>("vio/GYO_ekf", 1e-4)),
iuc(vk::getParam<double>("svo/IUC", 1e-1)), vo_ekf_t(vk::getParam<double>("vio/VO_ekf_translation", 1e-4)),
abc(vk::getParam<double>("svo/ABC", 1e-1)), vo_ekf_o(vk::getParam<double>("vio/VO_ekf_orientation", 1e-4)),
gbc(vk::getParam<double>("svo/GBC", 1e-1)), cmd_ekf_t(vk::getParam<double>("vio/CMD_ekf_translation", 1e-4)),
ebp(vk::getParam<double>("svo/EBP", 1e-2)), cmd_ekf_o(vk::getParam<double>("vio/CMD_ekf_orientation", 1e-4))
nrp(vk::getParam<double>("svo/NRP", 1e-5)),
svo_ekf(vk::getParam<double>("svo/svo_ekf", 30)),
acc_x_off(vk::getParam<double>("svo/Acceleration_offset_x", 0.0)),
acc_y_off(vk::getParam<double>("svo/Acceleration_offset_y", 0.0)),
gyr_z_off(vk::getParam<double>("svo/Gyroscope_offset_z", 0.0)),
cmd_cov(vk::getParam<double>("svo/Cmd_Cov", 0.0))
{} {}
Config& Config::getInstance() Config& Config::getInstance()
GPU_version/vio/src/feature_detection.cpp
+10 -5
View File
@@ -72,8 +72,8 @@ void FastDetector::detect(
const double detection_threshold, const double detection_threshold,
list<shared_ptr<Feature>>& fts) list<shared_ptr<Feature>>& fts)
{ {
Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f));
std::vector<cv::KeyPoint> keypoints; std::vector<cv::KeyPoint> keypoints;
fts.clear();
for(int L=0; L<n_pyr_levels_; ++L) for(int L=0; L<n_pyr_levels_; ++L)
{ {
if(L>img_pyr.size())return; if(L>img_pyr.size())return;
@@ -85,13 +85,20 @@ void FastDetector::detect(
cl_int icorner[1]={0}; cl_int icorner[1]={0};
gpu_fast_->load(0,2,1,icorner); gpu_fast_->load(0,2,1,icorner);
gpu_fast_->run(0,img.cols,img.rows); gpu_fast_->run(0,img.cols,img.rows);
cl_int count[1]; cl_int count[1]={0};
gpu_fast_->read(0,2,1,count); gpu_fast_->read(0,2,1,count);
if(count[0]<1 ){ if(count[0]<1 ){
ROS_ERROR("Can not communicate with GPU"); ROS_ERROR("Can not communicate with GPU");
exit(0); exit(0);
} }
int size=count[0]; const int size=count[0];
if(size<1 || size > 2000){
gpu_fast_->release(0,0);
gpu_fast_->release(0,1);
gpu_fast_->release(0,2);
free(fast_corner);
return;
}
cl_int2* fast_corners=(cl_int2*)calloc(size, sizeof(cl_int2)); cl_int2* fast_corners=(cl_int2*)calloc(size, sizeof(cl_int2));
gpu_fast_->read(0,1,size,fast_corners); gpu_fast_->read(0,1,size,fast_corners);
for(uint i=0;i<size;++i) for(uint i=0;i<size;++i)
@@ -115,11 +122,9 @@ void FastDetector::detect(
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4); cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
cv::Mat descriptor; cv::Mat descriptor;
extractor->compute(frame->img(), keypoints, descriptor); extractor->compute(frame->img(), keypoints, descriptor);
fts.clear();
for(auto&& p:_for(keypoints)){ for(auto&& p:_for(keypoints)){
fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64))); fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
} }
resetGrid();
} }
} // namespace feature_detection } // namespace feature_detection
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -64,7 +64,7 @@ FrameHandlerMono::~FrameHandlerMono()
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time) void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
{ {
ukfPtr_.setImuTime();
if(!startFrameProcessingCommon(timestamp)){ if(!startFrameProcessingCommon(timestamp)){
return; return;
} }
@@ -171,7 +171,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_=last_frame_; new_frame_=last_frame_;
return RESULT_FAILURE; return RESULT_FAILURE;
} }
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0), auto result=ukfPtr_.UpdateVO(new_frame_->T_f_w_.se2().translation()(0),
new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch()); new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch());
new_frame_->T_f_w_ =result.second; new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first; new_frame_->Cov_ = result.first;
GPU_version/vio/src/initialization.cpp
+2 -2
View File
@@ -51,7 +51,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
{ {
trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, features_ref_, disparities_); trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, features_ref_, disparities_);
if(disparities_.size() < 1){ if(disparities_.size() < 1){
ukf_->UpdateSvo(0.0,0.0,0.0); ukf_->UpdateVO(0.0,0.0,0.0);
return NO_KEYFRAME; return NO_KEYFRAME;
} }
if(disparities_.size() < 20){ if(disparities_.size() < 20){
@@ -64,7 +64,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
double disparity = vk::getMean(disparities_,min,max); double disparity = vk::getMean(disparities_,min,max);
if(disparity < Config::initMinDisparity()){ if(disparity < Config::initMinDisparity()){
auto result=ukf_->get_location(); auto result=ukf_->get_location();
if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0); if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateVO(0.0,result.second.se2().translation().y(),0.0);
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] Init: px average disparity is:%f ,While minimum is: %f KLT tracked : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(), fprintf(log_,"[%s] Init: px average disparity is:%f ,While minimum is: %f KLT tracked : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
disparity, disparity,
GPU_version/vio/src/reprojector.cpp
+4
View File
@@ -109,6 +109,7 @@ namespace vio {
} }
overlap_kfs.reserve(options_.max_n_kfs); overlap_kfs.reserve(options_.max_n_kfs);
std::vector<int> added_keypoints; std::vector<int> added_keypoints;
int points_count=0;
for (auto &&it_frame:_for(close_kfs)) { for (auto &&it_frame:_for(close_kfs)) {
if (it_frame.index > options_.max_n_kfs)continue; if (it_frame.index > options_.max_n_kfs)continue;
overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0)); overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
@@ -153,6 +154,7 @@ namespace vio {
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++; overlap_kfs.back().second++;
++points_count;
}else{ }else{
if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue; if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
@@ -163,10 +165,12 @@ namespace vio {
added_keypoints.push_back(match.trainIdx); added_keypoints.push_back(match.trainIdx);
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++; overlap_kfs.back().second++;
++points_count;
} }
} }
++it_ref; ++it_ref;
} }
if(points_count>300)return;
} }
/* std::cerr<<"here\n"; /* std::cerr<<"here\n";
exit(0);*/ exit(0);*/