add generalized and non-linear ICP
registration tests: refactor, strengthen, assert instead of print