Fix bugs and start pointset registration