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

more comment

Browse Source
This commit is contained in:
gfuse
2021-10-02 16:32:03 +02:00
parent d1fb2b4914
commit 5216608883
4 changed files with 1207 additions and 1447 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+168 -272
View File
@@ -1,272 +1,168 @@
[14:17:35.009] New key frame [14:54:18.504] New key frame
[14:17:35.009] BA loop run [14:54:18.504] BA loop run
[14:17:35.010] init error: 148.036207 [14:54:18.505] init error: 24.763610
[14:17:35.011] end error: 0.026000 [14:54:18.506] end error: 0.000054
[14:17:35.957] New key frame [14:54:19.426] New key frame
[14:17:35.957] BA loop run [14:54:19.426] BA loop run
[14:17:35.958] init error: 38918.365174 [14:54:19.426] init error: 12.320394
[14:17:35.958] end error: 0.013444 [14:54:19.428] end error: 0.002815
[14:17:36.925] New key frame [14:54:19.527] New key frame
[14:17:36.925] BA loop run [14:54:19.527] BA loop run
[14:17:36.926] init error: 212.526317 [14:54:19.528] init error: 323.515875
[14:17:36.927] end error: 0.000262 [14:54:19.529] end error: 0.003837
[14:17:37.760] New key frame [14:54:19.645] New key frame
[14:17:37.760] BA loop run [14:54:19.645] BA loop run
[14:17:37.760] init error: 138.384450 [14:54:19.645] init error: 2.917523
[14:17:37.761] end error: 0.000041 [14:54:19.647] end error: 0.000002
[14:17:37.847] New key frame [14:54:19.756] New key frame
[14:17:37.847] BA loop run [14:54:19.756] BA loop run
[14:17:37.847] init error: 89.222508 [14:54:19.756] init error: 2.562036
[14:17:37.848] end error: 0.000252 [14:54:19.758] end error: 0.041513
[14:17:38.645] New key frame [14:54:19.866] New key frame
[14:17:38.645] BA loop run [14:54:19.866] BA loop run
[14:17:38.645] init error: 10.820945 [14:54:19.867] init error: 1910.325804
[14:17:38.646] end error: 0.000143 [14:54:19.867] end error: 0.000167
[14:17:39.342] New key frame [14:54:19.963] New key frame
[14:17:39.342] BA loop run [14:54:19.963] BA loop run
[14:17:39.343] init error: 8.056582 [14:54:19.963] init error: 289.670025
[14:17:39.343] end error: 0.000025 [14:54:19.964] end error: 0.000167
[14:17:39.768] New key frame [14:54:20.055] New key frame
[14:17:39.769] BA loop run [14:54:20.055] BA loop run
[14:17:39.769] init error: 17.415217 [14:54:20.055] init error: 194.073707
[14:17:39.770] end error: 0.000767 [14:54:20.060] end error: 0.074585
[14:17:40.130] New key frame [14:54:20.501] New key frame
[14:17:40.130] BA loop run [14:54:20.501] BA loop run
[14:17:40.130] init error: 32.854374 [14:54:20.501] init error: 4052.719150
[14:17:40.131] end error: 0.000032 [14:54:20.504] end error: 0.478617
[14:17:40.925] New key frame [14:54:21.096] New key frame
[14:17:40.925] BA loop run [14:54:21.096] BA loop run
[14:17:40.925] init error: 78.734056 [14:54:21.097] init error: 597.219624
[14:17:40.926] end error: 0.000000 [14:54:21.101] end error: 0.836308
[14:17:41.579] New key frame [14:54:21.315] New key frame
[14:17:41.579] BA loop run [14:54:21.315] BA loop run
[14:17:41.579] init error: 60.909114 [14:54:21.316] init error: 6935.758919
[14:17:41.580] end error: 0.000000 [14:54:21.322] end error: 3.265738
[14:17:41.938] New key frame [14:54:21.763] New key frame
[14:17:41.938] BA loop run [14:54:21.763] BA loop run
[14:17:41.939] init error: 46.031892 [14:54:21.764] init error: 161.973034
[14:17:41.940] end error: 0.000042 [14:54:21.975] end error: 2.271218
[14:17:42.150] New key frame [14:54:22.267] New key frame
[14:17:42.150] BA loop run [14:54:22.267] BA loop run
[14:17:42.150] init error: 1076.813205 [14:54:22.268] init error: 95.065037
[14:17:42.152] end error: 0.000812 [14:54:22.391] end error: 2.446271
[14:17:42.345] New key frame [14:54:22.941] New key frame
[14:17:42.345] BA loop run [14:54:22.941] BA loop run
[14:17:42.346] init error: 754.379730 [14:54:22.943] init error: 4603.510437
[14:17:42.347] end error: 0.081436 [14:54:22.951] end error: 2.320479
[14:17:42.756] New key frame [14:54:24.537] New key frame
[14:17:42.756] BA loop run [14:54:24.537] BA loop run
[14:17:42.757] init error: 2677.765414 [14:54:24.540] init error: 2029.462857
[14:17:42.895] end error: 0.001519 [14:54:24.722] end error: 4.087532
[14:17:43.028] New key frame [14:54:25.356] New key frame
[14:17:43.028] BA loop run [14:54:25.356] BA loop run
[14:17:43.029] init error: 146.947081 [14:54:25.358] init error: 75690.392815
[14:17:43.145] end error: 0.002729 [14:54:25.518] end error: 5.770364
[14:17:43.437] New key frame [14:54:25.673] New key frame
[14:17:43.437] BA loop run [14:54:25.673] BA loop run
[14:17:43.438] init error: 13982.056448 [14:54:25.674] init error: 15240.984229
[14:17:43.553] end error: 0.004119 [14:54:26.122] end error: 4.056428
[14:17:43.771] New key frame [14:54:26.123] New key frame
[14:17:43.771] BA loop run [14:54:26.123] BA loop run
[14:17:43.772] init error: 76.715863 [14:54:26.127] init error: 386.422521
[14:17:43.773] end error: 0.007292 [14:54:26.710] end error: 3.468697
[14:17:43.858] New key frame [14:54:27.018] New key frame
[14:17:43.858] BA loop run [14:54:27.018] BA loop run
[14:17:43.858] init error: 215.511731 [14:54:27.020] init error: 2927.060707
[14:17:43.860] end error: 0.000221 [14:54:27.234] end error: 3.722009
[14:17:43.938] New key frame [14:54:27.501] New key frame
[14:17:43.938] BA loop run [14:54:27.501] BA loop run
[14:17:43.939] init error: 81.875323 [14:54:27.503] init error: 68846.950097
[14:17:43.940] end error: 0.000025 [14:54:27.510] end error: 3.166685
[14:17:44.118] New key frame [14:54:27.930] New key frame
[14:17:44.118] BA loop run [14:54:27.930] BA loop run
[14:17:44.118] init error: 2422.342876 [14:54:27.932] init error: 13881.061617
[14:17:44.119] end error: 0.000097 [14:54:27.997] end error: 8.823857
[14:17:44.221] New key frame [14:54:28.144] New key frame
[14:17:44.221] BA loop run [14:54:28.144] BA loop run
[14:17:44.221] init error: 207.581916 [14:54:28.147] init error: 3859.481635
[14:17:44.223] end error: 0.000000 [14:54:28.329] end error: 8.667055
[14:17:44.328] New key frame [14:54:28.590] New key frame
[14:17:44.328] BA loop run [14:54:28.590] BA loop run
[14:17:44.328] init error: 162.566432 [14:54:28.592] init error: 4412.399485
[14:17:44.329] end error: 0.001409 [14:54:28.992] end error: 7.702882
[14:17:44.422] New key frame [14:54:28.994] New key frame
[14:17:44.422] BA loop run [14:54:28.994] BA loop run
[14:17:44.423] init error: 14435.424627 [14:54:28.998] init error: 2083.696145
[14:17:44.425] end error: 7.540367 [14:54:29.013] end error: 11.568292
[14:17:44.519] New key frame [14:54:29.168] New key frame
[14:17:44.519] BA loop run [14:54:29.168] BA loop run
[14:17:44.520] init error: 133.788853 [14:54:29.171] init error: 5318.544608
[14:17:44.524] end error: 1.809725 [14:54:29.355] end error: 80.936368
[14:17:44.638] New key frame [14:54:29.366] New key frame
[14:17:44.638] BA loop run [14:54:29.366] BA loop run
[14:17:44.639] init error: 1324.756419 [14:54:29.370] init error: 518178.847730
[14:17:44.641] end error: 0.000203 [14:54:29.381] end error: 234.161606
[14:17:44.720] New key frame [14:54:29.759] New key frame
[14:17:44.720] BA loop run [14:54:29.759] BA loop run
[14:17:44.721] init error: 566.029171 [14:54:29.761] init error: 122235.358731
[14:17:44.723] end error: 0.000364 [14:54:29.769] end error: 62.169895
[14:17:44.827] New key frame [14:54:30.253] New key frame
[14:17:44.827] BA loop run [14:54:30.253] BA loop run
[14:17:44.828] init error: 13851.507144 [14:54:30.255] init error: 1847824.989597
[14:17:44.829] end error: 0.000267 [14:54:30.262] end error: 75.517423
[14:17:44.934] New key frame [14:54:31.363] New key frame
[14:17:44.934] BA loop run [14:54:31.363] BA loop run
[14:17:44.935] init error: 187.782764 [14:54:31.365] init error: 43049.124812
[14:17:44.937] end error: 0.000267 [14:54:31.372] end error: 45.707574
[14:17:45.035] New key frame [14:54:33.254] New key frame
[14:17:45.035] BA loop run [14:54:33.254] BA loop run
[14:17:45.035] init error: 839.892009 [14:54:33.256] init error: 332144.508586
[14:17:45.037] end error: 0.000267 [14:54:33.262] end error: 24.205326
[14:17:45.135] New key frame [14:54:40.425] New key frame
[14:17:45.135] BA loop run [14:54:40.425] BA loop run
[14:17:45.136] init error: 82.259093 [14:54:40.427] init error: 8015.415519
[14:17:45.138] end error: 0.004693 [14:54:40.435] end error: 32.395425
[14:17:45.222] New key frame [14:54:40.540] New key frame
[14:17:45.222] BA loop run [14:54:40.540] BA loop run
[14:17:45.223] init error: 1073.580439 [14:54:40.542] init error: 12811.599311
[14:17:45.224] end error: 0.001541 [14:54:40.552] end error: 21.742364
[14:17:45.328] New key frame [14:54:41.772] New key frame
[14:17:45.328] BA loop run [14:54:41.772] BA loop run
[14:17:45.329] init error: 893.975632 [14:54:41.775] init error: 7821.177608
[14:17:45.331] end error: 0.001541 [14:54:41.786] end error: 21.977430
[14:17:45.435] New key frame [14:54:41.903] New key frame
[14:17:45.435] BA loop run [14:54:41.903] BA loop run
[14:17:45.436] init error: 162.307891 [14:54:41.905] init error: 78.574760
[14:17:45.439] end error: 0.001541 [14:54:42.003] end error: 9.756280
[14:17:45.537] New key frame [14:54:42.173] New key frame
[14:17:45.538] BA loop run [14:54:42.173] BA loop run
[14:17:45.539] init error: 494869.499643 [14:54:42.176] init error: 955.246709
[14:17:45.541] end error: 0.502589 [14:54:42.717] end error: 16.657278
[14:17:45.636] New key frame [14:54:42.722] New key frame
[14:17:45.636] BA loop run [14:54:42.727] BA loop run
[14:17:45.637] init error: 713.370566 [14:54:42.733] init error: 41010.712911
[14:17:45.638] end error: 0.058017 [14:54:43.336] end error: 85.405648
[14:17:45.744] New key frame [14:54:43.340] New key frame
[14:17:45.744] BA loop run [14:54:43.340] BA loop run
[14:17:45.745] init error: 749.167754 [14:54:43.345] init error: 75196195.162744
[14:17:45.749] end error: 0.779043 [14:54:43.675] end error: 462.211381
[14:17:45.846] New key frame [14:54:43.692] New key frame
[14:17:45.846] BA loop run [14:54:43.692] BA loop run
[14:17:45.847] init error: 12412.756356 [14:54:43.698] init error: 803.385900
[14:17:45.850] end error: 1.941401 [14:54:44.528] end error: 40.007984
[14:17:45.938] New key frame [14:54:44.740] New key frame
[14:17:45.938] BA loop run [14:54:44.740] BA loop run
[14:17:45.939] init error: 1009.645679 [14:54:44.744] init error: 508388.132806
[14:17:45.943] end error: 1.298569 [14:54:45.066] end error: 85.428830
[14:17:46.130] New key frame [14:54:45.068] New key frame
[14:17:46.130] BA loop run [14:54:45.068] BA loop run
[14:17:46.131] init error: 1805.845656 [14:54:45.075] init error: 50137.336823
[14:17:46.136] end error: 52.687598 [14:54:45.344] end error: 39.905726
[14:17:46.225] New key frame [14:54:45.346] New key frame
[14:17:46.225] BA loop run [14:54:45.349] BA loop run
[14:17:46.227] init error: 1384.023739 [14:54:45.366] init error: 21223.109285
[14:17:46.231] end error: 3.293355 [14:54:46.055] end error: 52.832861
[14:17:46.332] New key frame [14:54:46.058] New key frame
[14:17:46.332] BA loop run [14:54:46.058] BA loop run
[14:17:46.334] init error: 1916.816311 [14:54:46.064] init error: 907829.961200
[14:17:46.340] end error: 15.678229 [14:54:46.734] end error: 86.036304
[14:17:46.422] New key frame
[14:17:46.422] BA loop run
[14:17:46.424] init error: 1364.949035
[14:17:46.430] end error: 11.994543
[14:17:46.542] New key frame
[14:17:46.542] BA loop run
[14:17:46.544] init error: 1803.705818
[14:17:46.549] end error: 13.248052
[14:17:46.735] New key frame
[14:17:46.735] BA loop run
[14:17:46.737] init error: 359.627244
[14:17:46.744] end error: 15.917097
[14:17:46.848] New key frame
[14:17:46.848] BA loop run
[14:17:46.850] init error: 132.767637
[14:17:46.859] end error: 7.496239
[14:17:46.942] New key frame
[14:17:46.942] BA loop run
[14:17:46.945] init error: 273.140713
[14:17:46.955] end error: 9.084497
[14:17:47.035] New key frame
[14:17:47.035] BA loop run
[14:17:47.038] init error: 44858.514613
[14:17:47.092] end error: 23.858194
[14:17:47.261] New key frame
[14:17:47.261] BA loop run
[14:17:47.265] init error: 19240.174352
[14:17:47.425] end error: 33.269102
[14:17:47.443] New key frame
[14:17:47.443] BA loop run
[14:17:47.451] init error: 9490.855291
[14:17:47.791] end error: 60.008907
[14:17:47.877] New key frame
[14:17:47.877] BA loop run
[14:17:47.885] init error: 250641.349869
[14:17:48.203] end error: 95.233779
[14:17:49.274] New key frame
[14:17:49.274] BA loop run
[14:17:49.278] init error: 9949.519439
[14:17:49.559] end error: 50.564651
[14:17:50.848] New key frame
[14:17:50.848] BA loop run
[14:17:50.852] init error: 5178.825342
[14:17:50.979] end error: 22.268369
[14:17:51.763] New key frame
[14:17:51.764] BA loop run
[14:17:51.767] init error: 543.547794
[14:17:52.061] end error: 17.817167
[14:17:52.065] New key frame
[14:17:52.066] BA loop run
[14:17:52.077] init error: 1339.153605
[14:17:52.261] end error: 20.246683
[14:17:56.550] New key frame
[14:17:56.550] BA loop run
[14:17:56.554] init error: 8303.291564
[14:17:56.629] end error: 26.594103
[14:17:57.059] New key frame
[14:17:57.059] BA loop run
[14:17:57.063] init error: 91935.690165
[14:17:57.271] end error: 38.526509
[14:17:57.852] New key frame
[14:17:57.852] BA loop run
[14:17:57.858] init error: 232131.159180
[14:17:57.996] end error: 61.418385
[14:17:58.081] New key frame
[14:17:58.081] BA loop run
[14:17:58.089] init error: 9070.775705
[14:17:58.447] end error: 34.291902
[14:17:58.494] New key frame
[14:17:58.494] BA loop run
[14:17:58.500] init error: 68366.090407
[14:17:58.991] end error: 113.614379
[14:17:59.249] New key frame
[14:17:59.249] BA loop run
[14:17:59.253] init error: 1635180.603207
[14:17:59.360] end error: 198.804844
[14:18:00.085] New key frame
[14:18:00.085] BA loop run
[14:18:00.090] init error: 131349.804393
[14:18:00.273] end error: 1609.872889
[14:18:00.682] New key frame
[14:18:00.682] BA loop run
[14:18:00.687] init error: 12215847.631416
[14:18:00.909] end error: 523.178192
[14:18:01.335] New key frame
[14:18:01.335] BA loop run
[14:18:01.341] init error: 1425.392849
[14:18:01.786] end error: 104.404989
[14:18:01.795] New key frame
[14:18:01.795] BA loop run
[14:18:01.803] init error: 65647.241336
[14:18:02.125] end error: 85.889376
[14:18:02.141] New key frame
[14:18:02.141] BA loop run
[14:18:02.149] init error: 556607.881609
[14:18:02.371] end error: 312.798019
[14:18:02.682] New key frame
[14:18:02.682] BA loop run
[14:18:02.688] init error: 1285.331214
[14:18:02.907] end error: 54.310029
[14:18:02.923] New key frame
[14:18:02.923] BA loop run
[14:18:02.932] init error: 63079.696761
[14:18:03.756] end error: 27.679050
GPU_version/vio/frame_handler_log.txt
+1019 -1172
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/src/math_utils.cpp
+19 -2
View File
@@ -19,15 +19,32 @@ Vector3d
triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
const Vector3d& feature1, const Vector3d& feature2 ) const Vector3d& feature1, const Vector3d& feature2 )
{ {
Vector3d R_F2 = R * feature2; Vector3d R_F2 = R * feature2;//T*A=R*A+t
Vector2d b; Vector2d b;
/*
* b=cross(t,f1)
* corss(t,Rf2)
* */
b[0] = t.dot(feature1); b[0] = t.dot(feature1);
b[1] = t.dot(R_F2);//cross(t,R*f2) b[1] = t.dot(R_F2);//cross(t,R*f2)
Matrix2d A; Matrix2d A;
/**
* A<< cross(f1,f1), -z
* z , -corss(R*f2,R*f2)
*
* det(A)= -corss(R*f2,R*f2)*cross(f1,f1) + pow(z,2)
*/
A(0,0) = feature1.dot(feature1); A(0,0) = feature1.dot(feature1);
A(1,0) = feature1.dot(R_F2); A(1,0) = feature1.dot(R_F2);//z
A(0,1) = -A(1,0); A(0,1) = -A(1,0);
A(1,1) = -R_F2.dot(R_F2); A(1,1) = -R_F2.dot(R_F2);
/*
* A_inv<< cross(f1,f1)/det(A) , z/det(A)
* -z/det(A) , -corss(R*f2,R*f2)/det(A)
*
* lambda[0]= cross(t,f1)*cross(f1,f1)/det(A) + corss(t,Rf2)*z/det(A)
* lambda[1]= -cross(t,f1)*z/det(A) - corss(t,Rf2) * corss(R*f2,R*f2)/det(A)
* */
Vector2d lambda = A.inverse() * b; Vector2d lambda = A.inverse() * b;
Vector3d xm = lambda[0] * feature1; Vector3d xm = lambda[0] * feature1;
Vector3d xn = t + lambda[1] * R_F2; Vector3d xn = t + lambda[1] * R_F2;
GPU_version/vio/src/reprojector.cpp
+1 -1
View File
@@ -145,7 +145,7 @@ namespace vio {
SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3(); SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
// pose with respect to reference frame // pose with respect to reference frame
Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(), Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
frame->c2f(px),(*it_ref)->f); (*it_ref)->f,frame->c2f(px));
// point in world frame // point in world frame
std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref)); std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
frame->addFeature(std::make_shared<Feature>(frame, frame->addFeature(std::make_shared<Feature>(frame,