Fixed keypoint reading and writing
Everything works again. Matrices back in yaml files.
Refactored feature matching