Fix the second bug related to issue #29
Merge branch 'master' of github.com:ethz-asl/libpointmatcher