How to save slam estimated trajectory for evo evaluate

abscanxcc commented 1 year ago

i want to save the estimated trajectory from slam system for evo i tried to save the data like this but i find it is different from the output using ORB_SLAM2 could someone teach me how to save the trajectory in a correct way?


class Traj:
    def __init__(self) -> None:
        self.ses = []        
    def append(self, t, q, timestamp):
        self.ses.append(f'{timestamp} {t[0]} {t[1]} {t[2]} {q.x()} {q.y()} {q.z()} {q.w()}\n')
    def save(self, file_path):
        with open(file_path, 'w') as f:
            for s in self.ses:
                f.write(s)

# def main -> while loop
if img is not None:
    time_start = time.time()                  
    slam.track(img, img_id, timestamp)  # main SLAM function
    TRAJ.append(slam.tracking.f_cur.tcw, slam.tracking.f_cur.quaternion, timestamp)
...

slam.quit()
TRAJ.save('output.txt')

ORB_SLAM2 output:

1341845688.561749 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 0.000000000 1.000000000
1341845688.593654 -0.007665934 -0.000903384 -0.002267529 0.001068144 -0.002141687 0.003122602 0.999992251
1341845688.629688 -0.012080790 -0.002912590 -0.013541372 0.001709714 -0.006462066 0.006715524 0.999955118
1341845688.663680 -0.012869255 -0.000281037 -0.015576986 0.004135095 -0.011875746 0.012413991 0.999843895
1341845688.697719 -0.016297463 -0.007826334 -0.020093242 0.006437330 -0.016998125 0.018521359 0.999663234
1341845688.729899 -0.018993620 -0.008006354 -0.026770055 0.010261443 -0.022736214 0.021241823 0.999463141
1341845688.761762 -0.004576349 0.002465011 -0.023948893 0.014276086 -0.028370073 0.020946579 0.999276042
1341845688.797687 -0.002894551 0.008339200 -0.020725235 0.016346155 -0.027029518 0.019734837 0.999306142
1341845688.829892 -0.005654691 0.012002264 -0.022449892 0.016384108 -0.024536967 0.017827895 0.999405682
1341845688.865774 -0.010583993 0.014003478 -0.023346752 0.016376702 -0.023874054 0.018919393 0.999401748
1341845688.929779 -0.018626541 0.018410981 -0.037423823 0.016890060 -0.029376652 0.020882333 0.999207497
1341845688.965631 -0.015068507 0.022202065 -0.035465192 0.017291747 -0.032659329 0.019101003 0.999134362
1341845688.997750 -0.008713639 0.031604972 -0.038524613 0.016863339 -0.033067830 0.015545724 0.999189913
1341845689.029751 -0.011352038 0.039969079 -0.036654677 0.013996737 -0.030239126 0.010605120 0.999388456

My output:

1341845688.561749 0.0 0.0 0.0 0.0 0.0 0.0 1.0
1341845688.593654 0.18590414840242278 3.1291080640529533e-14 2.1156316506151728e-13 -1.5045639289878153e-16 9.030903774208449e-16 -1.3695186101749028e-15 1.0
1341845688.629688 0.1041444735032357 0.004055743530027708 0.06860903261063744 -0.0005468233756680184 0.007387941620612827 -0.004436027128239304 0.9999627197881548
1341845688.66368 0.1162231408939824 -0.030088692548868735 0.06894297105367177 -0.003293192700167066 0.012726961493134592 -0.010030336703541235 0.999863276492644
1341845688.697719 0.12230393504285499 0.0005301188133760038 0.05438374193201695 -0.005339069575933595 0.018317457180983557 -0.014756811503941768 0.9997090584828785
1341845688.729899 0.08330706553412749 -0.03240612035827504 0.05231723263198659 -0.00924524611284535 0.025442831368680453 -0.018006209275595576 0.9994713423524365
1341845688.761762 0.11660125735604761 -0.288533963553802 0.05916108716467946 -0.016370840579262486 0.027259185196922196 -0.01573197212804857 0.9993705206048908
1341845688.797687 0.20938039943110034 -0.5160460019739481 0.1662612434476589 -0.022167052378181316 0.024422329843703003 -0.010305869069040557 0.9994028020055777
1341845688.829892 0.2724504671407873 -0.5257319932164769 0.25746406661791427 -0.02219729035574358 0.021564387967645243 -0.009702822035975119 0.9994739179773436
1341845688.865774 0.29985108283500794 -0.46581402721298326 0.328909950192147 -0.021270383256513124 0.021208013886261555 -0.013449913603042624 0.999458298663427
1341845688.929779 0.3882610463526344 -0.45892476951211747 0.48927596392984163 -0.02101625037367277 0.027099214325283567 -0.018007651903231354 0.9992495555546249
1341845688.965631 0.4034705280979693 -0.5680170133373412 0.4579343404574361 -0.022465706784494322 0.029662732118317242 -0.015334551933537992 0.999189804721279
1341845688.99775 0.4336877573027356 -0.6284629244323033 0.4992335235980256 -0.02120784017611048 0.028590201129412688 -0.01080034393720642 0.9993078507073195
1341845689.029751 0.4915297360222449 -0.664973641668609 0.5121783612773484 -0.017534272995032324 0.02503819973785446 -0.0052564136126956075 0.9995188882359128
1341845689.065755 0.5476687003202655 -0.6977573637603934 0.5679206445705772 -0.014243233229526614 0.022822908943001475 -0.0034990704973958736 0.999631933083474
1341845689.097725 0.553686384091326 -0.7453460155379238 0.5880414902686367 -0.01119981413950568 0.02196740871426796 -0.005400776383373152 0.9996813636014616
1341845689.133718 0.5473427315487064 -0.7737033840007819 0.5581451266628129 -0.008973782302578745 0.020778338078837496 -0.00822142425017404 0.9997100279987022
1341845689.165884 0.6174310223195113 -0.787621327523831 0.617072431935236 -0.006253756542540977 0.018734500443524375 -0.010708383701889652 0.9997475879144352
1341845689.198264 0.5974419084894774 -0.7952797033024445 0.5880470580868593 -0.0028585586201524166 0.018643029718368623 -0.01322976169216325 0.9997345845228641
1341845689.234322 0.5424774512839943 -0.7680435099775463 0.5326490898324885 -9.599532643936045e-05 0.018506909543350353 -0.015758739230802295 0.9997045299596812
1341845689.265745 0.5674982037190589 -0.7297356096724548 0.5368112698400846 0.0017181394229580712 0.01752747347042693 -0.017815347414186222 0.9996861752906152
1341845689.297769 0.5700196400804239 -0.7689989895014718 0.5428441965706401 0.002496145976960301 0.017816145843411707 -0.018058580228223334 0.9996750681509912
1341845689.33365 0.5078308737960288 -0.75039005323924 0.47798340846223647 0.003747980736419176 0.017654826567189686 -0.018367607092376153 0.9996683903920249
1341845689.365881 0.520871942197322 -0.7053493724199811 0.4938960363243826 0.004081404965243778 0.017161825228115317 -0.01829930519838561 0.9996769224692574
1341845689.401592 0.5352200239145737 -0.7156585069632726 0.5166037822962963 0.0026656499955382113 0.017755771275964023 -0.01774790025038002 0.99968126867177
1341845689.433744 0.5123883984215382 -0.6799460454408472 0.5141585309529508 0.0025673675658834188 0.019072708274426947 -0.017657930045571712 0.9996588607766981
1341845689.465651 0.47915953250692406 -0.58602857850772 0.5006597698887589 0.0010789245925780858 0.021130782045202568 -0.018644443071045784 0.9996022762651414

antopost commented 1 year ago

Were you able to solve this issue?

luigifreda commented 1 year ago

Hi, thanks for your feedback. I think it is very helpful to consider that:

ORBSLAM was designed and runs under C++ in a multithreading way,
pyslam was designed and runs under Python, where the GIL does not allow real multithreading (everything runs on a single core)

As stated in the main README _"You can use this framework as a baseline to play with local features, VO techniques and create your own (proof of concept) VO/SLAM pipeline in python. When you test it, consider that's a work in progress, a development framework written in Python, without any pretense of having state-of-the-art localization accuracy or real-time performances". pyslam was NOT designed to have real-time performance.

That being said, you can save "current" estimates as explained here https://github.com/luigifreda/pyslam/issues/97#issuecomment-1762851078 A function to save the "final" estimates is currently missing. Work in progress.

As for the EVO format, it's a question of understanding its requirements.

Hope this helps. Luigi

luigifreda / pyslam

How to save slam estimated trajectory for evo evaluate #84