Help with translating IMU trajectory to poses using Replica reader.

[kjrosscras]

Trying to use my own dataset from the Eagle lidar scanner with this, I've managed to create a way to render out a depth image for the corresponding RGB images. But I'm not sure how to translate the poses to use with this.

Raw IMU trajectory:
recalculate_trj.txt

How I'm translating:

`import numpy as np
import pandas as pd
from pathlib import Path
from scipy.spatial.transform import Rotation as R, Slerp

IMG_FOLDER = "undistorted"
POSE_CSV = "recalculate_trj.txt"
OUT_PATH = "pose_converted.txt"

image_timestamps = sorted([float(Path(f).stem) for f in Path(IMG_FOLDER).glob("*.jpg")])

poses = pd.read_csv(POSE_CSV, header=None, sep=r'\s+')
poses.columns = ['timestamp', 'x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']

Negate qw for correct orientation

poses['qw'] = -poses['qw']

Prepare data

poses.sort_values('timestamp', inplace=True)
poses.drop_duplicates(subset='timestamp', inplace=True)
pose_times = poses['timestamp'].to_numpy()
positions = poses[['x', 'y', 'z']].to_numpy()
rotations = R.from_quat(poses[['qx', 'qy', 'qz', 'qw']].to_numpy())
slerp = Slerp(pose_times, rotations)

=== COORDINATE SYSTEM CONVERSION (to Replica format) ===

Your convention: x = forward, y = right, z = up

Replica expects: z = forward, x = right, y = down

axis_flip = np.array([
[0, 0, 1],
[1, 0, 0],
[0, -1, 0]
])

=== GENERATE OUTPUT ===

with open(OUT_PATH, "w") as f:
for ts in image_timestamps:
if ts < pose_times[0] or ts > pose_times[-1]:
continue
idx_after = np.searchsorted(pose_times, ts)
idx_before = idx_after - 1
ratio = (ts - pose_times[idx_before]) / (pose_times[idx_after] - pose_times[idx_before])
pos_interp = positions[idx_before] + ratio * (positions[idx_after] - positions[idx_before])
rot_interp = slerp(ts).as_matrix()

    pose_mat = np.eye(4)
    pose_mat[:3, :3] = rot_interp
    pose_mat[:3, 3] = pos_interp

    # Transform to Replica convention
    pose_mat[:3, :3] = axis_flip @ pose_mat[:3, :3]
    pose_mat[:3, 3] = axis_flip @ pose_mat[:3, 3]

    f.write(" ".join(f"{v:.18e}" for v in pose_mat.flatten()) + "\n")

`

Calculated pose per frame:
pose.txt

Any suggestions would be great.

[goldoak]

Hi, you should use the extrinsic parameters between the IMU and your RGB sensor to first transform the IMU pose to the RGB pose. Ensure that the processed poses represent the transformation from the camera coordinate system to the world coordinate system, as this is what all GSFusion readers expect. After that, you can test your poses using GSFusion. You will quickly find out if your provided poses are correct through the GUI, as any incorrect poses will result in a chaotic visualization in the window.