I've enjoyed using your code, but for a certain application I'm working on, I extracted your eq_point function inside icp.m and found what might be an issue.
There are some scenarios where the returned rotation matrix has det(R)==-1, and for my application this is an issue. I understand that in many real-world datasets this might never come up, but the fix seems easy.
It has already been noted that the closed form SVD-based solution you are using for [R,T] estimation from correspondences can sometimes be problematic.
You can checkout the following classic paper from Umeyama:
my solution was to change on line of eq_point to do the following:
R = V*diag([1 1 det(U*V')])*U';
I haven't thoroughly tested this, but it *seems* to fix the problem for me.
Cheers and happy hacking,
Dr. Tomasz Malisiewicz
MIT CSAIL Postdoctoral Research Scholar