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

solve the seg errors and chnage the g2o version to manegment_memory beranch

Browse Source
This commit is contained in:
gfuse
2021-10-16 19:12:26 +02:00
parent ea5573b455
commit 16637a4f0c
11 changed files with 2588 additions and 1867 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+658 -224
View File
@@ -1,224 +1,658 @@
[12:03:41.417] New key frame
[12:03:41.417] BA loop run
[12:03:41.419] init error: 1.701205
[12:03:41.420] end error: 1.009137
[12:03:42.450] New key frame
[12:03:42.450] BA loop run
[12:03:42.452] init error: 6.237241
[12:03:42.453] end error: 0.489862
[12:03:42.648] New key frame
[12:03:42.648] BA loop run
[12:03:42.650] init error: 18.028230
[12:03:42.652] end error: 2.508618
[12:03:42.933] New key frame
[12:03:42.933] BA loop run
[12:03:42.937] init error: 36.102278
[12:03:42.942] end error: 4.830597
[12:03:43.247] New key frame
[12:03:43.247] BA loop run
[12:03:43.251] init error: 33.122877
[12:03:43.255] end error: 5.928311
[12:03:43.601] New key frame
[12:03:43.601] BA loop run
[12:03:43.607] init error: 39.321419
[12:03:43.615] end error: 13.236725
[12:03:44.013] New key frame
[12:03:44.013] BA loop run
[12:03:44.020] init error: 42.204503
[12:03:44.027] end error: 12.819218
[12:03:44.412] New key frame
[12:03:44.412] BA loop run
[12:03:44.422] init error: 52.348360
[12:03:44.435] end error: 20.717566
[12:03:44.859] New key frame
[12:03:44.859] BA loop run
[12:03:44.869] init error: 72.326163
[12:03:44.878] end error: 48.291812
[12:03:45.349] New key frame
[12:03:45.349] BA loop run
[12:03:45.359] init error: 77.093405
[12:03:45.371] end error: 45.229245
[12:03:45.907] New key frame
[12:03:45.907] BA loop run
[12:03:45.918] init error: 52.638919
[12:03:45.929] end error: 32.273172
[12:03:46.446] New key frame
[12:03:46.446] BA loop run
[12:03:46.459] init error: 89.777557
[12:03:46.474] end error: 61.190620
[12:03:47.109] New key frame
[12:03:47.109] BA loop run
[12:03:47.126] init error: 97.152057
[12:03:47.143] end error: 69.287463
[12:03:47.816] New key frame
[12:03:47.816] BA loop run
[12:03:47.831] init error: 99.116283
[12:03:47.845] end error: 66.357638
[12:03:48.329] New key frame
[12:03:48.329] BA loop run
[12:03:48.343] init error: 99.484118
[12:03:48.358] end error: 67.706517
[12:03:49.068] New key frame
[12:03:49.068] BA loop run
[12:03:49.084] init error: 119.223442
[12:03:49.099] end error: 73.552946
[12:03:49.970] New key frame
[12:03:49.970] BA loop run
[12:03:49.986] init error: 106.027200
[12:03:50.001] end error: 68.441734
[12:03:51.711] New key frame
[12:03:51.711] BA loop run
[12:03:51.725] init error: 70.560324
[12:03:51.736] end error: 57.569337
[12:03:53.093] New key frame
[12:03:53.094] BA loop run
[12:03:53.116] init error: 103.350472
[12:03:53.134] end error: 51.536080
[12:03:54.627] New key frame
[12:03:54.627] BA loop run
[12:03:54.647] init error: 125.083965
[12:03:54.665] end error: 64.421823
[12:03:56.359] New key frame
[12:03:56.359] BA loop run
[12:03:56.380] init error: 130.588163
[12:03:56.400] end error: 72.136725
[12:03:58.040] New key frame
[12:03:58.040] BA loop run
[12:03:58.063] init error: 174.737917
[12:03:58.084] end error: 92.886043
[12:03:59.575] New key frame
[12:03:59.575] BA loop run
[12:03:59.597] init error: 143.136133
[12:03:59.615] end error: 97.968820
[12:04:01.123] New key frame
[12:04:01.123] BA loop run
[12:04:01.144] init error: 151.510506
[12:04:01.166] end error: 102.378463
[12:04:03.072] New key frame
[12:04:03.072] BA loop run
[12:04:03.095] init error: 155.979793
[12:04:03.120] end error: 113.121948
[12:04:04.754] New key frame
[12:04:04.754] BA loop run
[12:04:04.779] init error: 181.844751
[12:04:04.801] end error: 133.088599
[12:04:06.496] New key frame
[12:04:06.496] BA loop run
[12:04:06.528] init error: 202.141490
[12:04:06.558] end error: 165.511598
[12:04:08.474] New key frame
[12:04:08.474] BA loop run
[12:04:08.498] init error: 259.418164
[12:04:08.520] end error: 206.366686
[12:04:10.145] New key frame
[12:04:10.146] BA loop run
[12:04:10.169] init error: 259.091854
[12:04:10.188] end error: 207.428913
[12:04:12.348] New key frame
[12:04:12.348] BA loop run
[12:04:12.370] init error: 181.705955
[12:04:12.386] end error: 166.767867
[12:04:14.020] New key frame
[12:04:14.020] BA loop run
[12:04:14.044] init error: 214.785175
[12:04:14.061] end error: 182.617615
[12:04:19.641] New key frame
[12:04:19.641] BA loop run
[12:04:19.671] init error: 771.169429
[12:04:19.686] end error: 395.558168
[12:04:22.579] New key frame
[12:04:22.579] BA loop run
[12:04:22.609] init error: 1169.384753
[12:04:22.628] end error: 404.458621
[12:04:29.656] New key frame
[12:04:29.656] BA loop run
[12:04:29.684] init error: 591.965362
[12:04:29.697] end error: 315.026841
[12:04:32.758] New key frame
[12:04:32.758] BA loop run
[12:04:32.789] init error: 480.827370
[12:04:32.804] end error: 366.377149
[12:04:34.911] New key frame
[12:04:34.911] BA loop run
[12:04:34.946] init error: 549.858008
[12:04:34.966] end error: 413.140415
[12:04:36.688] New key frame
[12:04:36.688] BA loop run
[12:04:36.719] init error: 659.142107
[12:04:36.736] end error: 458.711528
[12:04:39.397] New key frame
[12:04:39.397] BA loop run
[12:04:39.427] init error: 671.995324
[12:04:39.444] end error: 574.540631
[12:04:41.789] New key frame
[12:04:41.789] BA loop run
[12:04:41.814] init error: 776.453923
[12:04:41.828] end error: 768.396840
[12:04:44.200] New key frame
[12:04:44.200] BA loop run
[12:04:44.223] init error: 626.692321
[12:04:44.235] end error: 552.409475
[12:04:46.599] New key frame
[12:04:46.599] BA loop run
[12:04:46.619] init error: 521.987400
[12:04:46.630] end error: 466.837199
[12:04:47.958] New key frame
[12:04:47.958] BA loop run
[12:04:47.980] init error: 455.683188
[12:04:47.991] end error: 364.066169
[12:04:49.604] New key frame
[12:04:49.604] BA loop run
[12:04:49.623] init error: 485.524351
[12:04:49.632] end error: 411.181207
[12:04:51.534] New key frame
[12:04:51.535] BA loop run
[12:04:51.549] init error: 228.858900
[12:04:51.555] end error: 146.154664
[12:04:53.142] New key frame
[12:04:53.142] BA loop run
[12:04:53.160] init error: 350.238517
[12:04:53.168] end error: 267.636461
[12:04:54.386] New key frame
[12:04:54.386] BA loop run
[12:04:54.405] init error: 457.128205
[12:04:54.416] end error: 340.386048
[12:05:05.589] New key frame
[12:05:05.589] BA loop run
[12:05:05.596] init error: 25.690490
[12:05:05.597] end error: 9.485745
[12:05:05.779] New key frame
[12:05:05.779] BA loop run
[12:05:05.785] init error: 87.741382
[12:05:05.786] end error: 11.538819
[12:05:05.973] New key frame
[12:05:05.973] BA loop run
[12:05:05.979] init error: 28.194039
[12:05:05.979] end error: 2.489291
[12:05:06.142] New key frame
[12:05:06.142] BA loop run
[12:05:06.147] init error: 9.138186
[12:05:06.147] end error: 2.698424
[12:05:06.312] New key frame
[12:05:06.312] BA loop run
[12:05:06.318] init error: 2.996002
[12:05:06.319] end error: 2.624229
[12:05:06.447] New key frame
[12:05:06.447] BA loop run
[12:05:06.453] init error: 13.122931
[12:05:06.454] end error: 9.273583
[12:05:06.575] New key frame
[12:05:06.575] BA loop run
[12:05:06.581] init error: 11.353129
[12:05:06.582] end error: 9.504530
[12:05:06.848] New key frame
[12:05:06.848] BA loop run
[12:05:06.853] init error: 3.687899
[12:05:06.853] end error: 3.702094
[12:05:07.103] New key frame
[12:05:07.103] BA loop run
[12:05:07.108] init error: 3.702785
[12:05:07.108] end error: 3.702846
[12:05:12.138] New key frame
[12:05:12.138] BA loop run
[12:05:12.143] init error: 6.161187
[12:05:12.143] end error: 3.664383
[19:07:57.571] New key frame
[19:07:57.571] BA loop run
[19:07:57.572] init error: 20.124301
[19:07:57.575] end error: 0.000366
[19:07:57.725] New key frame
[19:07:57.725] BA loop run
[19:07:57.726] init error: 3.326193
[19:07:57.729] end error: 0.063434
[19:07:57.892] New key frame
[19:07:57.892] BA loop run
[19:07:58.033] New key frame
[19:07:58.033] BA loop run
[19:07:58.033] init error: 5.224831
[19:07:58.034] end error: 0.000000
[19:07:58.206] New key frame
[19:07:58.206] BA loop run
[19:07:58.206] init error: 16.211948
[19:07:58.208] end error: 0.000001
[19:07:58.406] New key frame
[19:07:58.406] BA loop run
[19:07:58.409] init error: 48.486817
[19:07:58.413] end error: 2.767577
[19:07:58.593] New key frame
[19:07:58.593] BA loop run
[19:07:58.595] init error: 348.821161
[19:07:58.600] end error: 1.276149
[19:07:58.892] New key frame
[19:07:58.892] BA loop run
[19:07:58.894] init error: 1915.998266
[19:07:58.899] end error: 3.539466
[19:07:59.206] New key frame
[19:07:59.206] BA loop run
[19:07:59.208] init error: 62.901709
[19:07:59.216] end error: 2.655298
[19:07:59.583] New key frame
[19:07:59.583] BA loop run
[19:07:59.585] init error: 36.592363
[19:07:59.592] end error: 0.916429
[19:07:59.821] New key frame
[19:07:59.821] BA loop run
[19:07:59.822] init error: 42.686031
[19:07:59.828] end error: 1.124509
[19:08:00.077] New key frame
[19:08:00.077] BA loop run
[19:08:00.078] init error: 306.849947
[19:08:00.086] end error: 2.161691
[19:08:00.316] New key frame
[19:08:00.316] BA loop run
[19:08:00.318] init error: 43.793156
[19:08:00.326] end error: 0.286283
[19:08:00.565] New key frame
[19:08:00.565] BA loop run
[19:08:00.567] init error: 114.305784
[19:08:00.577] end error: 2.788313
[19:08:00.829] New key frame
[19:08:00.829] BA loop run
[19:08:00.831] init error: 244.921992
[19:08:00.840] end error: 3.144747
[19:08:01.075] New key frame
[19:08:01.075] BA loop run
[19:08:01.077] init error: 175.755635
[19:08:01.085] end error: 2.467674
[19:08:01.352] New key frame
[19:08:01.352] BA loop run
[19:08:01.354] init error: 2023.324703
[19:08:01.366] end error: 2.591742
[19:08:01.561] New key frame
[19:08:01.561] BA loop run
[19:08:01.563] init error: 57.058299
[19:08:01.570] end error: 2.626874
[19:08:01.755] New key frame
[19:08:01.755] BA loop run
[19:08:01.757] init error: 729.558334
[19:08:01.763] end error: 1.215458
[19:08:01.957] New key frame
[19:08:01.957] BA loop run
[19:08:01.960] init error: 297.638335
[19:08:01.968] end error: 3.091007
[19:08:02.150] New key frame
[19:08:02.150] BA loop run
[19:08:02.152] init error: 144.108217
[19:08:02.162] end error: 5.496201
[19:08:02.356] New key frame
[19:08:02.356] BA loop run
[19:08:02.358] init error: 177.430184
[19:08:02.366] end error: 3.954758
[19:08:02.558] New key frame
[19:08:02.558] BA loop run
[19:08:02.560] init error: 127.969103
[19:08:02.573] end error: 3.019139
[19:08:02.801] New key frame
[19:08:02.802] BA loop run
[19:08:02.804] init error: 87.389767
[19:08:02.819] end error: 1.922565
[19:08:03.105] New key frame
[19:08:03.105] BA loop run
[19:08:03.107] init error: 3304.183444
[19:08:03.121] end error: 2.712068
[19:08:03.338] New key frame
[19:08:03.338] BA loop run
[19:08:03.340] init error: 226.922840
[19:08:03.350] end error: 2.476651
[19:08:03.594] New key frame
[19:08:03.594] BA loop run
[19:08:03.596] init error: 93.843590
[19:08:03.606] end error: 2.491450
[19:08:03.762] New key frame
[19:08:03.762] BA loop run
[19:08:03.765] init error: 1540.880986
[19:08:03.775] end error: 3.174820
[19:08:03.905] New key frame
[19:08:03.905] BA loop run
[19:08:03.908] init error: 59.127683
[19:08:03.916] end error: 1.047741
[19:08:04.135] New key frame
[19:08:04.135] BA loop run
[19:08:04.139] init error: 165.054727
[19:08:04.150] end error: 4.635068
[19:08:04.399] New key frame
[19:08:04.399] BA loop run
[19:08:04.403] init error: 84.080127
[19:08:04.414] end error: 0.297043
[19:08:04.637] New key frame
[19:08:04.637] BA loop run
[19:08:04.642] init error: 79.358421
[19:08:04.654] end error: 0.269855
[19:08:04.840] New key frame
[19:08:04.840] BA loop run
[19:08:04.845] init error: 126.204088
[19:08:04.886] end error: 0.000484
[19:08:05.064] New key frame
[19:08:05.064] BA loop run
[19:08:05.069] init error: 942.581476
[19:08:05.081] end error: 0.000467
[19:08:05.255] New key frame
[19:08:05.255] BA loop run
[19:08:05.259] init error: 36.688360
[19:08:05.308] end error: 1.078818
[19:08:05.607] New key frame
[19:08:05.607] BA loop run
[19:08:05.613] init error: 354592.408913
[19:08:05.626] end error: 526.740896
[19:08:05.961] New key frame
[19:08:05.961] BA loop run
[19:08:05.965] init error: 340.419486
[19:08:05.979] end error: 3.914580
[19:08:06.888] New key frame
[19:08:06.888] BA loop run
[19:08:06.892] init error: 5395.758872
[19:08:06.901] end error: 1.489281
[19:08:07.302] New key frame
[19:08:07.302] BA loop run
[19:08:07.306] init error: 143.068603
[19:08:07.322] end error: 3.227177
[19:08:07.706] New key frame
[19:08:07.706] BA loop run
[19:08:07.710] init error: 12406.169850
[19:08:07.719] end error: 22.960628
[19:08:08.376] New key frame
[19:08:08.376] BA loop run
[19:08:08.380] init error: 23441.247347
[19:08:08.389] end error: 11.344908
[19:08:08.769] New key frame
[19:08:08.769] BA loop run
[19:08:08.772] init error: 311.001814
[19:08:08.785] end error: 2.741961
[19:08:09.146] New key frame
[19:08:09.146] BA loop run
[19:08:09.151] init error: 2233.139659
[19:08:09.165] end error: 5.287275
[19:08:09.507] New key frame
[19:08:09.507] BA loop run
[19:08:09.512] init error: 141.205930
[19:08:09.522] end error: 4.366998
[19:08:09.909] New key frame
[19:08:09.909] BA loop run
[19:08:09.914] init error: 3004.172520
[19:08:09.926] end error: 6.696819
[19:08:10.343] New key frame
[19:08:10.343] BA loop run
[19:08:10.348] init error: 717.712530
[19:08:10.358] end error: 19.996458
[19:08:10.763] New key frame
[19:08:10.764] BA loop run
[19:08:10.769] init error: 381.142383
[19:08:10.781] end error: 18.124120
[19:08:11.149] New key frame
[19:08:11.149] BA loop run
[19:08:11.153] init error: 110.654655
[19:08:11.167] end error: 9.119094
[19:08:11.667] New key frame
[19:08:11.667] BA loop run
[19:08:11.673] init error: 148.874228
[19:08:11.688] end error: 6.669374
[19:08:12.204] New key frame
[19:08:12.204] BA loop run
[19:08:12.211] init error: 132.139573
[19:08:12.232] end error: 8.932782
[19:08:12.732] New key frame
[19:08:12.732] BA loop run
[19:08:12.740] init error: 132.865428
[19:08:12.761] end error: 13.242387
[19:08:13.253] New key frame
[19:08:13.253] BA loop run
[19:08:13.262] init error: 132.371013
[19:08:13.281] end error: 13.640638
[19:08:13.812] New key frame
[19:08:13.812] BA loop run
[19:08:13.824] init error: 420.872838
[19:08:13.847] end error: 71.267133
[19:08:14.437] New key frame
[19:08:14.437] BA loop run
[19:08:14.445] init error: 246.103669
[19:08:14.466] end error: 35.143164
[19:08:14.901] New key frame
[19:08:14.901] BA loop run
[19:08:14.910] init error: 595.048774
[19:08:14.930] end error: 6.163617
[19:08:15.429] New key frame
[19:08:15.429] BA loop run
[19:08:15.440] init error: 217.000308
[19:08:15.469] end error: 7.208416
[19:08:16.142] New key frame
[19:08:16.142] BA loop run
[19:08:16.149] init error: 581.567047
[19:08:16.169] end error: 2.480761
[19:08:16.652] New key frame
[19:08:16.652] BA loop run
[19:08:16.658] init error: 144.297517
[19:08:16.677] end error: 3.885534
[19:08:17.390] New key frame
[19:08:17.390] BA loop run
[19:08:17.397] init error: 205.203008
[19:08:17.419] end error: 5.931317
[19:08:18.184] New key frame
[19:08:18.184] BA loop run
[19:08:18.192] init error: 2362.207276
[19:08:18.217] end error: 8.164335
[19:08:18.863] New key frame
[19:08:18.863] BA loop run
[19:08:18.872] init error: 15541.837434
[19:08:18.897] end error: 30.216289
[19:08:19.517] New key frame
[19:08:19.517] BA loop run
[19:08:19.530] init error: 5645.073677
[19:08:19.572] end error: 28.499126
[19:08:20.298] New key frame
[19:08:20.298] BA loop run
[19:08:20.307] init error: 12194.098173
[19:08:20.336] end error: 13.802437
[19:08:20.989] New key frame
[19:08:20.989] BA loop run
[19:08:21.001] init error: 768.314319
[19:08:21.037] end error: 24.252743
[19:08:21.889] New key frame
[19:08:21.889] BA loop run
[19:08:21.900] init error: 4254.552392
[19:08:21.924] end error: 44.963277
[19:08:22.816] New key frame
[19:08:22.817] BA loop run
[19:08:22.828] init error: 372.261075
[19:08:22.864] end error: 9.733314
[19:08:23.453] New key frame
[19:08:23.453] BA loop run
[19:08:23.466] init error: 7895.034185
[19:08:23.499] end error: 14.295506
[19:08:24.035] New key frame
[19:08:24.035] BA loop run
[19:08:24.047] init error: 983.457179
[19:08:24.077] end error: 27.183696
[19:08:24.707] New key frame
[19:08:24.707] BA loop run
[19:08:24.719] init error: 238.905484
[19:08:24.752] end error: 31.446009
[19:08:25.574] New key frame
[19:08:25.574] BA loop run
[19:08:25.586] init error: 6639.216114
[19:08:25.615] end error: 63.603546
[19:08:26.418] New key frame
[19:08:26.418] BA loop run
[19:08:26.431] init error: 1527.439349
[19:08:26.467] end error: 102.500410
[19:08:27.104] New key frame
[19:08:27.104] BA loop run
[19:08:27.118] init error: 1682.446704
[19:08:27.158] end error: 63.194832
[19:08:28.170] New key frame
[19:08:28.170] BA loop run
[19:08:28.184] init error: 44402.645376
[19:08:28.221] end error: 100.878931
[19:08:29.319] New key frame
[19:08:29.319] BA loop run
[19:08:29.335] init error: 23143.431080
[19:08:29.375] end error: 106.133547
[19:08:29.885] New key frame
[19:08:29.885] BA loop run
[19:08:29.901] init error: 11041.869810
[19:08:29.945] end error: 92.381053
[19:08:30.974] New key frame
[19:08:30.974] BA loop run
[19:08:30.996] init error: 12458.156341
[19:08:31.043] end error: 284.208753
[19:08:32.234] New key frame
[19:08:32.234] BA loop run
[19:08:32.252] init error: 3241.041272
[19:08:32.303] end error: 138.828491
[19:08:33.769] New key frame
[19:08:33.769] BA loop run
[19:08:33.789] init error: 342796.721782
[19:08:33.834] end error: 870.263472
[19:08:34.914] New key frame
[19:08:34.914] BA loop run
[19:08:34.928] init error: 663960.189876
[19:08:34.965] end error: 872.220583
[19:08:36.148] New key frame
[19:08:36.148] BA loop run
[19:08:36.160] init error: 44034623.483747
[19:08:36.190] end error: 2783.771215
[19:08:37.044] New key frame
[19:08:37.044] BA loop run
[19:08:37.058] init error: 15634.066874
[19:08:37.090] end error: 433.156869
[19:08:38.420] New key frame
[19:08:38.420] BA loop run
[19:08:38.432] init error: 503468.223023
[19:08:38.458] end error: 847.134752
[19:08:39.613] New key frame
[19:08:39.613] BA loop run
[19:08:39.625] init error: 389294.375516
[19:08:39.648] end error: 690.928109
[19:08:40.874] New key frame
[19:08:40.874] BA loop run
[19:08:40.889] init error: 2377335.676515
[19:08:40.915] end error: 1104.021857
[19:08:43.363] New key frame
[19:08:43.363] BA loop run
[19:08:43.376] init error: 1446751.118520
[19:08:43.400] end error: 1436.698234
[19:08:44.738] New key frame
[19:08:44.738] BA loop run
[19:08:44.746] init error: 671993.539226
[19:08:44.771] end error: 1126.179519
[19:08:46.569] New key frame
[19:08:46.570] BA loop run
[19:08:46.578] init error: 38227755.723560
[19:08:46.595] end error: 1152.006000
[19:08:47.336] New key frame
[19:08:47.336] BA loop run
[19:08:47.343] init error: 552349.637205
[19:08:47.357] end error: 514.800584
[19:08:48.048] New key frame
[19:08:48.048] BA loop run
[19:08:48.054] init error: 1662425.910845
[19:08:48.065] end error: 346.807761
[19:08:48.696] New key frame
[19:08:48.696] BA loop run
[19:08:48.701] init error: 1551.689280
[19:08:48.713] end error: 34.723154
[19:08:50.537] New key frame
[19:08:50.537] BA loop run
[19:08:50.540] init error: 17253.587149
[19:08:50.547] end error: 41.350147
[19:08:50.944] New key frame
[19:08:50.944] BA loop run
[19:08:50.947] init error: 41829.307931
[19:08:50.952] end error: 13.714445
[19:08:51.401] New key frame
[19:08:51.401] BA loop run
[19:08:51.405] init error: 6269.468046
[19:08:51.412] end error: 1.252471
[19:08:51.879] New key frame
[19:08:51.879] BA loop run
[19:08:51.883] init error: 80889.919449
[19:08:51.889] end error: 5.937364
[19:08:52.330] New key frame
[19:08:52.330] BA loop run
[19:08:52.334] init error: 9300.475177
[19:08:52.340] end error: 6.408737
[19:08:52.723] New key frame
[19:08:52.723] BA loop run
[19:08:52.726] init error: 6762.686568
[19:08:52.732] end error: 30.922879
[19:08:53.046] New key frame
[19:08:53.046] BA loop run
[19:08:53.049] init error: 700473.491322
[19:08:53.055] end error: 53.988333
[19:08:53.337] New key frame
[19:08:53.337] BA loop run
[19:08:53.340] init error: 137404.671135
[19:08:53.345] end error: 28.566632
[19:08:53.632] New key frame
[19:08:53.632] BA loop run
[19:08:53.636] init error: 2455.155716
[19:08:53.642] end error: 7.978690
[19:08:53.928] New key frame
[19:08:53.928] BA loop run
[19:08:53.932] init error: 12036.376719
[19:08:53.939] end error: 30.499619
[19:08:54.198] New key frame
[19:08:54.198] BA loop run
[19:08:54.203] init error: 1078.009475
[19:08:54.211] end error: 50.562398
[19:08:54.516] New key frame
[19:08:54.516] BA loop run
[19:08:54.521] init error: 6000.783577
[19:08:54.531] end error: 182.849311
[19:08:54.817] New key frame
[19:08:54.817] BA loop run
[19:08:54.822] init error: 17020.616100
[19:08:54.831] end error: 47.082725
[19:08:55.144] New key frame
[19:08:55.144] BA loop run
[19:08:55.151] init error: 367551.921240
[19:08:55.161] end error: 329.161299
[19:08:55.452] New key frame
[19:08:55.452] BA loop run
[19:08:55.460] init error: 921304.542850
[19:08:55.473] end error: 1085.927310
[19:08:55.776] New key frame
[19:08:55.776] BA loop run
[19:08:55.784] init error: 52108.764078
[19:08:55.800] end error: 356.900435
[19:08:56.139] New key frame
[19:08:56.139] BA loop run
[19:08:56.148] init error: 347174.233661
[19:08:56.164] end error: 128.466734
[19:08:56.569] New key frame
[19:08:56.569] BA loop run
[19:08:56.578] init error: 115657.173290
[19:08:56.595] end error: 567.610467
[19:08:57.088] New key frame
[19:08:57.088] BA loop run
[19:08:57.097] init error: 34133.221553
[19:08:57.113] end error: 105.756833
[19:08:57.655] New key frame
[19:08:57.655] BA loop run
[19:08:57.666] init error: 1690132.313171
[19:08:57.687] end error: 232.489549
[19:08:58.406] New key frame
[19:08:58.406] BA loop run
[19:08:58.419] init error: 37636.651818
[19:08:58.444] end error: 303.571524
[19:08:59.291] New key frame
[19:08:59.291] BA loop run
[19:08:59.305] init error: 113246.107218
[19:08:59.336] end error: 1694.849522
[19:09:00.011] New key frame
[19:09:00.011] BA loop run
[19:09:00.030] init error: 156038.558897
[19:09:00.063] end error: 447.036669
[19:09:00.744] New key frame
[19:09:00.744] BA loop run
[19:09:00.764] init error: 2348.101843
[19:09:00.797] end error: 1079.336873
[19:09:01.371] New key frame
[19:09:01.372] BA loop run
[19:09:01.391] init error: 8593.408104
[19:09:01.428] end error: 1483.332738
[19:09:02.517] New key frame
[19:09:02.517] BA loop run
[19:09:02.537] init error: 602342.957103
[19:09:02.575] end error: 632.161000
[19:09:03.518] New key frame
[19:09:03.518] BA loop run
[19:09:03.541] init error: 2532.563297
[19:09:03.595] end error: 1354.499854
[19:09:05.686] New key frame
[19:09:05.686] BA loop run
[19:09:05.702] init error: 2417.686791
[19:09:05.743] end error: 257.207984
[19:09:06.762] New key frame
[19:09:06.762] BA loop run
[19:09:06.781] init error: 92905.664997
[19:09:06.825] end error: 552.618345
[19:09:07.734] New key frame
[19:09:07.734] BA loop run
[19:09:07.752] init error: 76574.633398
[19:09:07.790] end error: 536.851969
[19:09:08.316] New key frame
[19:09:08.317] BA loop run
[19:09:08.336] init error: 17765.492579
[19:09:08.381] end error: 537.227541
[19:09:08.888] New key frame
[19:09:08.888] BA loop run
[19:09:08.907] init error: 2713.787254
[19:09:08.946] end error: 344.511587
[19:09:09.764] New key frame
[19:09:09.764] BA loop run
[19:09:09.784] init error: 28287.606737
[19:09:09.830] end error: 495.225474
[19:09:10.843] New key frame
[19:09:10.843] BA loop run
[19:09:10.866] init error: 182562.043663
[19:09:10.920] end error: 1447.150602
[19:09:11.932] New key frame
[19:09:11.932] BA loop run
[19:09:11.957] init error: 295804.193193
[19:09:12.011] end error: 863.894614
[19:09:12.832] New key frame
[19:09:12.832] BA loop run
[19:09:12.857] init error: 30989.020287
[19:09:12.932] end error: 865.154263
[19:09:13.632] New key frame
[19:09:13.632] BA loop run
[19:09:13.657] init error: 2005.115013
[19:09:13.713] end error: 754.150107
[19:09:14.562] New key frame
[19:09:14.562] BA loop run
[19:09:14.590] init error: 2930.989857
[19:09:14.704] end error: 847.762853
[19:09:15.411] New key frame
[19:09:15.411] BA loop run
[19:09:15.439] init error: 8141.425072
[19:09:15.505] end error: 656.188305
[19:09:16.068] New key frame
[19:09:16.068] BA loop run
[19:09:16.090] init error: 7883.882089
[19:09:16.139] end error: 549.824669
[19:09:16.817] New key frame
[19:09:16.817] BA loop run
[19:09:16.838] init error: 31683.980719
[19:09:16.885] end error: 561.011736
[19:09:17.385] New key frame
[19:09:17.385] BA loop run
[19:09:17.406] init error: 59327.635941
[19:09:17.472] end error: 483.543798
[19:09:17.892] New key frame
[19:09:17.892] BA loop run
[19:09:17.913] init error: 4670.716461
[19:09:17.968] end error: 379.145547
[19:09:18.604] New key frame
[19:09:18.604] BA loop run
[19:09:18.627] init error: 5209.772044
[19:09:18.698] end error: 406.523648
[19:09:19.314] New key frame
[19:09:19.314] BA loop run
[19:09:19.342] init error: 24325.460496
[19:09:19.396] end error: 1069.182673
[19:09:20.044] New key frame
[19:09:20.044] BA loop run
[19:09:20.067] init error: 67332.643981
[19:09:20.120] end error: 451.879759
[19:09:20.768] New key frame
[19:09:20.768] BA loop run
[19:09:20.795] init error: 5823.475524
[19:09:20.849] end error: 715.265627
[19:09:21.296] New key frame
[19:09:21.296] BA loop run
[19:09:21.317] init error: 12157.100550
[19:09:21.366] end error: 441.857794
[19:09:22.053] New key frame
[19:09:22.053] BA loop run
[19:09:22.074] init error: 28622.816252
[19:09:22.123] end error: 800.104694
[19:09:22.991] New key frame
[19:09:22.991] BA loop run
[19:09:23.011] init error: 3101.906475
[19:09:23.071] end error: 354.338690
[19:09:23.883] New key frame
[19:09:23.883] BA loop run
[19:09:23.912] init error: 135469.987048
[19:09:23.966] end error: 1135.546090
[19:09:24.720] New key frame
[19:09:24.720] BA loop run
[19:09:24.747] init error: 13523.226465
[19:09:24.816] end error: 666.512385
[19:09:25.480] New key frame
[19:09:25.481] BA loop run
[19:09:25.503] init error: 435607.138491
[19:09:25.555] end error: 595.250050
[19:09:26.154] New key frame
[19:09:26.154] BA loop run
[19:09:26.175] init error: 15305.018933
[19:09:26.244] end error: 393.300600
[19:09:26.639] New key frame
[19:09:26.639] BA loop run
[19:09:26.663] init error: 5086.688488
[19:09:26.771] end error: 1158.814927
[19:09:27.192] New key frame
[19:09:27.192] BA loop run
[19:09:27.214] init error: 169567.864815
[19:09:27.259] end error: 906.772537
[19:09:27.636] New key frame
[19:09:27.636] BA loop run
[19:09:27.656] init error: 2315.138570
[19:09:27.710] end error: 429.474969
[19:09:28.021] New key frame
[19:09:28.021] BA loop run
[19:09:28.041] init error: 2193.575695
[19:09:28.083] end error: 646.445919
[19:09:28.645] New key frame
[19:09:28.645] BA loop run
[19:09:28.665] init error: 5229.595941
[19:09:28.710] end error: 561.443113
[19:09:29.317] New key frame
[19:09:29.317] BA loop run
[19:09:29.338] init error: 392563.566338
[19:09:29.382] end error: 312.687692
[19:09:29.969] New key frame
[19:09:29.969] BA loop run
[19:09:29.991] init error: 22890.618899
[19:09:30.036] end error: 187.744379
[19:09:30.596] New key frame
[19:09:30.596] BA loop run
[19:09:30.620] init error: 44921.900136
[19:09:30.676] end error: 372.990847
[19:09:31.359] New key frame
[19:09:31.359] BA loop run
[19:09:31.389] init error: 10506.844043
[19:09:31.454] end error: 1413.312563
[19:09:32.052] New key frame
[19:09:32.052] BA loop run
[19:09:32.085] init error: 8061.284444
[19:09:32.146] end error: 695.109379
[19:09:32.677] New key frame
[19:09:32.677] BA loop run
[19:09:32.710] init error: 22051.193591
[19:09:32.791] end error: 487.391619
[19:09:33.253] New key frame
[19:09:33.253] BA loop run
[19:09:33.292] init error: 408549.445554
[19:09:33.360] end error: 2152.703612
[19:09:34.085] New key frame
[19:09:34.086] BA loop run
[19:09:34.123] init error: 426652.390483
[19:09:34.210] end error: 895.246709
[19:09:34.790] New key frame
[19:09:34.790] BA loop run
[19:09:34.823] init error: 146290.284901
[19:09:34.905] end error: 952.559704
[19:09:35.659] New key frame
[19:09:35.659] BA loop run
[19:09:35.691] init error: 881388.768973
[19:09:35.774] end error: 1705.674421
[19:09:36.533] New key frame
[19:09:36.533] BA loop run
[19:09:36.562] init error: 76209.846512
[19:09:36.633] end error: 1204.167804
[19:09:37.454] New key frame
[19:09:37.454] BA loop run
[19:09:37.483] init error: 11119.525258
[19:09:37.554] end error: 1859.468648
[19:09:38.341] New key frame
[19:09:38.341] BA loop run
[19:09:38.367] init error: 5625.588972
[19:09:38.466] end error: 2318.181275
[19:09:39.275] New key frame
[19:09:39.275] BA loop run
[19:09:39.302] init error: 13654.263186
[19:09:39.377] end error: 801.690669
[19:09:40.328] New key frame
[19:09:40.328] BA loop run
[19:09:40.363] init error: 3104.981592
[19:09:40.433] end error: 707.488389
[19:09:41.352] New key frame
[19:09:41.352] BA loop run
[19:09:41.377] init error: 2056.058613
[19:09:41.444] end error: 877.723256
GPU_version/vio/frame_handler_log.txt
+1855 -1551
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -52,7 +52,7 @@ namespace vio {
Features fts_; //!< List of features in the image.
vector<std::shared_ptr<Feature>> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
bool is_keyframe_; //!< Was this frames selected as keyframe?
g2o::VertexSE3Expmap* v_kf_=NULL; //!< Temporary pointer to the g2o node object of the keyframe.
std::shared_ptr<g2o::VertexSE3Expmap> v_kf_=NULL; //!< Temporary pointer to the g2o node object of the keyframe.
int last_published_ts_; //!< Timestamp of last publishing.
Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
GPU_version/vio/include/vio/global_optimizer.h
+8 -11
View File
@@ -77,27 +77,23 @@ protected:
boost::thread* thread_;
Map& map_;
size_t v_id_ = 0;
std::unique_ptr<g2o::SparseOptimizer> optimizer_=NULL;
std::shared_ptr<g2o::CameraParameters> cam_params_=NULL;
#if VIO_DEBUG
FILE* log_=nullptr;
#endif
/// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3{
g2o::EdgeProjectXYZ2UV* edge;
g2o::VertexSE3Expmap* expmap;
g2o::VertexSBAPointXYZ* point;
};
/// Create a g2o vertice from a keyframe object.
g2o::VertexSE3Expmap* createG2oFrameSE3(
std::shared_ptr<g2o::VertexSE3Expmap> createG2oFrameSE3(
FramePtr kf);
/// Creates a g2o vertice from a mappoint object.
g2o::VertexSBAPointXYZ* createG2oPoint(
std::shared_ptr<g2o::VertexPointXYZ> createG2oPoint(
Vector3d pos);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
g2o::VertexSE3Expmap* v_kf,
g2o::VertexSBAPointXYZ* v_mp,
std::shared_ptr<g2o::EdgeProjectXYZ2UV> createG2oEdgeSE3(
std::shared_ptr<g2o::VertexSE3Expmap> v_kf,
std::shared_ptr<g2o::VertexPointXYZ>v_mp,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
@@ -105,6 +101,7 @@ protected:
/// A thread that is continuously optimizing the map.
/// Global bundle adjustment.
void updateLoop();
void reset_map();
};
} // namespace vio
GPU_version/vio/include/vio/point.h
+1 -2
View File
@@ -49,8 +49,7 @@ public:
Vector3d pos_; //!< 3d pos of the point in the world coordinate frame.
list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
std::pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>> v_pt_=std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>()); //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
std::shared_ptr<g2o::VertexPointXYZ> v_pt_=NULL; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice.
PointType type_; //!< Quality of the point.
int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point.
GPU_version/vio/include/vio/reprojector.h
+1 -2
View File
@@ -75,9 +75,8 @@ private:
/// will search a maching feature in the image.
struct Candidate {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
std::shared_ptr<Point> pt; //!< 3D point.
Vector2d px; //!< projected 2D pixel location.
Candidate(std::shared_ptr<Point> pt, Vector2d& px) : pt(pt), px(px) {}
Candidate(Vector2d& px) : px(px) {}
};
typedef std::list<Candidate > Cell;
typedef std::vector<Cell*> CandidateGrid;
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<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 -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/frame_handler_base.cpp
-1
View File
@@ -54,7 +54,6 @@ bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
if(stage_ == STAGE_PAUSED)
return false;
// some cleanup from last iteration, can't do before because of visualization
map_.emptyTrash();
return true;
}
GPU_version/vio/src/global_optimizer.cpp
+59 -68
View File
@@ -34,7 +34,19 @@ namespace vio {
BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL)
{
optimizer_=std::make_unique<g2o::SparseOptimizer>();
optimizer_->setVerbose(false);
/*g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();*/
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver=g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>>();
std::unique_ptr<g2o::OptimizationAlgorithm> solver(new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
optimizer_->setAlgorithm(std::move(solver));
// setup camera
cam_params_ =std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
cam_params_->setId(0);
if (!optimizer_->addParameter(cam_params_)) {
assert(false && "Camera initialization in BA");
}
#if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
assert(log_);
@@ -78,7 +90,8 @@ namespace vio {
vio::time_in_HH_MM_SS_MMM().c_str());
#endif
new_keyframe_=false;
// init g2o
g2o::OptimizableGraph::VertexContainer points;
ba_mux_.lock();
// Go through all Keyframes
v_id_ = 0;
@@ -86,6 +99,7 @@ namespace vio {
{
// New Keyframe Vertex
(*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf);
optimizer_->addVertex((*it_kf)->v_kf_);
for(auto& it_ftr:(*it_kf)->fts_)
{
if(it_ftr->point==NULL)continue;
@@ -93,140 +107,117 @@ namespace vio {
// for each keyframe add edges to all observed mapoints
if(it_ftr->point->pos_.hasNaN())continue;
if(it_ftr->point->pos_.norm()==0.)continue;
if(it_ftr->point->v_pt_.first == NULL)
if(it_ftr->point->v_pt_ == NULL)
{
// mappoint-vertex doesn't exist yet. create a new one:
it_ftr->point->v_pt_.first = createG2oPoint(it_ftr->point->pos_);
it_ftr->point->v_pt_ = createG2oPoint(it_ftr->point->pos_);
optimizer_->addVertex(it_ftr->point->v_pt_);
}
it_ftr->point->v_pt_.second.push_back(createG2oEdgeSE3((*it_kf)->v_kf_, it_ftr->point->v_pt_.first, vk::project2d(it_ftr->f),
optimizer_->addEdge(createG2oEdgeSE3((*it_kf)->v_kf_, it_ftr->point->v_pt_, vk::project2d(it_ftr->f),
true,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
}
}
ba_mux_.unlock();
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
/*g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();*/
g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
double* pam=map_.keyframes_.back()->cam_->params();
// setup camera
g2o::CameraParameters* cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false && "Camera initialization in BA");
}
// init g2o
g2o::OptimizableGraph::VertexContainer points;
auto end=map_.keyframes_.end();
auto end_1=end--;
for(auto it=map_.keyframes_.begin(); it!=map_.keyframes_.end();++it){
optimizer.addVertex((*it)->v_kf_);
for(auto& p:(*it)->fts_){
if(p->point == NULL)continue;
if(p->point->v_pt_.first == NULL || p->point->v_pt_.second.empty())continue;
if(p->point->v_pt_.second.size()>1){
optimizer.addVertex(p->point->v_pt_.first);///TODO
points.push_back(p->point->v_pt_.first);
for(auto&& e:p->point->v_pt_.second)optimizer.addEdge(e);
p->point->v_pt_.second.clear();
}
}
if(it == end_1-- || it ==end || it==map_.keyframes_.end()) (*it)->v_kf_->setFixed(false);
if((*it)->v_kf_->dimension()==3)points.push_back((*it)->v_kf_);
for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer_->vertices().begin();
it != optimizer_->vertices().end(); ++it) {
auto v = std::static_pointer_cast<g2o::OptimizableGraph::Vertex>(it->second);
if (v->dimension() == 3 && v->edges().size()>2) points.push_back(v);
}
// Optimization
if(points.empty() || optimizer.vertices().empty()){
optimizer.clear();
if(points.empty() || optimizer_->vertices().empty()){
optimizer_->clear();
ba_mux_.unlock();
continue;
}
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer_->initializeOptimization();
optimizer_->computeActiveErrors();
g2o::StructureOnlySolver<3> structure_only_ba;
structure_only_ba.calc(points, vio::Config::lobaNumIter());
#if VIO_DEBUG
fprintf(log_,"[%s] init error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer_->activeChi2());
#endif
if(optimizer.optimize(vio::Config::lobaNumIter())<1){
optimizer.clear();
if(optimizer_->optimize(vio::Config::lobaNumIter())<1){
optimizer_->clear();
ba_mux_.unlock();
continue;
}
#if VIO_DEBUG
fprintf(log_,"[%s] end error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer_->activeChi2());
#endif
ba_mux_.lock();
// Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
it_kf != map_.keyframes_.end();++it_kf)
{
(*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
(*it_kf)->v_kf_->estimate().translation()));
(*it_kf)->v_kf_ = NULL;
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
{
if((*it_ftr)->point == NULL)
continue;
if((*it_ftr)->point->v_pt_.first == NULL)
if((*it_ftr)->point->v_pt_ == NULL)
continue; // mp was updated before
(*it_ftr)->point->pos_ = (*it_ftr)->point->v_pt_.first->estimate();
(*it_ftr)->point->v_pt_ = std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>());
(*it_ftr)->point->pos_ = (*it_ftr)->point->v_pt_->estimate();
(*it_ftr)->point->v_pt_.reset();
}
}
optimizer_->clear();
ba_mux_.unlock();
optimizer.clear();
}
}
g2o::VertexSE3Expmap*
std::shared_ptr<g2o::VertexSE3Expmap>
BA_Glob::createG2oFrameSE3(FramePtr frame)
{
g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
std::shared_ptr<g2o::VertexSE3Expmap> v= std::make_shared<g2o::VertexSE3Expmap>();
++v_id_;
v->setId(v_id_);
v->setFixed(true);
v->setFixed(false);
v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
return v;
}
g2o::VertexSBAPointXYZ*
std::shared_ptr<g2o::VertexPointXYZ>
BA_Glob::createG2oPoint(Vector3d pos)
{
++v_id_;
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
std::shared_ptr<g2o::VertexPointXYZ> v =std::make_shared<g2o::VertexPointXYZ>();
v->setId(v_id_);
v->setFixed(false);
//v->setFixed(false);
v->setMarginalized(true);
v->setEstimate(pos);
return v;
}
g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
g2o::VertexSBAPointXYZ* v_point,
std::shared_ptr<g2o::EdgeProjectXYZ2UV> BA_Glob::createG2oEdgeSE3(
std::shared_ptr<g2o::VertexSE3Expmap> v_frame,
std::shared_ptr<g2o::VertexPointXYZ> v_point,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight)
{
g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
std::shared_ptr<g2o::EdgeProjectXYZ2UV> e= std::make_shared<g2o::EdgeProjectXYZ2UV>();
e->vertices()[0]=v_point;
e->vertices()[1]=v_frame;
e->setMeasurement(f_up);
e->information() = weight*Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
e->setRobustKernel(rk);
e->setParameterId(0, 0); //old: e->setId(v_point->id());
return e;
}
void BA_Glob::reset_map(){
for(auto&& f:map_.keyframes_){
f->v_kf_.reset();
for(auto p:f->fts_)
if(p->point!=NULL)
p->point->v_pt_.reset();
}
}
} // namespace vio
GPU_version/vio/src/point.cpp
+2 -4
View File
@@ -29,8 +29,7 @@ Point::Point(const Vector3d& pos) :
id_(point_counter_++),
pos_(pos),
n_obs_(0),
v_pt_(std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>())),
v_pt_(NULL),
last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN),
n_failed_reproj_(0),
@@ -41,8 +40,7 @@ Point::Point(const Vector3d& pos, std::shared_ptr<Feature> ftr) :
id_(point_counter_++),
pos_(pos),
n_obs_(1),
v_pt_(std::make_pair<g2o::VertexSBAPointXYZ*,std::vector<g2o::EdgeProjectXYZ2UV*>>(NULL,
std::vector<g2o::EdgeProjectXYZ2UV*>())),
v_pt_(NULL),
last_frame_overlap_id_(-10),
type_(TYPE_UNKNOWN),
n_failed_reproj_(0),
GPU_version/vio/src/reprojector.cpp
+2 -2
View File
@@ -153,7 +153,7 @@ namespace vio {
added_keypoints.push_back(match.trainIdx);
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
frame->fts_.back()->point->type_=vio::Point::TYPE_UNKNOWN;
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
grid_.cells.at(k)->push_back(Candidate( px));
overlap_kfs.back().second++;
++points_count;
}else{
@@ -164,7 +164,7 @@ namespace vio {
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
(*it_ref)->point->type_=vio::Point::TYPE_CANDIDATE;
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( px));
overlap_kfs.back().second++;
++points_count;
}