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.

[lericson]

I have named the generator cv_interp because this is essentially making a constant velocity assumption.

[EmmanuelMess]

I have named the generator cv_interp because this is essentially making a constant velocity assumption.

Maybe the velocity is not constant during a turn. How does this keep a constant angular and linear velocity in the turn?

[lericson]

I don't understand this question, I'm afraid. The code simply uses linear interpolation to compute the estimated state at each time of the reference trajectory. This implies a constant linear velocity with respect to the world, and constant angular velocity.

[EmmanuelMess]

I don't understand this question, I'm afraid. The code simply uses linear interpolation to compute the estimated state at each time of the reference trajectory. This implies a constant linear velocity with respect to the world, and constant angular velocity.

To keep angular velocity constant, you have to take into account the linear velocity. The solution you provide is not trivially correct AFAIK. I would seek a mathematical proof before implementing as is. (here is an explanation, but the essence of the problem is the fact that the coordinate frame rotates as you move)

[lericson]

Oh, I think I see where the confusion arises. In this case, both the linear and angular velocity are assumed to be constant in the world frame. So the robot is moving in a straight line (via LERP), and rotating smoothly (via quaternion SLERP) between orientations, over the same time interval.

It's like imagining the robot moving along the straight line between its two nearest sampling points, while simultaneously rotating - both motions occurring at constant rates with respect to the world frame, not the robot’s own body frame.

[lericson]

To be clear, the linear velocity in the body frame is most certainly not constant.