Cleanup code
Merge branch 'master' into minimap_test