Cleanup code and improve reg speed

Improve speed for image registration