ICP: Fix and speed up

! Fix ICP (use V and Ut instead of Vt and U; transform the second matrix
  instead of overwriting it with a transformation of the first one)
* Speed ICP up by using std::shuffle for the random selection (may make
  things slower for low point counts, but that was fast anyway; but is
  very important for large point counts) and threading the nearest
  neighbor search