Last active
August 11, 2022 04:27
-
-
Save ymd-stella/3c942086d1d976f4f909d4205257011f to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment