def transform_trajectory(trajectory, R, p): """Create new trajectory relative the given transformation If R1(t) and p1(t) is the rotation and translation given by inout trajectory, then this function returns a new trajectory that fulfills the following. Let X1 = R1(t).T[X - p1(t)] be the coordinate of point X in input trajectory coordinates. Then X2 = R X1 + p is the same point in the coordinate frame of the new trajectory Since X2 = R2(t).T [X - p2(t)] then we have R2(t) = R1(t)R and p2(t) = p1(t) + R1(t)p """ ts_q1 = trajectory.sampled.rotationKeyFrames q1 = ts_q1.values ts_p1 = trajectory.sampled.positionKeyFrames p1 = ts_p1.values # Rotation Q = Quaternion.fromMatrix(R) q2 = q1 * Q ts_q2 = TimeSeries(ts_q1.timestamps, q2) # Translation p2 = p1 + q1.rotateVector(p) ts_p2 = TimeSeries(ts_p1.timestamps, p2) sampled = SampledTrajectory(positionKeyFrames=ts_p2, rotationKeyFrames=ts_q2) smoothen = False # MUST be the same as used in Dataset class splined = SplinedTrajectory(sampled, smoothRotations=smoothen) return splined