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

correct frame for initialize

Browse Source
This commit is contained in:
gfuse
2021-09-07 11:07:07 +02:00
parent 2ada3523cc
commit 1599bbe487
14 changed files with 614 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-8
View File
@@ -1,8 +0,0 @@
[13:53:07.139] init depth filter
[13:55:51.997] DepthFilter: RESET.
[13:55:57.552] add key frame
[13:55:57.552] update seed
[13:55:57.552] init seeds
[13:55:57.555] DepthFilter: Initialized 83 new seeds
[13:56:18.563] DepthFilter stop thread invoked.
[13:56:18.563] DepthFilter interrupt and join thread
GPU_version/vio/frame_handler_log.txt
+443 -123
View File
@@ -1,123 +1,443 @@
[13:55:52.003] Init: Selected first frame.
[13:55:52.103] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.199] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.299] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.400] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.499] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.599] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.699] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.797] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:52.898] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.003] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.098] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.199] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.298] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.398] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.498] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.597] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.700] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.806] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:53.900] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:54.003] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:54.103] Init: distance between two frames:0.000000 ,angle between two frames:0.000000
[13:55:54.199] Init: distance between two frames:0.008098 ,angle between two frames:-0.001413
[13:55:54.299] Init: distance between two frames:0.005687 ,angle between two frames:-0.008137
[13:55:54.407] Init: distance between two frames:0.005817 ,angle between two frames:-0.018202
[13:55:54.507] Init: distance between two frames:0.005256 ,angle between two frames:-0.021341
[13:55:54.600] Init: distance between two frames:0.005647 ,angle between two frames:-0.009766
[13:55:54.701] Init: distance between two frames:0.005229 ,angle between two frames:-0.008498
[13:55:54.801] Init: distance between two frames:0.005693 ,angle between two frames:0.001719
[13:55:54.901] Init: distance between two frames:0.005210 ,angle between two frames:-0.008292
[13:55:55.001] Init: distance between two frames:0.005547 ,angle between two frames:-0.015455
[13:55:55.300] Init: distance between two frames:0.016741 ,angle between two frames:0.002021
[13:55:55.403] Init: distance between two frames:0.005574 ,angle between two frames:-0.023727
[13:55:55.503] Init: distance between two frames:0.005524 ,angle between two frames:-0.000275
[13:55:55.600] Init: distance between two frames:0.005563 ,angle between two frames:-0.011462
[13:55:55.701] Init: distance between two frames:0.005445 ,angle between two frames:0.013509
[13:55:55.801] Init: distance between two frames:0.005600 ,angle between two frames:-0.000241
[13:55:55.901] Init: distance between two frames:0.005498 ,angle between two frames:0.001560
[13:55:56.000] Init: distance between two frames:0.005522 ,angle between two frames:0.004637
[13:55:56.102] Init: distance between two frames:0.005248 ,angle between two frames:-0.006459
[13:55:56.203] Init: distance between two frames:0.005829 ,angle between two frames:-0.005902
[13:55:56.305] Init: distance between two frames:0.005226 ,angle between two frames:-0.010043
[13:55:56.402] Init: distance between two frames:0.005506 ,angle between two frames:0.001260
[13:55:56.502] Init: distance between two frames:0.005776 ,angle between two frames:-0.008069
[13:55:56.602] Init: distance between two frames:0.005224 ,angle between two frames:0.002812
[13:55:56.704] Init: distance between two frames:0.005528 ,angle between two frames:0.006773
[13:55:56.802] Init: distance between two frames:0.005590 ,angle between two frames:-0.004552
[13:55:56.902] Init: distance between two frames:0.005306 ,angle between two frames:0.018989
[13:55:57.002] Init: distance between two frames:0.005527 ,angle between two frames:0.009266
[13:55:57.102] Init: distance between two frames:0.005535 ,angle between two frames:-0.003895
[13:55:57.202] Init: distance between two frames:0.006042 ,angle between two frames:-0.005678
[13:55:57.303] Init: distance between two frames:0.005375 ,angle between two frames:0.002087
[13:55:57.401] Init: distance between two frames:0.005455 ,angle between two frames:-0.005637
[13:55:57.552] Init: distance between two frames:1.071640 ,angle between two frames:0.180774
[13:55:57.552] Init: Selected first frame.
[13:55:57.682] After Reprojection Map nPoint:16 ,nCell:15 ,nMatches:4 ,distance between two frames x:0.456077 ,z=-0.308324,angle between two frames:0.009333
[13:55:57.682] After pose optimization, distance between two frames x:0.456077 ,z=-0.308324,angle between two frames:0.009333
[13:55:57.777] After Reprojection Map nPoint:4 ,nCell:4 ,nMatches:3 ,distance between two frames x:0.444207 ,z=-0.197349,angle between two frames:0.011885
[13:55:57.870] After Reprojection Map nPoint:3 ,nCell:3 ,nMatches:2 ,distance between two frames x:-0.915788 ,z=1.264063,angle between two frames:0.060910
[13:55:58.080] After Reprojection Map nPoint:2 ,nCell:2 ,nMatches:1 ,distance between two frames x:-0.207997 ,z=0.968835,angle between two frames:0.027259
[13:55:58.177] After Reprojection Map nPoint:1 ,nCell:1 ,nMatches:1 ,distance between two frames x:0.244283 ,z=-0.037634,angle between two frames:0.003677
[13:55:58.278] After Reprojection Map nPoint:1 ,nCell:1 ,nMatches:0 ,distance between two frames x:0.812641 ,z=-0.028099,angle between two frames:0.011898
[13:55:58.466] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005370 ,z=-0.001219,angle between two frames:0.018277
[13:55:58.581] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000004,angle between two frames:0.008013
[13:55:58.667] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000001,angle between two frames:0.001890
[13:55:58.775] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000009 ,z=-0.000010,angle between two frames:0.006365
[13:55:58.869] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000005 ,z=-0.000010,angle between two frames:0.000411
[13:55:58.970] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.000203
[13:55:59.064] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000008 ,z=-0.000075,angle between two frames:0.003287
[13:55:59.564] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.021895 ,z=-0.003651,angle between two frames:0.021524
[13:56:00.170] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.027362 ,z=-0.006364,angle between two frames:0.085969
[13:56:00.564] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.015903 ,z=-0.004426,angle between two frames:0.015006
[13:56:00.664] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000003 ,z=-0.000011,angle between two frames:0.001058
[13:56:00.777] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000003 ,z=0.000002,angle between two frames:0.000613
[13:56:01.164] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.016086 ,z=-0.003829,angle between two frames:0.024067
[13:56:01.264] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=0.000003,angle between two frames:0.001916
[13:56:01.364] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000003 ,z=-0.000018,angle between two frames:0.001095
[13:56:01.464] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000011 ,z=0.000003,angle between two frames:0.002453
[13:56:02.064] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.026612 ,z=-0.007134,angle between two frames:0.070804
[13:56:02.169] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000021 ,z=-0.000080,angle between two frames:0.001478
[13:56:02.363] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005007 ,z=-0.001536,angle between two frames:0.017243
[13:56:02.463] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000001,angle between two frames:0.001778
[13:56:02.565] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000008 ,z=-0.000018,angle between two frames:0.000692
[13:56:02.764] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005113 ,z=-0.001562,angle between two frames:0.006603
[13:56:02.865] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000021 ,z=-0.000078,angle between two frames:0.002404
[13:56:02.966] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000017 ,z=0.000001,angle between two frames:0.003004
[13:56:03.064] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000007 ,z=-0.000008,angle between two frames:0.002762
[13:56:03.171] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000005 ,z=-0.000005,angle between two frames:0.004046
[13:56:03.264] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000009 ,z=0.000009,angle between two frames:0.002007
[13:56:03.364] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.000411
[13:56:03.666] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.010716 ,z=-0.003146,angle between two frames:0.031622
[13:56:04.164] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.020633 ,z=-0.007259,angle between two frames:0.038141
[13:56:04.265] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000002,angle between two frames:0.000542
[13:56:04.464] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005238 ,z=-0.001791,angle between two frames:0.023024
[13:56:04.563] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000008 ,z=-0.000046,angle between two frames:0.000343
[13:56:04.665] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=0.000001,angle between two frames:0.002538
[13:56:04.765] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000012 ,z=-0.000005,angle between two frames:0.001983
[13:56:04.971] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005352 ,z=-0.001606,angle between two frames:0.000162
[13:56:05.166] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005324 ,z=-0.001748,angle between two frames:0.002488
[13:56:05.371] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005314 ,z=-0.001922,angle between two frames:0.026127
[13:56:05.698] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.010681 ,z=-0.004039,angle between two frames:0.020154
[13:56:05.773] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000001,angle between two frames:0.000479
[13:56:05.984] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.005159 ,z=-0.002048,angle between two frames:0.010591
[13:56:06.096] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=0.000000,angle between two frames:0.000905
[13:56:06.178] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000008 ,z=-0.000022,angle between two frames:0.001117
[13:56:06.287] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000023 ,z=0.000004,angle between two frames:0.001649
[13:56:06.375] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=0.000003,angle between two frames:0.000796
[13:56:06.470] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000004 ,z=0.000011,angle between two frames:0.003529
[13:56:06.564] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000036 ,z=-0.000111,angle between two frames:0.000367
[13:56:06.765] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000019 ,z=-0.000176,angle between two frames:0.002309
[13:56:06.864] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000156 ,z=-0.000101,angle between two frames:0.003402
[13:56:06.964] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000006 ,z=-0.000020,angle between two frames:0.004292
[13:56:07.066] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000005,angle between two frames:0.000914
[13:56:07.178] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.000731
[13:56:07.266] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000002,angle between two frames:0.000396
[13:56:07.367] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.000795
[13:56:07.466] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000002,angle between two frames:0.000653
[13:56:07.565] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.000792
[13:56:07.668] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.000924
[13:56:07.772] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.000731
[13:56:07.864] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000002,angle between two frames:0.000526
[13:56:07.967] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000002,angle between two frames:0.000643
[13:56:08.064] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000002,angle between two frames:0.000659
[11:01:55.988] Init: frame zero: -0.000004, -0.000076 -0.031748
[11:01:55.988] Init: Selected first frame.
[11:01:56.086] Init: px average disparity is:0.037345 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.086] Init: distance between two frames x:0.000004 ,z=0.000076,angle between two frames: 0.031748
[11:01:56.184] Init: px average disparity is:0.050550 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.184] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.284] Init: px average disparity is:0.054696 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.284] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.386] Init: px average disparity is:0.047857 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.386] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.482] Init: px average disparity is:0.055471 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.482] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.583] Init: px average disparity is:0.057133 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.583] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.685] Init: px average disparity is:0.051904 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.685] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.782] Init: px average disparity is:0.061241 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.782] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.884] Init: px average disparity is:0.050830 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.884] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:56.988] Init: px average disparity is:0.101803 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:56.988] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.086] Init: px average disparity is:7.279913 ,While minimum is: 30.000000 KLT tracked : 146
[11:01:57.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.188] Init: px average disparity is:14.299447 ,While minimum is: 30.000000 KLT tracked : 136
[11:01:57.188] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.284] Init: px average disparity is:17.174474 ,While minimum is: 30.000000 KLT tracked : 130
[11:01:57.284] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.387] Init: px average disparity is:16.165114 ,While minimum is: 30.000000 KLT tracked : 129
[11:01:57.387] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.485] Init: px average disparity is:13.631511 ,While minimum is: 30.000000 KLT tracked : 129
[11:01:57.485] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.584] Init: px average disparity is:11.967167 ,While minimum is: 30.000000 KLT tracked : 129
[11:01:57.584] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.683] Init: px average disparity is:9.226998 ,While minimum is: 30.000000 KLT tracked : 128
[11:01:57.683] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.786] Init: px average disparity is:9.280991 ,While minimum is: 30.000000 KLT tracked : 127
[11:01:57.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.886] Init: px average disparity is:8.696010 ,While minimum is: 30.000000 KLT tracked : 126
[11:01:57.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:57.986] Init: px average disparity is:10.812092 ,While minimum is: 30.000000 KLT tracked : 126
[11:01:57.986] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.084] Init: px average disparity is:10.220709 ,While minimum is: 30.000000 KLT tracked : 126
[11:01:58.084] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.187] Init: px average disparity is:9.905617 ,While minimum is: 30.000000 KLT tracked : 126
[11:01:58.187] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.284] Init: px average disparity is:10.536242 ,While minimum is: 30.000000 KLT tracked : 126
[11:01:58.284] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.385] Init: px average disparity is:9.821336 ,While minimum is: 30.000000 KLT tracked : 126
[11:01:58.385] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.496] Init: px average disparity is:11.334300 ,While minimum is: 30.000000 KLT tracked : 125
[11:01:58.496] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.585] Init: px average disparity is:11.068508 ,While minimum is: 30.000000 KLT tracked : 125
[11:01:58.585] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.685] Init: px average disparity is:12.364304 ,While minimum is: 30.000000 KLT tracked : 125
[11:01:58.685] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.786] Init: px average disparity is:9.340030 ,While minimum is: 30.000000 KLT tracked : 116
[11:01:58.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.886] Init: px average disparity is:8.380981 ,While minimum is: 30.000000 KLT tracked : 105
[11:01:58.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:58.984] Init: px average disparity is:9.289314 ,While minimum is: 30.000000 KLT tracked : 105
[11:01:58.984] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.086] Init: px average disparity is:8.568369 ,While minimum is: 30.000000 KLT tracked : 104
[11:01:59.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.186] Init: px average disparity is:8.583950 ,While minimum is: 30.000000 KLT tracked : 104
[11:01:59.186] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.294] Init: px average disparity is:10.034264 ,While minimum is: 30.000000 KLT tracked : 103
[11:01:59.294] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.385] Init: px average disparity is:10.455943 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.385] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.485] Init: px average disparity is:12.370028 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.485] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.586] Init: px average disparity is:11.320209 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.586] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.684] Init: px average disparity is:10.469113 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.684] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.784] Init: px average disparity is:10.335327 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.785] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.885] Init: px average disparity is:10.202416 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:01:59.986] Init: px average disparity is:10.288406 ,While minimum is: 30.000000 KLT tracked : 102
[11:01:59.986] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.086] Init: px average disparity is:10.270807 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.184] Init: px average disparity is:10.249354 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.184] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.285] Init: px average disparity is:10.251442 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.285] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.384] Init: px average disparity is:10.286003 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.384] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.485] Init: px average disparity is:10.152584 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.485] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.586] Init: px average disparity is:10.132516 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.586] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.688] Init: px average disparity is:10.194353 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.688] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.785] Init: px average disparity is:10.172247 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.785] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.885] Init: px average disparity is:10.240655 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.885] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:00.988] Init: px average disparity is:10.233074 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:00.988] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.085] Init: px average disparity is:10.133897 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.085] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.186] Init: px average disparity is:10.163725 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.186] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.285] Init: px average disparity is:10.222874 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.285] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.388] Init: px average disparity is:10.261694 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.388] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.486] Init: px average disparity is:10.267448 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.486] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.588] Init: px average disparity is:10.135959 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.588] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.687] Init: px average disparity is:10.252439 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.687] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.786] Init: px average disparity is:10.227142 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.888] Init: px average disparity is:10.259772 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.888] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:01.985] Init: px average disparity is:10.251031 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:01.985] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.086] Init: px average disparity is:10.260130 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.188] Init: px average disparity is:10.247340 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.188] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.288] Init: px average disparity is:10.186346 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.288] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.385] Init: px average disparity is:10.137828 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.385] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.487] Init: px average disparity is:10.226063 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.487] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.587] Init: px average disparity is:10.320538 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.587] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.688] Init: px average disparity is:10.223420 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.688] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.788] Init: px average disparity is:10.347357 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.788] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.889] Init: px average disparity is:10.157928 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.890] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:02.987] Init: px average disparity is:10.222194 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:02.987] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.097] Init: px average disparity is:10.251769 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.097] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.184] Init: px average disparity is:10.222131 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.185] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.288] Init: px average disparity is:10.232337 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.288] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.385] Init: px average disparity is:10.237536 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.385] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.487] Init: px average disparity is:10.282337 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.487] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.585] Init: px average disparity is:10.270175 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.585] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.687] Init: px average disparity is:10.206514 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.687] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.786] Init: px average disparity is:10.241267 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.886] Init: px average disparity is:10.141367 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:03.985] Init: px average disparity is:10.248750 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:03.985] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.085] Init: px average disparity is:10.221989 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.085] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.185] Init: px average disparity is:10.216355 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.185] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.287] Init: px average disparity is:10.219806 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.287] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.384] Init: px average disparity is:10.257169 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.384] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.484] Init: px average disparity is:10.158695 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.484] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.586] Init: px average disparity is:10.264931 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.587] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.685] Init: px average disparity is:10.282706 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.685] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.785] Init: px average disparity is:10.188245 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.785] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.886] Init: px average disparity is:10.237752 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:04.985] Init: px average disparity is:10.276601 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:04.985] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.084] Init: px average disparity is:10.241680 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.084] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.185] Init: px average disparity is:10.251192 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.185] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.285] Init: px average disparity is:10.171053 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.285] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.385] Init: px average disparity is:10.244646 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.385] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.484] Init: px average disparity is:10.203797 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.484] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.585] Init: px average disparity is:10.201256 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.585] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.687] Init: px average disparity is:10.136497 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.687] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.786] Init: px average disparity is:10.135943 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.885] Init: px average disparity is:10.248781 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.885] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:05.986] Init: px average disparity is:10.247617 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:05.986] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.086] Init: px average disparity is:10.267410 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.187] Init: px average disparity is:10.266867 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.187] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.287] Init: px average disparity is:10.264840 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.287] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.388] Init: px average disparity is:10.337304 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.388] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.485] Init: px average disparity is:10.128865 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.485] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.585] Init: px average disparity is:10.269877 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.585] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.687] Init: px average disparity is:10.337045 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.688] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.785] Init: px average disparity is:10.129750 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.785] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.885] Init: px average disparity is:10.168965 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.885] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:06.987] Init: px average disparity is:10.191896 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:06.987] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.087] Init: px average disparity is:10.238642 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.087] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.185] Init: px average disparity is:10.223198 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.185] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.285] Init: px average disparity is:10.282554 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.285] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.385] Init: px average disparity is:10.176952 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.385] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.486] Init: px average disparity is:10.238094 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.486] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.586] Init: px average disparity is:10.211476 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.586] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.685] Init: px average disparity is:10.233704 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.685] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.787] Init: px average disparity is:10.269705 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.787] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.891] Init: px average disparity is:10.287704 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.891] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:07.985] Init: px average disparity is:10.135574 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:07.985] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.086] Init: px average disparity is:10.249979 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.184] Init: px average disparity is:10.185653 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.184] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.284] Init: px average disparity is:10.219771 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.284] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.387] Init: px average disparity is:10.173549 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.387] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.487] Init: px average disparity is:10.147496 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.487] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.587] Init: px average disparity is:10.248013 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.587] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.689] Init: px average disparity is:10.263723 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.689] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.789] Init: px average disparity is:10.254862 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.789] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.886] Init: px average disparity is:10.270866 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:08.986] Init: px average disparity is:10.216670 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:08.986] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.086] Init: px average disparity is:10.217332 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.186] Init: px average disparity is:10.240237 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.186] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.285] Init: px average disparity is:10.193919 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.285] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.388] Init: px average disparity is:10.352347 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.388] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.488] Init: px average disparity is:10.186599 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.488] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.588] Init: px average disparity is:10.249947 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.588] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.685] Init: px average disparity is:10.126938 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.686] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.786] Init: px average disparity is:10.134148 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.887] Init: px average disparity is:10.290771 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.887] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:09.992] Init: px average disparity is:10.226093 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:09.992] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.087] Init: px average disparity is:10.265580 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.087] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.186] Init: px average disparity is:10.237849 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.186] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.287] Init: px average disparity is:10.284607 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.287] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.386] Init: px average disparity is:10.243096 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.386] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.487] Init: px average disparity is:10.328327 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.487] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.586] Init: px average disparity is:10.264396 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.586] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.687] Init: px average disparity is:10.196906 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.687] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.786] Init: px average disparity is:10.149259 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.786] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.887] Init: px average disparity is:10.172362 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.887] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:10.986] Init: px average disparity is:10.299432 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:10.986] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.086] Init: px average disparity is:10.251815 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.086] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.189] Init: px average disparity is:10.272047 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.189] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.288] Init: px average disparity is:10.222494 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.288] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.388] Init: px average disparity is:10.276493 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.388] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.488] Init: px average disparity is:10.266866 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.488] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.586] Init: px average disparity is:10.253621 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.586] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.686] Init: px average disparity is:10.212135 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.686] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.789] Init: px average disparity is:10.234589 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.789] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.885] Init: px average disparity is:10.168199 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.885] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:11.986] Init: px average disparity is:10.137210 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:11.987] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.085] Init: px average disparity is:10.244415 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.085] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.185] Init: px average disparity is:10.245816 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.185] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.286] Init: px average disparity is:10.135106 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.286] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.384] Init: px average disparity is:10.191597 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.384] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.486] Init: px average disparity is:10.293023 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.487] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.585] Init: px average disparity is:10.175095 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.585] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.687] Init: px average disparity is:10.151985 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.687] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.787] Init: px average disparity is:10.254288 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.788] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.887] Init: px average disparity is:10.171402 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.887] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:12.988] Init: px average disparity is:10.345112 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:12.988] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.087] Init: px average disparity is:10.209089 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.087] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.186] Init: px average disparity is:10.150739 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.186] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.288] Init: px average disparity is:10.227470 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.288] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.387] Init: px average disparity is:10.200847 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.387] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.487] Init: px average disparity is:10.265641 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.487] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.586] Init: px average disparity is:10.258678 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.586] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.685] Init: px average disparity is:10.129732 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.685] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.787] Init: px average disparity is:9.102508 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.787] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.885] Init: px average disparity is:11.568022 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.886] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:13.986] Init: px average disparity is:21.810193 ,While minimum is: 30.000000 KLT tracked : 102
[11:02:13.986] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.091] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:30.827348 ,While minimum is: 30.000000 KLT tracked: 102
[11:02:14.091] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.196] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:39.974605 ,While minimum is: 30.000000 KLT tracked: 102
[11:02:14.196] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.295] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:48.333544 ,While minimum is: 30.000000 KLT tracked: 102
[11:02:14.295] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.491] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:77.938278 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:14.491] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.590] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:90.717428 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:14.590] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.690] Init: Homography RANSAC (inlier) is:12 ,While 40 inliers minimum required. px average disparity is:106.873798 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:14.690] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.793] Init: Homography RANSAC (inlier) is:4 ,While 40 inliers minimum required. px average disparity is:118.779647 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:14.793] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.891] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:127.640758 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:14.891] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:14.990] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:140.627478 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:14.990] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.092] Init: Homography RANSAC (inlier) is:5 ,While 40 inliers minimum required. px average disparity is:158.715614 ,While minimum is: 30.000000 KLT tracked: 99
[11:02:15.092] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.193] Init: Homography RANSAC (inlier) is:5 ,While 40 inliers minimum required. px average disparity is:169.205942 ,While minimum is: 30.000000 KLT tracked: 98
[11:02:15.193] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.293] Init: Homography RANSAC (inlier) is:5 ,While 40 inliers minimum required. px average disparity is:183.609603 ,While minimum is: 30.000000 KLT tracked: 98
[11:02:15.293] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.394] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:197.180236 ,While minimum is: 30.000000 KLT tracked: 95
[11:02:15.394] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.494] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:209.895206 ,While minimum is: 30.000000 KLT tracked: 93
[11:02:15.494] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.602] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:222.922431 ,While minimum is: 30.000000 KLT tracked: 93
[11:02:15.602] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.699] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:240.840291 ,While minimum is: 30.000000 KLT tracked: 92
[11:02:15.699] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.802] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:255.005166 ,While minimum is: 30.000000 KLT tracked: 89
[11:02:15.802] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:15.912] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:267.040170 ,While minimum is: 30.000000 KLT tracked: 89
[11:02:15.912] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:16.010] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:279.245505 ,While minimum is: 30.000000 KLT tracked: 89
[11:02:16.010] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:16.396] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:317.753528 ,While minimum is: 30.000000 KLT tracked: 85
[11:02:16.396] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.056] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:325.193446 ,While minimum is: 30.000000 KLT tracked: 82
[11:02:27.056] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.257] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:302.640580 ,While minimum is: 30.000000 KLT tracked: 82
[11:02:27.257] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.472] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:277.556248 ,While minimum is: 30.000000 KLT tracked: 82
[11:02:27.472] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.515] Init: Homography RANSAC (inlier) is:0 ,While 40 inliers minimum required. px average disparity is:261.254929 ,While minimum is: 30.000000 KLT tracked: 80
[11:02:27.515] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.603] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:245.062521 ,While minimum is: 30.000000 KLT tracked: 80
[11:02:27.603] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.799] Init: Homography RANSAC (inlier) is:3 ,While 40 inliers minimum required. px average disparity is:215.034152 ,While minimum is: 30.000000 KLT tracked: 80
[11:02:27.799] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:27.902] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:203.837870 ,While minimum is: 30.000000 KLT tracked: 79
[11:02:27.902] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.000] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:188.600403 ,While minimum is: 30.000000 KLT tracked: 79
[11:02:28.000] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.097] Init: Homography RANSAC (inlier) is:9 ,While 40 inliers minimum required. px average disparity is:172.574220 ,While minimum is: 30.000000 KLT tracked: 79
[11:02:28.097] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.191] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:160.910590 ,While minimum is: 30.000000 KLT tracked: 78
[11:02:28.191] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.293] Init: Homography RANSAC (inlier) is:1 ,While 40 inliers minimum required. px average disparity is:144.128868 ,While minimum is: 30.000000 KLT tracked: 78
[11:02:28.293] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.392] Init: Homography RANSAC (inlier) is:6 ,While 40 inliers minimum required. px average disparity is:128.974416 ,While minimum is: 30.000000 KLT tracked: 78
[11:02:28.392] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.491] Init: Homography RANSAC (inlier) is:6 ,While 40 inliers minimum required. px average disparity is:118.914593 ,While minimum is: 30.000000 KLT tracked: 77
[11:02:28.491] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.594] Init: Homography RANSAC (inlier) is:6 ,While 40 inliers minimum required. px average disparity is:105.187966 ,While minimum is: 30.000000 KLT tracked: 77
[11:02:28.594] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.694] Init: Homography RANSAC (inlier) is:6 ,While 40 inliers minimum required. px average disparity is:90.291915 ,While minimum is: 30.000000 KLT tracked: 77
[11:02:28.694] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.793] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:74.358445 ,While minimum is: 30.000000 KLT tracked: 77
[11:02:28.793] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:28.991] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:49.854464 ,While minimum is: 30.000000 KLT tracked: 77
[11:02:28.991] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:29.091] Init: Homography RANSAC (inlier) is:11 ,While 40 inliers minimum required. px average disparity is:38.269844 ,While minimum is: 30.000000 KLT tracked: 77
[11:02:29.091] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:29.188] Init: px average disparity is:26.081087 ,While minimum is: 30.000000 KLT tracked : 77
[11:02:29.188] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:29.287] Init: px average disparity is:14.597041 ,While minimum is: 30.000000 KLT tracked : 77
[11:02:29.287] Init: distance between two frames x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[11:02:29.386] Init: px average disparity is:8.958921 ,While mi
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -132,7 +132,7 @@ namespace vio {
SE2 inv=T_f_w_.inverse();
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real())-M_PI_2, Vector3d::UnitY())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1)));
return out;
GPU_version/vio/include/vio/global.h
+2 -2
View File
@@ -122,8 +122,8 @@ namespace vio
//Todo add 15 roll orientation
//Camera frame z front, x right, y down -> right hands
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX()) // roll
* AngleAxisd(pitch()-M_PI_2, Vector3d::UnitY())// pitch
q = AngleAxisd(-0.261799, Vector3d::UnitX()) // roll
* AngleAxisd(pitch(), Vector3d::UnitY())// pitch
* AngleAxisd(0.0, Vector3d::UnitZ()); //yaw
SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
return tem;
GPU_version/vio/include/vio/initialization.h
+44 -21
View File
@@ -19,7 +19,8 @@
#include <vio/global.h>
#include <vio/cl_class.h>
//#include "../../gtsam/imu_integration.h"
#include <vio/ukf.h>
#include <vio/homography.h>
namespace vio {
@@ -37,7 +38,7 @@ public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FramePtr frame_ref_;
KltHomographyInit(opencl* gpu_fast_):gpu_fast_(gpu_fast_),T_cur_from_ref_(0.0,0.0,0.0) {};
KltHomographyInit(opencl* gpu_fast_,UKF* ukf,FILE* log=nullptr):gpu_fast_(gpu_fast_),T_cur_from_ref_(0.0,0.0,0.0),ukf_(ukf),log_(log) {};
~KltHomographyInit() {};
InitResult addFirstFrame(FramePtr frame_ref);
InitResult addSecondFrame(FramePtr frame_ref);
@@ -53,6 +54,40 @@ protected:
vector<Vector3d> xyz_in_cur_; //!< 3D points computed during the geometric check.
SE2_5 T_cur_from_ref_; //!< computed transformation between the first two frames.
opencl* gpu_fast_;
FILE* log_=nullptr;
UKF* ukf_= nullptr;
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE2_5& T_cur_from_ref)
{
vector<Vector2d > uv_ref(f_ref.size());
vector<Vector2d > uv_cur(f_cur.size());
for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
{
uv_ref[i] = vk::project2d(f_ref[i]);
uv_cur[i] = vk::project2d(f_cur[i]);
}
vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
vector<int> outliers;
assert(ukf_!= nullptr);
auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
auto check=ukf_->get_location();
if((check.second.se3().translation()-Homography.T_c2_from_c1.translation()).norm()>0.1)return;
auto res=ukf_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
Homography.T_c2_from_c1=res.second.se3();
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
T_cur_from_ref=SE2_5(Homography.T_c2_from_c1);
}
};
/// Detect Fast corners in the image.
@@ -61,26 +96,14 @@ void detectFeatures(
vector<cv::Point2f>& px_vec,
vector<Vector3d>& f_vec,
opencl* gpu_fast);
/// Compute optical flow (Lucas Kanade) for selected keypoints.
void trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities);
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE2_5& T_cur_from_ref);
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities);
} // namespace initialization
} // namespace vio
GPU_version/vio/include/vio/ukf.h
+11 -11
View File
@@ -14,6 +14,9 @@ public:
state_=init;
cov_.setIdentity();
cov_*=0.0001;
state_h_=init;
cov_h_.setIdentity();
cov_h_*=0.0001;
};
virtual ~Base(){
};
@@ -36,7 +39,6 @@ public:
0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))),
0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
predict_up=true;
}
void correct(double ddx, double ddy, double dpitch, double time){
double dt=time-t_imu;
@@ -75,12 +77,11 @@ public:
cov_h_=G*cov_*G.transpose()+R;
}
void correct(double x, double z,double pitch,size_t match,double time){
void correct(double x, double z,double pitch){
Eigen::Matrix<double,3,3> H;
Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k;
++match;
H<<1.0,0,0,
0,1.0,0,
0,0,1.0;
@@ -91,11 +92,9 @@ public:
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+(k*E);
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
predict_up=false;
}
Eigen::Matrix<double,3,3> cov_;
Eigen::Matrix<double, 3, 1> state_;
bool predict_up=false;
private:
double t_cmd=0.0;
Eigen::Matrix<double,3,3> cov_h_;
@@ -116,7 +115,7 @@ public:
lock=true;
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(filter_->predict_up)filter_->correct(x,y,theta,1e-9*time.toNSec());
filter_->correct(x,y,theta,1e-9*time.toNSec());
lock=false;
};
//IMU frame y front, x right, z up -> left hands (theta counts from y)
@@ -126,16 +125,17 @@ public:
filter_->predict(x,y,theta,1e-9*time.toNSec());
lock=false;
};
//Camera frame z front, x right, y down -> right hands (pitch counts from x)
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*/,size_t match,ros::Time& time) {
//Camera frame z front, x right, y down -> right 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
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*/) {
while(lock)usleep(5);
lock=true;
//if(filter_->predict_up)filter_->correct(z,x,pitch,match,1e-9*time.toNSec());
filter_->correct(-1.0*x,-1.0*z,pitch);
lock=false;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_2)));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-1.0*filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)+M_PI_2)));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-1.0*filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
}
private:
Base* filter_= nullptr;
GPU_version/vio/kernel/compute-residual.cl
+3 -3
View File
@@ -22,11 +22,11 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
{
ref.x*=-1.0;
ref.y*=-1.0;
ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
ref.z = 3.141592653589793238462643383279502884197169399375 + ref.z;
float3 error=ref+cur;
float yaw=0.0;
float pitch=error.z-1.57079632679489661923;
float roll=0.26;
float pitch=error.z;
float roll=-0.26;
float R00=cos(yaw)*cos(pitch);//cos(pitch)
float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -5,12 +5,12 @@ svo:
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.
init_min_tracked: 100
init_min_disparity: 80
init_min_disparity: 30
structureoptim_max_pts: 50
kfselect_mindist: 0.01
poseoptim_thresh: 4.0
init_min_inliers: 40
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.001 #gyro white noise in continuous in EKF
svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
GYO_noise: 100 #gyro white noise in continuous in EKF
svo_ekf: 0.01 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+5 -5
View File
@@ -258,14 +258,14 @@ namespace vio {
continue;
}
// check if point is visible in the current image
SE2 T(it->ftr->frame->T_f_w_.pitch()+frame->T_f_w_.pitch()+M_PI,it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
SE2 T(frame->T_f_w_.pitch()-it->ftr->frame->T_f_w_.pitch(),frame->T_f_w_.se2().translation()+it->ftr->frame->T_f_w_.inverse().translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real())-M_PI_2, Vector3d::UnitY())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
SE3 T_cur_ref(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_cur_ref*(1.0/it->mu * it->ftr->f) );
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
#endif
@@ -295,7 +295,7 @@ namespace vio {
++n_failed_matches;
continue;
}
double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
double tau = computeTau(T_cur_ref.inverse(), it->ftr->f, z, px_error_angle);
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
// update the estimate
GPU_version/vio/src/frame_handler_mono.cpp
+14 -19
View File
@@ -65,12 +65,14 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
gpu_fast_->write_buf(1,9,300,J);
float chi2[300]={0.01};
gpu_fast_->write_buf(1,10,300,chi2);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
initialize();
#if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+");
assert(log_);
chmod((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(), ACCESSPERMS);
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_,&ukfPtr_,log_);
#else
klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_&ukfPtr_);
#endif
}
@@ -140,26 +142,19 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
#if VIO_DEBUG
fprintf(log_,"[%s] Init: distance between two frames:%f ,angle between two frames x:%f ,z=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch());
fprintf(log_,"[%s] Init: distance between two frames x:%f ,z=%f,angle between two frames: %f \n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch());
#endif
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_);
new_frame_->T_f_w_ = result.second;
new_frame_->Cov_ = result.first;
if(res == initialization::NO_KEYFRAME){
return RESULT_NO_KEYFRAME;
}else if(res == initialization::FAILURE){
stage_ = STAGE_FIRST_FRAME;
return RESULT_FAILURE;
}
new_frame_->setKeyframe();
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
@@ -191,8 +186,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fprintf(log_,"[%s] After Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
n_point,
reprojector_.n_trials_,reprojector_.n_matches_,
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
if( reprojector_.n_trials_ < 10){
@@ -208,8 +203,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);*/
#if VIO_DEBUG
fprintf(log_,"[%s] After pose optimization, distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
@@ -220,13 +215,13 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
return RESULT_FAILURE;
}
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch());
new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first;
#if VIO_DEBUG
fprintf(log_,"[%s] Update EKF and 3D points, distance between two frames x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
last_frame_->T_f_w_.se2().translation().x()-new_frame_->T_f_w_.se2().translation().x(),
last_frame_->T_f_w_.se2().translation().y()-new_frame_->T_f_w_.se2().translation().y(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
GPU_version/vio/src/homography.cpp
+4 -4
View File
@@ -43,13 +43,13 @@ namespace vk {
vector<cv::Point2f> src_pts(fts_c1.size()), dst_pts(fts_c1.size());
for(size_t i=0; i<fts_c1.size(); ++i)
{
src_pts[i] = cv::Point2f(fts_c1[i][0], fts_c1[i][1]);
dst_pts[i] = cv::Point2f(fts_c2[i][0], fts_c2[i][1]);
src_pts[i] = cv::Point2f(fts_c1[i][0], fts_c1[i][1]);//refrance
dst_pts[i] = cv::Point2f(fts_c2[i][0], fts_c2[i][1]);//curent
}
// TODO: replace this function to remove dependency from opencv!
// cv::Mat cvH = cv::findHomography(src_pts, dst_pts, CV_RANSAC, 2./error_multiplier2);
cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC, 2./error_multiplier2);
cv::Mat cvH = cv::findHomography(src_pts, dst_pts, cv::RANSAC, 2./error_multiplier2);//2./error_multiplier2
H_c2_from_c1(0,0) = cvH.at<double>(0,0);
H_c2_from_c1(0,1) = cvH.at<double>(0,1);
H_c2_from_c1(0,2) = cvH.at<double>(0,2);
@@ -92,7 +92,7 @@ namespace vk {
q.w() *= 1/q.norm();
q.x()=0.0;
q.z()=0.0;
T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(), 0.0, decompositions.front().T.translation().z()));
T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(), decompositions.front().T.translation().y(), decompositions.front().T.translation().z()));
return true;
}
GPU_version/vio/src/initialization.cpp
+76 -89
View File
@@ -21,7 +21,6 @@
#include <vio/initialization.h>
#include <vio/feature_detection.h>
#include <vio/math_utils.h>
#include <vio/homography.h>
namespace vio {
namespace initialization {
@@ -35,6 +34,12 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
return FAILURE;
}
#if VIO_DEBUG
fprintf(log_,"[%s] Init: frame zero: %f, %f %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
frame_ref->T_f_w_.se2().translation().x(),
frame_ref->T_f_w_.se2().translation().y(),
frame_ref->T_f_w_.pitch());
#endif
frame_ref_ = frame_ref;
px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
return SUCCESS;
@@ -46,25 +51,30 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
if(disparities_.size() < 1)
return NO_KEYFRAME;
double disparity = vk::getMedian(disparities_);
if(disparity < Config::initMinDisparity()){
SVO_INFO_STREAM("Init: px average disparity is: "<<disparity<<" While: "<<Config::initMinDisparity()<<" minimum required."
<<" KLT tracked : "<<disparities_.size());
return NO_KEYFRAME;
}
if(disparity < Config::initMinDisparity()){
#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(),
disparity,
Config::initMinDisparity(),
disparities_.size());
#endif
return NO_KEYFRAME;
}
computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
inliers_, xyz_in_cur_, T_cur_from_ref_);
if(inliers_.size() < Config::initMinInliers() && disparity < 150)
{
SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<<inliers_.size()<<" While: "<<Config::initMinInliers()<<" inliers minimum required.\n"
<<"Init: px average disparity is: "<<disparity<<" While: "<<Config::initMinDisparity()<<" minimum required.");
if(inliers_.size() < Config::initMinInliers()){
#if VIO_DEBUG
fprintf(log_,"[%s] Init: Homography RANSAC (inlier) is:%d ,While %d inliers minimum required. px average disparity is:%f ,While minimum is: %f KLT tracked: %d\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
inliers_.size(),
Config::initMinInliers(),
disparity,
Config::initMinDisparity(),
disparities_.size());
#endif
return NO_KEYFRAME;
}else if(inliers_.size() < Config::initMinInliers() && disparity > 150){
SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<<inliers_.size()<<" While: "<<Config::initMinInliers()<<" inliers minimum required.\n"
<<"Init: px average disparity is: "<<disparity<<" While: "<<Config::initMinDisparity()<<" minimum required. Reinitialized");
return FAILURE;
}
// Rescale the map such that the mean scene depth is equal to the specified scale
@@ -96,9 +106,15 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
new_point->addFrameRef(ftr_ref);
}
}
SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<<inliers_.size()
<<"\nKLT disparity is: "<<disparity<<" px average disparity."
<<"\nThe number of feature: "<<frame_cur->fts_.size());
#if VIO_DEBUG
fprintf(log_,"[%s] Init finished: Homography RANSAC (inlier) is:%d ,While %d inliers minimum required. px average disparity is:%f ,While minimum is: %f KLT tracked: %d\n",
vio::time_in_HH_MM_SS_MMM().c_str(),
inliers_.size(),
Config::initMinInliers(),
disparity,
Config::initMinDisparity(),
disparities_.size());
#endif
return SUCCESS;
}
@@ -127,78 +143,49 @@ void detectFeatures(
delete ftr;
}
}
void trackKlt(
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities)
{
disparities.clear();
const double klt_win_size = 30.0;//30.0
const int klt_max_iter = 30;//30
const double klt_eps = 0.001;
vector<uchar> status;
vector<float> error;
vector<float> min_eig_vec;
cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
if(frame_ref->img_pyr_[0].empty() || frame_cur->img_pyr_[0].empty())return;
cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
vector<Vector3d>::iterator f_ref_it = f_ref.begin();
f_cur.clear(); //f_cur.reserve(px_cur.size());
//disparities.reserve(px_cur.size());
for(size_t i=0; px_ref_it != px_ref.end(); ++i)
{
if(!status[i])
FramePtr frame_ref,
FramePtr frame_cur,
vector<cv::Point2f>& px_ref,
vector<cv::Point2f>& px_cur,
vector<Vector3d>& f_ref,
vector<Vector3d>& f_cur,
vector<double>& disparities)
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
f_ref_it = f_ref.erase(f_ref_it);
continue;
}
f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y));
disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm());
++px_ref_it;
++px_cur_it;
++f_ref_it;
}
}
void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE2_5& T_cur_from_ref)
{
vector<Vector2d > uv_ref(f_ref.size());
vector<Vector2d > uv_cur(f_cur.size());
for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
{
uv_ref[i] = vk::project2d(f_ref[i]);
uv_cur[i] = vk::project2d(f_cur[i]);
}
vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
vector<int> outliers;
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
T_cur_from_ref=SE2_5(Homography.T_c2_from_c1);
}
disparities.clear();
const double klt_win_size = 30.0;//30.0
const int klt_max_iter = 30;//30
const double klt_eps = 0.001;
vector<uchar> status;
vector<float> error;
vector<float> min_eig_vec;
cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
if(frame_ref->img_pyr_[0].empty() || frame_cur->img_pyr_[0].empty())return;
cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
vector<Vector3d>::iterator f_ref_it = f_ref.begin();
f_cur.clear(); //f_cur.reserve(px_cur.size());
//disparities.reserve(px_cur.size());
for(size_t i=0; px_ref_it != px_ref.end(); ++i)
{
if(!status[i])
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
f_ref_it = f_ref.erase(f_ref_it);
continue;
}
f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y));
disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm());
++px_ref_it;
++px_cur_it;
++f_ref_it;
}
};
} // namespace initialization
} // namespace vio
GPU_version/vio/src/visualizer.cpp
+4 -4
View File
@@ -55,11 +55,11 @@ void Visualizer::publishMinimal(
//Rviz frame z up, x right, y forward -> right hands (yaw counts from y)
//Camera frame z front, x right, y down -> right hands (pitch counts from z)
//IMU frame y front, x right, z up -> left hands (theta counts from y)
Quaterniond q(AngleAxisd(-odom.second.pitch(), Vector3d::UnitZ()));
Quaterniond q(AngleAxisd(-odom.second.pitch()+M_PI_2, Vector3d::UnitZ()));
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.x = -1.0*odom.second.se2().translation()(0);
msg_pose_with_cov->pose.pose.position.y = -1.0*odom.second.se2().translation()(1);
msg_pose_with_cov->pose.pose.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x();
msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -74,7 +74,7 @@ void Visualizer::publishMinimal(
pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker(
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory",
pub_points_, Eigen::Vector3d(-1.0*odom.second.se2().translation()(0),-1.0*odom.second.se2().translation()(1),0.0), "trajectory",
ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
}
}
GPU_version/vio/src/vo_node.cpp
+4 -1
View File
@@ -119,10 +119,13 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
try {
cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
if(img.empty())return;
cv::Mat imgbul;
cv::Mat imgbul;//tem;
cv::Laplacian(img,imgbul,CV_64F);
cv::Scalar mean, stddev;
meanStdDev(imgbul, mean, stddev, cv::Mat());
//cv::cvtColor(img,tem,CV_GRAY2RGBA);
//cv::imshow("test",tem);
//cv::waitKey();
if(stddev.val[0] * stddev.val[0]< 50.0)return;
vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp);
} catch (cv_bridge::Exception& e) {