Depth map to point cloud

Perhaps you can adapt this example to your needs.