ICP that handle normales and arbitraty transformations for 3D meshes

Hello, In the research work done by Johnson and Hebert 1998, authors have developped a variant of ICP, in order to verify transformation and associated correspondances computed from the matching stage. This variant takes in account both position and normals to handle partial overlaps. In addition it handles too arbitrary transformations, based on the computed one from the matching stage.

So I am seeking for the implementation of this variant in matlab if it exists, and it is been a whole 2 days I am searching but worthless.