UZ-SLAMLab / ORB_SLAM3

ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
GNU General Public License v3.0
6.45k stars 2.53k forks source link

The timestamp problem of tum-vi dataset #265

Open AcresJoe1 opened 3 years ago

AcresJoe1 commented 3 years ago

Thank you very much for your work. @jdtardos @jjgr3496 Why is it that the timestamp of monocular euroc is the same as that of input loading, but the timestamp of monocular tmu-vi is completely different from that of input loading? Here are the saved trajectories of these two datasets: (1)f_dataset-MH01_mono.txt 1403636579963555584.000000 -0.000136528 0.018019525 0.006424963 -0.011999285 0.005858626 -0.003159937 0.999905825 1403636580013555456.000000 -0.000292776 0.023962667 0.008088766 -0.016854534 0.005779970 -0.001192409 0.999840558 1403636580063555584.000000 -0.000480495 0.029613828 0.009697663 -0.020557934 0.005988440 -0.000634232 0.999770522 1403636580113555456.000000 -0.000458905 0.036159169 0.011573208 -0.022359943 0.005410744 0.002378339 0.999732494 1403636580163555584.000000 -0.000673709 0.041793048 0.013072382 -0.024127690 0.004974772 0.004022802 0.999688387 1403636580213555456.000000 -0.000738881 0.046774935 0.014287969 -0.026610069 0.004400382 0.003976642 0.999628305 1403636580263555584.000000 -0.001001044 0.050838012 0.015418543 -0.029865382 0.004633134 0.002160637 0.999540865 1403636580313555456.000000 -0.001212217 0.055651244 0.016411416 -0.033441052 0.004713817 0.000094046 0.999429584 .......

(2)f_dataset-room1_512_monoi.txt 1520530313549745664.000000 0.032059722 0.001185614 0.002466401 -0.000886317 0.008851318 0.011195094 0.999897599 1520530313599746816.000000 0.030612076 0.001153023 0.002311439 -0.001251669 0.008568821 0.012217074 0.999887705 1520530313649744896.000000 0.030282259 0.001354882 0.001770983 -0.001431506 0.008543415 0.013359715 0.999873102 1520530313699746048.000000 0.031414580 0.000769881 0.002338739 -0.000319169 0.008480185 0.014441068 0.999859571 1520530313749747968.000000 0.031072002 0.000279755 0.002818754 0.000868355 0.008764772 0.015604269 0.999839306 1520530313799749888.000000 0.044247001 0.002539435 -0.000243437 0.001034567 0.010291521 0.013812623 0.999850571 1520530313849751040.000000 0.031194288 0.000402364 0.002806168 0.001347453 0.009761748 0.017103676 0.999805033 1520530313899752960.000000 0.016918806 -0.000325218 0.001352911 -0.000153172 0.011616229 0.017715391 0.999775410 1520530313949754112.000000 0.017047651 -0.000097296 0.001152300 -0.001872233 0.012751647 0.018231180 0.999750555 .......

sandeepnmenon commented 2 years ago

+1 I am observing the same thing. Is there any correction being done for tum_vi ? @AcresJoe1 Did you find the issue? I see there is a divising by 1e9 of the timestamp in the LoadImages code. https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/4452a3c4ab75b1cde34e5505a36ec3f9edcdc4c4/Examples/Monocular/mono_tum_vi.cc#L252

But this is done in EUROC as well.

sandeepnmenon commented 2 years ago

I removed the division by 1e9 shown above and in the SaveTrajectoryEuRoC folder. https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/4452a3c4ab75b1cde34e5505a36ec3f9edcdc4c4/src/System.cc#L769 Then add the division to the waiting time since waiting time becomes scaled and the issue is resolved.

        // Wait to load the next frame
        double T=0;
        if(ni<nImages[seq]-1)
            T = vTimestampsCam[seq][ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestampsCam[seq][ni-1];
        T/=1e9; // Added to reduce wait time
        if(ttrack<T)
            usleep((T-ttrack)*1e6); // 1e6