Implemented mean shift for 3d points