Trajectory alignment by interpolation [proof of concept implementation]

[lericson]

Hi, I did not find a way to do trajectory alignment by interpolation of the estimated trajectory, which I think is a fairly normal thing to have in a package such as this.

For each time t in the reference trajectory, the code below will find the nearest next timestep t_next and the one before, t_prev. It will then interpolate between the poses at t_next and t_prev according to t.

from warnings import warn

import numpy as np
from evo.core.trajectory import PoseTrajectory3D
from evo.core.transformations import quaternion_slerp as qslerp


def lerp(a, b, t):
    return (1.0-t)*a + t*b


# We need to do linear interpolation, ourselves...
def cv_interp(est, ref_timestamps, *, extrapolate_past_end=False):
    "Compute points along trajectory *est* at *timestamps* or trajectory *ref*."
    # Accept trajectories 
    if hasattr(ref_timestamps, 'timestamps'):
        ref_timestamps = ref_timestamps.timestamps

    ref_timestamps = np.array(ref_timestamps, copy=True)
    
    est_tran = est.positions_xyz
    est_quat = est.orientations_quat_wxyz
    
    # Index of closest next estimated timestamp for each pose in *est*.
    # Must be >= 1 since we look at the previous pose.
    i = 1

    for j, t_ref in enumerate(ref_timestamps):
        while i < est.num_poses and est.timestamps[i] <= t_ref:
            i += 1
        if not extrapolate_past_end and est.num_poses <= i:
            warn('reference trajectory ends after estimated, cut short')
            break
        t_prev = est.timestamps[i-1]
        t_next = est.timestamps[i]
        td     = (t_ref - t_prev) / (t_next - t_prev)
        quat_j = qslerp(est_quat[i-1], est_quat[i], td)
        tran_j =   lerp(est_tran[i-1], est_tran[i], td)
        yield np.r_[t_ref, tran_j, quat_j]


def trajectory_interpolation(est, timestamps):
    poses = np.array(list(cv_interp(est, timestamps)))
    times = poses[:, 0]
    trans = poses[:, 1:4]
    quats = poses[:, 4:8]
    return PoseTrajectory3D(trans, quats, times, meta=est.meta)

I'm sure there is a better way to do this with the stuff in lie.py but the above code works.