GPS CSV to KITTI

This script converts GPS measurments from frames/gps.csv into KITTI-format poses.txt, producing one 3x4 pose matrix per frame.GPS poses are interpolated to each frame timestamp using frames_manifest.csv.

Overview

The script performs the following steps:

1. Load GPSsamples from frames/gps.csv

2. Load frame timestamps from frames/frames_manifest.csv

3. Interpolate GPS position and heading from each frame
4. Convert latitude, longitude, and altitude into local coordinates
5. Build KITTI 3x4 pose matrix for each frame
6. Write the result to frames/poses.txt

Coordinate System

The Output using following coordinate mapping: - x = East - y = Down - z = North

The translation ver´ctor is defined as:

t= [east, -up, north]

This matches a trajectory view where x and z from the horizontal plane.

Input Files

frames/gps.csv

Expected columns: - timestamp_ns - latitude - longitude - altitude_m

frames/frames_manifest.csv

Expected columns:

• cam0_ts_ns
• cam1_ts_ns

The frame timestamp is computed as the average of the two camera timestamps:

ts = (int(row["cam0_ts_ns"])+ int(row["cam1_ts_ns"])) / 2

Output File

frames/poses.txt

Each line contains one flattened 3x4 KITTI pose matrix:

r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz

Code

"""
Convert frames/gps.csv to KITTI-format poses.txt (one 3x4 matrix per frame).

Coordinate mapping (matches plot_trajectory X vs Z = horizontal plane):
  X = East,  Y = -(altitude above origin, i.e. down),  Z = North

Rotation from GPS heading (clockwise from North) → yaw around Y-down axis.
GPS poses are interpolated to each frame timestamp from frames_manifest.csv.
"""

import csv
import math
import numpy as np

EARTH_R = 6_371_000.0  # metres


def lat_lon_alt_to_enu(lat, lon, alt, lat0, lon0, alt0):
    dlat = math.radians(lat - lat0)
    dlon = math.radians(lon - lon0)
    north = dlat * EARTH_R
    east  = dlon * EARTH_R * math.cos(math.radians(lat0))
    up    = alt - alt0
    return east, north, up


def heading_to_rotation(heading_deg):
    """Yaw-only rotation around Y-down axis from compass heading (CW from North)."""
    theta = math.radians(heading_deg)
    c, s = math.cos(theta), math.sin(theta)
    return np.array([
        [ c, 0, s],
        [ 0, 1, 0],
        [-s, 0, c],
    ], dtype=np.float64)


def interp_angle(a0, a1, t):
    """Linear interpolation of angles with wraparound (degrees)."""
    diff = ((a1 - a0 + 180) % 360) - 180
    return (a0 + diff * t) % 360


gps_rows = []
with open("frames/gps.csv") as f:
    for row in csv.DictReader(f):
        gps_rows.append({
            "ts":      int(row["timestamp_ns"]),
            "lat":     float(row["latitude"]),
            "lon":     float(row["longitude"]),
            "alt":     float(row["altitude_m"]),
            "heading": float(row["heading_deg"]),
        })

gps_rows.sort(key=lambda r: r["ts"])
lat0  = gps_rows[0]["lat"]
lon0  = gps_rows[0]["lon"]
alt0  = gps_rows[0]["alt"]
gps_ts = np.array([r["ts"] for r in gps_rows], dtype=np.float64)

frame_ts = []
with open("frames/frames_manifest.csv") as f:
    for row in csv.DictReader(f):
        ts = (int(row["cam0_ts_ns"]) + int(row["cam1_ts_ns"])) / 2
        frame_ts.append(ts)


def interp_gps(query_ts):
    idx = np.searchsorted(gps_ts, query_ts)
    if idx == 0:
        return gps_rows[0]
    if idx >= len(gps_rows):
        return gps_rows[-1]
    r0, r1 = gps_rows[idx - 1], gps_rows[idx]
    t = (query_ts - r0["ts"]) / (r1["ts"] - r0["ts"])
    return {
        "lat":     r0["lat"]     + t * (r1["lat"]     - r0["lat"]),
        "lon":     r0["lon"]     + t * (r1["lon"]     - r0["lon"]),
        "alt":     r0["alt"]     + t * (r1["alt"]     - r0["alt"]),
        "heading": interp_angle(r0["heading"], r1["heading"], t),
    }


poses = []
for ts in frame_ts:
    g = interp_gps(ts)
    east, north, up = lat_lon_alt_to_enu(g["lat"], g["lon"], g["alt"], lat0, lon0, alt0)

    t = np.array([east, -up, north], dtype=np.float64)
    R = heading_to_rotation(g["heading"])

    T = np.eye(3, 4, dtype=np.float64)
    T[:3, :3] = R
    T[:3,  3] = t
    poses.append(T)

out_path = "frames/poses.txt"
with open(out_path, "w") as f:
    for T in poses:
        line = " ".join(f"{v:.6e}" for v in T.flatten())
        f.write(line + "\n")

print(f"Written {len(poses)} poses → {out_path}")
print(f"Origin: lat={lat0:.6f}, lon={lon0:.6f}, alt={alt0:.1f} m")
east_range = max(T[0,3] for T in poses) - min(T[0,3] for T in poses)
north_range = max(T[2,3] for T in poses) - min(T[2,3] for T in poses)
print(f"East span: {east_range:.1f} m,  North span: {north_range:.1f} m")

Function Details

lat_lon_alt_to_enu

Converts latitude, longitude, and altitude into local coordinates relative to the first GPS sample.

def lat_lon_alt_to_enu(lat, lon, alt, lat0, lon0, alt0):
    dlat = math.radians(lat - lat0)
    dlon = math.radians(lon - lon0)
    north = dlat * EARTH_R
    east  = dlon * EARTH_R * math.cos(math.radians(lat0))
    up    = alt - alt0
    return east, north, up

heading_to_rotation

Converts GPS heading into a yaw-only rotation matrix around the Y-down axis.

def heading_to_rotation(heading_deg):
    theta = math.radians(heading_deg)
    c, s = math.cos(theta), math.sin(theta)
    return np.array([
        [ c, 0, s],
        [ 0, 1, 0],
        [-s, 0, c],
    ], dtype=np.float64)

interp_angle

Interpolates heading values while handling wraparound at 360 degrees.

def interp_angle(a0, a1, t):
    diff = ((a1 - a0 + 180) % 360) - 180
    return (a0 + diff * t) % 360

interp_gps

Interpolates GPS position and heading for a given frame timestamp.

def interp_gps(query_ts):
    idx = np.searchsorted(gps_ts, query_ts)
    if idx == 0:
        return gps_rows[0]
    if idx >= len(gps_rows):
        return gps_rows[-1]
    r0, r1 = gps_rows[idx - 1], gps_rows[idx]
    t = (query_ts - r0["ts"]) / (r1["ts"] - r0["ts"])
    return {
        "lat":     r0["lat"]     + t * (r1["lat"]     - r0["lat"]),
        "lon":     r0["lon"]     + t * (r1["lon"]     - r0["lon"]),
        "alt":     r0["alt"]     + t * (r1["alt"]     - r0["alt"]),
        "heading": interp_angle(r0["heading"], r1["heading"], t),
    }

Usage

Run the script with:

python gps_to_kitti.py

Example Console Output

Written 250 poses → frames/poses.txt
Origin: lat=50.123456, lon=6.123456, alt=145.2 m
East span: 85.3 m, North span: 122.7 m

Notes

• The Earth is approximated as a sphere
• This method is suitable for small local areas
• Only yaw is used for orientation
• Roll and pitch are ignored
• Frame timestamps outside the GPS range use the nearest GPS sample