ICP is implemented and it works!  Also created a test program to make a video of the iterations