Integrated Steve's code with mine (PCL Octree)