Reads keyframe poses from map data in sqlite3 format.

read_poses_from_db.py

#!/usr/bin/env python3
import argparse
import sqlite3
import struct
import numpy as np


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')
    for keyfrm in cur:
        # 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)
        map_to_camera_affine_cv = np.linalg.inv(pose_cw)
        with np.printoptions(suppress=True):
            print(map_to_camera_affine_cv)
    cur.close()
    conn.close()


if __name__ == "__main__":
    main()