From 4781b38efdbdd018593e7f5821316f4c2fa607a2 Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 28 Sep 2021 19:07:43 +0200 Subject: [PATCH] debug freak matching --- GPU_version/vio/CMakeLists.txt | 1 - GPU_version/vio/depth_filter_log.txt | 0 GPU_version/vio/frame_handler_log.txt | 249 ++++++++++++++++++ GPU_version/vio/include/vio/feature.h | 25 ++ .../vio/include/vio/feature_detection.h | 6 +- GPU_version/vio/include/vio/matcher.h | 1 + GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/param/vo_fast.yaml | 4 +- GPU_version/vio/src/feature_alignment.cpp | 6 +- GPU_version/vio/src/feature_detection.cpp | 19 +- GPU_version/vio/src/frame_handler_mono.cpp | 2 +- GPU_version/vio/src/initialization.cpp | 11 +- GPU_version/vio/src/matcher.cpp | 2 +- GPU_version/vio/src/reprojector.cpp | 58 ++-- 14 files changed, 324 insertions(+), 62 deletions(-) create mode 100755 GPU_version/vio/depth_filter_log.txt create mode 100755 GPU_version/vio/frame_handler_log.txt diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index a79eba9..9cabae0 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -31,7 +31,6 @@ FIND_PACKAGE(OpenCL REQUIRED) FIND_PACKAGE(G2O REQUIRED) #FIND_PACKAGE(SuiteSparse) - FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation) add_service_files(FILES diff --git a/GPU_version/vio/depth_filter_log.txt b/GPU_version/vio/depth_filter_log.txt new file mode 100755 index 0000000..e69de29 diff --git a/GPU_version/vio/frame_handler_log.txt b/GPU_version/vio/frame_handler_log.txt new file mode 100755 index 0000000..a13e405 --- /dev/null +++ b/GPU_version/vio/frame_handler_log.txt @@ -0,0 +1,249 @@ +[19:03:41.830] frame process finished the id is: 0 the obs is:0 +[19:03:41.919] frame process finished the id is: 1 the obs is:0 +[19:03:42.009] frame process finished the id is: 2 the obs is:0 +[19:03:42.112] frame process finished the id is: 3 the obs is:0 +[19:03:42.209] frame process finished the id is: 4 the obs is:0 +[19:03:42.308] frame process finished the id is: 5 the obs is:0 +[19:03:42.408] frame process finished the id is: 6 the obs is:0 +[19:03:42.508] frame process finished the id is: 7 the obs is:0 +[19:03:42.607] frame process finished the id is: 8 the obs is:0 +[19:03:42.709] frame process finished the id is: 9 the obs is:0 +[19:03:42.746] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[19:03:42.815] frame process finished the id is: 10 the obs is:0 +[19:03:42.835] Odometry x=0.000000, y=0.000000, theta=-0.000000 +[19:03:42.908] frame process finished the id is: 11 the obs is:0 +[19:03:42.933] Odometry x=0.000000, y=0.000035, theta=-0.014739 +[19:03:43.008] frame process finished the id is: 12 the obs is:0 +[19:03:43.032] Odometry x=-0.000016, y=-0.000458, theta=-0.029702 +[19:03:43.108] frame process finished the id is: 13 the obs is:0 +[19:03:43.136] Odometry x=-0.000030, y=0.000053, theta=-0.040488 +[19:03:43.208] frame process finished the id is: 14 the obs is:0 +[19:03:43.234] Odometry x=-0.000002, y=0.000114, theta=-0.067076 +[19:03:43.308] frame process finished the id is: 15 the obs is:0 +[19:03:43.333] Odometry x=0.000002, y=0.000144, theta=-0.072754 +[19:03:43.408] frame process finished the id is: 16 the obs is:0 +[19:03:43.432] Odometry x=0.000199, y=0.000115, theta=-0.093481 +[19:03:43.517] frame process finished the id is: 17 the obs is:0 +[19:03:43.533] Odometry x=0.000439, y=0.000166, theta=-0.108529 +[19:03:43.608] frame process finished the id is: 18 the obs is:0 +[19:03:43.634] Odometry x=0.000517, y=0.000660, theta=-0.136896 +[19:03:43.708] frame process finished the id is: 19 the obs is:0 +[19:03:43.734] Odometry x=0.000513, y=0.000632, theta=-0.156055 +[19:03:43.808] frame process finished the id is: 20 the obs is:0 +[19:03:43.835] Odometry x=0.000528, y=0.000706, theta=-0.171952 +[19:03:43.909] frame process finished the id is: 21 the obs is:0 +[19:03:43.937] Odometry x=0.000633, y=0.001607, theta=-0.186130 +[19:03:44.010] frame process finished the id is: 22 the obs is:0 +[19:03:44.031] Odometry x=0.000712, y=0.001578, theta=-0.213187 +[19:03:44.113] frame process finished the id is: 23 the obs is:0 +[19:03:44.135] Odometry x=0.000723, y=0.001702, theta=-0.238404 +[19:03:44.210] Init: frame zero: 0.000000, 0.000000 0.000000 +[19:03:44.210] Init: Selected first frame. +[19:03:44.210] frame process finished the id is: 24 the obs is:0 +[19:03:44.229] Odometry x=0.000725, y=0.001732, theta=-0.250716 +[19:03:44.310] Init: Homography RANSAC (inlier) is:20 ,While 30 inliers minimum required. px average disparity is:14.526801 ,While minimum is: 10.000000 KLT tracked: 128 +[19:03:44.310] Init: distance between the first and current frame is x:0.000000 ,z=0.000000,angle between two frames: 0.000000 +[19:03:44.310] frame process finished the id is: 25 the obs is:0 +[19:03:44.332] Odometry x=0.000949, y=0.001734, theta=-0.279430 +[19:03:44.408] Init finished: Homography RANSAC (inlier) is:32 ,While 30 inliers minimum required. px average disparity is:26.256217 ,While minimum is: 10.000000 KLT tracked: 128 +[19:03:44.408] Init: distance between the first and current frame is x:0.000966 ,z=0.001920,angle between two frames: -0.296072 +[19:03:44.408] Init: Selected Second frame. The number of features: 32 depth mean:0.005307 min:0.002690 +[19:03:44.408] frame process finished the id is: 26 the obs is:32 +[19:03:44.435] Odometry x=0.000966, y=0.001920, theta=-0.296072 +[19:03:44.531] Odometry x=0.000975, y=0.001961, theta=-0.317395 +[19:03:44.633] Odometry x=0.001046, y=0.002359, theta=-0.336592 +[19:03:44.732] Odometry x=0.001104, y=0.002685, theta=-0.355629 +[19:03:44.834] Odometry x=0.001159, y=0.002986, theta=-0.377720 +[19:03:44.932] Odometry x=0.001584, y=0.002417, theta=-0.401479 +[19:03:45.032] Odometry x=0.001610, y=0.002432, theta=-0.416364 +[19:03:45.132] Odometry x=0.001706, y=0.002866, theta=-0.436506 +[19:03:45.232] Odometry x=0.002272, y=0.002762, theta=-0.461576 +[19:03:45.334] Odometry x=0.002354, y=0.003092, theta=-0.480614 +[19:03:45.434] Odometry x=0.002441, y=0.003028, theta=-0.500689 +[19:03:45.536] Odometry x=0.002518, y=0.002993, theta=-0.523179 +[19:03:45.636] Odometry x=0.002620, y=0.003166, theta=-0.544427 +[19:03:45.733] Odometry x=0.002642, y=0.003191, theta=-0.560905 +[19:03:45.833] Odometry x=0.002756, y=0.003916, theta=-0.581583 +[19:03:45.934] Odometry x=0.002788, y=0.004004, theta=-0.600291 +[19:03:46.034] Odometry x=0.003736, y=0.003123, theta=-0.625975 +[19:03:46.132] Odometry x=0.003734, y=0.003124, theta=-0.638540 +[19:03:46.234] Odometry x=0.003737, y=0.003125, theta=-0.656723 +[19:03:46.328] Odometry x=0.003808, y=0.003306, theta=-0.679839 +[19:03:46.424] Odometry x=0.003845, y=0.003391, theta=-0.693880 +[19:03:46.526] Odometry x=0.004147, y=0.004122, theta=-0.715479 +[19:03:46.628] Odometry x=0.004188, y=0.004159, theta=-0.740028 +[19:03:46.734] Odometry x=0.004320, y=0.004462, theta=-0.751417 +[19:03:46.828] Odometry x=0.004689, y=0.004159, theta=-0.776666 +[19:03:46.925] Odometry x=0.004845, y=0.003863, theta=-0.798853 +[19:03:47.025] Odometry x=0.004982, y=0.003988, theta=-0.811391 +[19:03:47.131] Odometry x=0.004986, y=0.003990, theta=-0.826543 +[19:03:47.227] Odometry x=0.005416, y=0.004450, theta=-0.859956 +[19:03:47.325] Odometry x=0.005503, y=0.004525, theta=-0.880350 +[19:03:47.425] Odometry x=0.005598, y=0.004628, theta=-0.899165 +[19:03:47.529] Odometry x=0.005898, y=0.004681, theta=-0.916101 +[19:03:47.629] Odometry x=0.006133, y=0.004716, theta=-0.938758 +[19:03:47.729] Odometry x=0.006647, y=0.005388, theta=-0.960163 +[19:03:47.829] Odometry x=0.006661, y=0.005367, theta=-0.980779 +[19:03:47.925] Odometry x=0.006784, y=0.005512, theta=-0.998739 +[19:03:48.028] Odometry x=0.007789, y=0.006762, theta=-1.014226 +[19:03:48.130] Odometry x=0.008178, y=0.007550, theta=-1.031749 +[19:03:48.225] Odometry x=0.008186, y=0.007549, theta=-1.050436 +[19:03:48.325] Odometry x=0.008208, y=0.007527, theta=-1.070095 +[19:03:48.425] Odometry x=0.008229, y=0.007454, theta=-1.092333 +[19:03:48.525] Odometry x=0.008394, y=0.007627, theta=-1.112394 +[19:03:48.626] Odometry x=0.008490, y=0.007914, theta=-1.127474 +[19:03:48.727] Odometry x=0.008507, y=0.007920, theta=-1.151183 +[19:03:48.829] Odometry x=0.008551, y=0.007974, theta=-1.176545 +[19:03:48.935] Odometry x=0.008610, y=0.007789, theta=-1.193454 +[19:03:49.033] Odometry x=0.008623, y=0.007737, theta=-1.208680 +[19:03:49.129] Odometry x=0.008761, y=0.007959, theta=-1.235255 +[19:03:49.231] Odometry x=0.008825, y=0.007868, theta=-1.257892 +[19:03:49.335] Odometry x=0.008851, y=0.007808, theta=-1.286898 +[19:03:49.428] Odometry x=0.008761, y=0.007739, theta=-1.287849 +[19:03:49.529] Odometry x=0.008803, y=0.007764, theta=-1.307717 +[19:03:49.631] Odometry x=0.009279, y=0.007995, theta=-1.341373 +[19:03:49.726] Odometry x=0.009278, y=0.007995, theta=-1.345607 +[19:03:49.825] Odometry x=0.009493, y=0.006214, theta=-1.377549 +[19:03:49.926] Odometry x=0.009491, y=0.006213, theta=-1.387627 +[19:03:50.030] Odometry x=0.009506, y=0.006066, theta=-1.422065 +[19:03:50.132] Odometry x=0.009511, y=0.005999, theta=-1.442972 +[19:03:50.229] Odometry x=0.009511, y=0.005998, theta=-1.460928 +[19:03:50.326] Odometry x=0.009603, y=0.004593, theta=-1.490979 +[19:03:50.431] Odometry x=0.009603, y=0.004587, theta=-1.485082 +[19:03:50.529] Odometry x=0.009655, y=0.004310, theta=-1.508717 +[19:03:50.626] Odometry x=0.009730, y=0.004140, theta=-1.528381 +[19:03:50.731] Odometry x=0.010140, y=0.002926, theta=-1.551244 +[19:03:50.826] Odometry x=0.010168, y=0.002738, theta=-1.567563 +[19:03:50.931] Odometry x=0.011157, y=0.001043, theta=-1.591306 +[19:03:51.028] Odometry x=0.011442, y=0.000775, theta=-1.625367 +[19:03:51.131] Odometry x=0.011494, y=0.000718, theta=-1.635808 +[19:03:51.228] Odometry x=0.011406, y=0.000731, theta=-1.646737 +[19:03:51.326] Odometry x=0.011459, y=0.000726, theta=-1.664412 +[19:03:51.431] Odometry x=0.011455, y=0.000553, theta=-1.687280 +[19:03:51.527] Odometry x=0.011507, y=0.000545, theta=-1.704368 +[19:03:51.627] Odometry x=0.011572, y=0.000527, theta=-1.726423 +[19:03:51.736] Odometry x=0.011790, y=0.000435, theta=-1.747009 +[19:03:51.829] Odometry x=0.011833, y=0.000382, theta=-1.773826 +[19:03:51.930] Odometry x=0.012100, y=0.000249, theta=-1.789493 +[19:03:52.032] Odometry x=0.012885, y=-0.000437, theta=-1.809109 +[19:03:52.133] Odometry x=0.012946, y=-0.002033, theta=-1.831527 +[19:03:52.232] Odometry x=0.012954, y=-0.002035, theta=-1.846979 +[19:03:52.330] Odometry x=0.012956, y=-0.002037, theta=-1.868389 +[19:03:52.426] Odometry x=0.013007, y=-0.001713, theta=-1.885884 +[19:03:52.533] Odometry x=0.013007, y=-0.001713, theta=-1.927876 +[19:03:52.630] Odometry x=0.013023, y=-0.002070, theta=-1.928569 +[19:03:52.726] Odometry x=0.012664, y=-0.001853, theta=-1.950556 +[19:03:52.832] Odometry x=0.012648, y=-0.001941, theta=-1.971663 +[19:03:52.934] Odometry x=0.012652, y=-0.001967, theta=-1.992112 +[19:03:53.031] Odometry x=0.012734, y=-0.002040, theta=-2.013194 +[19:03:53.134] Odometry x=0.013053, y=-0.002340, theta=-2.041808 +[19:03:53.231] Odometry x=0.012848, y=-0.002407, theta=-2.055036 +[19:03:53.334] Odometry x=0.012812, y=-0.003671, theta=-2.075133 +[19:03:53.431] Odometry x=0.012815, y=-0.003675, theta=-2.094489 +[19:03:53.534] Odometry x=0.013023, y=-0.003912, theta=-2.118870 +[19:03:53.631] Odometry x=0.013076, y=-0.003965, theta=-2.134809 +[19:03:53.726] Odometry x=0.013136, y=-0.004042, theta=-2.154602 +[19:03:53.833] Odometry x=0.013188, y=-0.004123, theta=-2.171104 +[19:03:53.930] Odometry x=0.012887, y=-0.005014, theta=-2.198842 +[19:03:54.031] Odometry x=0.012882, y=-0.005036, theta=-2.222160 +[19:03:54.131] Odometry x=0.012881, y=-0.005038, theta=-2.238859 +[19:03:54.226] Odometry x=0.012881, y=-0.005039, theta=-2.256346 +[19:03:54.332] Odometry x=0.012910, y=-0.005085, theta=-2.278863 +[19:03:54.432] Odometry x=0.012609, y=-0.005084, theta=-2.295296 +[19:03:54.536] Odometry x=0.012549, y=-0.006042, theta=-2.325994 +[19:03:54.632] Odometry x=0.012478, y=-0.006188, theta=-2.336751 +[19:03:54.733] Odometry x=0.012664, y=-0.005981, theta=-2.362241 +[19:03:54.835] Odometry x=0.012083, y=-0.009130, theta=-2.380840 +[19:03:54.931] Odometry x=0.012084, y=-0.009157, theta=-2.401395 +[19:03:55.026] Odometry x=0.010170, y=-0.012687, theta=-2.426134 +[19:03:55.131] Odometry x=0.009932, y=-0.013095, theta=-2.441288 +[19:03:55.234] Odometry x=0.009926, y=-0.013179, theta=-2.459024 +[19:03:55.335] Odometry x=0.008940, y=-0.014067, theta=-2.480464 +[19:03:55.425] Odometry x=0.008955, y=-0.014154, theta=-2.502435 +[19:03:55.532] Odometry x=0.008929, y=-0.014190, theta=-2.519539 +[19:03:55.628] Odometry x=0.009107, y=-0.014691, theta=-2.536558 +[19:03:55.727] Odometry x=0.008473, y=-0.014609, theta=-2.561450 +[19:03:55.834] Odometry x=0.008448, y=-0.014656, theta=-2.581408 +[19:03:55.930] Odometry x=0.008504, y=-0.014871, theta=-2.607664 +[19:03:56.031] Odometry x=0.008514, y=-0.014904, theta=-2.628230 +[19:03:56.127] Odometry x=0.008498, y=-0.014898, theta=-2.644026 +[19:03:56.234] Odometry x=0.008660, y=-0.015495, theta=-2.665277 +[19:03:56.336] Odometry x=0.008834, y=-0.015391, theta=-2.674092 +[19:03:56.433] Odometry x=0.008803, y=-0.015431, theta=-2.710954 +[19:03:56.533] Odometry x=0.008780, y=-0.015329, theta=-2.722390 +[19:03:56.635] Odometry x=0.008781, y=-0.015351, theta=-2.737733 +[19:03:56.729] Odometry x=0.008777, y=-0.015355, theta=-2.763386 +[19:03:56.836] Odometry x=0.008776, y=-0.015351, theta=-2.784200 +[19:03:56.927] Odometry x=0.008822, y=-0.015319, theta=-2.801707 +[19:03:57.030] Odometry x=0.008791, y=-0.015302, theta=-2.824509 +[19:03:57.132] Odometry x=0.008785, y=-0.015290, theta=-2.839928 +[19:03:57.230] Odometry x=0.008830, y=-0.015827, theta=-2.860562 +[19:03:57.329] Odometry x=0.008738, y=-0.015895, theta=-2.881298 +[19:03:57.429] Odometry x=0.008739, y=-0.015897, theta=-2.902525 +[19:03:57.527] Odometry x=0.008733, y=-0.015907, theta=-2.917228 +[19:03:57.629] Odometry x=0.008718, y=-0.015918, theta=-2.943145 +[19:03:57.736] Odometry x=0.008713, y=-0.015920, theta=-2.964944 +[19:03:57.833] Odometry x=0.007364, y=-0.016168, theta=-2.982891 +[19:03:57.933] Odometry x=0.007335, y=-0.016174, theta=-2.998146 +[19:03:58.029] Odometry x=0.007346, y=-0.016186, theta=-3.024654 +[19:03:58.132] Odometry x=0.007419, y=-0.017356, theta=-3.039007 +[19:03:58.230] Odometry x=0.007430, y=-0.017456, theta=-3.059337 +[19:03:58.331] Odometry x=0.007551, y=-0.017429, theta=-3.078037 +[19:03:58.433] Odometry x=0.007552, y=-0.017453, theta=-3.096738 +[19:03:58.530] Odometry x=0.007567, y=-0.018106, theta=-3.118657 +[19:03:58.631] Odometry x=0.007568, y=-0.018106, theta=-3.137664 +[19:03:58.731] Odometry x=0.007570, y=-0.018102, theta=3.127204 +[19:03:58.826] Odometry x=0.007495, y=-0.017425, theta=3.104639 +[19:03:58.926] Odometry x=0.007501, y=-0.017425, theta=3.089735 +[19:03:59.028] Odometry x=0.007501, y=-0.017424, theta=3.067425 +[19:03:59.128] Odometry x=0.007502, y=-0.017425, theta=3.048360 +[19:03:59.224] Odometry x=0.007499, y=-0.017488, theta=3.027172 +[19:03:59.328] Odometry x=0.007504, y=-0.017553, theta=3.010471 +[19:03:59.435] Odometry x=0.007496, y=-0.017542, theta=2.991329 +[19:03:59.528] Odometry x=0.008968, y=-0.017968, theta=2.962129 +[19:03:59.629] Odometry x=0.008960, y=-0.018059, theta=2.942655 +[19:03:59.725] Odometry x=0.008926, y=-0.018060, theta=2.931529 +[19:03:59.833] Odometry x=0.008942, y=-0.018064, theta=2.908479 +[19:03:59.929] Odometry x=0.008928, y=-0.018184, theta=2.888073 +[19:04:00.025] Odometry x=0.008920, y=-0.018240, theta=2.869678 +[19:04:00.126] Odometry x=0.008974, y=-0.018269, theta=2.843394 +[19:04:00.231] Odometry x=0.009053, y=-0.018319, theta=2.827975 +[19:04:00.330] Odometry x=0.008876, y=-0.019114, theta=2.809267 +[19:04:00.428] Odometry x=0.008893, y=-0.019109, theta=2.784339 +[19:04:00.531] Odometry x=0.009001, y=-0.019189, theta=2.761568 +[19:04:00.632] Odometry x=0.009011, y=-0.019196, theta=2.739901 +[19:04:00.728] Odometry x=0.009014, y=-0.019204, theta=2.720543 +[19:04:00.835] Odometry x=0.008974, y=-0.019382, theta=2.706724 +[19:04:00.932] Odometry x=0.008849, y=-0.019886, theta=2.688520 +[19:04:01.031] Odometry x=0.008965, y=-0.019993, theta=2.667348 +[19:04:01.131] Odometry x=0.008965, y=-0.019992, theta=2.644108 +[19:04:01.228] Odometry x=0.008959, y=-0.020017, theta=2.627208 +[19:04:01.335] Odometry x=0.008941, y=-0.020082, theta=2.604776 +[19:04:01.436] Odometry x=0.008923, y=-0.020110, theta=2.587249 +[19:04:01.532] Odometry x=0.008963, y=-0.019989, theta=2.575068 +[19:04:01.634] Odometry x=0.008968, y=-0.019995, theta=2.551009 +[19:04:01.737] Odometry x=0.009010, y=-0.020033, theta=2.537524 +[19:04:01.833] Odometry x=0.008602, y=-0.019470, theta=2.514726 +[19:04:01.933] Odometry x=0.008673, y=-0.019627, theta=2.488872 +[19:04:02.032] Odometry x=0.008504, y=-0.019354, theta=2.471651 +[19:04:02.131] Odometry x=0.008467, y=-0.019444, theta=2.455800 +[19:04:02.228] Odometry x=0.008463, y=-0.019448, theta=2.434341 +[19:04:02.336] Odometry x=0.008491, y=-0.019426, theta=2.412975 +[19:04:02.436] Odometry x=0.008141, y=-0.020016, theta=2.391785 +[19:04:02.532] Odometry x=0.008128, y=-0.020043, theta=2.371354 +[19:04:02.637] Odometry x=0.008126, y=-0.020038, theta=2.350333 +[19:04:02.730] Odometry x=0.008055, y=-0.020178, theta=2.334459 +[19:04:02.834] Odometry x=0.007819, y=-0.021943, theta=2.313763 +[19:04:02.932] Odometry x=0.007909, y=-0.022144, theta=2.292213 +[19:04:03.027] Odometry x=0.007905, y=-0.022139, theta=2.275083 +[19:04:03.134] Odometry x=0.007509, y=-0.022709, theta=2.254945 +[19:04:03.230] Odometry x=0.007436, y=-0.022965, theta=2.241917 +[19:04:03.330] Odometry x=0.007229, y=-0.023300, theta=2.215787 +[19:04:03.431] Odometry x=0.007214, y=-0.023319, theta=2.196995 +[19:04:03.526] Odometry x=0.007153, y=-0.023402, theta=2.180305 +[19:04:03.627] Odometry x=0.007195, y=-0.023549, theta=2.158489 +[19:04:03.731] Odometry x=0.007050, y=-0.023255, theta=2.137535 +[19:04:03.835] Odometry x=0.006992, y=-0.023071, theta=2.117542 +[19:04:03.930] Odometry x=0.006981, y=-0.023039, theta=2.098960 +[19:04:04.031] Odometry x=0.006274, y=-0.020648, theta=2.077011 +[19:04:04.126] Odometry \ No newline at end of file diff --git a/GPU_version/vio/include/vio/feature.h b/GPU_version/vio/include/vio/feature.h index 829a11a..35dac2e 100644 --- a/GPU_version/vio/include/vio/feature.h +++ b/GPU_version/vio/include/vio/feature.h @@ -38,6 +38,8 @@ struct Feature int level; //!< Image pyramid level where feature was extracted. std::shared_ptr point; //!< Pointer to 3D point which corresponds to the feature. Vector2d grad; //!< Dominant gradient direction for edglets, normalized. + float score=0.0; + uint8_t descriptor[64]={0}; //!< descriptor of the feature in the frame in which the feature was detected. Feature(std::shared_ptr _frame, const Vector2d& _px, int _level) : type(CORNER), @@ -66,6 +68,29 @@ struct Feature point(_point), grad(1.0,0.0) {} + Feature(std::shared_ptr _frame, std::shared_ptr _point, const Vector2d& _px, const Vector3d& _f, const float _score ,int _level,uint8_t* _descriptor) : + type(CORNER), + frame(_frame), + px(_px), + f(_f), + level(_level), + point(_point), + score(_score), + grad(1.0,0.0) + { + memcpy(descriptor,_descriptor, sizeof(uint8_t)*64); + } + Feature(std::shared_ptr _frame, const Vector2d& _px, const float _score ,int _level,uint8_t* _descriptor) : + type(CORNER), + frame(_frame), + px(_px), + f(frame->cam_->cam2world(px)), + level(_level), + score(_score), + grad(1.0,0.0) + { + memcpy(descriptor,_descriptor, sizeof(uint8_t)*64); + } }; } // namespace vio diff --git a/GPU_version/vio/include/vio/feature_detection.h b/GPU_version/vio/include/vio/feature_detection.h index f53d3dd..f60cd64 100644 --- a/GPU_version/vio/include/vio/feature_detection.h +++ b/GPU_version/vio/include/vio/feature_detection.h @@ -60,8 +60,7 @@ public: std::shared_ptr frame, const ImgPyr& img_pyr, const double detection_threshold, - Features& fts, - cv::Mat* descriptors= nullptr) = 0; + Features& fts) = 0; /// Flag the grid cell as occupied void setGridOccpuancy(const Vector2d& px); @@ -104,8 +103,7 @@ public: std::shared_ptr frame, const ImgPyr& img_pyr, const double detection_threshold, - list>& fts, - cv::Mat* descriptors= nullptr); + list>& fts); opencl* gpu_fast_; }; diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h index e73e49e..1ede992 100644 --- a/GPU_version/vio/include/vio/matcher.h +++ b/GPU_version/vio/include/vio/matcher.h @@ -126,6 +126,7 @@ public: uint i=0; void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res){ cv::Mat Ref,Cur; + if(!res)return; cv::cvtColor(cur,Cur,cv::COLOR_GRAY2RGB); cv::cvtColor(ref,Ref,cv::COLOR_GRAY2RGB); cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2); diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index d12f08d..7c4cbdf 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -1,6 +1,6 @@ - + diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index e2597ef..534bda8 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -5,11 +5,11 @@ svo: quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. init_min_tracked: 100 - init_min_disparity: 20 + init_min_disparity: 10 structureoptim_max_pts: 50 kfselect_mindist: 0.01 poseoptim_thresh: 8.0 - init_min_inliers: 25 + init_min_inliers: 30 ACC_noise: 0.5 #acc white noise in continuous in EKF GYO_noise: 0.5 #gyro white noise in continuous in EKF svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 43dc2f5..caf03c6 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -202,7 +202,7 @@ bool align2D( bool sign=true;// + // termination condition - const float min_update_squared = 0.001;//0.001 + const float min_update_squared = 0.01;//0.001 const int cur_step = cur_img.step.p[0]; // float chi2 = 0; Vector3f update; update.setZero(); @@ -337,7 +337,7 @@ bool align2D_SSE2( float v = cur_px_estimate.y(); // termination condition - const float min_update_squared = 0.001; + const float min_update_squared = 0.01; const int cur_step = cur_img.step.p[0]; const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16 float chi2 = 0; @@ -502,7 +502,7 @@ bool align2D_NEON ( float v = cur_px_estimate.y(); // termination condition - const float min_update_squared = 0.001; + const float min_update_squared = 0.01; const int cur_step = cur_img.step.p[0]; Vector3f update; Vector3f Jres; diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index b3ba690..a3035f8 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/GPU_version/vio/src/feature_detection.cpp @@ -17,6 +17,7 @@ #include #include #include +#include namespace vio { namespace feature_detection { @@ -69,8 +70,7 @@ void FastDetector::detect( std::shared_ptr frame, const ImgPyr& img_pyr, const double detection_threshold, - list>& fts, - cv::Mat* descriptors){ + list>& fts){ Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f)); std::vector keypoints; fts.clear(); @@ -91,7 +91,7 @@ void FastDetector::detect( cl_int count[1]={0}; gpu_fast_->read(0,2,1,count); cl_int2 fast_corners[350000]={0}; - gpu_fast_->read(0,1,count[0],fast_corners); + gpu_fast_->read(0,1,350000,fast_corners); if(count[0]<1 ){ ROS_ERROR("Can not communicate with GPU"); exit(0); @@ -110,19 +110,18 @@ void FastDetector::detect( const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y); if(score > corners.at(k).score){ corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f); - fts.push_back(make_shared(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level)); - if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7)); + keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score)); } } } - if(fts.size()<1){ + if(keypoints.size()<1){ ROS_ERROR("GPU Driver crash try again!"); assert(false); } - if(descriptors){ - cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 500); - extractor->compute(frame->img(), keypoints, *descriptors); - } + cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 50); + cv::Mat descriptor; + extractor->compute(frame->img(), keypoints, descriptor); + for(auto&& p:_for(keypoints))fts.push_back(make_shared(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64))); resetGrid(); }; diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index df6b6fb..8d6e8cb 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -168,7 +168,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() #endif ROS_INFO("VIO initialized :)"); ROS_INFO("Running ..."); - ba_glob_->new_key_frame(); + ba_glob_->new_key_frame(); return RESULT_IS_KEYFRAME; } diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index 7896f7c..a5e8a2a 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -91,19 +91,14 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) if(frame_cur->cam_->isInFrame(Vector2d(px_cur_.at(id).x,px_cur_.at(id).y).cast(), 10) && frame_ref_->cam_->isInFrame((*first)->px.cast(), 10)){ Vector3d pos = xyz_in_cur_.at(id);//frame_cur->se3() * (xyz_in_cur_.at(id)/* *scale */); -/*#if VIO_DEBUG - fprintf(log_,"[%s] inlier: x=%f, y=%f, z=%f pos: x=%f y=%f z=%f\n", - vio::time_in_HH_MM_SS_MMM().c_str(), - xyz_in_cur_.at(id).x(),xyz_in_cur_.at(id).y(),xyz_in_cur_.at(id).z(), - pos.x(),pos.y(),pos.z()); -#endif*/ + std::shared_ptr new_point = std::make_shared(pos); std::shared_ptr ftr_cur=std::make_shared(frame_cur, new_point, Vector2d(px_cur_.at(id).x,px_cur_.at(id).y), - frame_cur->c2f(px_cur_.at(id).x,px_cur_.at(id).y), (*first)->level); + frame_cur->c2f(px_cur_.at(id).x,px_cur_.at(id).y), (*first)->score,(*first)->level,(*first)->descriptor); frame_cur->addFeature(ftr_cur); new_point->addFrameRef(ftr_cur); - std::shared_ptr ftr_ref=std::make_shared(frame_ref_, new_point, (*first)->px, (*first)->f, (*first)->level); + std::shared_ptr ftr_ref=std::make_shared(frame_ref_, new_point, (*first)->px, (*first)->f, (*first)->score,(*first)->level,(*first)->descriptor); frame_ref_->addFeature(ftr_ref); new_point->addFrameRef(ftr_ref); } diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 6e99ee9..791ee77 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -184,7 +184,7 @@ bool Matcher::findMatchDirect( cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, options_.align_max_iter, px_scaled); } - //debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[search_level_],ref_ftr_->px,px_cur,px_scaled * (1<frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[search_level_],ref_ftr_->px,px_cur,px_scaled * (1<id_<1)return; resetGrid(); - cv::Mat descriptors_cur; - Features keypoints_cur; - cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 500); - cv::Ptr matcher = cv::DescriptorMatcher::create("BruteForce"); + Features keypoints; + cv::Ptr matcher = cv::BFMatcher::create(cv::NORM_HAMMING2,false); feature_detection::FastDetector detector( frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels()); - detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur); + detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints); list > close_kfs; map_.getCloseKeyframes(frame, close_kfs); if (!last_frame->fts_.empty()) @@ -91,27 +89,35 @@ namespace vio { for (auto &&it_frame:_for(close_kfs)) { if (it_frame.index > options_.max_n_kfs)continue; std::vector keypoints_kfs; - std::vector matches; - cv::Mat descriptors_ref; + std::vector keypoints_cur; + std::vector matches; overlap_kfs.push_back(pair(it_frame.item.first, 0)); - for (auto &&it_ftr:it_frame.item.first->fts_) { - keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 1)); + cv::Mat ref_des=cv::Mat(it_frame.item.first->fts_.size(),64,CV_8UC1); + for (auto &&it_ftr:_for(it_frame.item.first->fts_)) { + keypoints_kfs.push_back(cv::KeyPoint(it_ftr.item->px.x(), it_ftr.item->px.y(), 7.f,it_ftr.item->score)); + memcpy(ref_des.data+(it_ftr.index*64),it_ftr.item->descriptor,sizeof(uint8_t)*64); } - std::vector keypoints_cu; - for (auto &&f:keypoints_cur) { - keypoints_cu.push_back(cv::KeyPoint(f->px.x(), f->px.y(), 1)); + cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1); + for (auto &&f:_for(keypoints)) { + keypoints_cur.push_back(cv::KeyPoint(f.item->px.x(), f.item->px.y(), 7.f,f.item->score)); + memcpy(cur_des.data+(f.index*64),f.item->descriptor,sizeof(uint8_t)*64); } - extractor->compute(it_frame.item.first->img_pyr_[0], keypoints_kfs, descriptors_ref); - matcher->match(descriptors_ref, descriptors_cur, matches); - cv::Mat imgMatch; - cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cu,matches, imgMatch); + matcher->match(ref_des, cur_des, matches); +/* cv::Mat imgMatch,key_point_ref,key_point_cur; + cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cur,matches, imgMatch); + cv::drawKeypoints(it_frame.item.first->img_pyr_[0],keypoints_kfs,key_point_ref); + cv::drawKeypoints(frame->img_pyr_[0],keypoints_cur,key_point_cur); cv::imshow("img",imgMatch); + cv::imshow("key_point_ref",key_point_ref); + cv::imshow("key_point_current",key_point_cur); cv::waitKey(); exit(0); + std::cerr<fts_.size()<<","<fts_)) { - for (auto &&match:matches) { + for (auto &&match:matches.at(f.index)) { if (match.queryIdx != f.index)continue; if(keypoints_cur.size() < match.trainIdx)continue; + if(match.distance > 30.0 )continue; list>::iterator point=keypoints_cur.begin(); std::advance(point,match.trainIdx); if(!(*point))continue; @@ -126,14 +132,14 @@ namespace vio { SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3(); Vector3d new_point=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(), frame->c2f(px),f.item->f); - if(new_point.z()<0.0)break; + if(new_point.z()<0.0)continue; frame->fts_.push_back(std::make_shared(it_frame.item.first, std::make_shared(it_frame.item.first->se3()*new_point,f.item), px,f.item->f,(*point)->level)); frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); overlap_kfs.back().second++; - break; + //break; }else{ if (f.item->point->last_frame_overlap_id_ == frame->id_)continue; f.item->point->last_frame_overlap_id_ = frame->id_; @@ -146,28 +152,19 @@ namespace vio { (int) (*point)->px.y()); grid_.cells.at(k)->push_back(Candidate(f.item->point, px)); overlap_kfs.back().second++; - break; + //break; } } } } { - int cl = 0; for (auto&& cell : grid_.cells) { -#if VIO_DEBUG - if(cell->size()>=1) { - fprintf(log_, "[%s] %d-%d/%d : \n", vio::time_in_HH_MM_SS_MMM().c_str(), frame->id_, ++cl, - grid_.cells.size()); - } -#endif - //std::cerr<id_<<"-"<<++cl<<"/"< (size_t) Config::maxFts()) break; } } - //std::exit(154); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -178,7 +175,6 @@ namespace vio { } bool Reprojector::reprojectCell(Cell &cell, FramePtr frame, FILE* log_) { - //cv::Mat image = frame->img(); if(cell.size()<1){ return false; } @@ -201,7 +197,7 @@ namespace vio { //cv::circle(image, pointb,1,(255, 0, 0), 1); Vector2d uvb(it->px.x(), it->px.y()); #if VIO_DEBUG - fprintf(log_,"befor : %f %f after : ", it->px.x(), it->px.y()); + fprintf(log_,"before : %f %f after : ", it->px.x(), it->px.y()); #endif //std::cerr<<"before : "<px.x()<<" "<px.y()<<" after : "; bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);