1 files changed, 11598 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/localization/data/GroundData/log1_pose.txt b/SD-VBS/benchmarks/localization/data/GroundData/log1_pose.txt
new file mode 100755
index 0000000..b14576a
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/data/GroundData/log1_pose.txt
@@ -0,0 +1,11598 @@
+1116844391.527211 0.000651 0.001841 1.611056 4422625.792709 483672.630756 -33.995671
+1116844391.577349 -0.000456 0.002267 1.947426 4422625.792709 483672.630756 -33.995671
+1116844391.627188 -0.000622 0.002630 1.934604 4422625.792709 483672.630756 -33.995673
+1116844391.677171 0.000059 0.003221 1.824546 4422625.792709 483672.630756 -33.995673
+1116844391.727197 -0.000313 0.003639 1.830604 4422625.792709 483672.630756 -33.995673
+1116844391.777182 0.000393 0.004790 1.739825 4422625.792709 483672.630756 -33.995673
+1116844391.827188 0.000948 0.004988 1.643562 4422625.792709 483672.630756 -33.995673
+1116844391.877183 0.000563 0.005426 1.638871 4422625.792709 483672.630756 -33.995673
+1116844391.927194 0.001852 0.005653 1.359098 4422625.792709 483672.630756 -33.995673
+1116844391.977173 -0.000023 0.006351 1.749055 4422625.792709 483672.630756 -33.995673
+1116844392.027175 -0.000426 0.006716 1.827708 4422625.792709 483672.630756 -33.995673
+1116844392.077171 -0.003659 0.005830 2.384925 4422625.792709 483672.630756 -33.995673
+1116844392.127201 -0.004068 0.006781 2.359339 4422625.792709 483672.630756 -33.995673
+1116844392.177181 -0.003152 0.007870 2.230047 4422625.792709 483672.630756 -33.995673
+1116844392.227177 -0.003669 0.008703 2.207214 4422625.792709 483672.630756 -33.995673
+1116844392.277184 -0.004468 0.009142 2.270689 4422625.792709 483672.630756 -33.995673
+1116844392.327192 -0.003146 0.010104 2.138541 4422625.792709 483672.630756 -33.995673
+1116844392.377239 -0.003488 0.009758 2.170015 4422625.792709 483672.630756 -33.995673
+1116844392.427166 -0.002866 0.011032 2.106180 4422625.792709 483672.630756 -33.995673
+1116844392.527196 -0.002926 0.013294 2.118804 4422625.832853 483672.594586 -33.995010
+1116844392.577223 -0.003773 0.013313 2.142214 4422625.832853 483672.594586 -33.995010
+1116844392.627194 -0.005117 0.013002 2.221826 4422625.832853 483672.594586 -33.995010
+1116844392.677199 -0.004930 0.013527 2.200774 4422625.832853 483672.594586 -33.995010
+1116844392.727188 -0.004496 0.013857 2.181864 4422625.832853 483672.594586 -33.995010
+1116844392.777241 -0.005582 0.013657 2.248963 4422625.832853 483672.594586 -33.995010
+1116844392.827182 -0.007065 0.013569 2.324012 4422625.832853 483672.594586 -33.995010
+1116844392.877175 -0.006229 0.014770 2.273278 4422625.832853 483672.594586 -33.995010
+1116844392.927192 -0.006734 0.014917 2.292420 4422625.832853 483672.594586 -33.995010
+1116844392.977341 -0.007620 0.015078 2.336955 4422625.832853 483672.594586 -33.995010
+1116844393.027177 -0.008346 0.015023 2.361104 4422625.832853 483672.594586 -33.995010
+1116844393.077182 -0.009223 0.015303 2.393396 4422625.832853 483672.594586 -33.995010
+1116844393.127186 -0.008513 0.016500 2.353534 4422625.832853 483672.594586 -33.995010
+1116844393.177218 -0.008351 0.017097 2.329110 4422625.832853 483672.594586 -33.995010
+1116844393.227188 -0.008610 0.017754 2.339470 4422625.832853 483672.594586 -33.995010
+1116844393.277175 -0.009877 0.018198 2.372309 4422625.832853 483672.594586 -33.995010
+1116844393.327190 -0.009867 0.018640 2.327581 4422625.832853 483672.594586 -33.995010
+1116844393.377209 -0.010519 0.018972 2.343713 4422625.832853 483672.594586 -33.995010
+1116844393.427175 -0.011177 0.019038 2.373834 4422625.832853 483672.594586 -33.995010
+1116844393.527213 -0.013236 0.018770 2.433140 4422625.876835 483672.566446 -33.993900
+1116844393.577192 -0.012969 0.018908 2.402878 4422625.876835 483672.566446 -33.993900
+1116844393.627190 -0.013083 0.019217 2.403674 4422625.876835 483672.566446 -33.993898
+1116844393.677175 -0.013154 0.020095 2.401067 4422625.876835 483672.566446 -33.993898
+1116844393.727191 -0.014223 0.019624 2.459349 4422625.876835 483672.566446 -33.993898
+1116844393.777212 -0.014929 0.019828 2.474869 4422625.876835 483672.566446 -33.993898
+1116844393.827196 -0.014958 0.020261 2.444587 4422625.876835 483672.566446 -33.993898
+1116844393.877180 -0.014900 0.020460 2.419538 4422625.876835 483672.566446 -33.993898
+1116844393.927186 -0.015271 0.020337 2.421429 4422625.876835 483672.566446 -33.993898
+1116844393.977190 -0.014993 0.021189 2.388787 4422625.876835 483672.566446 -33.993898
+1116844394.027169 -0.015754 0.020640 2.408157 4422625.876835 483672.566446 -33.993898
+1116844394.077172 -0.015829 0.020956 2.391688 4422625.876835 483672.566446 -33.993898
+1116844394.127192 -0.016770 0.020874 2.410886 4422625.876835 483672.566446 -33.993898
+1116844394.177277 -0.017103 0.021196 2.417233 4422625.876835 483672.566446 -33.993898
+1116844394.227174 -0.018296 0.021426 2.434966 4422625.876835 483672.566446 -33.993898
+1116844394.277184 -0.019188 0.021685 2.445102 4422625.876835 483672.566446 -33.993898
+1116844394.327189 -0.019277 0.021580 2.452040 4422625.876835 483672.566446 -33.993898
+1116844394.377196 -0.019929 0.022195 2.454693 4422625.876835 483672.566446 -33.993898
+1116844394.427165 -0.020062 0.021468 2.454670 4422625.923072 483672.541346 -33.994208
+1116844394.477178 -0.020814 0.020747 2.462955 4422625.923158 483672.540886 -33.995434
+1116844394.527180 -0.021304 0.020149 2.480808 4422625.923160 483672.540875 -33.995463
+1116844394.577195 -0.022058 0.020583 2.493409 4422625.923160 483672.540874 -33.995464
+1116844394.627181 -0.021547 0.020922 2.489491 4422625.923160 483672.540874 -33.995464
+1116844394.677180 -0.022027 0.020880 2.508210 4422625.923160 483672.540874 -33.995464
+1116844394.727179 -0.022833 0.020792 2.518270 4422625.923160 483672.540874 -33.995464
+1116844394.777192 -0.022528 0.022197 2.498824 4422625.923160 483672.540874 -33.995464
+1116844394.827162 -0.023382 0.022631 2.512057 4422625.923160 483672.540874 -33.995464
+1116844394.877225 -0.023465 0.023911 2.496000 4422625.923160 483672.540874 -33.995464
+1116844394.927182 -0.024253 0.024172 2.495688 4422625.923160 483672.540874 -33.995464
+1116844394.977194 -0.024891 0.023852 2.509264 4422625.923160 483672.540874 -33.995464
+1116844395.027169 -0.024325 0.023607 2.516960 4422625.923160 483672.540874 -33.995464
+1116844395.077179 -0.024439 0.023851 2.526288 4422625.923160 483672.540874 -33.995464
+1116844395.127181 -0.025780 0.023827 2.535200 4422625.923160 483672.540874 -33.995464
+1116844395.177192 -0.026530 0.023859 2.526626 4422625.923160 483672.540874 -33.995464
+1116844395.227170 -0.027004 0.024038 2.537738 4422625.923160 483672.540874 -33.995464
+1116844395.277186 -0.027579 0.024305 2.530980 4422625.923160 483672.540874 -33.995464
+1116844395.327194 -0.028100 0.025341 2.535445 4422625.923160 483672.540874 -33.995464
+1116844395.377202 -0.028895 0.025603 2.542622 4422625.923160 483672.540874 -33.995464
+1116844395.427196 -0.029742 0.025956 2.541459 4422625.923160 483672.540874 -33.995464
+1116844395.477182 -0.030241 0.024953 2.555278 4422625.975800 483672.506574 -33.994981
+1116844395.527211 -0.029836 0.026577 2.527135 4422625.975805 483672.506566 -33.994956
+1116844395.577189 -0.029946 0.026297 2.526402 4422625.975805 483672.506566 -33.994955
+1116844395.627186 -0.030836 0.026469 2.530437 4422625.975804 483672.506567 -33.994955
+1116844395.677186 -0.031063 0.026257 2.531839 4422625.975803 483672.506567 -33.994955
+1116844395.727194 -0.030482 0.026661 2.531326 4422625.975802 483672.506568 -33.994954
+1116844395.777216 -0.031274 0.027439 2.530370 4422625.975800 483672.506570 -33.994953
+1116844395.827177 -0.030814 0.028250 2.510859 4422625.975800 483672.506570 -33.994953
+1116844395.877182 -0.030549 0.028330 2.506000 4422625.975800 483672.506570 -33.994953
+1116844395.927200 -0.030393 0.029855 2.494759 4422625.975800 483672.506570 -33.994953
+1116844395.977208 -0.030983 0.029401 2.500719 4422625.975799 483672.506570 -33.994953
+1116844396.027176 -0.031854 0.030160 2.506419 4422625.975799 483672.506571 -33.994953
+1116844396.081664 -0.031396 0.030928 2.491861 4422625.975799 483672.506571 -33.994953
+1116844396.127141 -0.032261 0.031110 2.494210 4422625.975799 483672.506571 -33.994953
+1116844396.177194 -0.031865 0.032287 2.474892 4422625.975799 483672.506571 -33.994953
+1116844396.227178 -0.032541 0.032600 2.455604 4422625.975799 483672.506571 -33.994953
+1116844396.277180 -0.031478 0.034156 2.444693 4422625.975799 483672.506571 -33.994953
+1116844396.327193 -0.030425 0.035006 2.429470 4422625.975799 483672.506571 -33.994953
+1116844396.377196 -0.030635 0.035845 2.429695 4422625.975799 483672.506571 -33.994953
+1116844396.427284 -0.030920 0.035842 2.424656 4422625.975799 483672.506571 -33.994953
+1116844396.527211 -0.031232 0.036287 2.418021 4422626.025201 483672.488462 -33.995130
+1116844396.577205 -0.032104 0.035731 2.414095 4422626.025201 483672.488462 -33.995130
+1116844396.627191 -0.031566 0.036379 2.401192 4422626.025194 483672.488468 -33.995128
+1116844396.677181 -0.031200 0.036797 2.392880 4422626.025165 483672.488494 -33.995115
+1116844396.727189 -0.033323 0.034819 2.393541 4422626.025106 483672.488550 -33.995089
+1116844396.777201 -0.035168 0.034667 2.435004 4422626.025001 483672.488639 -33.995043
+1116844396.827177 -0.036497 0.036257 2.467322 4422626.024857 483672.488753 -33.995006
+1116844396.877187 -0.040016 0.033075 2.508711 4422626.024623 483672.488911 -33.994900
+1116844396.927195 -0.041322 0.021561 2.657442 4422626.024347 483672.488926 -33.994691
+1116844396.977200 -0.030708 0.018821 2.654793 4422626.023913 483672.489160 -33.994531
+1116844397.027174 -0.021829 0.018693 2.584506 4422626.023280 483672.489693 -33.994279
+1116844397.077199 -0.027023 0.017040 2.520066 4422626.022543 483672.490447 -33.993926
+1116844397.127190 -0.030244 0.016309 2.467260 4422626.021879 483672.491187 -33.993609
+1116844397.177196 -0.037078 0.015354 2.538771 4422626.021461 483672.491277 -33.993524
+1116844397.227174 -0.039972 0.013477 2.552004 4422626.020859 483672.491593 -33.993709
+1116844397.277183 -0.042810 0.015281 2.506513 4422626.020857 483672.491728 -33.993766
+1116844397.327195 -0.042499 0.023922 2.472222 4422626.020288 483672.492446 -33.994292
+1116844397.377197 -0.045353 0.019967 2.510702 4422626.020020 483672.492429 -33.994524
+1116844397.427178 -0.040944 0.028459 2.335051 4422626.020328 483672.493637 -33.994478
+1116844397.527190 -0.007221 0.054573 1.614963 4422626.058033 483673.062698 -33.993397
+1116844397.577198 -0.010626 0.056099 1.610089 4422626.058538 483673.062766 -33.993157
+1116844397.627201 -0.005840 0.053285 1.603093 4422626.055301 483673.199424 -33.992851
+1116844397.677181 0.005623 0.051254 1.598037 4422626.051166 483673.271780 -33.993720
+1116844397.727195 0.003271 0.052653 1.587552 4422626.050553 483673.343216 -33.993208
+1116844397.777230 0.016250 0.053270 1.578259 4422626.049358 483673.416628 -33.992839
+1116844397.827193 0.019343 0.055152 1.567199 4422626.047223 483673.490521 -33.992040
+1116844397.877179 0.021989 0.049909 1.551817 4422626.047121 483673.562742 -33.990379
+1116844397.927189 0.023805 0.043741 1.517923 4422626.045239 483673.636068 -33.989545
+1116844397.977192 0.020289 0.046535 1.483267 4422626.046843 483673.710334 -33.988024
+1116844398.027172 0.018421 0.046388 1.412739 4422626.053291 483673.784867 -33.986671
+1116844398.077181 0.019954 0.054614 1.342952 4422626.063474 483673.859487 -33.985637
+1116844398.127191 0.019810 0.055671 1.262205 4422626.079698 483673.932140 -33.984345
+1116844398.177208 0.034093 0.060950 1.171303 4422626.102430 483674.003538 -33.983397
+1116844398.227179 0.043033 0.068258 1.074085 4422626.128018 483674.072279 -33.982895
+1116844398.277172 0.059469 0.074653 0.989079 4422626.165336 483674.135926 -33.980856
+1116844398.327193 0.059466 0.059845 0.868806 4422626.199630 483674.197328 -33.980224
+1116844398.377185 0.049192 0.068963 0.752577 4422626.244619 483674.252160 -33.978916
+1116844398.427172 0.054256 0.060935 0.655934 4422626.304165 483674.303533 -33.977597
+1116844398.477175 0.033908 0.057141 0.778013 4422625.997924 483674.428575 -33.997905
+1116844398.527196 0.025755 0.065059 0.681514 4422625.961746 483674.452082 -34.003683
+1116844398.577186 0.025872 0.058357 0.621544 4422626.012549 483674.497816 -34.005908
+1116844398.627192 0.028646 0.054039 0.483050 4422626.073197 483674.539103 -34.007188
+1116844398.677174 0.028977 0.052225 0.407884 4422626.138831 483674.574768 -34.007952
+1116844398.727202 0.044739 0.037231 0.306847 4422626.214593 483674.605571 -34.008200
+1116844398.777186 0.048051 0.038477 0.225925 4422626.290699 483674.628864 -34.008234
+1116844398.827170 0.047935 0.044452 0.168478 4422626.357590 483674.646549 -34.009691
+1116844398.877187 0.053995 0.038872 0.085778 4422626.431262 483674.659726 -34.010435
+1116844398.927194 0.054770 0.028769 0.018222 4422626.512958 483674.666135 -34.011034
+1116844398.977187 0.053433 0.041844 -0.051487 4422626.582283 483674.669296 -34.012360
+1116844399.027176 0.053511 0.026718 -0.094298 4422626.640758 483674.670417 -34.017149
+1116844399.077177 0.071359 0.012123 -0.157173 4422626.720106 483674.662560 -34.018896
+1116844399.127577 0.082867 0.011833 -0.228078 4422626.788242 483674.652653 -34.019414
+1116844399.177193 0.088919 0.003099 -0.285502 4422626.864231 483674.636302 -34.020209
+1116844399.227173 0.090348 0.007188 -0.332935 4422626.942778 483674.614511 -34.020989
+1116844399.277206 0.092034 0.001910 -0.382561 4422627.000044 483674.596763 -34.025405
+1116844399.327192 0.097272 -0.021757 -0.419521 4422627.080005 483674.565621 -34.026334
+1116844399.377198 0.097447 -0.014739 -0.452900 4422627.154101 483674.533122 -34.027030
+1116844399.427180 0.085850 -0.011768 -0.475828 4422627.216583 483674.504141 -34.027659
+1116844399.477175 0.068927 -0.013280 -0.243599 4422626.909148 483674.726760 -34.046610
+1116844399.527215 0.048907 -0.013172 -0.261960 4422626.982100 483674.709650 -34.04