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

local commit

Browse Source
This commit is contained in:
VahdPourAmin
2021-09-07 17:34:30 +02:00
parent 4b9f739d4d
commit c7c4feb68d
5 changed files with 3 additions and 18487 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-17528
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
-956
View File
@@ -1,956 +0,0 @@
[17:09:48.429] Init: frame zero: 0.000000, 0.000000 0.000000
[17:09:48.429] Init: Selected first frame.
[17:09:48.527] Init: px average disparity is:0.046673 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:48.527] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:48.624] Init: px average disparity is:0.050710 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:48.624] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:48.723] Init: px average disparity is:0.062790 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:48.723] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:48.823] Init: px average disparity is:0.069989 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:48.823] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:48.923] Init: px average disparity is:0.046144 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:48.923] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.023] Init: px average disparity is:0.048902 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.023] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.127] Init: px average disparity is:0.060309 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.127] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.225] Init: px average disparity is:0.052158 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.225] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.324] Init: px average disparity is:0.056654 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.324] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.423] Init: px average disparity is:0.055116 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.423] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.523] Init: px average disparity is:0.052830 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.523] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.625] Init: px average disparity is:0.053570 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.625] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.724] Init: px average disparity is:0.055715 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.724] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.826] Init: px average disparity is:0.059457 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.826] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:49.925] Init: px average disparity is:0.067845 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:49.925] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.025] Init: px average disparity is:0.056338 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:50.025] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.127] Init: px average disparity is:0.136545 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:50.127] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.226] Init: px average disparity is:1.497322 ,While minimum is: 30.000000 KLT tracked : 109
[17:09:50.226] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.328] Init: px average disparity is:6.213272 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:50.328] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.424] Init: px average disparity is:4.487548 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:50.424] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.526] Init: px average disparity is:8.260767 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:50.526] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.624] Init: px average disparity is:12.090768 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:50.624] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.724] Init: px average disparity is:17.964640 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:50.724] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:50.924] Init: px average disparity is:13.701610 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:50.924] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.227] Init: px average disparity is:10.527424 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:51.227] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.326] Init: px average disparity is:16.135485 ,While minimum is: 30.000000 KLT tracked : 107
[17:09:51.326] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.425] Init: px average disparity is:13.199271 ,While minimum is: 30.000000 KLT tracked : 106
[17:09:51.426] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.526] Init: px average disparity is:18.945413 ,While minimum is: 30.000000 KLT tracked : 106
[17:09:51.526] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.626] Init: px average disparity is:21.501833 ,While minimum is: 30.000000 KLT tracked : 106
[17:09:51.626] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.728] Init: px average disparity is:24.921740 ,While minimum is: 30.000000 KLT tracked : 106
[17:09:51.728] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.828] Init: px average disparity is:28.628273 ,While minimum is: 30.000000 KLT tracked : 104
[17:09:51.828] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[17:09:51.943] homography: x=0.035802 y=0.051909 z=0.135398 roll=0.000000 pitch=-0.021768 yaw=0.000000 ekf: x=-0.001716 z=0.109814 pitch=-0.029114
[17:09:51.943] homography after fuse: x=0.031675 y=0.000000 z=0.132583 pitch=-0.024646 number of outlier: 71
[17:09:51.943] Init finished: Homography RANSAC (inlier) is:33 ,While 20 inliers minimum required. px average disparity is:35.447556 ,While minimum is: 30.000000 KLT tracked: 104
[17:09:51.943] Init: distance between the first and current frame is x:0.031675 ,z=0.132583,angle between two frames: -0.024646
[17:09:51.943] Init: Selected Second frame. The number of features: 33
[17:09:52.136] After Reprojection Map nPoint:17 ,nCell:15 ,nMatches:6 ,distance between two frames x:-0.012543 ,z=-0.002927,angle between two frames:0.018256
[17:09:52.136] After pose optimization, distance between two frames x:-0.012543 ,z=-0.002927,angle between two frames:0.018256
[17:09:52.136] Update EKF and 3D points, distance between two frames x:-0.012616 ,z=-0.002361,angle between two frames:0.017115
[17:09:52.218] After Reprojection Map nPoint:6 ,nCell:6 ,nMatches:3 ,distance between two frames x:0.054602 ,z=-0.038448,angle between two frames:0.058261
[17:09:52.305] After Reprojection Map nPoint:3 ,nCell:3 ,nMatches:2 ,distance between two frames x:-0.017513 ,z=0.034222,angle between two frames:0.027745
[17:09:52.406] After Reprojection Map nPoint:2 ,nCell:2 ,nMatches:1 ,distance between two frames x:-0.037735 ,z=0.048615,angle between two frames:0.049061
[17:09:52.506] After Reprojection Map nPoint:1 ,nCell:1 ,nMatches:1 ,distance between two frames x:-0.215026 ,z=0.105374,angle between two frames:0.235546
[17:09:52.602] After Reprojection Map nPoint:1 ,nCell:1 ,nMatches:1 ,distance between two frames x:-0.000951 ,z=0.048662,angle between two frames:0.017731
[17:09:52.709] After Reprojection Map nPoint:1 ,nCell:1 ,nMatches:0 ,distance between two frames x:0.086582 ,z=0.022150,angle between two frames:0.064802
[17:09:52.805] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000006,angle between two frames:0.002648
[17:09:52.896] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000005 ,z=-0.000001,angle between two frames:0.001970
[17:09:52.995] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000006 ,z=-0.000010,angle between two frames:0.008429
[17:09:53.093] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000010 ,z=0.000003,angle between two frames:0.004813
[17:09:53.215] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000402 ,z=0.005668,angle between two frames:0.001464
[17:09:53.294] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000014 ,z=0.000001,angle between two frames:0.000741
[17:09:53.392] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000016 ,z=-0.000002,angle between two frames:0.001627
[17:09:53.491] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000442 ,z=0.005484,angle between two frames:0.006666
[17:09:53.607] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000010 ,z=0.000003,angle between two frames:0.003621
[17:09:53.697] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000000,angle between two frames:0.000960
[17:09:53.794] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000004 ,z=-0.000007,angle between two frames:0.003734
[17:09:53.900] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000032 ,z=0.000001,angle between two frames:0.006322
[17:09:54.001] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000002 ,z=-0.000005,angle between two frames:0.003426
[17:09:54.098] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000335
[17:09:54.194] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000004,angle between two frames:0.002054
[17:09:54.310] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000167 ,z=0.005496,angle between two frames:0.000808
[17:09:54.394] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.001699
[17:09:54.496] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000262 ,z=0.005483,angle between two frames:0.002436
[17:09:54.605] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000051,angle between two frames:0.002884
[17:09:54.697] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.005841
[17:09:54.805] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000000,angle between two frames:0.002911
[17:09:54.897] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.003418
[17:09:55.009] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000004 ,z=-0.000005,angle between two frames:0.005679
[17:09:55.098] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000001,angle between two frames:0.004268
[17:09:55.196] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000002 ,z=-0.000005,angle between two frames:0.004611
[17:09:55.304] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000015 ,z=0.000026,angle between two frames:0.000247
[17:09:55.396] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000006,angle between two frames:0.000832
[17:09:55.503] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.000423
[17:09:55.593] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.000642
[17:09:55.717] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000379 ,z=0.005489,angle between two frames:0.000400
[17:09:55.796] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.000866
[17:09:55.910] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000008,angle between two frames:0.000944
[17:09:56.022] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000000,angle between two frames:0.000278
[17:09:56.100] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000001,angle between two frames:0.002044
[17:09:56.201] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000001,angle between two frames:0.000000
[17:09:56.303] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000014,angle between two frames:0.006408
[17:09:56.402] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000041 ,z=0.000001,angle between two frames:0.003228
[17:09:56.497] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000003,angle between two frames:0.005125
[17:09:56.607] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000004 ,z=-0.000004,angle between two frames:0.002957
[17:09:56.699] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000006,angle between two frames:0.004095
[17:09:56.795] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=0.000016,angle between two frames:0.006552
[17:09:56.894] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000032,angle between two frames:0.002312
[17:09:56.994] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000001,angle between two frames:0.005925
[17:09:57.094] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000007,angle between two frames:0.008434
[17:09:57.200] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000004,angle between two frames:0.002160
[17:09:57.298] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000002 ,z=-0.000003,angle between two frames:0.008127
[17:09:57.413] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000003 ,z=-0.000017,angle between two frames:0.000113
[17:09:57.503] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=0.000000,angle between two frames:0.001628
[17:09:57.594] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.002460
[17:09:57.692] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000494 ,z=0.005472,angle between two frames:0.000118
[17:09:57.803] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000553 ,z=0.005484,angle between two frames:0.000014
[17:09:57.905] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000000,angle between two frames:0.004000
[17:09:58.002] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000026,angle between two frames:0.001752
[17:09:58.102] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000000,angle between two frames:0.001818
[17:09:58.209] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000003 ,z=-0.000020,angle between two frames:0.007039
[17:09:58.304] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000001,angle between two frames:0.000298
[17:09:58.404] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000004,angle between two frames:0.004567
[17:09:58.500] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.003086
[17:09:58.605] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000001,angle between two frames:0.001875
[17:09:58.703] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000255
[17:09:58.806] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000016 ,z=-0.000004,angle between two frames:0.000151
[17:09:58.900] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000008,angle between two frames:0.000562
[17:09:59.004] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000015,angle between two frames:0.000182
[17:09:59.122] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.004209
[17:09:59.227] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.003085
[17:09:59.331] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000930
[17:09:59.436] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000020,angle between two frames:0.000860
[17:09:59.519] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.000351
[17:09:59.599] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000004,angle between two frames:0.002124
[17:09:59.704] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000007 ,z=-0.000001,angle between two frames:0.000608
[17:09:59.797] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000004,angle between two frames:0.005574
[17:09:59.904] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000018 ,z=-0.000008,angle between two frames:0.004527
[17:09:59.994] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000004,angle between two frames:0.001156
[17:10:00.100] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.001174 ,z=0.005371,angle between two frames:0.010286
[17:10:00.204] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000005 ,z=-0.000002,angle between two frames:0.001829
[17:10:00.293] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000005 ,z=-0.000002,angle between two frames:0.001397
[17:10:00.407] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.001436 ,z=0.005595,angle between two frames:0.002215
[17:10:00.506] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000002 ,z=-0.000006,angle between two frames:0.003430
[17:10:00.604] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000021 ,z=-0.000006,angle between two frames:0.003879
[17:10:00.703] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000006 ,z=-0.000027,angle between two frames:0.003191
[17:10:00.807] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000001,angle between two frames:0.001126
[17:10:00.898] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000001,angle between two frames:0.002192
[17:10:01.006] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000015,angle between two frames:0.007290
[17:10:01.094] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000005 ,z=0.000016,angle between two frames:0.001200
[17:10:01.214] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000020 ,z=-0.000077,angle between two frames:0.002533
[17:10:01.323] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000001,angle between two frames:0.001729
[17:10:01.425] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000001,angle between two frames:0.000862
[17:10:01.506] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000003 ,z=-0.000001,angle between two frames:0.002707
[17:10:01.598] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000001,angle between two frames:0.001356
[17:10:01.698] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000029 ,z=-0.000043,angle between two frames:0.003402
[17:10:01.791] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000001,angle between two frames:0.000347
[17:10:01.901] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.001483 ,z=0.005359,angle between two frames:0.002604
[17:10:02.001] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000003 ,z=-0.000013,angle between two frames:0.001389
[17:10:02.095] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000008 ,z=-0.000003,angle between two frames:0.000702
[17:10:02.193] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000023,angle between two frames:0.004420
[17:10:02.299] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000008 ,z=0.000014,angle between two frames:0.002311
[17:10:02.395] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=0.000000,angle between two frames:0.000054
[17:10:02.495] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000009 ,z=-0.000007,angle between two frames:0.007261
[17:10:02.612] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000000,angle between two frames:0.003862
[17:10:02.723] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000001,angle between two frames:0.002592
[17:10:02.809] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000000,angle between two frames:0.000414
[17:10:02.908] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000004,angle between two frames:0.001396
[17:10:03.017] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000003 ,z=0.000005,angle between two frames:0.003645
[17:10:03.102] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.000000
[17:10:03.205] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000002,angle between two frames:0.000552
[17:10:03.304] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000015 ,z=-0.000016,angle between two frames:0.000247
[17:10:03.400] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.003713
[17:10:03.523] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000003 ,z=-0.000007,angle between two frames:0.000436
[17:10:03.628] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.001264
[17:10:03.731] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.000445
[17:10:03.819] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000005 ,z=-0.000002,angle between two frames:0.000578
[17:10:03.895] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.000000
[17:10:04.000] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000003 ,z=-0.000011,angle between two frames:0.006776
[17:10:04.093] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000008,angle between two frames:0.005632
[17:10:04.195] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.001621 ,z=0.005030,angle between two frames:0.009600
[17:10:04.294] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000004,angle between two frames:0.003089
[17:10:04.405] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.001561 ,z=0.005409,angle between two frames:0.007116
[17:10:04.506] After Reprojection Map nPoint:20 ,nCell:18 ,nMatches:6 ,distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.005618
[17:10:04.506] After pose optimization, distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.005618
[17:10:04.506] Update EKF and 3D points, distance between two frames x:-0.000133 ,z=0.000550,angle between two frames:0.011824
[17:10:04.600] After Reprojection Map nPoint:22 ,nCell:21 ,nMatches:6 ,distance between two frames x:0.033275 ,z=0.011406,angle between two frames:0.018234
[17:10:04.600] After pose optimization, distance between two frames x:0.033275 ,z=0.011406,angle between two frames:0.018234
[17:10:04.600] Update EKF and 3D points, distance between two frames x:0.029331 ,z=0.010962,angle between two frames:0.019044
[17:10:04.702] After Reprojection Map nPoint:24 ,nCell:21 ,nMatches:5 ,distance between two frames x:-0.014796 ,z=-0.000537,angle between two frames:0.013891
[17:10:04.702] After pose optimization, distance between two frames x:-0.014796 ,z=-0.000537,angle between two frames:0.013891
[17:10:04.702] Update EKF and 3D points, distance between two frames x:-0.014639 ,z=-0.000037,angle between two frames:0.019858
[17:10:04.807] After Reprojection Map nPoint:23 ,nCell:18 ,nMatches:7 ,distance between two frames x:0.032228 ,z=0.054512,angle between two frames:0.000301
[17:10:04.808] After pose optimization, distance between two frames x:0.032228 ,z=0.054512,angle between two frames:0.000301
[17:10:04.808] Update EKF and 3D points, distance between two frames x:0.028552 ,z=0.049248,angle between two frames:0.017651
[17:10:04.909] After Reprojection Map nPoint:30 ,nCell:23 ,nMatches:6 ,distance between two frames x:-0.011333 ,z=-0.014214,angle between two frames:0.008278
[17:10:04.909] After pose optimization, distance between two frames x:-0.011333 ,z=-0.014214,angle between two frames:0.008278
[17:10:04.909] Update EKF and 3D points, distance between two frames x:-0.011504 ,z=-0.013854,angle between two frames:0.012961
[17:10:05.005] After Reprojection Map nPoint:25 ,nCell:21 ,nMatches:7 ,distance between two frames x:-0.001726 ,z=0.002463,angle between two frames:0.006755
[17:10:05.005] After pose optimization, distance between two frames x:-0.001726 ,z=0.002463,angle between two frames:0.006755
[17:10:05.005] Update EKF and 3D points, distance between two frames x:-0.001781 ,z=0.002886,angle between two frames:0.015592
[17:10:05.097] After Reprojection Map nPoint:28 ,nCell:23 ,nMatches:5 ,distance between two frames x:0.014773 ,z=0.010847,angle between two frames:0.005439
[17:10:05.098] After pose optimization, distance between two frames x:0.014773 ,z=0.010847,angle between two frames:0.005439
[17:10:05.098] Update EKF and 3D points, distance between two frames x:0.012759 ,z=0.010324,angle between two frames:0.008436
[17:10:05.217] After Reprojection Map nPoint:25 ,nCell:21 ,nMatches:5 ,distance between two frames x:0.006243 ,z=0.008106,angle between two frames:0.000762
[17:10:05.217] After pose optimization, distance between two frames x:0.006243 ,z=0.008106,angle between two frames:0.000762
[17:10:05.217] Update EKF and 3D points, distance between two frames x:0.004701 ,z=0.007632,angle between two frames:0.009954
[17:10:05.337] After Reprojection Map nPoint:27 ,nCell:22 ,nMatches:5 ,distance between two frames x:0.023418 ,z=0.003116,angle between two frames:0.014871
[17:10:05.337] After pose optimization, distance between two frames x:0.023418 ,z=0.003116,angle between two frames:0.014871
[17:10:05.337] Update EKF and 3D points, distance between two frames x:0.020137 ,z=0.003156,angle between two frames:0.012976
[17:10:05.447] After Reprojection Map nPoint:30 ,nCell:24 ,nMatches:6 ,distance between two frames x:0.004310 ,z=0.009452,angle between two frames:0.009071
[17:10:05.447] After pose optimization, distance between two frames x:0.004310 ,z=0.009452,angle between two frames:0.009071
[17:10:05.447] Update EKF and 3D points, distance between two frames x:0.002526 ,z=0.008997,angle between two frames:0.006199
[17:10:05.538] After Reprojection Map nPoint:32 ,nCell:26 ,nMatches:6 ,distance between two frames x:-0.007600 ,z=0.018278,angle between two frames:0.000580
[17:10:05.538] After pose optimization, distance between two frames x:-0.007600 ,z=0.018278,angle between two frames:0.000580
[17:10:05.538] Update EKF and 3D points, distance between two frames x:-0.007548 ,z=0.016636,angle between two frames:0.011275
[17:10:05.624] After Reprojection Map nPoint:19 ,nCell:14 ,nMatches:6 ,distance between two frames x:0.007294 ,z=-0.011390,angle between two frames:0.040673
[17:10:05.624] After pose optimization, distance between two frames x:0.007294 ,z=-0.011390,angle between two frames:0.040673
[17:10:05.624] Update EKF and 3D points, distance between two frames x:0.006304 ,z=-0.010123,angle between two frames:0.031072
[17:10:05.710] After Reprojection Map nPoint:18 ,nCell:13 ,nMatches:6 ,distance between two frames x:0.026219 ,z=0.002848,angle between two frames:0.010941
[17:10:05.710] After pose optimization, distance between two frames x:0.026219 ,z=0.002848,angle between two frames:0.010941
[17:10:05.710] Update EKF and 3D points, distance between two frames x:0.022778 ,z=0.003549,angle between two frames:0.005307
[17:10:05.816] After Reprojection Map nPoint:17 ,nCell:14 ,nMatches:2 ,distance between two frames x:0.011534 ,z=-0.004911,angle between two frames:0.028595
[17:10:05.816] After pose optimization, distance between two frames x:0.011534 ,z=-0.004911,angle between two frames:0.028595
[17:10:05.816] Update EKF and 3D points, distance between two frames x:0.008864 ,z=-0.003514,angle between two frames:0.011509
[17:10:05.916] After Reprojection Map nPoint:27 ,nCell:22 ,nMatches:6 ,distance between two frames x:0.007650 ,z=-0.002538,angle between two frames:0.017499
[17:10:05.916] After pose optimization, distance between two frames x:0.007650 ,z=-0.002538,angle between two frames:0.017499
[17:10:05.916] Update EKF and 3D points, distance between two frames x:0.005668 ,z=-0.001165,angle between two frames:0.001130
[17:10:06.021] After Reprojection Map nPoint:21 ,nCell:18 ,nMatches:8 ,distance between two frames x:0.017141 ,z=-0.002302,angle between two frames:0.016672
[17:10:06.021] After pose optimization, distance between two frames x:0.017141 ,z=-0.002302,angle between two frames:0.016672
[17:10:06.021] Update EKF and 3D points, distance between two frames x:0.014358 ,z=-0.000969,angle between two frames:0.003278
[17:10:06.138] After Reprojection Map nPoint:26 ,nCell:21 ,nMatches:7 ,distance between two frames x:0.003893 ,z=0.001695,angle between two frames:0.003991
[17:10:06.138] After pose optimization, distance between two frames x:0.003893 ,z=0.001695,angle between two frames:0.003991
[17:10:06.138] Update EKF and 3D points, distance between two frames x:0.002296 ,z=0.002562,angle between two frames:0.008523
[17:10:06.246] After Reprojection Map nPoint:7 ,nCell:6 ,nMatches:3 ,distance between two frames x:-0.002713 ,z=0.002641,angle between two frames:0.005340
[17:10:06.336] After Reprojection Map nPoint:3 ,nCell:3 ,nMatches:2 ,distance between two frames x:0.011690 ,z=-0.004094,angle between two frames:0.023451
[17:10:06.419] After Reprojection Map nPoint:2 ,nCell:2 ,nMatches:1 ,distance between two frames x:-0.010556 ,z=0.000116,angle between two frames:0.005439
[17:10:06.524] After Reprojection Map nPoint:1 ,nCell:1 ,nMatches:0 ,distance between two frames x:-0.020999 ,z=0.018250,angle between two frames:0.064472
[17:10:06.609] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000001,angle between two frames:0.000401
[17:10:06.697] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000653
[17:10:06.808] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.001806
[17:10:06.895] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000000,angle between two frames:0.001137
[17:10:06.998] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000000,angle between two frames:0.001051
[17:10:07.096] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.001686
[17:10:07.191] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.002238 ,z=0.005403,angle between two frames:0.000676
[17:10:07.305] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.002001 ,z=0.004770,angle between two frames:0.001893
[17:10:07.392] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000690
[17:10:07.495] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000001,angle between two frames:0.005996
[17:10:07.599] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.002735
[17:10:07.703] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.000504
[17:10:07.794] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000942
[17:10:07.900] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000003,angle between two frames:0.000774
[17:10:08.003] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=0.000000,angle between two frames:0.001873
[17:10:08.093] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000429
[17:10:08.206] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000002,angle between two frames:0.005235
[17:10:08.300] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000047
[17:10:08.402] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000003,angle between two frames:0.000467
[17:10:08.493] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.001112
[17:10:08.592] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.000366
[17:10:08.699] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.002168 ,z=0.005059,angle between two frames:0.003260
[17:10:08.800] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.007137
[17:10:08.893] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000005 ,z=-0.000014,angle between two frames:0.005742
[17:10:09.001] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.002334 ,z=0.005065,angle between two frames:0.010100
[17:10:09.104] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.002553
[17:10:09.194] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000000 ,z=-0.000000,angle between two frames:0.000484
[17:10:09.295] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.007320
[17:10:09.399] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000000,angle between two frames:0.003771
[17:10:09.500] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000002 ,z=-0.000001,angle between two frames:0.000004
[17:10:09.598] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.000001 ,z=-0.000001,angle between two frames:0.000538
[17:10:09.701] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.001181
[17:10:09.792] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.001718
[17:10:09.905] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:-0.002549 ,z=0.004964,angle between two frames:0.000550
[17:10:09.993] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.001029
[17:10:10.098] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000002 ,z=-0.000005,angle between two frames:0.000912
[17:10:10.199] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.002796
[17:10:10.303] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000001,angle between two frames:0.001078
[17:10:10.397] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000001 ,z=-0.000002,angle between two frames:0.000335
[17:10:10.506] After Reprojection Map nPoint:0 ,nCell:0 ,nMatches:0 ,distance between two frames x:0.000000 ,z=-0.000000,angle between two frames:0.004025
[17:10:10.622] After Reprojection Map nPoint:24 ,nCell:20 ,nMatches:8 ,distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.001126
[17:10:10.622] After pose optimization, distance between two frames x:0.000000 ,z=0.000000,angle between two frames:0.001126
[17:10:10.622] Update EKF and 3D points, distance between two frames x:-0.000266 ,z=0.000534,angle between two frames:0.000631
[17:10:10.706] After Reprojection Map nPoint:29 ,nCell:22 ,nMatches:6 ,distance between two frames x:0.000261 ,z=0.003612,angle between two frames:0.000151
[17:10:10.706] After pose optimization, distance between two frames x:0.000261 ,z=0.003612,angle between two frames:0.000151
[17:10:10.706] Update EKF and 3D points, distance between two frames x:-0.000137 ,z=0.003933,angle between two frames:0.004893
[17:10:10.808] After Reprojection Map nPoint:25 ,nCell:22 ,nMatches:9 ,distance between two frames x:0.013038 ,z=0.004465,angle between two frames:0.009983
[17:10:10.808] After pose optimization, distance between two frames x:0.013038 ,z=0.004465,angle between two frames:0.009983
[17:10:10.808] Update EKF and 3D points, distance between two frames x:0.011183 ,z=0.004612,angle between two frames:0.018640
[17:10:10.933] After Reprojection Map nPoint:27 ,nCell:22 ,nMatches:9 ,distance between two frames x:0.017358 ,z=-0.012946,angle between two frames:0.031611
[17:10:10.933] After pose optimization, distance between two frames x:0.017358 ,z=-0.012946,angle between two frames:0.031611
[17:10:10.933] Update EKF and 3D points, distance between two frames x:0.014526 ,z=-0.010942,angle between two frames:0.025445
[17:10:11.016] After Reprojection Map nPoint:30 ,nCell:23 ,nMatches:8 ,distance between two frames x:-0.004723 ,z=0.007114,angle between two frames:0.002977
[17:10:11.016] After pose optimization, distance between two frames x:-0.004723 ,z=0.007114,angle between two frames:0.002977
[17:10:11.016] Update EKF and 3D points, distance between two frames x:-0.005466 ,z=0.007537,angle between two frames:0.000074
[17:10:11.104] After Reprojection Map nPoint:26 ,nCell:26 ,nMatches:8 ,distance between two frames x:0.001195 ,z=0.000081,angle between two frames:0.004297
[17:10:11.104] After pose optimization, distance between two frames x:0.001195 ,z=0.000081,angle between two frames:0.004297
[17:10:11.104] Update EKF and 3D points, distance between two frames x:0.000512 ,z=0.000743,angle between two frames:0.002383
[17:10:11.215] After Reprojection Map nPoint:28 ,nCell:24 ,nMatches:11 ,distance between two frames x:0.003729 ,z=0.003002,angle between two frames:0.004866
[17:10:11.215] After pose optimization, distance between two frames x:0.003729 ,z=0.003002,angle between two frames:0.004866
[17:10:11.215] Update EKF and 3D points, distance between two frames x:0.002786 ,z=0.003420,angle between two frames:0.004285
[17:10:11.318] After Reprojection Map nPoint:30 ,nCell:23 ,nMatches:11 ,distance between two frames x:0.003067 ,z=-0.000956,angle between two frames:0.009484
[17:10:11.318] After pose optimization, distance between two frames x:0.003067 ,z=-0.000956,angle between two frames:0.009484
[17:10:11.318] Update EKF and 3D points, distance between two frames x:0.002108 ,z=-0.000185,angle between two frames:0.006041
[17:10:11.397] After Reprojection Map nPoint:34 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.004026 ,z=0.004345,angle between two frames:0.008403
[17:10:11.397] After pose optimization, distance between two frames x:-0.004026 ,z=0.004345,angle between two frames:0.008403
[17:10:11.397] Update EKF and 3D points, distance between two frames x:-0.004216 ,z=0.004656,angle between two frames:0.005937
[17:10:11.507] After Reprojection Map nPoint:36 ,nCell:24 ,nMatches:12 ,distance between two frames x:0.000134 ,z=0.005834,angle between two frames:0.004127
[17:10:11.508] After pose optimization, distance between two frames x:0.000134 ,z=0.005834,angle between two frames:0.004127
[17:10:11.508] Update EKF and 3D points, distance between two frames x:-0.000244 ,z=0.005822,angle between two frames:0.004226
[17:10:11.602] After Reprojection Map nPoint:33 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.002906 ,z=0.003655,angle between two frames:0.007766
[17:10:11.602] After pose optimization, distance between two frames x:-0.002906 ,z=0.003655,angle between two frames:0.007766
[17:10:11.602] Update EKF and 3D points, distance between two frames x:-0.003015 ,z=0.003774,angle between two frames:0.004232
[17:10:11.701] After Reprojection Map nPoint:33 ,nCell:26 ,nMatches:13 ,distance between two frames x:0.000736 ,z=0.001785,angle between two frames:0.001095
[17:10:11.701] After pose optimization, distance between two frames x:0.000736 ,z=0.001785,angle between two frames:0.001095
[17:10:11.701] Update EKF and 3D points, distance between two frames x:0.000320 ,z=0.002156,angle between two frames:0.000958
[17:10:11.799] After Reprojection Map nPoint:35 ,nCell:31 ,nMatches:17 ,distance between two frames x:-0.005291 ,z=0.006783,angle between two frames:0.011606
[17:10:11.799] After pose optimization, distance between two frames x:-0.005291 ,z=0.006783,angle between two frames:0.011606
[17:10:11.799] Update EKF and 3D points, distance between two frames x:-0.005145 ,z=0.006688,angle between two frames:0.007718
[17:10:11.899] After Reprojection Map nPoint:34 ,nCell:26 ,nMatches:11 ,distance between two frames x:-0.008922 ,z=0.009292,angle between two frames:0.016846
[17:10:11.899] After pose optimization, distance between two frames x:-0.008922 ,z=0.009292,angle between two frames:0.016846
[17:10:11.899] Update EKF and 3D points, distance between two frames x:-0.008175 ,z=0.008766,angle between two frames:0.012526
[17:10:12.009] After Reprojection Map nPoint:33 ,nCell:22 ,nMatches:9 ,distance between two frames x:0.004527 ,z=0.006382,angle between two frames:0.016541
[17:10:12.009] After pose optimization, distance between two frames x:0.004527 ,z=0.006382,angle between two frames:0.016541
[17:10:12.009] Update EKF and 3D points, distance between two frames x:0.003995 ,z=0.006035,angle between two frames:0.010667
[17:10:12.110] After Reprojection Map nPoint:32 ,nCell:28 ,nMatches:9 ,distance between two frames x:-0.006171 ,z=0.003370,angle between two frames:0.001978
[17:10:12.110] After pose optimization, distance between two frames x:-0.006171 ,z=0.003370,angle between two frames:0.001978
[17:10:12.110] Update EKF and 3D points, distance between two frames x:-0.005957 ,z=0.003422,angle between two frames:0.001140
[17:10:12.200] After Reprojection Map nPoint:37 ,nCell:31 ,nMatches:12 ,distance between two frames x:-0.014291 ,z=0.027234,angle between two frames:0.035344
[17:10:12.200] After pose optimization, distance between two frames x:-0.014291 ,z=0.027234,angle between two frames:0.035344
[17:10:12.200] Update EKF and 3D points, distance between two frames x:-0.012907 ,z=0.024756,angle between two frames:0.021508
[17:10:12.306] After Reprojection Map nPoint:41 ,nCell:33 ,nMatches:11 ,distance between two frames x:-0.006895 ,z=0.004813,angle between two frames:0.014050
[17:10:12.306] After pose optimization, distance between two frames x:-0.006895 ,z=0.004813,angle between two frames:0.014050
[17:10:12.306] Update EKF and 3D points, distance between two frames x:-0.005918 ,z=0.003942,angle between two frames:0.006273
[17:10:12.429] After Reprojection Map nPoint:38 ,nCell:26 ,nMatches:11 ,distance between two frames x:0.008007 ,z=-0.004703,angle between two frames:0.006641
[17:10:12.429] After pose optimization, distance between two frames x:0.008007 ,z=-0.004703,angle between two frames:0.006641
[17:10:12.429] Update EKF and 3D points, distance between two frames x:0.007168 ,z=-0.003906,angle between two frames:0.011053
[17:10:12.536] After Reprojection Map nPoint:37 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.000103 ,z=0.011799,angle between two frames:0.003251
[17:10:12.536] After pose optimization, distance between two frames x:-0.000103 ,z=0.011799,angle between two frames:0.003251
[17:10:12.536] Update EKF and 3D points, distance between two frames x:-0.000657 ,z=0.011325,angle between two frames:0.000059
[17:10:12.630] After Reprojection Map nPoint:39 ,nCell:31 ,nMatches:14 ,distance between two frames x:0.001092 ,z=0.000088,angle between two frames:0.000683
[17:10:12.630] After pose optimization, distance between two frames x:0.001092 ,z=0.000088,angle between two frames:0.000683
[17:10:12.630] Update EKF and 3D points, distance between two frames x:0.000519 ,z=0.000435,angle between two frames:0.003714
[17:10:12.718] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:12 ,distance between two frames x:0.003776 ,z=-0.003471,angle between two frames:0.013430
[17:10:12.718] After pose optimization, distance between two frames x:0.003776 ,z=-0.003471,angle between two frames:0.013430
[17:10:12.718] Update EKF and 3D points, distance between two frames x:0.002898 ,z=-0.002446,angle between two frames:0.009640
[17:10:12.798] After Reprojection Map nPoint:39 ,nCell:31 ,nMatches:13 ,distance between two frames x:-0.000949 ,z=0.004070,angle between two frames:0.005759
[17:10:12.798] After pose optimization, distance between two frames x:-0.000949 ,z=0.004070,angle between two frames:0.005759
[17:10:12.798] Update EKF and 3D points, distance between two frames x:-0.001439 ,z=0.004533,angle between two frames:0.003063
[17:10:12.907] After Reprojection Map nPoint:37 ,nCell:27 ,nMatches:13 ,distance between two frames x:-0.023438 ,z=0.021903,angle between two frames:0.025710
[17:10:12.907] After pose optimization, distance between two frames x:-0.023438 ,z=0.021903,angle between two frames:0.025710
[17:10:12.907] Update EKF and 3D points, distance between two frames x:-0.021293 ,z=0.020170,angle between two frames:0.014679
[17:10:13.007] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:12 ,distance between two frames x:-0.002504 ,z=0.005468,angle between two frames:0.012105
[17:10:13.007] After pose optimization, distance between two frames x:-0.002504 ,z=0.005468,angle between two frames:0.012105
[17:10:13.007] Update EKF and 3D points, distance between two frames x:-0.001777 ,z=0.004821,angle between two frames:0.002413
[17:10:13.111] After Reprojection Map nPoint:33 ,nCell:29 ,nMatches:13 ,distance between two frames x:0.004431 ,z=0.016192,angle between two frames:0.008578
[17:10:13.111] After pose optimization, distance between two frames x:0.004431 ,z=0.016192,angle between two frames:0.008578
[17:10:13.111] Update EKF and 3D points, distance between two frames x:0.003917 ,z=0.014734,angle between two frames:0.002879
[17:10:13.203] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:12 ,distance between two frames x:0.009536 ,z=-0.013629,angle between two frames:0.019879
[17:10:13.203] After pose optimization, distance between two frames x:0.009536 ,z=-0.013629,angle between two frames:0.019879
[17:10:13.203] Update EKF and 3D points, distance between two frames x:0.008038 ,z=-0.012078,angle between two frames:0.014604
[17:10:13.323] After Reprojection Map nPoint:37 ,nCell:32 ,nMatches:12 ,distance between two frames x:-0.006090 ,z=0.005325,angle between two frames:0.003762
[17:10:13.323] After pose optimization, distance between two frames x:-0.006090 ,z=0.005325,angle between two frames:0.003762
[17:10:13.323] Update EKF and 3D points, distance between two frames x:-0.006199 ,z=0.005806,angle between two frames:0.005627
[17:10:13.404] After Reprojection Map nPoint:36 ,nCell:29 ,nMatches:10 ,distance between two frames x:0.017574 ,z=0.028885,angle between two frames:0.011262
[17:10:13.404] After pose optimization, distance between two frames x:0.017574 ,z=0.028885,angle between two frames:0.011262
[17:10:13.404] Update EKF and 3D points, distance between two frames x:0.015328 ,z=0.026406,angle between two frames:0.006921
[17:10:13.527] After Reprojection Map nPoint:38 ,nCell:33 ,nMatches:15 ,distance between two frames x:-0.012567 ,z=0.003430,angle between two frames:0.008776
[17:10:13.527] After pose optimization, distance between two frames x:-0.012567 ,z=0.003430,angle between two frames:0.008776
[17:10:13.527] Update EKF and 3D points, distance between two frames x:-0.012216 ,z=0.002752,angle between two frames:0.004860
[17:10:13.636] After Reprojection Map nPoint:43 ,nCell:33 ,nMatches:15 ,distance between two frames x:0.012585 ,z=0.002864,angle between two frames:0.008298
[17:10:13.637] After pose optimization, distance between two frames x:0.012585 ,z=0.002864,angle between two frames:0.008298
[17:10:13.637] Update EKF and 3D points, distance between two frames x:0.011055 ,z=0.002860,angle between two frames:0.004807
[17:10:13.721] After Reprojection Map nPoint:41 ,nCell:33 ,nMatches:13 ,distance between two frames x:-0.006580 ,z=0.001915,angle between two frames:0.005605
[17:10:13.721] After pose optimization, distance between two frames x:-0.006580 ,z=0.001915,angle between two frames:0.005605
[17:10:13.721] Update EKF and 3D points, distance between two frames x:-0.006641 ,z=0.002250,angle between two frames:0.008454
[17:10:13.801] After Reprojection Map nPoint:37 ,nCell:29 ,nMatches:14 ,distance between two frames x:-0.006161 ,z=0.010338,angle between two frames:0.019537
[17:10:13.801] After pose optimization, distance between two frames x:-0.006161 ,z=0.010338,angle between two frames:0.019537
[17:10:13.801] Update EKF and 3D points, distance between two frames x:-0.005757 ,z=0.009856,angle between two frames:0.015581
[17:10:13.913] After Reprojection Map nPoint:34 ,nCell:25 ,nMatches:11 ,distance between two frames x:0.000386 ,z=-0.003042,angle between two frames:0.008941
[17:10:13.913] After pose optimization, distance between two frames x:0.000386 ,z=-0.003042,angle between two frames:0.008941
[17:10:13.913] Update EKF and 3D points, distance between two frames x:0.000217 ,z=-0.002317,angle between two frames:0.010130
[17:10:14.007] After Reprojection Map nPoint:39 ,nCell:32 ,nMatches:13 ,distance between two frames x:-0.001680 ,z=0.003076,angle between two frames:0.001930
[17:10:14.007] After pose optimization, distance between two frames x:-0.001680 ,z=0.003076,angle between two frames:0.001930
[17:10:14.007] Update EKF and 3D points, distance between two frames x:-0.001815 ,z=0.003523,angle between two frames:0.002688
[17:10:14.101] After Reprojection Map nPoint:41 ,nCell:35 ,nMatches:13 ,distance between two frames x:-0.001308 ,z=0.001655,angle between two frames:0.002085
[17:10:14.101] After pose optimization, distance between two frames x:-0.001308 ,z=0.001655,angle between two frames:0.002085
[17:10:14.101] Update EKF and 3D points, distance between two frames x:-0.001477 ,z=0.002169,angle between two frames:0.005445
[17:10:14.204] After Reprojection Map nPoint:40 ,nCell:31 ,nMatches:10 ,distance between two frames x:-0.003078 ,z=0.005439,angle between two frames:0.005854
[17:10:14.204] After pose optimization, distance between two frames x:-0.003078 ,z=0.005439,angle between two frames:0.005854
[17:10:14.204] Update EKF and 3D points, distance between two frames x:-0.003054 ,z=0.005531,angle between two frames:0.002386
[17:10:14.309] After Reprojection Map nPoint:34 ,nCell:31 ,nMatches:15 ,distance between two frames x:-0.001556 ,z=0.001450,angle between two frames:0.008606
[17:10:14.309] After pose optimization, distance between two frames x:-0.001556 ,z=0.001450,angle between two frames:0.008606
[17:10:14.309] Update EKF and 3D points, distance between two frames x:-0.001657 ,z=0.001857,angle between two frames:0.016677
[17:10:14.403] After Reprojection Map nPoint:36 ,nCell:31 ,nMatches:12 ,distance between two frames x:-0.002665 ,z=0.003312,angle between two frames:0.002902
[17:10:14.403] After pose optimization, distance between two frames x:-0.002665 ,z=0.003312,angle between two frames:0.002902
[17:10:14.403] Update EKF and 3D points, distance between two frames x:-0.002686 ,z=0.003620,angle between two frames:0.007017
[17:10:14.499] After Reprojection Map nPoint:35 ,nCell:30 ,nMatches:11 ,distance between two frames x:0.002173 ,z=0.020319,angle between two frames:0.007681
[17:10:14.499] After pose optimization, distance between two frames x:0.002173 ,z=0.020319,angle between two frames:0.007681
[17:10:14.499] Update EKF and 3D points, distance between two frames x:0.001634 ,z=0.018702,angle between two frames:0.000907
[17:10:14.603] After Reprojection Map nPoint:33 ,nCell:32 ,nMatches:12 ,distance between two frames x:0.021088 ,z=0.009215,angle between two frames:0.011503
[17:10:14.603] After pose optimization, distance between two frames x:0.021088 ,z=0.009215,angle between two frames:0.011503
[17:10:14.604] Update EKF and 3D points, distance between two frames x:0.018297 ,z=0.008189,angle between two frames:0.010234
[17:10:14.698] After Reprojection Map nPoint:35 ,nCell:32 ,nMatches:14 ,distance between two frames x:-0.018707 ,z=0.031542,angle between two frames:0.041269
[17:10:14.698] After pose optimization, distance between two frames x:-0.018707 ,z=0.031542,angle between two frames:0.041269
[17:10:14.698] Update EKF and 3D points, distance between two frames x:-0.017870 ,z=0.028209,angle between two frames:0.024310
[17:10:14.805] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:17 ,distance between two frames x:-0.017253 ,z=0.017764,angle between two frames:0.028365
[17:10:14.805] After pose optimization, distance between two frames x:-0.017253 ,z=0.017764,angle between two frames:0.028365
[17:10:14.805] Update EKF and 3D points, distance between two frames x:-0.015342 ,z=0.015214,angle between two frames:0.012364
[17:10:14.907] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:17 ,distance between two frames x:0.004442 ,z=-0.002355,angle between two frames:0.008704
[17:10:14.907] After pose optimization, distance between two frames x:0.004442 ,z=-0.002355,angle between two frames:0.008704
[17:10:14.907] Update EKF and 3D points, distance between two frames x:0.004318 ,z=-0.002407,angle between two frames:0.013218
[17:10:15.016] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:14 ,distance between two frames x:0.006286 ,z=-0.006044,angle between two frames:0.009933
[17:10:15.016] After pose optimization, distance between two frames x:0.006286 ,z=-0.006044,angle between two frames:0.009933
[17:10:15.016] Update EKF and 3D points, distance between two frames x:0.005278 ,z=-0.004850,angle between two frames:0.006604
[17:10:15.104] After Reprojection Map nPoint:36 ,nCell:25 ,nMatches:12 ,distance between two frames x:0.020827 ,z=-0.002728,angle between two frames:0.021751
[17:10:15.104] After pose optimization, distance between two frames x:0.020827 ,z=-0.002728,angle between two frames:0.021751
[17:10:15.104] Update EKF and 3D points, distance between two frames x:0.017885 ,z=-0.001477,angle between two frames:0.012097
[17:10:15.203] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:13 ,distance between two frames x:-0.014179 ,z=0.013114,angle between two frames:0.023882
[17:10:15.203] After pose optimization, distance between two frames x:-0.014179 ,z=0.013114,angle between two frames:0.023882
[17:10:15.203] Update EKF and 3D points, distance between two frames x:-0.013872 ,z=0.012600,angle between two frames:0.019183
[17:10:15.317] After Reprojection Map nPoint:34 ,nCell:26 ,nMatches:14 ,distance between two frames x:-0.005966 ,z=0.020962,angle between two frames:0.017762
[17:10:15.317] After pose optimization, distance between two frames x:-0.005966 ,z=0.020962,angle between two frames:0.017762
[17:10:15.317] Update EKF and 3D points, distance between two frames x:-0.005467 ,z=0.019007,angle between two frames:0.012329
[17:10:15.407] After Reprojection Map nPoint:34 ,nCell:27 ,nMatches:12 ,distance between two frames x:0.002735 ,z=0.006088,angle between two frames:0.002893
[17:10:15.408] After pose optimization, distance between two frames x:0.002735 ,z=0.006088,angle between two frames:0.002893
[17:10:15.408] Update EKF and 3D points, distance between two frames x:0.002335 ,z=0.005306,angle between two frames:0.003124
[17:10:15.502] After Reprojection Map nPoint:32 ,nCell:27 ,nMatches:11 ,distance between two frames x:-0.015823 ,z=0.015409,angle between two frames:0.025855
[17:10:15.502] After pose optimization, distance between two frames x:-0.015823 ,z=0.015409,angle between two frames:0.025855
[17:10:15.502] Update EKF and 3D points, distance between two frames x:-0.014470 ,z=0.013999,angle between two frames:0.017975
[17:10:15.613] After Reprojection Map nPoint:36 ,nCell:30 ,nMatches:13 ,distance between two frames x:-0.006363 ,z=0.016232,angle between two frames:0.021176
[17:10:15.614] After pose optimization, distance between two frames x:-0.006363 ,z=0.016232,angle between two frames:0.021176
[17:10:15.614] Update EKF and 3D points, distance between two frames x:-0.005462 ,z=0.014533,angle between two frames:0.014986
[17:10:15.705] After Reprojection Map nPoint:36 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.021981 ,z=0.024056,angle between two frames:0.038554
[17:10:15.705] After pose optimization, distance between two frames x:-0.021981 ,z=0.024056,angle between two frames:0.038554
[17:10:15.705] Update EKF and 3D points, distance between two frames x:-0.019473 ,z=0.021363,angle between two frames:0.023119
[17:10:15.798] After Reprojection Map nPoint:35 ,nCell:26 ,nMatches:11 ,distance between two frames x:-0.000231 ,z=0.002324,angle between two frames:0.008077
[17:10:15.798] After pose optimization, distance between two frames x:-0.000231 ,z=0.002324,angle between two frames:0.008077
[17:10:15.798] Update EKF and 3D points, distance between two frames x:0.000430 ,z=0.001674,angle between two frames:0.001740
[17:10:15.915] After Reprojection Map nPoint:35 ,nCell:28 ,nMatches:12 ,distance between two frames x:-0.002708 ,z=0.005687,angle between two frames:0.005727
[17:10:15.915] After pose optimization, distance between two frames x:-0.002708 ,z=0.005687,angle between two frames:0.005727
[17:10:15.915] Update EKF and 3D points, distance between two frames x:-0.002423 ,z=0.005405,angle between two frames:0.001131
[17:10:16.017] After Reprojection Map nPoint:35 ,nCell:26 ,nMatches:12 ,distance between two frames x:-0.012649 ,z=0.016481,angle between two frames:0.023688
[17:10:16.017] After pose optimization, distance between two frames x:-0.012649 ,z=0.016481,angle between two frames:0.023688
[17:10:16.017] Update EKF and 3D points, distance between two frames x:-0.011394 ,z=0.015119,angle between two frames:0.008032
[17:10:16.103] After Reprojection Map nPoint:38 ,nCell:26 ,nMatches:15 ,distance between two frames x:-0.000577 ,z=0.002135,angle between two frames:0.000690
[17:10:16.103] After pose optimization, distance between two frames x:-0.000577 ,z=0.002135,angle between two frames:0.000690
[17:10:16.103] Update EKF and 3D points, distance between two frames x:-0.000325 ,z=0.002013,angle between two frames:0.006822
[17:10:16.200] After Reprojection Map nPoint:38 ,nCell:29 ,nMatches:13 ,distance between two frames x:0.001084 ,z=0.000593,angle between two frames:0.004862
[17:10:16.200] After pose optimization, distance between two frames x:0.001084 ,z=0.000593,angle between two frames:0.004862
[17:10:16.200] Update EKF and 3D points, distance between two frames x:0.000785 ,z=0.001098,angle between two frames:0.007630
[17:10:16.301] After Reprojection Map nPoint:33 ,nCell:25 ,nMatches:12 ,distance between two frames x:0.005959 ,z=-0.005309,angle between two frames:0.017077
[17:10:16.301] After pose optimization, distance between two frames x:0.005959 ,z=-0.005309,angle between two frames:0.017077
[17:10:16.301] Update EKF and 3D points, distance between two frames x:0.004951 ,z=-0.003990,angle between two frames:0.016601
[17:10:16.402] After Reprojection Map nPoint:36 ,nCell:30 ,nMatches:12 ,distance between two frames x:-0.018054 ,z=0.022286,angle between two frames:0.021671
[17:10:16.402] After pose optimization, distance between two frames x:-0.018054 ,z=0.022286,angle between two frames:0.021671
[17:10:16.402] Update EKF and 3D points, distance between two frames x:-0.016635 ,z=0.020811,angle between two frames:0.009700
[17:10:16.498] After Reprojection Map nPoint:32 ,nCell:27 ,nMatches:12 ,distance between two frames x:-0.015799 ,z=0.028877,angle between two frames:0.035165
[17:10:16.498] After pose optimization, distance between two frames x:-0.015799 ,z=0.028877,angle between two frames:0.035165
[17:10:16.498] Update EKF and 3D points, distance between two frames x:-0.013783 ,z=0.025682,angle between two frames:0.018112
[17:10:16.602] After Reprojection Map nPoint:37 ,nCell:34 ,nMatches:16 ,distance between two frames x:-0.013066 ,z=0.020722,angle between two frames:0.032262
[17:10:16.602] After pose optimization, distance between two frames x:-0.013066 ,z=0.020722,angle between two frames:0.032262
[17:10:16.602] Update EKF and 3D points, distance between two frames x:-0.011155 ,z=0.017874,angle between two frames:0.012761
[17:10:16.702] After Reprojection Map nPoint:40 ,nCell:32 ,nMatches:12 ,distance between two frames x:0.007025 ,z=-0.007085,angle between two frames:0.010798
[17:10:16.702] After pose optimization, distance between two frames x:0.007025 ,z=-0.007085,angle between two frames:0.010798
[17:10:16.702] Update EKF and 3D points, distance between two frames x:0.006666 ,z=-0.006700,angle between two frames:0.011110
[17:10:16.804] After Reprojection Map nPoint:38 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.003251 ,z=0.017556,angle between two frames:0.013610
[17:10:16.804] After pose optimization, distance between two frames x:-0.003251 ,z=0.017556,angle between two frames:0.013610
[17:10:16.804] Update EKF and 3D points, distance between two frames x:-0.003239 ,z=0.016316,angle between two frames:0.010483
[17:10:16.900] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:10 ,distance between two frames x:-0.007056 ,z=0.002158,angle between two frames:0.010936
[17:10:16.900] After pose optimization, distance between two frames x:-0.007056 ,z=0.002158,angle between two frames:0.010936
[17:10:16.900] Update EKF and 3D points, distance between two frames x:-0.006436 ,z=0.002054,angle between two frames:0.008075
[17:10:17.001] After Reprojection Map nPoint:38 ,nCell:28 ,nMatches:12 ,distance between two frames x:0.017731 ,z=0.011241,angle between two frames:0.000469
[17:10:17.002] After pose optimization, distance between two frames x:0.017731 ,z=0.011241,angle between two frames:0.000469
[17:10:17.002] Update EKF and 3D points, distance between two frames x:0.015736 ,z=0.010532,angle between two frames:0.000037
[17:10:17.104] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.006607 ,z=-0.004304,angle between two frames:0.003753
[17:10:17.105] After pose optimization, distance between two frames x:-0.006607 ,z=-0.004304,angle between two frames:0.003753
[17:10:17.105] Update EKF and 3D points, distance between two frames x:-0.006757 ,z=-0.003532,angle between two frames:0.000344
[17:10:17.204] After Reprojection Map nPoint:37 ,nCell:32 ,nMatches:12 ,distance between two frames x:-0.006596 ,z=0.006322,angle between two frames:0.007640
[17:10:17.204] After pose optimization, distance between two frames x:-0.006596 ,z=0.006322,angle between two frames:0.007640
[17:10:17.204] Update EKF and 3D points, distance between two frames x:-0.006132 ,z=0.006457,angle between two frames:0.008690
[17:10:17.302] After Reprojection Map nPoint:36 ,nCell:23 ,nMatches:9 ,distance between two frames x:0.000038 ,z=0.011071,angle between two frames:0.010458
[17:10:17.302] After pose optimization, distance between two frames x:0.000038 ,z=0.011071,angle between two frames:0.010458
[17:10:17.302] Update EKF and 3D points, distance between two frames x:-0.000027 ,z=0.010488,angle between two frames:0.009832
[17:10:17.410] After Reprojection Map nPoint:39 ,nCell:23 ,nMatches:12 ,distance between two frames x:-0.004677 ,z=0.005744,angle between two frames:0.012605
[17:10:17.410] After pose optimization, distance between two frames x:-0.004677 ,z=0.005744,angle between two frames:0.012605
[17:10:17.410] Update EKF and 3D points, distance between two frames x:-0.004386 ,z=0.005484,angle between two frames:0.009468
[17:10:17.503] After Reprojection Map nPoint:38 ,nCell:26 ,nMatches:12 ,distance between two frames x:-0.007591 ,z=0.008730,angle between two frames:0.010154
[17:10:17.503] After pose optimization, distance between two frames x:-0.007591 ,z=0.008730,angle between two frames:0.010154
[17:10:17.503] Update EKF and 3D points, distance between two frames x:-0.006854 ,z=0.008250,angle between two frames:0.002247
[17:10:17.610] After Reprojection Map nPoint:38 ,nCell:26 ,nMatches:11 ,distance between two frames x:-0.003503 ,z=0.007347,angle between two frames:0.005357
[17:10:17.610] After pose optimization, distance between two frames x:-0.003503 ,z=0.007347,angle between two frames:0.005357
[17:10:17.610] Update EKF and 3D points, distance between two frames x:-0.003054 ,z=0.006889,angle between two frames:0.001364
[17:10:17.706] After Reprojection Map nPoint:39 ,nCell:26 ,nMatches:13 ,distance between two frames x:-0.002527 ,z=0.003715,angle between two frames:0.002492
[17:10:17.706] After pose optimization, distance between two frames x:-0.002527 ,z=0.003715,angle between two frames:0.002492
[17:10:17.706] Update EKF and 3D points, distance between two frames x:-0.002312 ,z=0.003747,angle between two frames:0.004374
[17:10:17.814] After Reprojection Map nPoint:37 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.005163 ,z=0.007693,angle between two frames:0.002675
[17:10:17.814] After pose optimization, distance between two frames x:-0.005163 ,z=0.007693,angle between two frames:0.002675
[17:10:17.814] Update EKF and 3D points, distance between two frames x:-0.004733 ,z=0.007424,angle between two frames:0.005364
[17:10:17.902] After Reprojection Map nPoint:35 ,nCell:32 ,nMatches:13 ,distance between two frames x:-0.013926 ,z=0.023890,angle between two frames:0.024522
[17:10:17.902] After pose optimization, distance between two frames x:-0.013926 ,z=0.023890,angle between two frames:0.024522
[17:10:17.902] Update EKF and 3D points, distance between two frames x:-0.012454 ,z=0.021726,angle between two frames:0.009613
[17:10:18.001] After Reprojection Map nPoint:34 ,nCell:27 ,nMatches:14 ,distance between two frames x:0.016098 ,z=-0.014238,angle between two frames:0.031137
[17:10:18.001] After pose optimization, distance between two frames x:0.016098 ,z=-0.014238,angle between two frames:0.031137
[17:10:18.001] Update EKF and 3D points, distance between two frames x:0.014623 ,z=-0.012867,angle between two frames:0.028077
[17:10:18.120] After Reprojection Map nPoint:35 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.023576 ,z=0.037941,angle between two frames:0.039043
[17:10:18.120] After pose optimization, distance between two frames x:-0.023576 ,z=0.037941,angle between two frames:0.039043
[17:10:18.120] Update EKF and 3D points, distance between two frames x:-0.021706 ,z=0.034815,angle between two frames:0.024970
[17:10:18.201] After Reprojection Map nPoint:39 ,nCell:27 ,nMatches:12 ,distance between two frames x:0.000291 ,z=-0.000159,angle between two frames:0.003476
[17:10:18.201] After pose optimization, distance between two frames x:0.000291 ,z=-0.000159,angle between two frames:0.003476
[17:10:18.201] Update EKF and 3D points, distance between two frames x:0.000684 ,z=-0.000617,angle between two frames:0.001939
[17:10:18.301] After Reprojection Map nPoint:40 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.018661 ,z=0.037650,angle between two frames:0.040575
[17:10:18.301] After pose optimization, distance between two frames x:-0.018661 ,z=0.037650,angle between two frames:0.040575
[17:10:18.301] Update EKF and 3D points, distance between two frames x:-0.016657 ,z=0.033879,angle between two frames:0.022466
[17:10:18.404] After Reprojection Map nPoint:42 ,nCell:32 ,nMatches:14 ,distance between two frames x:0.007018 ,z=0.015915,angle between two frames:0.008076
[17:10:18.404] After pose optimization, distance between two frames x:0.007018 ,z=0.015915,angle between two frames:0.008076
[17:10:18.404] Update EKF and 3D points, distance between two frames x:0.006732 ,z=0.013439,angle between two frames:0.000106
[17:10:18.507] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:15 ,distance between two frames x:0.000500 ,z=0.002804,angle between two frames:0.002393
[17:10:18.507] After pose optimization, distance between two frames x:0.000500 ,z=0.002804,angle between two frames:0.002393
[17:10:18.507] Update EKF and 3D points, distance between two frames x:0.000148 ,z=0.002235,angle between two frames:0.000789
[17:10:18.605] After Reprojection Map nPoint:38 ,nCell:27 ,nMatches:11 ,distance between two frames x:-0.029368 ,z=0.039846,angle between two frames:0.047998
[17:10:18.605] After pose optimization, distance between two frames x:-0.029368 ,z=0.039846,angle between two frames:0.047998
[17:10:18.605] Update EKF and 3D points, distance between two frames x:-0.026435 ,z=0.035828,angle between two frames:0.030650
[17:10:18.699] After Reprojection Map nPoint:35 ,nCell:27 ,nMatches:13 ,distance between two frames x:0.008954 ,z=-0.005621,angle between two frames:0.004800
[17:10:18.699] After pose optimization, distance between two frames x:0.008954 ,z=-0.005621,angle between two frames:0.004800
[17:10:18.699] Update EKF and 3D points, distance between two frames x:0.008790 ,z=-0.005796,angle between two frames:0.004814
[17:10:18.803] After Reprojection Map nPoint:33 ,nCell:27 ,nMatches:12 ,distance between two frames x:-0.001022 ,z=0.001044,angle between two frames:0.002070
[17:10:18.803] After pose optimization, distance between two frames x:-0.001022 ,z=0.001044,angle between two frames:0.002070
[17:10:18.803] Update EKF and 3D points, distance between two frames x:-0.001141 ,z=0.001471,angle between two frames:0.004269
[17:10:18.898] After Reprojection Map nPoint:35 ,nCell:31 ,nMatches:14 ,distance between two frames x:0.002849 ,z=0.016708,angle between two frames:0.010667
[17:10:18.899] After pose optimization, distance between two frames x:0.002849 ,z=0.016708,angle between two frames:0.010667
[17:10:18.899] Update EKF and 3D points, distance between two frames x:0.002323 ,z=0.015564,angle between two frames:0.008190
[17:10:18.999] After Reprojection Map nPoint:35 ,nCell:31 ,nMatches:13 ,distance between two frames x:-0.016494 ,z=0.039583,angle between two frames:0.043137
[17:10:18.999] After pose optimization, distance between two frames x:-0.016494 ,z=0.039583,angle between two frames:0.043137
[17:10:18.999] Update EKF and 3D points, distance between two frames x:-0.014993 ,z=0.035349,angle between two frames:0.030722
[17:10:19.112] After Reprojection Map nPoint:33 ,nCell:24 ,nMatches:10 ,distance between two frames x:-0.009296 ,z=0.011380,angle between two frames:0.021619
[17:10:19.112] After pose optimization, distance between two frames x:-0.009296 ,z=0.011380,angle between two frames:0.021619
[17:10:19.112] Update EKF and 3D points, distance between two frames x:-0.007921 ,z=0.009303,angle between two frames:0.012140
[17:10:19.198] After Reprojection Map nPoint:34 ,nCell:29 ,nMatches:15 ,distance between two frames x:-0.008422 ,z=0.037837,angle between two frames:0.034870
[17:10:19.198] After pose optimization, distance between two frames x:-0.008422 ,z=0.037837,angle between two frames:0.034870
[17:10:19.198] Update EKF and 3D points, distance between two frames x:-0.007156 ,z=0.033486,angle between two frames:0.017525
[17:10:19.306] After Reprojection Map nPoint:39 ,nCell:28 ,nMatches:12 ,distance between two frames x:0.000101 ,z=0.001920,angle between two frames:0.003296
[17:10:19.306] After pose optimization, distance between two frames x:0.000101 ,z=0.001920,angle between two frames:0.003296
[17:10:19.306] Update EKF and 3D points, distance between two frames x:0.000376 ,z=0.000832,angle between two frames:0.005541
[17:10:19.404] After Reprojection Map nPoint:32 ,nCell:19 ,nMatches:10 ,distance between two frames x:-0.016461 ,z=0.037529,angle between two frames:0.031490
[17:10:19.404] After pose optimization, distance between two frames x:-0.016461 ,z=0.037529,angle between two frames:0.031490
[17:10:19.404] Update EKF and 3D points, distance between two frames x:-0.014682 ,z=0.033582,angle between two frames:0.011884
[17:10:19.498] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:12 ,distance between two frames x:-0.011288 ,z=0.026447,angle between two frames:0.028675
[17:10:19.499] After pose optimization, distance between two frames x:-0.011288 ,z=0.026447,angle between two frames:0.028675
[17:10:19.499] Update EKF and 3D points, distance between two frames x:-0.008436 ,z=0.020842,angle between two frames:0.011104
[17:10:19.607] After Reprojection Map nPoint:33 ,nCell:29 ,nMatches:11 ,distance between two frames x:-0.001207 ,z=0.016176,angle between two frames:0.009894
[17:10:19.607] After pose optimization, distance between two frames x:-0.001207 ,z=0.016176,angle between two frames:0.009894
[17:10:19.607] Update EKF and 3D points, distance between two frames x:-0.000499 ,z=0.013553,angle between two frames:0.005899
[17:10:19.707] After Reprojection Map nPoint:36 ,nCell:29 ,nMatches:15 ,distance between two frames x:-0.051238 ,z=0.072551,angle between two frames:0.086987
[17:10:19.707] After pose optimization, distance between two frames x:-0.051238 ,z=0.072551,angle between two frames:0.086987
[17:10:19.707] Update EKF and 3D points, distance between two frames x:-0.045483 ,z=0.064245,angle between two frames:0.045739
[17:10:19.803] After Reprojection Map nPoint:33 ,nCell:26 ,nMatches:12 ,distance between two frames x:0.004006 ,z=0.014069,angle between two frames:0.006938
[17:10:19.803] After pose optimization, distance between two frames x:0.004006 ,z=0.014069,angle between two frames:0.006938
[17:10:19.803] Update EKF and 3D points, distance between two frames x:0.005405 ,z=0.010284,angle between two frames:0.009158
[17:10:19.899] After Reprojection Map nPoint:34 ,nCell:26 ,nMatches:11 ,distance between two frames x:0.011706 ,z=0.016791,angle between two frames:0.006662
[17:10:19.899] After pose optimization, distance between two frames x:0.011706 ,z=0.016791,angle between two frames:0.006662
[17:10:19.899] Update EKF and 3D points, distance between two frames x:0.010692 ,z=0.014003,angle between two frames:0.001219
[17:10:20.005] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:12 ,distance between two frames x:-0.000661 ,z=0.002957,angle between two frames:0.002306
[17:10:20.005] After pose optimization, distance between two frames x:-0.000661 ,z=0.002957,angle between two frames:0.002306
[17:10:20.005] Update EKF and 3D points, distance between two frames x:-0.001029 ,z=0.002310,angle between two frames:0.000864
[17:10:20.103] After Reprojection Map nPoint:38 ,nCell:33 ,nMatches:15 ,distance between two frames x:-0.014107 ,z=0.021237,angle between two frames:0.021872
[17:10:20.103] After pose optimization, distance between two frames x:-0.014107 ,z=0.021237,angle between two frames:0.021872
[17:10:20.103] Update EKF and 3D points, distance between two frames x:-0.012774 ,z=0.019237,angle between two frames:0.011806
[17:10:20.228] After Reprojection Map nPoint:32 ,nCell:26 ,nMatches:9 ,distance between two frames x:0.007706 ,z=0.000096,angle between two frames:0.007152
[17:10:20.228] After pose optimization, distance between two frames x:0.007706 ,z=0.000096,angle between two frames:0.007152
[17:10:20.228] Update EKF and 3D points, distance between two frames x:0.007200 ,z=0.000016,angle between two frames:0.013020
[17:10:20.330] After Reprojection Map nPoint:34 ,nCell:30 ,nMatches:15 ,distance between two frames x:-0.002454 ,z=0.012483,angle between two frames:0.008277
[17:10:20.330] After pose optimization, distance between two frames x:-0.002454 ,z=0.012483,angle between two frames:0.008277
[17:10:20.330] Update EKF and 3D points, distance between two frames x:-0.002464 ,z=0.011704,angle between two frames:0.005194
[17:10:20.408] After Reprojection Map nPoint:38 ,nCell:29 ,nMatches:11 ,distance between two frames x:0.007770 ,z=-0.018211,angle between two frames:0.020345
[17:10:20.408] After pose optimization, distance between two frames x:0.007770 ,z=-0.018211,angle between two frames:0.020345
[17:10:20.408] Update EKF and 3D points, distance between two frames x:0.006808 ,z=-0.015866,angle between two frames:0.013740
[17:10:20.503] After Reprojection Map nPoint:39 ,nCell:35 ,nMatches:15 ,distance between two frames x:-0.001559 ,z=0.018956,angle between two frames:0.010212
[17:10:20.503] After pose optimization, distance between two frames x:-0.001559 ,z=0.018956,angle between two frames:0.010212
[17:10:20.503] Update EKF and 3D points, distance between two frames x:-0.001810 ,z=0.018202,angle between two frames:0.008414
[17:10:20.606] After Reprojection Map nPoint:40 ,nCell:35 ,nMatches:13 ,distance between two frames x:0.009366 ,z=-0.011995,angle between two frames:0.013587
[17:10:20.606] After pose optimization, distance between two frames x:0.009366 ,z=-0.011995,angle between two frames:0.013587
[17:10:20.606] Update EKF and 3D points, distance between two frames x:0.008125 ,z=-0.010268,angle between two frames:0.006035
[17:10:20.705] After Reprojection Map nPoint:39 ,nCell:34 ,nMatches:15 ,distance between two frames x:0.005688 ,z=0.009791,angle between two frames:0.001266
[17:10:20.705] After pose optimization, distance between two frames x:0.005688 ,z=0.009791,angle between two frames:0.001266
[17:10:20.705] Update EKF and 3D points, distance between two frames x:0.004536 ,z=0.009901,angle between two frames:0.002942
[17:10:20.800] After Reprojection Map nPoint:42 ,nCell:29 ,nMatches:14 ,distance between two frames x:-0.001801 ,z=0.014185,angle between two frames:0.009873
[17:10:20.800] After pose optimization, distance between two frames x:-0.001801 ,z=0.014185,angle between two frames:0.009873
[17:10:20.800] Update EKF and 3D points, distance between two frames x:-0.002095 ,z=0.013205,angle between two frames:0.007209
[17:10:20.907] After Reprojection Map nPoint:40 ,nCell:27 ,nMatches:12 ,distance between two frames x:0.003506 ,z=0.010072,angle between two frames:0.007224
[17:10:20.907] After pose optimization, distance between two frames x:0.003506 ,z=0.010072,angle between two frames:0.007224
[17:10:20.907] Update EKF and 3D points, distance between two frames x:0.002906 ,z=0.009266,angle between two frames:0.006362
[17:10:21.008] After Reprojection Map nPoint:39 ,nCell:26 ,nMatches:12 ,distance between two frames x:0.003812 ,z=-0.007676,angle between two frames:0.009599
[17:10:21.008] After pose optimization, distance between two frames x:0.003812 ,z=-0.007676,angle between two frames:0.009599
[17:10:21.008] Update EKF and 3D points, distance between two frames x:0.003099 ,z=-0.006525,angle between two frames:0.003287
[17:10:21.113] After Reprojection Map nPoint:38 ,nCell:30 ,nMatches:12 ,distance between two frames x:-0.016875 ,z=0.017338,angle between two frames:0.023546
[17:10:21.113] After pose optimization, distance between two frames x:-0.016875 ,z=0.017338,angle between two frames:0.023546
[17:10:21.113] Update EKF and 3D points, distance between two frames x:-0.015356 ,z=0.016423,angle between two frames:0.017294
[17:10:21.211] After Reprojection Map nPoint:36 ,nCell:28 ,nMatches:13 ,distance between two frames x:-0.004348 ,z=0.025359,angle between two frames:0.020922
[17:10:21.211] After pose optimization, distance between two frames x:-0.004348 ,z=0.025359,angle between two frames:0.020922
[17:10:21.211] Update EKF and 3D points, distance between two frames x:-0.003446 ,z=0.022859,angle between two frames:0.010920
[17:10:21.327] After Reprojection Map nPoint:38 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.003880 ,z=0.003979,angle between two frames:0.007004
[17:10:21.327] After pose optimization, distance between two frames x:-0.003880 ,z=0.003979,angle between two frames:0.007004
[17:10:21.327] Update EKF and 3D points, distance between two frames x:-0.003227 ,z=0.003272,angle between two frames:0.004095
[17:10:21.414] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:12 ,distance between two frames x:-0.009001 ,z=0.031877,angle between two frames:0.028502
[17:10:21.414] After pose optimization, distance between two frames x:-0.009001 ,z=0.031877,angle between two frames:0.028502
[17:10:21.415] Update EKF and 3D points, distance between two frames x:-0.007872 ,z=0.028734,angle between two frames:0.014580
[17:10:21.522] After Reprojection Map nPoint:42 ,nCell:35 ,nMatches:17 ,distance between two frames x:0.039151 ,z=-0.000147,angle between two frames:0.024047
[17:10:21.523] After pose optimization, distance between two frames x:0.039151 ,z=-0.000147,angle between two frames:0.024047
[17:10:21.523] Update EKF and 3D points, distance between two frames x:0.035145 ,z=-0.000594,angle between two frames:0.023684
[17:10:21.608] After Reprojection Map nPoint:39 ,nCell:31 ,nMatches:11 ,distance between two frames x:-0.019596 ,z=0.011229,angle between two frames:0.011746
[17:10:21.608] After pose optimization, distance between two frames x:-0.019596 ,z=0.011229,angle between two frames:0.011746
[17:10:21.608] Update EKF and 3D points, distance between two frames x:-0.018896 ,z=0.010445,angle between two frames:0.001210
[17:10:21.699] After Reprojection Map nPoint:37 ,nCell:31 ,nMatches:13 ,distance between two frames x:0.008836 ,z=0.003328,angle between two frames:0.007871
[17:10:21.699] After pose optimization, distance between two frames x:0.008836 ,z=0.003328,angle between two frames:0.007871
[17:10:21.699] Update EKF and 3D points, distance between two frames x:0.007924 ,z=0.003293,angle between two frames:0.011237
[17:10:21.811] After Reprojection Map nPoint:40 ,nCell:28 ,nMatches:13 ,distance between two frames x:-0.004650 ,z=0.000859,angle between two frames:0.008050
[17:10:21.811] After pose optimization, distance between two frames x:-0.004650 ,z=0.000859,angle between two frames:0.008050
[17:10:21.811] Update EKF and 3D points, distance between two frames x:-0.004539 ,z=0.001354,angle between two frames:0.013363
[17:10:21.906] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:11 ,distance between two frames x:-0.008179 ,z=0.012981,angle between two frames:0.008497
[17:10:21.906] After pose optimization, distance between two frames x:-0.008179 ,z=0.012981,angle between two frames:0.008497
[17:10:21.906] Update EKF and 3D points, distance between two frames x:-0.007339 ,z=0.012288,angle between two frames:0.002382
[17:10:22.006] After Reprojection Map nPoint:42 ,nCell:31 ,nMatches:13 ,distance between two frames x:-0.006993 ,z=0.008219,angle between two frames:0.008404
[17:10:22.006] After pose optimization, distance between two frames x:-0.006993 ,z=0.008219,angle between two frames:0.008404
[17:10:22.006] Update EKF and 3D points, distance between two frames x:-0.006041 ,z=0.007697,angle between two frames:0.002297
[17:10:22.100] After Reprojection Map nPoint:42 ,nCell:36 ,nMatches:12 ,distance between two frames x:-0.001724 ,z=0.002033,angle between two frames:0.001947
[17:10:22.101] After pose optimization, distance between two frames x:-0.001724 ,z=0.002033,angle between two frames:0.001947
[17:10:22.101] Update EKF and 3D points, distance between two frames x:-0.001306 ,z=0.002235,angle between two frames:0.003683
[17:10:22.216] After Reprojection Map nPoint:39 ,nCell:31 ,nMatches:13 ,distance between two frames x:0.009077 ,z=0.005947,angle between two frames:0.003519
[17:10:22.216] After pose optimization, distance between two frames x:0.009077 ,z=0.005947,angle between two frames:0.003519
[17:10:22.216] Update EKF and 3D points, distance between two frames x:0.008126 ,z=0.005931,angle between two frames:0.001908
[17:10:22.312] After Reprojection Map nPoint:36 ,nCell:25 ,nMatches:11 ,distance between two frames x:-0.001084 ,z=-0.000575,angle between two frames:0.003460
[17:10:22.313] After pose optimization, distance between two frames x:-0.001084 ,z=-0.000575,angle between two frames:0.003460
[17:10:22.313] Update EKF and 3D points, distance between two frames x:-0.001392 ,z=0.000113,angle between two frames:0.001571
[17:10:22.421] After Reprojection Map nPoint:40 ,nCell:30 ,nMatches:14 ,distance between two frames x:-0.009700 ,z=0.019590,angle between two frames:0.018100
[17:10:22.421] After pose optimization, distance between two frames x:-0.009700 ,z=0.019590,angle between two frames:0.018100
[17:10:22.421] Update EKF and 3D points, distance between two frames x:-0.008834 ,z=0.018237,angle between two frames:0.011016
[17:10:22.500] After Reprojection Map nPoint:43 ,nCell:33 ,nMatches:11 ,distance between two frames x:-0.006572 ,z=0.012468,angle between two frames:0.007620
[17:10:22.500] After pose optimization, distance between two frames x:-0.006572 ,z=0.012468,angle between two frames:0.007620
[17:10:22.500] Update EKF and 3D points, distance between two frames x:-0.005641 ,z=0.011246,angle between two frames:0.004163
[17:10:22.618] After Reprojection Map nPoint:40 ,nCell:34 ,nMatches:11 ,distance between two frames x:-0.008812 ,z=0.015979,angle between two frames:0.019589
[17:10:22.618] After pose optimization, distance between two frames x:-0.008812 ,z=0.015979,angle between two frames:0.019589
[17:10:22.618] Update EKF and 3D points, distance between two frames x:-0.007616 ,z=0.014405,angle between two frames:0.015821
[17:10:22.715] After Reprojection Map nPoint:38 ,nCell:31 ,nMatches:13 ,distance between two frames x:-0.002268 ,z=0.017916,angle between two frames:0.013311
[17:10:22.715] After pose optimization, distance between two frames x:-0.002268 ,z=0.017916,angle between two frames:0.013311
[17:10:22.715] Update EKF and 3D points, distance between two frames x:-0.001691 ,z=0.016012,angle between two frames:0.011185
[17:10:22.810] After Reprojection Map nPoint:36 ,nCell:33 ,nMatches:13 ,distance between two frames x:-0.011423 ,z=0.004635,angle between two frames:0.011861
[17:10:22.810] After pose optimization, distance between two frames x:-0.011423 ,z=0.004635,angle between two frames:0.011861
[17:10:22.810] Update EKF and 3D points, distance between two frames x:-0.010039 ,z=0.004071,angle between two frames:0.009575
[17:10:22.910] After Reprojection Map nPoint:36 ,nCell:30 ,nMatches:12 ,distance between two frames x:0.002803 ,z=-0.004985,angle between two frames:0.004530
[17:10:22.910] After pose optimization, distance between two frames x:0.002803 ,z=-0.004985,angle between two frames:0.004530
[17:10:22.910] Update EKF and 3D points, distance between two frames x:0.002898 ,z=-0.004052,angle between two frames:0.003281
[17:10:23.003] After Reprojection Map nPoint:39 ,nCell:27 ,nMatches:11 ,distance between two frames x:-0.010027 ,z=0.016561,angle between two frames:0.010748
[17:10:23.003] After pose optimization, distance between two frames x:-0.010027 ,z=0.016561,angle between two frames:0.010748
[17:10:23.003] Update EKF and 3D points, distance between two frames x:-0.008965 ,z=0.015666,angle between two frames:0.004956
[17:10:23.100] After Reprojection Map nPoint:42 ,nCell:36 ,nMatches:14 ,distance between two frames x:-0.010708 ,z=0.024581,angle between two frames:0.019534
[17:10:23.100] After pose optimization, distance between two frames x:-0.010708 ,z=0.024581,angle between two frames:0.019534
[17:10:23.100] Update EKF and 3D points, distance between two frames x:-0.009220 ,z=0.022119,angle between two frames:0.007900
[17:10:23.201] After Reprojection Map nPoint:40 ,nCell:27 ,nMatches:12 ,distance between two frames x:0.001711 ,z=-0.008595,angle between two frames:0.011648
[17:10:23.201] After pose optimization, distance between two frames x:0.001711 ,z=-0.008595,angle between two frames:0.011648
[17:10:23.201] Update EKF and 3D points, distance between two frames x:0.001957 ,z=-0.007890,angle between two frames:0.013139
[17:10:23.304] After Reprojection Map nPoint:39 ,nCell:26 ,nMatches:11 ,distance between two frames x:0.002443 ,z=-0.002548,angle between two frames:0.006682
[17:10:23.304] After pose optimization, distance between two frames x:0.002443 ,z=-0.002548,angle between two frames:0.006682
[17:10:23.304] Update EKF and 3D points, distance between two frames x:0.002178 ,z=-0.001404,angle between two frames:0.007284
[17:10:23.400] After Reprojection Map nPoint:39 ,nCell:32 ,nMatches:14 ,distance between two frames x:-0.002314 ,z=0.000813,angle between two frames:0.007269
[17:10:23.400] After pose optimization, distance between two frames x:-0.002314 ,z=0.000813,angle between two frames:0.007269
[17:10:23.400] Update EKF and 3D points, distance between two frames x:-0.002234 ,z=0.001726,angle between two frames:0.010851
[17:10:23.514] After Reprojection Map nPoint:41 ,nCell:35 ,nMatches:14 ,distance between two frames x:0.001212 ,z=-0.000437,angle between two frames:0.007341
[17:10:23.514] After pose optimization, distance between two frames x:0.001212 ,z=-0.000437,angle between two frames:0.007341
[17:10:23.514] Update EKF and 3D points, distance between two frames x:0.001016 ,z=0.000529,angle between two frames:0.009567
[17:10:23.628] After Reprojection Map nPoint:39 ,nCell:23 ,nMatches:12 ,distance between two frames x:-0.000324 ,z=0.000311,angle between two frames:0.006163
[17:10:23.628] After pose optimization, distance between two frames x:-0.000324 ,z=0.000311,angle between two frames:0.006163
[17:10:23.628] Update EKF and 3D points, distance between two frames x:-0.000451 ,z=0.001207,angle between two frames:0.009939
[17:10:23.719] After Reprojection Map nPoint:42 ,nCell:31 ,nMatches:16 ,distance between two frames x:0.001295 ,z=-0.004732,angle between two frames:0.012517
[17:10:23.719] After pose optimization, distance between two frames x:0.001295 ,z=-0.004732,angle between two frames:0.012517
[17:10:23.719] Update EKF and 3D points, distance between two frames x:0.001015 ,z=-0.003334,angle between two frames:0.009743
[17:10:23.801] After Reprojection Map nPoint:45 ,nCell:32 ,nMatches:12 ,distance between two frames x:-0.001026 ,z=-0.002406,angle between two frames:0.003547
[17:10:23.801] After pose optimization, distance between two frames x:-0.001026 ,z=-0.002406,angle between two frames:0.003547
[17:10:23.801] Update EKF and 3D points, distance between two frames x:-0.001113 ,z=-0.001022,angle between two frames:0.001099
[17:10:23.911] After Reprojection Map nPoint:44 ,nCell:28 ,nMatches:13 ,distance between two frames x:0.002778 ,z=0.000135,angle between two frames:0.003201
[17:10:23.911] After pose optimization, distance between two frames x:0.002778 ,z=0.000135,angle between two frames:0.003201
[17:10:23.911] Update EKF and 3D points, distance between two frames x:0.002348 ,z=0.001159,angle between two frames:0.001144
[17:10:24.004] After Reprojection Map nPoint:43 ,nCell:34 ,nMatches:14 ,distance between two frames x:-0.000042 ,z=0.003144,angle between two frames:0.000789
[17:10:24.004] After pose optimization, distance between two frames x:-0.000042 ,z=0.003144,angle between two frames:0.000789
[17:10:24.004] Update EKF and 3D points, distance between two frames x:-0.000289 ,z=0.003751,angle between two frames:0.000243
[17:10:24.104] After Reprojection Map nPoint:42 ,nCell:36 ,nMatches:14 ,distance between two frames x:-0.001826 ,z=0.000946,angle between two frames:0.001438
[17:10:24.104] After pose optimization, distance between two frames x:-0.001826 ,z=0.000946,angle between two frames:0.001438
[17:10:24.104] Update EKF and 3D points, distance between two frames x:-0.001807 ,z=0.001652,angle between two frames:0.001768
[17:10:24.200] After Reprojection Map nPoint:36 ,nCell:28 ,nMatches:11 ,distance between two frames x:-0.000449 ,z=-0.002944,angle between two frames:0.003470
[17:10:24.200] After pose optimization, distance between two frames x:-0.000449 ,z=-0.002944,angle between two frames:0.003470
[17:10:24.200] Update EKF and 3D points, distance between two frames x:-0.000490 ,z=-0.001764,angle between two frames:0.001412
[17:10:24.306] After Reprojection Map nPoint:36 ,nCell:29 ,nMatches:13 ,distance between two frames x:0.004560 ,z=-0.001143,angle between two frames:0.001688
[17:10:24.306] After pose optimization, distance between two frames x:0.004560 ,z=-0.001143,angle between two frames:0.001688
[17:10:24.306] Update EKF and 3D points, distance between two frames x:0.003947 ,z=-0.000015,angle between two frames:0.002494
[17:10:24.415] After Reprojection Map nPoint:35 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.008307 ,z=0.008341,angle between two frames:0.010123
[17:10:24.415] After pose optimization, distance between two frames x:-0.008307 ,z=0.008341,angle between two frames:0.010123
[17:10:24.415] Update EKF and 3D points, distance between two frames x:-0.007688 ,z=0.008378,angle between two frames:0.012264
[17:10:24.505] After Reprojection Map nPoint:34 ,nCell:28 ,nMatches:14 ,distance between two frames x:-0.001236 ,z=-0.008296,angle between two frames:0.008628
[17:10:24.505] After pose optimization, distance between two frames x:-0.001236 ,z=-0.008296,angle between two frames:0.008628
[17:10:24.505] Update EKF and 3D points, distance between two frames x:-0.000977 ,z=-0.006765,angle between two frames:0.003112
[17:10:24.612] After Reprojection Map nPoint:37 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.002992 ,z=-0.003891,angle between two frames:0.008817
[17:10:24.612] After pose optimization, distance between two frames x:-0.002992 ,z=-0.003891,angle between two frames:0.008817
[17:10:24.612] Update EKF and 3D points, distance between two frames x:-0.002672 ,z=-0.002341,angle between two frames:0.007598
[17:10:24.711] After Reprojection Map nPoint:34 ,nCell:24 ,nMatches:12 ,distance between two frames x:0.005857 ,z=0.005732,angle between two frames:0.001116
[17:10:24.711] After pose optimization, distance between two frames x:0.005857 ,z=0.005732,angle between two frames:0.001116
[17:10:24.712] Update EKF and 3D points, distance between two frames x:0.005222 ,z=0.006230,angle between two frames:0.000617
[17:10:24.801] After Reprojection Map nPoint:36 ,nCell:27 ,nMatches:15 ,distance between two frames x:-0.005109 ,z=0.008339,angle between two frames:0.006010
[17:10:24.801] After pose optimization, distance between two frames x:-0.005109 ,z=0.008339,angle between two frames:0.006010
[17:10:24.801] Update EKF and 3D points, distance between two frames x:-0.004863 ,z=0.008191,angle between two frames:0.000698
[17:10:24.901] After Reprojection Map nPoint:39 ,nCell:25 ,nMatches:14 ,distance between two frames x:0.000058 ,z=-0.001300,angle between two frames:0.005305
[17:10:24.901] After pose optimization, distance between two frames x:0.000058 ,z=-0.001300,angle between two frames:0.005305
[17:10:24.901] Update EKF and 3D points, distance between two frames x:0.000036 ,z=-0.000627,angle between two frames:0.007368
[17:10:25.010] After Reprojection Map nPoint:36 ,nCell:20 ,nMatches:11 ,distance between two frames x:-0.000590 ,z=0.001612,angle between two frames:0.004112
[17:10:25.010] After pose optimization, distance between two frames x:-0.000590 ,z=0.001612,angle between two frames:0.004112
[17:10:25.010] Update EKF and 3D points, distance between two frames x:-0.000643 ,z=0.002261,angle between two frames:0.007693
[17:10:25.107] After Reprojection Map nPoint:42 ,nCell:31 ,nMatches:12 ,distance between two frames x:0.003703 ,z=0.008065,angle between two frames:0.004792
[17:10:25.107] After pose optimization, distance between two frames x:0.003703 ,z=0.008065,angle between two frames:0.004792
[17:10:25.107] Update EKF and 3D points, distance between two frames x:0.003164 ,z=0.007999,angle between two frames:0.001252
[17:10:25.203] After Reprojection Map nPoint:39 ,nCell:27 ,nMatches:12 ,distance between two frames x:-0.002006 ,z=-0.001719,angle between two frames:0.008876
[17:10:25.203] After pose optimization, distance between two frames x:-0.002006 ,z=-0.001719,angle between two frames:0.008876
[17:10:25.203] Update EKF and 3D points, distance between two frames x:-0.002086 ,z=-0.000950,angle between two frames:0.012509
[17:10:25.309] After Reprojection Map nPoint:38 ,nCell:25 ,nMatches:14 ,distance between two frames x:0.002978 ,z=0.005633,angle between two frames:0.000546
[17:10:25.309] After pose optimization, distance between two frames x:0.002978 ,z=0.005633,angle between two frames:0.000546
[17:10:25.309] Update EKF and 3D points, distance between two frames x:0.002501 ,z=0.005870,angle between two frames:0.003716
[17:10:25.404] After Reprojection Map nPoint:42 ,nCell:22 ,nMatches:13 ,distance between two frames x:-0.008300 ,z=0.020508,angle between two frames:0.014275
[17:10:25.404] After pose optimization, distance between two frames x:-0.008300 ,z=0.020508,angle between two frames:0.014275
[17:10:25.404] Update EKF and 3D points, distance between two frames x:-0.007663 ,z=0.018897,angle between two frames:0.006239
[17:10:25.500] After Reprojection Map nPoint:38 ,nCell:26 ,nMatches:13 ,distance between two frames x:0.018850 ,z=0.009349,angle between two frames:0.001642
[17:10:25.500] After pose optimization, distance between two frames x:0.018850 ,z=0.009349,angle between two frames:0.001642
[17:10:25.500] Update EKF and 3D points, distance between two frames x:0.016843 ,z=0.008357,angle between two frames:0.005839
[17:10:25.609] After Reprojection Map nPoint:43 ,nCell:31 ,nMatches:14 ,distance between two frames x:-0.007817 ,z=-0.003735,angle between two frames:0.004131
[17:10:25.609] After pose optimization, distance between two frames x:-0.007817 ,z=-0.003735,angle between two frames:0.004131
[17:10:25.609] Update EKF and 3D points, distance between two frames x:-0.007755 ,z=-0.003060,angle between two frames:0.003503
[17:10:25.706] After Reprojection Map nPoint:42 ,nCell:31 ,nMatches:12 ,distance between two frames x:-0.005074 ,z=0.009403,angle between two frames:0.007607
[17:10:25.706] After pose optimization, distance between two frames x:-0.005074 ,z=0.009403,angle between two frames:0.007607
[17:10:25.706] Update EKF and 3D points, distance between two frames x:-0.004606 ,z=0.009168,angle between two frames:0.005890
[17:10:25.802] After Reprojection Map nPoint:37 ,nCell:30 ,nMatches:13 ,distance between two frames x:-0.001624 ,z=0.009325,angle between two frames:0.007142
[17:10:25.802] After pose optimization, distance between two frames x:-0.001624 ,z=0.009325,angle between two frames:0.007142
[17:10:25.802] Update EKF and 3D points, distance between two frames x:-0.001400 ,z=0.008830,angle between two frames:0.008179
[17:10:25.930] After Reprojection Map nPoint:39 ,nCell:33 ,nMatches:15 ,distance between two frames x:0.000495 ,z=-0.001088,angle between two frames:0.000780
[17:10:25.930] After pose optimization, distance between two frames x:0.000495 ,z=-0.001088,angle between two frames:0.000780
[17:10:25.930] Update EKF and 3D points, distance between two frames x:0.000408 ,z=-0.000529,angle between two frames:0.003060
[17:10:26.022] After Reprojection Map nPoint:41 ,nCell:37 ,nMatches:14 ,distance between two frames x:-0.008267 ,z=0.023351,angle between two frames:0.020302
[17:10:26.022] After pose optimization, distance between two frames x:-0.008267 ,z=0.023351,angle between two frames:0.020302
[17:10:26.022] Update EKF and 3D points, distance between two frames x:-0.007495 ,z=0.021579,angle between two frames:0.014867
[17:10:26.104] After Reprojection Map nPoint:39 ,nCell:32 ,nMatches:14 ,distance between two frames x:0.014306 ,z=0.015177,angle between two frames:0.007957
[17:10:26.104] After pose optimization, distance between two frames x:0.014306 ,z=0.015177,angle between two frames:0.007957
[17:10:26.104] Update EKF and 3D points, distance between two frames x:0.012894 ,z=0.013475,angle between two frames:0.003828
[17:10:26.200] After Reprojection Map nPoint:37 ,nCell:28 ,nMatches:14 ,distance between two frames x:-0.007905 ,z=0.036328,angle between two frames:0.025258
[17:10:26.200] After pose optimization, distance between two frames x:-0.007905 ,z=0.036328,angle between two frames:0.025258
[17:10:26.200] Update EKF and 3D points, distance between two frames x:-0.007607 ,z=0.032307,angle between two frames:0.017902
[17:10:26.299] After Reprojection Map nPoint:42 ,nCell:31 ,nMatches:13 ,distance between two frames x:-0.001581 ,z=0.001383,angle between two frames:0.006973
[17:10:26.299] After pose optimization, distance between two frames x:-0.001581 ,z=0.001383,angle between two frames:0.006973
[17:10:26.299] Update EKF and 3D points, distance between two frames x:-0.001390 ,z=0.000451,angle between two frames:0.003578
[17:10:26.401] After Reprojection Map nPoint:42 ,nCell:35 ,nMatches:14 ,distance between two frames x:-0.006695 ,z=0.031239,angle between two frames:0.023341
[17:10:26.401] After pose optimization, distance between two frames x:-0.006695 ,z=0.031239,angle between two frames:0.023341
[17:10:26.401] Update EKF and 3D points, distance between two frames x:-0.005970 ,z=0.028033,angle between two frames:0.012040
[17:10:26.518] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:11 ,distance between two frames x:0.015097 ,z=0.008202,angle between two frames:0.000909
[17:10:26.518] After pose optimization, distance between two frames x:0.015097 ,z=0.008202,angle between two frames:0.000909
[17:10:26.518] Update EKF and 3D points, distance between two frames x:0.013593 ,z=0.006825,angle between two frames:0.007884
[17:10:26.606] After Reprojection Map nPoint:39 ,nCell:30 ,nMatches:13 ,distance between two frames x:-0.015287 ,z=0.024587,angle between two frames:0.018480
[17:10:26.606] After pose optimization, distance between two frames x:-0.015287 ,z=0.024587,angle between two frames:0.018480
[17:10:26.606] Update EKF and 3D points, distance between two frames x:-0.014197 ,z=0.021989,angle between two frames:0.009516
[17:10:26.704] After Reprojection Map nPoint:37 ,nCell:33 ,nMatches:14 ,distance between two frames x:-0.001554 ,z=0.000218,angle between two frames:0.003029
[17:10:26.704] After pose optimization, distance between two frames x:-0.001554 ,z=0.000218,angle between two frames:0.003029
[17:10:26.704] Update EKF and 3D points, distance between two frames x:-0.001095 ,z=-0.000093,angle between two frames:0.003140
[17:10:26.812] After Reprojection Map nPoint:39 ,nCell:26 ,nMatches:15 ,distance between two frames x:-0.005579 ,z=0.009734,angle between two frames:0.000967
[17:10:26.812] After pose optimization, distance between two frames x:-0.005579 ,z=0.009734,angle between two frames:0.000967
[17:10:26.812] Update EKF and 3D points, distance between two frames x:-0.004896 ,z=0.009183,angle between two frames:0.004634
[17:10:26.908] After Reprojection Map nPoint:43 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.002356 ,z=0.019778,angle between two frames:0.012219
[17:10:26.908] After pose optimization, distance between two frames x:-0.002356 ,z=0.019778,angle between two frames:0.012219
[17:10:26.908] Update EKF and 3D points, distance between two frames x:-0.001947 ,z=0.017979,angle between two frames:0.002855
[17:10:27.008] After Reprojection Map nPoint:41 ,nCell:34 ,nMatches:14 ,distance between two frames x:0.006046 ,z=0.007809,angle between two frames:0.000930
[17:10:27.008] After pose optimization, distance between two frames x:0.006046 ,z=0.007809,angle between two frames:0.000930
[17:10:27.008] Update EKF and 3D points, distance between two frames x:0.005433 ,z=0.006931,angle between two frames:0.005915
[17:10:27.103] After Reprojection Map nPoint:41 ,nCell:32 ,nMatches:12 ,distance between two frames x:-0.001442 ,z=0.007354,angle between two frames:0.001391
[17:10:27.103] After pose optimization, distance between two frames x:-0.001442 ,z=0.007354,angle between two frames:0.001391
[17:10:27.103] Update EKF and 3D points, distance between two frames x:-0.001589 ,z=0.006863,angle between two frames:0.007357
[17:10:27.203] After Reprojection Map nPoint:40 ,nCell:32 ,nMatches:14 ,distance between two frames x:-0.004536 ,z=0.004382,angle between two frames:0.000581
[17:10:27.204] After pose optimization, distance between two frames x:-0.004536 ,z=0.004382,angle between two frames:0.000581
[17:10:27.204] Update EKF and 3D points, distance between two frames x:-0.004179 ,z=0.004331,angle between two frames:0.004228
[17:10:27.306] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:13 ,distance between two frames x:0.009688 ,z=-0.009182,angle between two frames:0.012889
[17:10:27.306] After pose optimization, distance between two frames x:0.009688 ,z=-0.009182,angle between two frames:0.012889
[17:10:27.306] Update EKF and 3D points, distance between two frames x:0.008651 ,z=-0.007592,angle between two frames:0.008566
[17:10:27.415] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.003305 ,z=0.004745,angle between two frames:0.002769
[17:10:27.415] After pose optimization, distance between two frames x:-0.003305 ,z=0.004745,angle between two frames:0.002769
[17:10:27.415] Update EKF and 3D points, distance between two frames x:-0.003387 ,z=0.005363,angle between two frames:0.003278
[17:10:27.507] After Reprojection Map nPoint:43 ,nCell:34 ,nMatches:16 ,distance between two frames x:-0.002012 ,z=0.010734,angle between two frames:0.007644
[17:10:27.507] After pose optimization, distance between two frames x:-0.002012 ,z=0.010734,angle between two frames:0.007644
[17:10:27.507] Update EKF and 3D points, distance between two frames x:-0.001906 ,z=0.010358,angle between two frames:0.008400
[17:10:27.611] After Reprojection Map nPoint:43 ,nCell:35 ,nMatches:12 ,distance between two frames x:0.000340 ,z=0.003407,angle between two frames:0.004445
[17:10:27.611] After pose optimization, distance between two frames x:0.000340 ,z=0.003407,angle between two frames:0.004445
[17:10:27.611] Update EKF and 3D points, distance between two frames x:0.000252 ,z=0.003502,angle between two frames:0.004081
[17:10:27.718] After Reprojection Map nPoint:39 ,nCell:32 ,nMatches:11 ,distance between two frames x:0.004596 ,z=-0.006499,angle between two frames:0.007896
[17:10:27.718] After pose optimization, distance between two frames x:0.004596 ,z=-0.006499,angle between two frames:0.007896
[17:10:27.718] Update EKF and 3D points, distance between two frames x:0.003978 ,z=-0.005153,angle between two frames:0.002252
[17:10:27.800] After Reprojection Map nPoint:37 ,nCell:25 ,nMatches:13 ,distance between two frames x:0.008481 ,z=-0.008197,angle between two frames:0.011044
[17:10:27.800] After pose optimization, distance between two frames x:0.008481 ,z=-0.008197,angle between two frames:0.011044
[17:10:27.800] Update EKF and 3D points, distance between two frames x:0.007238 ,z=-0.006210,angle between two frames:0.003699
[17:10:27.899] After Reprojection Map nPoint:37 ,nCell:31 ,nMatches:15 ,distance between two frames x:-0.008365 ,z=0.007460,angle between two frames:0.006620
[17:10:27.899] After pose optimization, distance between two frames x:-0.008365 ,z=0.007460,angle between two frames:0.006620
[17:10:27.899] Update EKF and 3D points, distance between two frames x:-0.007941 ,z=0.007917,angle between two frames:0.009098
[17:10:28.013] After Reprojection Map nPoint:39 ,nCell:33 ,nMatches:14 ,distance between two frames x:0.001688 ,z=0.003244,angle between two frames:0.005174
[17:10:28.013] After pose optimization, distance between two frames x:0.001688 ,z=0.003244,angle between two frames:0.005174
[17:10:28.013] Update EKF and 3D points, distance between two frames x:0.001567 ,z=0.003642,angle between two frames:0.006002
[17:10:28.107] After Reprojection Map nPoint:40 ,nCell:34 ,nMatches:15 ,distance between two frames x:-0.007682 ,z=0.008842,angle between two frames:0.006991
[17:10:28.107] After pose optimization, distance between two frames x:-0.007682 ,z=0.008842,angle between two frames:0.006991
[17:10:28.107] Update EKF and 3D points, distance between two frames x:-0.006956 ,z=0.008628,angle between two frames:0.001080
[17:10:28.220] After Reprojection Map nPoint:41 ,nCell:37 ,nMatches:14 ,distance between two frames x:-0.001905 ,z=0.003877,angle between two frames:0.001321
[17:10:28.220] After pose optimization, distance between two frames x:-0.001905 ,z=0.003877,angle between two frames:0.001321
[17:10:28.220] Update EKF and 3D points, distance between two frames x:-0.001529 ,z=0.003949,angle between two frames:0.001633
[17:10:28.299] After Reprojection Map nPoint:39 ,nCell:37 ,nMatches:15 ,distance between two frames x:-0.004442 ,z=0.015211,angle between two frames:0.009987
[17:10:28.299] After pose optimization, distance between two frames x:-0.004442 ,z=0.015211,angle between two frames:0.009987
[17:10:28.299] Update EKF and 3D points, distance between two frames x:-0.003904 ,z=0.014144,angle between two frames:0.001779
[17:10:28.404] After Reprojection Map nPoint:32 ,nCell:27 ,nMatches:14 ,distance between two frames x:0.008659 ,z=0.000209,angle between two frames:0.008868
[17:10:28.404] After pose optimization, distance between two frames x:0.008659 ,z=0.000209,angle between two frames:0.008868
[17:10:28.404] Update EKF and 3D points, distance between two frames x:0.007802 ,z=0.000426,angle between two frames:0.008967
[17:10:28.500] After Reprojection Map nPoint:35 ,nCell:27 ,nMatches:15 ,distance between two frames x:-0.008195 ,z=0.011544,angle between two frames:0.010208
[17:10:28.500] After pose optimization, distance between two frames x:-0.008195 ,z=0.011544,angle between two frames:0.010208
[17:10:28.500] Update EKF and 3D points, distance between two frames x:-0.007667 ,z=0.010952,angle between two frames:0.003534
[17:10:28.603] After Reprojection Map nPoint:40 ,nCell:32 ,nMatches:13 ,distance between two frames x:0.004356 ,z=0.005089,angle between two frames:0.002983
[17:10:28.604] After pose optimization, distance between two frames x:0.004356 ,z=0.005089,angle between two frames:0.002983
[17:10:28.604] Update EKF and 3D points, distance between two frames x:0.003962 ,z=0.004909,angle between two frames:0.007244
[17:10:28.701] After Reprojection Map nPoint:38 ,nCell:31 ,nMatches:12 ,distance between two frames x:0.006293 ,z=0.014419,angle between two frames:0.001123
[17:10:28.701] After pose optimization, distance between two frames x:0.006293 ,z=0.014419,angle between two frames:0.001123
[17:10:28.701] Update EKF and 3D points, distance between two frames x:0.005355 ,z=0.013384,angle between two frames:0.004427
[17:10:28.809] After Reprojection Map nPoint:37 ,nCell:30 ,nMatches:11 ,distance between two frames x:-0.005756 ,z=-0.002550,angle between two frames:0.005131
[17:10:28.809] After pose optimization, distance between two frames x:-0.005756 ,z=-0.002550,angle between two frames:0.005131
[17:10:28.809] Update EKF and 3D points, distance between two frames x:-0.005530 ,z=-0.002049,angle between two frames:0.007942
[17:10:28.909] After Reprojection Map nPoint:32 ,nCell:25 ,nMatches:9 ,distance between two frames x:-0.001053 ,z=0.007696,angle between two frames:0.000694
[17:10:28.909] After pose optimization, distance between two frames x:-0.001053 ,z=0.007696,angle between two frames:0.000694
[17:10:28.909] Update EKF and 3D points, distance between two frames x:-0.000961 ,z=0.007592,angle between two frames:0.007482
[17:10:29.001] After Reprojection Map nPoint:38 ,nCell:32 ,nMatches:14 ,distance between two frames x:-0.003149 ,z=0.022897,angle between two frames:0.009244
[17:10:29.001] After pose optimization, distance between two frames x:-0.003149 ,z=0.022897,angle between two frames:0.009244
[17:10:29.001] Update EKF and 3D points, distance between two frames x:-0.002851 ,z=0.020434,angle between two frames:0.002309
[17:10:29.114] After Reprojection Map nPoint:39 ,nCell:32 ,nMatches:11 ,distance between two frames x:-0.001920 ,z=0.003102,angle between two frames:0.007906
[17:10:29.114] After pose optimization, distance between two frames x:-0.001920 ,z=0.003102,angle between two frames:0.007906
[17:10:29.114] Update EKF and 3D points, distance between two frames x:-0.001740 ,z=0.002724,angle between two frames:0.009394
[17:10:29.301] After Reprojection Map nPoint:27 ,nCell:20 ,nMatches:10 ,distance between two frames x:0.032461 ,z=-0.010914,angle between two frames:0.069700
[17:10:29.301] After pose optimization, distance between two frames x:0.032461 ,z=-0.010914,angle between two frames:0.069700
[17:10:29.301] Update EKF and 3D points, distance between two frames x:0.028653 ,z=-0.008748,angle between two frames:0.050870
[17:10:29.410] After Reprojection Map nPoint:31 ,nCell:26 ,nMatches:13 ,distance between two frames x:0.002107 ,z=0.011674,angle between two frames:0.012913
[17:10:29.410] After pose optimization, distance between two frames x:0.002107 ,z=0.011674,angle between two frames:0.012913
[17:10:29.410] Update EKF and 3D points, distance between two frames x:0.000480 ,z=0.011619,angle between two frames:0.017153
[17:10:29.610] After Reprojection Map nPoint:35 ,nCell:24 ,nMatches:10 ,distance between two frames x:-0.011674 ,z=0.021650,angle between two frames:0.060263
[17:10:29.610] After pose optimization, distance between two frames x:-0.011674 ,z=0.021650,angle between two frames:0.060263
[17:10:29.610] Update EKF and 3D points, distance between two frames x:-0.011119 ,z=0.020290,angle between two frames:0.053454
[17:10:29.710] After Reprojection Map nPoint:30 ,nCell:26 ,nMatches:10 ,distance between two frames x:-0.018840 ,z=0.022035,angle between two frames:0.026070
[17:10:29.710] After pose optimization, distance between two frames x:-0.018840 ,z=0.022035,angle between two frames:0.026070
[17:10:29.710] Update EKF and 3D points, distance between two frames x:-0.016666 ,z=0.019681,angle between two frames:0.007907
[17:10:29.798] After Reprojection Map nPoint:14 ,nCell:14 ,nMatches:6 ,distance between two frames x:0.009737 ,z=0.008697,angle between two frames:0.002926
[17:10:29.798] After pose optimization, distance between two frames x:0.009737 ,z=0.008697,angle between two frames:0.002926
[17:10:29.798] Update EKF and 3D points, distance between two frames x:0.009308 ,z=0.007475,angle between two frames:0.005177
[17:10:29.901] After Reprojection Map nPoint:25 ,nCell:21 ,nMatches:12 ,distance between two frames x:-0.015886 ,z=0.025953,angle between two frames:0.028868
[17:10:29.901] After pose optimization, distance between two frames x:-0.015886 ,z=0.025953,angle between two frames:0.028868
[17:10:29.901] Update EKF and 3D points, distance between two frames x:-0.014355 ,z=0.023239,angle between two frames:0.019198
[17:10:30.009] After Reprojection Map nPoint:33 ,nCell:27 ,nMatches:12 ,distance between two frames x:0.007090 ,z=-0.004893,angle between two frames:0.004875
[17:10:30.009] After pose optimization, distance between two frames x:0.007090 ,z=-0.004893,angle between two frames:0.004875
[17:10:30.009] Update EKF and 3D points, distance between two frames x:0.006744 ,z=-0.004674,angle between two frames:0.002060
[17:10:30.105] After Reprojection Map nPoint:35 ,nCell:25 ,nMatches:10 ,distance between two frames x:-0.008768 ,z=0.012129,angle between two frames:0.013816
[17:10:30.105] After pose optimization, distance between two frames x:-0.008768 ,z=0.012129,angle between two frames:0.013816
[17:10:30.105] Update EKF and 3D points, distance between two frames x:-0.007922 ,z=0.010868,angle between two frames:0.000999
[17:10:30.202] After Reprojection Map nPoint:31 ,nCell:28 ,nMatches:13 ,distance between two frames x:-0.002060 ,z=0.004652,angle between two frames:0.002452
[17:10:30.202] After pose optimization, distance between two frames x:-0.002060 ,z=0.004652,angle between two frames:0.002452
[17:10:30.202] Update EKF and 3D points, distance between two frames x:-0.001551 ,z=0.003707,angle between two frames:0.000463
[17:10:30.307] After Reprojection Map nPoint:31 ,nCell:24 ,nMatches:10 ,distance between two frames x:0.004252 ,z=0.007490,angle between two frames:0.003136
[17:10:30.307] After pose optimization, distance between two frames x:0.004252 ,z=0.007490,angle between two frames:0.003136
[17:10:30.307] Update EKF and 3D points, distance between two frames x:0.003958 ,z=0.006343,angle between two frames:0.010774
[17:10:30.411] After Reprojection Map nPoint:35 ,nCell:25 ,nMatches:11 ,distance between two frames x:-0.003510 ,z=0.014523,angle between two frames:0.009403
[17:10:30.411] After pose optimization, distance between two frames x:-0.003510 ,z=0.014523,angle between two frames:0.009403
[17:10:30.411] Update EKF and 3D points, distance between two frames x:-0.003227 ,z=0.012531,angle between two frames:0.000875
[17:10:30.501] After Reprojection Map nPoint:35 ,nCell:28 ,nMatches:11 ,distance between two frames x:-0.011670 ,z=0.013140,angle between two frames:0.009843
[17:10:30.501] After pose optimization, distance between two frames x:-0.011670 ,z=0.013140,angle between two frames:0.009843
[17:10:30.501] Update EKF and 3D points, distance between two frames x:-0.010290 ,z=0.011014,angle between two frames:0.004733
[17:10:30.606] After Reprojection Map nPoint:37 ,nCell:32 ,nMatches:12 ,distance between two frames x:0.002350 ,z=0.004234,angle between two frames:0.006579
[17:10:30.606] After pose optimization, distance between two frames x:0.002350 ,z=0.004234,angle between two frames:0.006579
[17:10:30.606] Update EKF and 3D points, distance between two frames x:0.002558 ,z=0.003044,angle between two frames:0.000355
[17:10:30.702] After Reprojection Map nPoint:35 ,nCell:27 ,nMatches:13 ,distance between two frames x:-0.009981 ,z=0.023934,angle between two frames:0.013081
[17:10:30.702] After pose optimization, distance between two frames x:-0.009981 ,z=0.023934,angle between two frames:0.013081
[17:10:30.702] Update EKF and 3D points, distance between two frames x:-0.008813 ,z=0.020893,angle between two frames:0.000891
[17:10:30.802] After Reprojection Map nPoint:38 ,nCell:32 ,nMatches:12 ,distance between two frames x:-0.001113 ,z=-0.000243,angle between two frames:0.004083
[17:10:30.802] After pose optimization, distance between two frames x:-0.001113 ,z=-0.000243,angle between two frames:0.004083
[17:10:30.802] Update EKF and 3D points, distance between two frames x:-0.000595 ,z=-0.001253,angle between two frames:0.007913
[17:10:30.906] After Reprojection Map nPoint:37 ,nCell:31 ,nMatches:15 ,distance between two frames x:-0.007401 ,z=0.026001,angle between two frames:0.022891
[17:10:30.906] After pose optimization, distance between two frames x:-0.007401 ,z=0.026001,angle between two frames:0.022891
[17:10:30.906] Update EKF and 3D points, distance between two frames x:-0.006395 ,z=0.022738,angle between two frames:0.010192
[17:10:31.018] After Reprojection Map nPoint:41 ,nCell:27 ,nMatches:11 ,distance between two frames x:0.009383 ,z=0.000811,angle between two frames:0.006124
[17:10:31.018] After pose optimization, distance between two frames x:0.009383 ,z=0.000811,angle between two frames:0.006124
[17:10:31.018] Update EKF and 3D points, distance between two frames x:0.008687 ,z=-0.000397,angle between two frames:0.008351
[17:10:31.105] After Reprojection Map nPoint:37 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.018846 ,z=0.028766,angle between two frames:0.023457
[17:10:31.105] After pose optimization, distance between two frames x:-0.018846 ,z=0.028766,angle between two frames:0.023457
[17:10:31.105] Update EKF and 3D points, distance between two frames x:-0.017010 ,z=0.025191,angle between two frames:0.013268
[17:10:31.200] After Reprojection Map nPoint:38 ,nCell:32 ,nMatches:15 ,distance between two frames x:-0.000917 ,z=0.006985,angle between two frames:0.006713
[17:10:31.200] After pose optimization, distance between two frames x:-0.000917 ,z=0.006985,angle between two frames:0.006713
[17:10:31.200] Update EKF and 3D points, distance between two frames x:-0.000192 ,z=0.004999,angle between two frames:0.000744
[17:10:31.306] After Reprojection Map nPoint:39 ,nCell:33 ,nMatches:12 ,distance between two frames x:0.007495 ,z=0.001709,angle between two frames:0.005300
[17:10:31.306] After pose optimization, distance between two frames x:0.007495 ,z=0.001709,angle between two frames:0.005300
[17:10:31.306] Update EKF and 3D points, distance between two frames x:0.006917 ,z=0.000845,angle between two frames:0.005796
[17:10:31.397] After Reprojection Map nPoint:41 ,nCell:37 ,nMatches:15 ,distance between two frames x:-0.012072 ,z=0.019668,angle between two frames:0.016776
[17:10:31.397] After pose optimization, distance between two frames x:-0.012072 ,z=0.019668,angle between two frames:0.016776
[17:10:31.397] Update EKF and 3D points, distance between two frames x:-0.010941 ,z=0.017210,angle between two frames:0.009248
[17:10:31.532] After Reprojection Map nPoint:39 ,nCell:31 ,nMatches:11 ,distance between two frames x:-0.002818 ,z=0.005901,angle between two frames:0.005929
[17:10:31.532] After pose optimization, distance between two frames x:-0.002818 ,z=0.005901,angle between two frames:0.005929
[17:10:31.532] Update EKF and 3D points, distance between two frames x:-0.002124 ,z=0.004415,angle between two frames:0.000448
[17:10:31.629] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:12 ,distance between two frames x:-0.004828 ,z=0.022761,angle between two frames:0.015500
[17:10:31.629] After pose optimization, distance between two frames x:-0.004828 ,z=0.022761,angle between two frames:0.015500
[17:10:31.629] Update EKF and 3D points, distance between two frames x:-0.004061 ,z=0.019752,angle between two frames:0.007286
[17:10:31.708] After Reprojection Map nPoint:39 ,nCell:29 ,nMatches:13 ,distance between two frames x:-0.008299 ,z=0.009597,angle between two frames:0.010044
[17:10:31.708] After pose optimization, distance between two frames x:-0.008299 ,z=0.009597,angle between two frames:0.010044
[17:10:31.708] Update EKF and 3D points, distance between two frames x:-0.007125 ,z=0.007518,angle between two frames:0.003403
[17:10:31.807] After Reprojection Map nPoint:40 ,nCell:35 ,nMatches:13 ,distance between two frames x:-0.000771 ,z=0.010401,angle between two frames:0.006636
[17:10:31.807] After pose optimization, distance between two frames x:-0.000771 ,z=0.010401,angle between two frames:0.006636
[17:10:31.807] Update EKF and 3D points, distance between two frames x:-0.000288 ,z=0.008550,angle between two frames:0.001523
[17:10:31.909] After Reprojection Map nPoint:39 ,nCell:33 ,nMatches:13 ,distance between two frames x:-0.005614 ,z=0.016216,angle between two frames:0.012362
[17:10:31.909] After pose optimization, distance between two frames x:-0.005614 ,z=0.016216,angle between two frames:0.012362
[17:10:31.909] Update EKF and 3D points, distance between two frames x:-0.004832 ,z=0.013802,angle between two frames:0.005350
[17:10:32.011] After Reprojection Map nPoint:36 ,nCell:32 ,nMatches:11 ,distance between two frames x:-0.002121 ,z=-0.000636,angle between two frames:0.001786
[17:10:32.011] After pose optimization, distance between two frames x:-0.002121 ,z=-0.000636,angle between two frames:0.001786
[17:10:32.011] Update EKF and 3D points, distance between two frames x:-0.001622 ,z=-0.001387,angle between two frames:0.001637
[17:10:32.101] After Reprojection Map nPoint:32 ,nCell:27 ,nMatches:10 ,distance between two frames x:-0.003512 ,z=0.022758,angle between two frames:0.012689
[17:10:32.101] After pose optimization, distance between two frames x:-0.003512 ,z=0.022758,angle between two frames:0.012689
[17:10:32.101] Update EKF and 3D points, distance between two frames x:-0.002956 ,z=0.019999,angle between two frames:0.006073
[17:10:32.199] After Reprojection Map nPoint:37 ,nCell:32 ,nMatches:13 ,distance between two frames x:-0.005539 ,z=0.012075,angle between two frames:0.010144
[17:10:32.200] After pose optimization, distance between two frames x:-0.005539 ,z=0.012075,angle between two frames:0.010144
[17:10:32.200] Update EKF and 3D points, distance between two frames x:-0.004741 ,z=0.009808,angle between two frames:0.003651
[17:10:32.314] After Reprojection Map nPoint:39 ,nCell:32 ,nMatches:13 ,distance between two frames x:0.003338 ,z=-0.001094,angle between two frames:0.001860
[17:10:32.314] After pose optimization, distance between two frames x:0.003338 ,z=-0.001094,angle between two frames:0.001860
[17:10:32.314] Update EKF and 3D points, distance between two frames x:0.003242 ,z=-0.001745,angle between two frames:0.003899
[17:10:32.400] After Reprojection Map nPoint:39 ,nCell:30 ,nMatches:12 ,distance between two frames x:-0.004086 ,z=0.002041,angle between two frames:0.002276
[17:10:32.400] After pose optimization, distance between two frames x:-0.004086 ,z=0.002041,angle between two frames:0.002276
[17:10:32.400] Update EKF and 3D points, distance between two frames x:-0.003669 ,z=0.001594,angle between two frames:0.000029
[17:10:32.499] After Reprojection Map nPoint:37 ,nCell:31 ,nMatches:10 ,distance between two frames x:0.002797 ,z=0.011744,angle between two frames:0.004464
[17:10:32.499] After pose optimization, distance between two frames x:0.002797 ,z=0.011744,angle between two frames:0.004464
[17:10:32.499] Update EKF and 3D points, distance between two frames x:0.002631 ,z=0.010300,angle between two frames:0.001184
[17:10:32.599] After Reprojection Map nPoint:38 ,nCell:29 ,nMatches:11 ,distance between two frames x:-0.003484 ,z=0.003054,angle between two frames:0.002668
[17:10:32.599] After pose optimization, distance between two frames x:-0.003484 ,z=0.003054,angle between two frames:0.002668
[17:10:32.599] Update EKF and 3D points, distance between two frames x:-0.003157 ,z=0.002227,angle between two frames:0.000195
[17:10:32.704] After Reprojection Map nPoint:39 ,nCell:35 ,nMatches:13 ,distance between two frames x:-0.001482 ,z=0.003069,angle between two frames:0.001593
[17:10:32.704] After pose optimization, distance between two frames x:-0.001482 ,z=0.003069,angle between two frames:0.001593
[17:10:32.704] Update EKF and 3D points, distance between two frames x:-0.001208 ,z=0.002450,angle between two frames:0.000795
[17:10:32.817] After Reprojection Map nPoint:39 ,nCell:33 ,nMatches:11 ,distance between two frames x:-0.005177 ,z=0.011430,angle between two frames:0.009030
[17:10:32.817] After pose optimization, distance between two frames x:-0.005177 ,z=0.011430,angle between two frames:0.009030
[17:10:32.817] Update EKF and 3D points, distance between two frames x:-0.004515 ,z=0.009962,angle between two frames:0.003808
[17:10:32.908] After Reprojection Map nPoint:36 ,nCell:32 ,nMatches:13 ,distance between two frames x:-0.006929 ,z=0.017493,angle between two frames:0.011577
[17:10:32.908] After pose optimization, distance between two frames x:-0.006929 ,z=0.017493,angle between two frames:0.011577
[17:10:32.908] Update EKF and 3D points, distance between two frames x:-0.005941 ,z=0.015069,angle between two frames:0.004954
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -131,7 +131,7 @@ namespace vio {
//TODO add 15 degree roll orientation
SE2 inv=T_f_w_.inverse();
Quaterniond q;
q = AngleAxisd(0.261799, Vector3d::UnitX())
q = AngleAxisd(0.0, Vector3d::UnitX())
* 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)));
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" launch-prefix="xterm -e gdb args" /-->
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -261,7 +261,7 @@ namespace vio {
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())
q = AngleAxisd(0.0, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_cur_ref(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));