fixed the point clouds registration. Code needs cleaning though.