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

change the triangulation and reproject pipline

Browse Source
This commit is contained in:
gfuse
2021-10-04 10:00:13 +02:00
parent 2b6a1461bb
commit 346dbb640c
7 changed files with 291 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/frame_handler_log.txt
+251 -86
View File
@@ -1,86 +1,251 @@
[19:59:17.117] frame process finished the id is: 0 the obs is:0 [09:36:51.193] Init: frame zero: 0.000000, 0.000000 0.000000
[19:59:17.212] frame process finished the id is: 1 the obs is:0 [09:36:51.193] Init: Selected first frame.
[19:59:17.310] Init: frame zero: 0.000000, 0.000000 0.000000 [09:36:51.193] frame process finished the id is: 0 the obs is:0
[19:59:17.310] Init: Selected first frame. [09:36:51.224] Init: px average disparity is:0.096920 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.310] frame process finished the id is: 2 the obs is:0 [09:36:51.224] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.345] Init: px average disparity is:0.152753 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.224] frame process finished the id is: 1 the obs is:0
[19:59:17.345] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.320] Init: px average disparity is:0.120159 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.345] frame process finished the id is: 3 the obs is:0 [09:36:51.320] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.444] Init: px average disparity is:0.221920 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.320] frame process finished the id is: 2 the obs is:0
[19:59:17.444] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.419] Init: px average disparity is:0.166353 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.444] frame process finished the id is: 4 the obs is:0 [09:36:51.419] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.553] Init: px average disparity is:0.152235 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.419] frame process finished the id is: 3 the obs is:0
[19:59:17.553] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.519] Init: px average disparity is:0.191240 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.553] frame process finished the id is: 5 the obs is:0 [09:36:51.519] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.642] Init: px average disparity is:0.197958 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.519] frame process finished the id is: 4 the obs is:0
[19:59:17.642] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.620] Init: px average disparity is:0.207745 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.642] frame process finished the id is: 6 the obs is:0 [09:36:51.620] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.745] Init: px average disparity is:0.221111 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.620] frame process finished the id is: 5 the obs is:0
[19:59:17.745] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.719] Init: px average disparity is:0.227178 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.745] frame process finished the id is: 7 the obs is:0 [09:36:51.719] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.851] Init: px average disparity is:0.224726 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.719] frame process finished the id is: 6 the obs is:0
[19:59:17.851] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.824] Init: px average disparity is:0.236689 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.851] frame process finished the id is: 8 the obs is:0 [09:36:51.824] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:17.952] Init: px average disparity is:0.158653 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:51.824] frame process finished the id is: 7 the obs is:0
[19:59:17.952] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:51.921] Init: px average disparity is:0.269724 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:17.952] frame process finished the id is: 9 the obs is:0 [09:36:51.921] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.039] Odometry x=0.000000, y=0.000000, theta=-0.000000 [09:36:51.921] frame process finished the id is: 8 the obs is:0
[19:59:18.049] Init: px average disparity is:0.179287 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:52.020] Init: px average disparity is:0.279740 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.049] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.020] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.049] frame process finished the id is: 10 the obs is:0 [09:36:52.020] frame process finished the id is: 9 the obs is:0
[19:59:18.132] Odometry x=0.000001, y=0.000003, theta=-0.033384 [09:36:52.121] Init: px average disparity is:0.324433 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.149] Init: px average disparity is:0.148421 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:52.121] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.149] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.121] frame process finished the id is: 10 the obs is:0
[19:59:18.149] frame process finished the id is: 11 the obs is:0 [09:36:52.221] Init: px average disparity is:0.332425 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.230] Odometry x=0.000002, y=0.000007, theta=-0.059169 [09:36:52.221] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.245] Init: px average disparity is:0.137330 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:52.221] frame process finished the id is: 11 the obs is:0
[19:59:18.245] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.322] Init: px average disparity is:0.360695 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.245] frame process finished the id is: 12 the obs is:0 [09:36:52.322] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.333] Odometry x=0.000009, y=0.000010, theta=-0.083266 [09:36:52.322] frame process finished the id is: 12 the obs is:0
[19:59:18.353] Init: px average disparity is:4.945143 ,While minimum is: 10.000000 KLT tracked : 109 [09:36:52.421] Init: px average disparity is:0.331429 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.353] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.421] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.353] frame process finished the id is: 13 the obs is:0 [09:36:52.421] frame process finished the id is: 13 the obs is:0
[19:59:18.431] Odometry x=0.000000, y=0.000029, theta=-0.014218 [09:36:52.523] Init: px average disparity is:0.364890 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.519] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:11.912397 ,While minimum is: 10.000000 KLT tracked: 109 [09:36:52.524] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.519] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.524] frame process finished the id is: 14 the obs is:0
[19:59:18.519] frame process finished the id is: 14 the obs is:0 [09:36:52.621] Init: px average disparity is:0.336525 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.533] Odometry x=0.000006, y=0.000037, theta=-0.041722 [09:36:52.621] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.619] Init: Homography RANSAC (inlier) is:0 ,While 30 inliers minimum required. px average disparity is:19.121424 ,While minimum is: 10.000000 KLT tracked: 109 [09:36:52.621] frame process finished the id is: 15 the obs is:0
[19:59:18.619] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.721] Init: px average disparity is:0.393741 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.619] frame process finished the id is: 15 the obs is:0 [09:36:52.721] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.631] Odometry x=0.000010, y=0.000047, theta=-0.082338 [09:36:52.721] frame process finished the id is: 16 the obs is:0
[19:59:18.716] Init: Homography RANSAC (inlier) is:1 ,While 30 inliers minimum required. px average disparity is:29.212896 ,While minimum is: 10.000000 KLT tracked: 109 [09:36:52.822] Init: px average disparity is:0.365818 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.716] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.822] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.716] frame process finished the id is: 16 the obs is:0 [09:36:52.822] frame process finished the id is: 17 the obs is:0
[19:59:18.733] Odometry x=0.000014, y=0.000051, theta=-0.116735 [09:36:52.922] Init: px average disparity is:0.362293 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.819] Init: Homography RANSAC (inlier) is:6 ,While 30 inliers minimum required. px average disparity is:40.866547 ,While minimum is: 10.000000 KLT tracked: 109 [09:36:52.922] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.819] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:52.922] frame process finished the id is: 18 the obs is:0
[19:59:18.819] frame process finished the id is: 17 the obs is:0 [09:36:53.021] Init: px average disparity is:0.361204 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.831] Odometry x=0.000012, y=0.000065, theta=-0.150406 [09:36:53.021] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.918] Init: Homography RANSAC (inlier) is:7 ,While 30 inliers minimum required. px average disparity is:51.065260 ,While minimum is: 10.000000 KLT tracked: 109 [09:36:53.021] frame process finished the id is: 19 the obs is:0
[19:59:18.918] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:53.120] Init: px average disparity is:0.348439 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:18.918] frame process finished the id is: 18 the obs is:0 [09:36:53.120] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:18.932] Odometry x=0.000032, y=0.000065, theta=-0.175625 [09:36:53.120] frame process finished the id is: 20 the obs is:0
[19:59:19.018] Init: Homography RANSAC (inlier) is:9 ,While 30 inliers minimum required. px average disparity is:58.238817 ,While minimum is: 10.000000 KLT tracked: 108 [09:36:53.221] Init: px average disparity is:0.350235 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.018] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:53.221] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.018] frame process finished the id is: 19 the obs is:0 [09:36:53.221] frame process finished the id is: 21 the obs is:0
[19:59:19.032] Odometry x=0.000037, y=0.000068, theta=-0.203385 [09:36:53.322] Init: px average disparity is:0.355781 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.118] Init: Homography RANSAC (inlier) is:9 ,While 30 inliers minimum required. px average disparity is:68.778966 ,While minimum is: 10.000000 KLT tracked: 106 [09:36:53.322] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.118] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:53.322] frame process finished the id is: 22 the obs is:0
[19:59:19.118] frame process finished the id is: 20 the obs is:0 [09:36:53.422] Init: px average disparity is:0.350137 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.133] Odometry x=0.000050, y=0.000075, theta=-0.234988 [09:36:53.422] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.218] Init: Homography RANSAC (inlier) is:23 ,While 30 inliers minimum required. px average disparity is:79.322258 ,While minimum is: 10.000000 KLT tracked: 105 [09:36:53.422] frame process finished the id is: 23 the obs is:0
[19:59:19.218] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 [09:36:53.521] Init: px average disparity is:0.351329 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.218] frame process finished the id is: 21 the obs is:0 [09:36:53.521] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.230] Odometry x=0.000058, y=0.000081, theta=-0.267405 [09:36:53.521] frame process finished the id is: 24 the obs is:0
[19:59:19.318] Init finished: Homography RANSAC (inlier) is:35 ,While 30 inliers minimum required. px average disparity is:90.575691 ,While minimum is: 10.000000 KLT tracked: 105 [09:36:53.621] Init: px average disparity is:0.309508 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.318] Init: distance between the first and current frame is x:0.000062 ,z=0.000083,angle between two frames: -0.296235 [09:36:53.621] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.318] Init: Selected Second frame. The number of features: 35 depth mean:-0.000641 min:-0.001309 [09:36:53.621] frame process finished the id is: 25 the obs is:0
[19:59:19.318] frame process finished the id is: 22 the obs is:35 [09:36:53.722] Init: px average disparity is:0.347856 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.330] Odometry x=0.000062, y=0.000083, theta=-0.296235 [09:36:53.722] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.434] Odometry x=0.000065, y=0.000086, theta=-0.324513 [09:36:53.722] frame process finished the id is: 26 the obs is:0
[19:59:19.531] Odometry x=0.000066, y=0.000090, theta=-0.357137 [09:36:53.821] Init: px average disparity is:0.323454 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.630] Odometry x=0.000075, y=0.000100, theta=-0.387188 [09:36:53.822] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[19:59:19.732] Odometry x=0.000075, y=0.000109, theta=-0.416785 [09:36:53.822] frame process finished the id is: 27 the obs is:0
[19:59:19.830] Odometry x=0.000082, y=0.000111, theta=-0.445093 [09:36:53.922] Init: px average disparity is:0.334123 ,While minimum is: 10.000000 KLT tracked : 1212
[19:59:19.932] Odomet [09:36:53.922] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:53.922] frame process finished the id is: 28 the obs is:0
[09:36:54.022] Init: px average disparity is:0.373803 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.022] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.022] frame process finished the id is: 29 the obs is:0
[09:36:54.121] Init: px average disparity is:0.323537 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.121] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.121] frame process finished the id is: 30 the obs is:0
[09:36:54.221] Init: px average disparity is:0.339853 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.221] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.221] frame process finished the id is: 31 the obs is:0
[09:36:54.322] Init: px average disparity is:0.355549 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.322] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.322] frame process finished the id is: 32 the obs is:0
[09:36:54.422] Init: px average disparity is:0.327988 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.422] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.422] frame process finished the id is: 33 the obs is:0
[09:36:54.522] Init: px average disparity is:0.334005 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.522] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.522] frame process finished the id is: 34 the obs is:0
[09:36:54.622] Init: px average disparity is:0.329450 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.622] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.622] frame process finished the id is: 35 the obs is:0
[09:36:54.721] Init: px average disparity is:0.353743 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.721] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.721] frame process finished the id is: 36 the obs is:0
[09:36:54.823] Init: px average disparity is:0.325713 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.823] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.823] frame process finished the id is: 37 the obs is:0
[09:36:54.925] Init: px average disparity is:0.336127 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:54.925] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:54.925] frame process finished the id is: 38 the obs is:0
[09:36:55.026] Init: px average disparity is:0.339370 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.026] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.026] frame process finished the id is: 39 the obs is:0
[09:36:55.128] Init: px average disparity is:0.341702 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.128] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.128] frame process finished the id is: 40 the obs is:0
[09:36:55.221] Init: px average disparity is:0.322163 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.221] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.221] frame process finished the id is: 41 the obs is:0
[09:36:55.320] Init: px average disparity is:0.327227 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.320] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.320] frame process finished the id is: 42 the obs is:0
[09:36:55.426] Init: px average disparity is:0.337917 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.426] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.426] frame process finished the id is: 43 the obs is:0
[09:36:55.521] Init: px average disparity is:0.347184 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.521] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.521] frame process finished the id is: 44 the obs is:0
[09:36:55.621] Init: px average disparity is:0.345335 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.621] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.621] frame process finished the id is: 45 the obs is:0
[09:36:55.727] Init: px average disparity is:0.349314 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.727] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.727] frame process finished the id is: 46 the obs is:0
[09:36:55.823] Init: px average disparity is:0.325428 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.823] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.823] frame process finished the id is: 47 the obs is:0
[09:36:55.923] Init: px average disparity is:0.333423 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:55.923] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:55.923] frame process finished the id is: 48 the obs is:0
[09:36:56.023] Init: px average disparity is:0.322207 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.023] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.023] frame process finished the id is: 49 the obs is:0
[09:36:56.123] Init: px average disparity is:0.332989 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.123] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.123] frame process finished the id is: 50 the obs is:0
[09:36:56.227] Init: px average disparity is:0.326103 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.227] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.227] frame process finished the id is: 51 the obs is:0
[09:36:56.328] Init: px average disparity is:0.338467 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.328] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.328] frame process finished the id is: 52 the obs is:0
[09:36:56.427] Init: px average disparity is:0.351026 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.427] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.427] frame process finished the id is: 53 the obs is:0
[09:36:56.527] Init: px average disparity is:0.344845 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.527] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.527] frame process finished the id is: 54 the obs is:0
[09:36:56.627] Init: px average disparity is:0.332508 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.627] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.627] frame process finished the id is: 55 the obs is:0
[09:36:56.730] Init: px average disparity is:0.340312 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.730] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.730] frame process finished the id is: 56 the obs is:0
[09:36:56.823] Init: px average disparity is:0.359988 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.823] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.823] frame process finished the id is: 57 the obs is:0
[09:36:56.924] Init: px average disparity is:0.342481 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:56.924] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:56.924] frame process finished the id is: 58 the obs is:0
[09:36:57.023] Init: px average disparity is:0.336252 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.024] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.024] frame process finished the id is: 59 the obs is:0
[09:36:57.122] Init: px average disparity is:0.332147 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.122] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.122] frame process finished the id is: 60 the obs is:0
[09:36:57.220] Init: px average disparity is:0.330074 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.220] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.220] frame process finished the id is: 61 the obs is:0
[09:36:57.319] Init: px average disparity is:0.344951 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.319] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.319] frame process finished the id is: 62 the obs is:0
[09:36:57.419] Init: px average disparity is:0.336023 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.419] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.419] frame process finished the id is: 63 the obs is:0
[09:36:57.519] Init: px average disparity is:0.336179 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.519] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.519] frame process finished the id is: 64 the obs is:0
[09:36:57.619] Init: px average disparity is:0.331562 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.619] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.619] frame process finished the id is: 65 the obs is:0
[09:36:57.719] Init: px average disparity is:0.343314 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.719] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.719] frame process finished the id is: 66 the obs is:0
[09:36:57.819] Init: px average disparity is:0.344914 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.819] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.819] frame process finished the id is: 67 the obs is:0
[09:36:57.922] Init: px average disparity is:0.343931 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:57.922] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:57.922] frame process finished the id is: 68 the obs is:0
[09:36:58.019] Init: px average disparity is:0.342843 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.019] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.019] frame process finished the id is: 69 the obs is:0
[09:36:58.120] Init: px average disparity is:0.319272 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.120] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.120] frame process finished the id is: 70 the obs is:0
[09:36:58.220] Init: px average disparity is:0.332929 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.220] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.220] frame process finished the id is: 71 the obs is:0
[09:36:58.320] Init: px average disparity is:0.335166 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.320] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.320] frame process finished the id is: 72 the obs is:0
[09:36:58.420] Init: px average disparity is:0.322458 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.420] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.420] frame process finished the id is: 73 the obs is:0
[09:36:58.520] Init: px average disparity is:0.333361 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.520] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.520] frame process finished the id is: 74 the obs is:0
[09:36:58.621] Init: px average disparity is:0.364620 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.621] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.621] frame process finished the id is: 75 the obs is:0
[09:36:58.720] Init: px average disparity is:0.361013 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.720] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.721] frame process finished the id is: 76 the obs is:0
[09:36:58.822] Init: px average disparity is:0.352700 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.822] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.822] frame process finished the id is: 77 the obs is:0
[09:36:58.921] Init: px average disparity is:0.342237 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:58.921] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:58.921] frame process finished the id is: 78 the obs is:0
[09:36:58.965] Odometry x=0.000000, y=0.000000, theta=-0.000000
[09:36:59.020] Init: px average disparity is:0.338066 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:59.020] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:59.020] frame process finished the id is: 79 the obs is:0
[09:36:59.063] Odometry x=0.000001, y=0.033330, theta=-0.000007
[09:36:59.120] Init: px average disparity is:0.342626 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:59.120] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:59.121] frame process finished the id is: 80 the obs is:0
[09:36:59.165] Odometry x=0.000005, y=0.062598, theta=0.000440
[09:36:59.226] Init: px average disparity is:6.551406 ,While minimum is: 10.000000 KLT tracked : 1212
[09:36:59.226] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000
[09:36:59.226] frame process finished the id is: 81 the obs is:0
[09:36:59.263] Odometry x=-0.000028, y=0.089868, theta=-0.006405
[09:36:59.319] Init: px average disparity is:3.524687 ,While minimum is: 10.000000 KLT tracked : 1212
GPU_version/vio/param/vo_fast.yaml
+2 -2
View File
@@ -4,12 +4,12 @@ svo:
loba_num_iter: 10 #Number of iterations in the local bundle adjustment. loba_num_iter: 10 #Number of iterations in the local bundle adjustment.
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad.
quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
init_min_tracked: 100 init_min_tracked: 120
init_min_disparity: 10 init_min_disparity: 10
structureoptim_max_pts: 50 structureoptim_max_pts: 50
kfselect_mindist: 0.01 kfselect_mindist: 0.01
poseoptim_thresh: 8.0 poseoptim_thresh: 8.0
init_min_inliers: 30 init_min_inliers: 50
ACC_noise: 0.5 #acc white noise in continuous in EKF ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF GYO_noise: 0.5 #gyro white noise in continuous in EKF
svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
GPU_version/vio/src/frame.cpp
-2
View File
@@ -178,8 +178,6 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
{ {
if((*it)->point != NULL) if((*it)->point != NULL)
{ {
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3().inverse()*(*it)->point->pos_));
if((e /= (1<<(*it)->level)).norm()>0.5)continue;
double z = frame->w2f((*it)->point->pos_).z(); double z = frame->w2f((*it)->point->pos_).z();
depth_vec.push_back(z); depth_vec.push_back(z);
depth_min = fmin(z, depth_min); depth_min = fmin(z, depth_min);
GPU_version/vio/src/frame_handler_mono.cpp
+1 -1
View File
@@ -286,7 +286,7 @@ bool FrameHandlerMono::needNewKf()
fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()); fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
#endif #endif
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.2 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true; if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.1 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true;
return false; return false;
} }
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){ void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
GPU_version/vio/src/matcher.cpp
+3 -11
View File
@@ -42,12 +42,9 @@ void getWarpMatrixAffine(
{ {
// Compute affine warp matrix A_ref_cur // Compute affine warp matrix A_ref_cur
const int halfpatch_size = 8; const int halfpatch_size = 8;
const Vector3d xyz_ref(f_ref*depth_ref);
Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref))); Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref))); Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2]; const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(f_ref)));
xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref))); const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref))); const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size; A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
@@ -81,11 +78,6 @@ bool warpAffine(
if(patch==NULL || img_ref.empty())return false; if(patch==NULL || img_ref.empty())return false;
const int patch_size = halfpatch_size*2 ; const int patch_size = halfpatch_size*2 ;
const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>(); const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
if(isnan(A_ref_cur(0,0)))
{
printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
return false;
}
// Perform the warp on a larger patch. // Perform the warp on a larger patch.
uint8_t* patch_ptr = patch; uint8_t* patch_ptr = patch;
const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref); const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
@@ -188,8 +180,8 @@ bool Matcher::findMatchDirect(
cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_, cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_,
options_.align_max_iter, px_scaled); options_.align_max_iter, px_scaled);
} }
debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px, /* debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px,
px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).norm()); px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).z());*/
px_cur = px_scaled; px_cur = px_scaled;
return success; return success;
} }
GPU_version/vio/src/math_utils.cpp
+22 -2
View File
@@ -7,6 +7,7 @@
#include <vio/math_utils.h> #include <vio/math_utils.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <Eigen/SVD>
namespace vk { namespace vk {
@@ -49,13 +50,32 @@ triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
Vector3d xm = lambda[0] * feature1; Vector3d xm = lambda[0] * feature1;
Vector3d xn = t + lambda[1] * R_F2; Vector3d xn = t + lambda[1] * R_F2;
return ( xm + xn )/2;*/ return ( xm + xn )/2;*/
Vector3d R_F2 = R * feature2;//T*A=R*A+t /* Vector3d R_F2 = R * feature2;//T*A=R*A+t
Vector3d z = feature1.cross(R_F2); Vector3d z = feature1.cross(R_F2);
Vector3d b0 = t.cross(feature1); Vector3d b0 = t.cross(feature1);
Vector3d b1 = t.cross(R_F2); Vector3d b1 = t.cross(R_F2);
Vector3d xm = ((z.dot(b1)/pow(z.norm(), 2))*(feature1)); Vector3d xm = ((z.dot(b1)/pow(z.norm(), 2))*(feature1));
Vector3d xn = t + ((z.dot(b0)/pow(z.norm(), 2))*R_F2); Vector3d xn = t + ((z.dot(b0)/pow(z.norm(), 2))*R_F2);
return ( xm + xn )/2;*/
Vector3d m0_hat = R * feature2;//T*A=R*A+t
Vector3d m1_hat = feature1;
Eigen::Matrix<double,3,2> x;
x<<m0_hat,m1_hat;
Eigen::Matrix<double,2,3> svd_in = x.transpose() * (Eigen::Matrix3d::Identity() - (t/t.norm())*(t/t.norm()).transpose());
Eigen::JacobiSVD<MatrixXd> svd( svd_in, ComputeFullU | ComputeFullV);
Vector3d n_p_hat = svd.matrixV().col(1);
Vector3d m0_p = m0_hat - (m0_hat.dot(n_p_hat)) * n_p_hat;
Vector3d m1_p = m1_hat - (m1_hat.dot(n_p_hat)) * n_p_hat;
Vector3d R_F2_p = m0_p;
Vector3d f1_p = m1_p;
Vector3d z = f1_p.cross(R_F2_p);
Vector3d b0 = t.cross(f1_p);
Vector3d b1 = t.cross(R_F2_p);
double lamda0 = z.dot(b0)/pow(z.norm(), 2);
double lamda1 = z.dot(b1)/pow(z.norm(), 2);
Vector3d xm = lamda1 * f1_p;
Vector3d xn = t + lamda0 * R_F2_p;
return ( xm + xn )/2; return ( xm + xn )/2;
} }
@@ -112,7 +132,7 @@ computeInliers(const vector<Vector3d>& features2, // c2
if(t.z()>0.0){ if(t.z()>0.0){
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*reference*/, features2[j]/*current*/ )); xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*reference*/, features2[j]/*current*/ ));
double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2); double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2); double e2 = reprojError(features2[j], SE3(R,t).inverse()*xyz_vec.back(), error_multiplier2);
if(e1 > reproj_thresh || e2 > reproj_thresh) if(e1 > reproj_thresh || e2 > reproj_thresh)
outliers.push_back(j); outliers.push_back(j);
else else
GPU_version/vio/src/reprojector.cpp
+12 -7
View File
@@ -133,12 +133,12 @@ namespace vio {
it_cur=keypoints.begin(); it_cur=keypoints.begin();
std::advance(it_cur,match.trainIdx); std::advance(it_cur,match.trainIdx);
if(!(*it_cur))continue; if(!(*it_cur))continue;
const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) * /* const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
grid_.grid_n_cols grid_.grid_n_cols
+ static_cast<int>((*it_cur)->px.x() / grid_.cell_size); + static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
if(k>grid_.cells.size()-1)continue; if(k>grid_.cells.size()-1)continue;
assert(grid_.cells.at(k) != nullptr); assert(grid_.cells.at(k) != nullptr);
if(grid_.cells.at(k)->size()> Config::gridSize()-1)continue; if(grid_.cells.at(k)->size()> Config::gridSize()-1)continue;*/
Vector2d px((int) (*it_cur)->px.x(), Vector2d px((int) (*it_cur)->px.x(),
(int) (*it_cur)->px.y()); (int) (*it_cur)->px.y());
if ((*it_ref)->point == NULL){ if ((*it_ref)->point == NULL){
@@ -148,6 +148,9 @@ namespace vio {
(*it_ref)->f,frame->c2f(px)); (*it_ref)->f,frame->c2f(px));
// point in world frame // point in world frame
std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref)); std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
if(!matcher_.findMatchDirect(*new_point, *frame, px))continue;
frame->addFeature(std::make_shared<Feature>(frame, frame->addFeature(std::make_shared<Feature>(frame,
new_point, new_point,
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor)); px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
@@ -155,31 +158,33 @@ namespace vio {
added_keypoints.push_back(match.trainIdx); added_keypoints.push_back(match.trainIdx);
frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
///TODO number of cell is not reflecting the number of point, check K value ///TODO number of cell is not reflecting the number of point, check K value
grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); //grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
overlap_kfs.back().second++; overlap_kfs.back().second++;
}else{ }else{
if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
(*it_ref)->point->last_frame_overlap_id_ = frame->id_; (*it_ref)->point->last_frame_overlap_id_ = frame->id_;
frame->addFeature(std::make_shared<Feature>(frame, frame->addFeature(std::make_shared<Feature>(frame,
(*it_ref)->point, (*it_ref)->point,
px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor)); px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
added_keypoints.push_back(match.trainIdx); added_keypoints.push_back(match.trainIdx);
///TODO number of cell is not reflecting the number of point, check K value ///TODO number of cell is not reflecting the number of point, check K value
grid_.cells.at(k)->push_back(Candidate((*it_ref)->point, px)); //grid_.cells.at(k)->push_back(Candidate((*it_ref)->point, px));
overlap_kfs.back().second++; overlap_kfs.back().second++;
} }
} }
++it_ref; ++it_ref;
} }
} }
{ /*{
for (auto&& cell : grid_.cells) { for (auto&& cell : grid_.cells) {
if (reprojectCell(*cell, frame, log_)) if (reprojectCell(*cell, frame, log_))
++n_matches_; ++n_matches_;
if (n_matches_ > (size_t) Config::maxFts()) if (n_matches_ > (size_t) Config::maxFts())
break; break;
} }
} }*/
exit(0); //exit(0);
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////