Finish new sort of pointset

Need to refractor all the sort into a single function, need to debug the
transform matrix to elastix transform