sparse ICP now works with indices
now returning the proper transformation