Fixed bug. I forgot to use a rotation matrix to visualize recorded trajectories.