def align_trajectory(trajectory0: Trajectory,
trajectory1: Trajectory) -> Trajectory:
# Align trajectory1 to trajectory0
assert isinstance(trajectory0, Trajectory)
assert isinstance(trajectory1, Trajectory)
t0 = max(trajectory0.begin_time(), trajectory1.begin_time())
t1 = min(trajectory0.end_time(), trajectory1.end_time())
if t0 >= t1:
return Trajectory()
num_points = 100
times = np.linspace(t0, t1, num_points).tolist()
points0 = [trajectory0[t].x() for t in times]
points1 = [trajectory1[t].x() for t in times]
S = umeyama(points0, points1).to_SE3()
trajectoryA = S * trajectory0
return trajectoryA
