fixed the point clouds registration. Code needs cleaning + refactoring.