kitti转tum python脚本

import numpy as np

def load_kitti_poses(file_path):
    """Load KITTI poses from a text file."""
    poses = []
    with open(file_path, 'r') as f:
        for line in f.readlines():
            T = np.fromstring(line, sep=' ').reshape(3, 4)
            poses.append(T)
    return poses

def convert_pose_to_tum_format(poses):
    """Convert KITTI poses to TUM format."""
    tum_poses = []
    for idx, T in enumerate(poses):
        timestamp = float(idx)  # Assuming index as timestamp, replace with actual timestamps if available
        R = T[:3, :3]
        t = T[:3, 3]
        
        # Convert rotation matrix to quaternion
        q = quaternion_from_matrix(R)
        
        tum_poses.append((timestamp, t[0], t[1], t[2], q[0], q[1], q[2], q[3]))
    return tum_poses

def quaternion_from_matrix(matrix):
    """Convert a rotation matrix to a quaternion."""
    q = np.empty((4, ))
    M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
    t = np.trace(M)
    if t > M[0, 0]:
        q[3] = t
        q[2] = M[0, 2] - M[2, 0]
        q[1] = M[1, 0] - M[0, 1]
        q[0] = M[2, 1] - M[1, 2]
    else:
        i = 0
        if M[1, 1] > M[0, 0]:
            i = 1
        if M[2, 2] > M[i, i]:
            i = 2
        j = (i + 1) % 3
        k = (i + 2) % 3
        t = M[i, i] - M[j, j] - M[k, k] + 1.0
        q[i] = t
        q[j] = M[j, i] + M[i, j]
        q[k] = M[i, k] + M[k, i]
        q[3] = M[k, j] - M[j, k]
        q = q[[3, 0, 1, 2]]
    q *= 0.5 / np.sqrt(t)
    return q

def save_to_tum(file_path, tum_poses):
    """Save poses in TUM format to a text file."""
    with open(file_path, 'w') as f:
        for pose in tum_poses:
            f.write("{:.6f} {:.6f} {:.6f} {:.6f} {:.6f} {:.6f} {:.6f} {:.6f}\n".format(*pose))

if __name__ == "__main__":
    kitti_file_path = '07.txt'
    tum_file_path = '07_tum.txt'
    
    poses = load_kitti_poses(kitti_file_path)
    tum_poses = convert_pose_to_tum_format(poses)
    save_to_tum(tum_file_path, tum_poses)
    
    print(f"Converted KITTI data to TUM format and saved to {tum_file_path}")

运行：

python3 kitti2tum.py