ICP minimize function finished.
ICP works on the first scan, but is very slow.
my icp3 implementation finally reproduced the same bad results as the other three implementations.