Add point cloud functionality and fix some code