read_poses_from_db_and_print_tum.py

#!/usr/bin/env python3
"""
You can plot with the following commands

```
python3 read_poses_from_db_and_print_tum.py map.db > pose.tum
evo_traj tum pose.tum -p --save_plot plot.pdf --plot_mode xz
```
"""
import argparse
import sqlite3
import struct
import numpy as np
from scipy.spatial.transform import Rotation as R


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("filepath", type=str)
    args = parser.parse_args()
    conn = sqlite3.connect(args.filepath)
    cur = conn.cursor()
    cur.execute('SELECT * FROM keyframes ORDER BY id')
    
    data = []
    i = 0
    for keyfrm in cur:
        t = keyfrm[2]
        # See https://github.com/stella-cv/stella_vslam/blob/04f87d943ffcc5edf3e09c1a9d2a806ef4b762b4/src/stella_vslam/data/map_database.cc#L749-L759.
        pose_cw_bytes = keyfrm[5]
        pose_cw_vec = struct.unpack("<16d", pose_cw_bytes)
        pose_cw = np.matrix(np.array(pose_cw_vec).reshape((4, 4)).T)
        pose_wc = np.linalg.inv(pose_cw)
        rot = R.from_matrix(pose_wc[:3, :3])
        qx, qy, qz, qw = rot.as_quat()
        x, y, z = pose_wc[0, 3], pose_wc[1, 3], pose_wc[2, 3]
        print("{} {} {} {} {} {} {} {}".format(t, x, y, z, qx, qy, qz, qw))


    cur.close()
    conn.close()


if __name__ == "__main__":
    main()