Add debug function for point clouds and correctly estimate angle.