Added pose reader folder -> Goal: Read the true robot pose through icp and write it to file