Improved trajectorization

Fixed a couple of bugs that lost information. Now trajectorization is
more efficient and matches trajectories better.