Example file written in cpp to encode trajectories, some minor bugfixes