Replaced NNVector3 by Eigen implementation
First (rigid body)  ICP implementation + fixes