Code refactoring: added PointCloudUtils
Tested new code: ok