C++ implementation of coherent point drift, taken from https://sites.google.com/site/myronenko/research/cpd