addition of matrix getter
this has been bugging me for a while...
partial icp implementation