Got ICP mapping working, working way better on this dataset than optical flow