[#18] Simplify TrajectoryPoint by using Pose instead of (x,y,z,heading)

Replace the x, y, z, and Complex32 heading fields of TrajectoryPoint with Pose pose.

Closes #18 (closed)