fixed some errors in ICP point to plane

Hacked around the mistakes by adding arbitrary values to the matrix
which is fed to the solver