Reads keyframe poses from map data in sqlite3 format and export it to a bag file..

read_poses_from_db_and_convert_to_bag.py

#!/usr/bin/env python3
"""
python3 read_poses_from_db_and_print_tum.py map.db
"""
import argparse
import sqlite3
import struct
import math
import numpy as np
from scipy.spatial.transform import Rotation as R

from rosbags.rosbag1 import Writer
from rosbags.serde import serialize_ros1
from rosbags.typesys.types import builtin_interfaces__msg__Time as Time
from rosbags.typesys.types import geometry_msgs__msg__Quaternion as Quaternion
from rosbags.typesys.types import geometry_msgs__msg__Point as Point
from rosbags.typesys.types import geometry_msgs__msg__Pose as Pose
from rosbags.typesys.types import geometry_msgs__msg__PoseStamped as PoseStamped
from rosbags.typesys.types import std_msgs__msg__Header as Header

def time_to_nsec(timestamp):
    return int(timestamp * 10e+9)

def time_to_ros_time(timestamp):
    f, i = math.modf(timestamp)
    return Time(int(i), int(f * 10e+9))

def main(writer):
    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')

    topic = "pose"
    frame_id = "map"
    conn_bag = writer.add_connection(topic, PoseStamped.__msgtype__)

    use_pseudo_time = True
    pseudo_time = 0.0
    pseudo_fps = 1.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]
        timestamp = pseudo_time if use_pseudo_time else t
        msg = PoseStamped(
            Header(time_to_ros_time(timestamp), frame_id),
            Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)))
        msg_ros = serialize_ros1(msg, PoseStamped.__msgtype__)
        writer.write(conn_bag, time_to_nsec(timestamp), msg_ros)
        pseudo_time += 1 / pseudo_fps

    cur.close()
    conn.close()


if __name__ == "__main__":
    output_filename = "output.bag"
    with Writer(output_filename) as writer:
        main(writer)