def findAndFitDrillBarrel(polyData=None):
    ''' Find the horizontal surfaces
    on the horizontal surfaces, find all the drills
    '''

    inputObj = om.findObjectByName('pointcloud snapshot')
    polyData = polyData or inputObj.polyData

    groundPoints, scenePoints =  removeGround(polyData, groundThickness=0.02, sceneHeightFromGround=0.50)

    scenePoints = thresholdPoints(scenePoints, 'dist_to_plane', [0.5, 1.7])

    if not scenePoints.GetNumberOfPoints():
        return

    normalEstimationSearchRadius = 0.10

    f = vtk.vtkPCLNormalEstimation()
    f.SetSearchRadius(normalEstimationSearchRadius)
    f.SetInput(scenePoints)
    f.Update()
    scenePoints = shallowCopy(f.GetOutput())

    normals = vtkNumpy.getNumpyFromVtk(scenePoints, 'normals')
    normalsDotUp = np.abs(np.dot(normals, [0,0,1]))

    vtkNumpy.addNumpyToVtk(scenePoints, normalsDotUp, 'normals_dot_up')

    surfaces = thresholdPoints(scenePoints, 'normals_dot_up', [0.95, 1.0])


    updatePolyData(groundPoints, 'ground points', parent=getDebugFolder(), visible=False)
    updatePolyData(scenePoints, 'scene points', parent=getDebugFolder(), colorByName='normals_dot_up', visible=False)
    updatePolyData(surfaces, 'surfaces', parent=getDebugFolder(), visible=False)

    clusters = extractClusters(surfaces, clusterTolerance=0.15, minClusterSize=50)

    fitResults = []

    viewFrame = SegmentationContext.getGlobalInstance().getViewFrame()
    forwardDirection = np.array([1.0, 0.0, 0.0])
    viewFrame.TransformVector(forwardDirection, forwardDirection)
    robotOrigin = viewFrame.GetPosition()
    robotForward =forwardDirection

    #print 'robot origin:', robotOrigin
    #print 'robot forward:', robotForward
    centroid =[]

    for clusterId, cluster in enumerate(clusters):
        clusterObj = updatePolyData(cluster, 'surface cluster %d' % clusterId, color=[1,1,0], parent=getDebugFolder(), visible=False)

        origin, edges, _ = getOrientedBoundingBox(cluster)
        edgeLengths = [np.linalg.norm(edge) for edge in edges[:2]]

        skipCluster = False
        for edgeLength in edgeLengths:
            #print 'cluster %d edge length: %f' % (clusterId, edgeLength)
            if edgeLength < 0.35 or edgeLength > 0.75:
                skipCluster = True

        if skipCluster:
            continue

        clusterObj.setSolidColor([0, 0, 1])
        centroid = np.average(vtkNumpy.getNumpyFromVtk(cluster, 'Points'), axis=0)

        try:
            drillFrame = segmentDrillBarrelFrame(centroid, polyData=scenePoints, forwardDirection=robotForward)
            if drillFrame is not None:
                fitResults.append((clusterObj, drillFrame))
        except:
            print traceback.format_exc()
            print 'fit drill failed for cluster:', clusterId

    if not fitResults:
        return

    sortFittedDrills(fitResults, robotOrigin, robotForward)

    return centroid
dataset/ETHPy150Open RobotLocomotion/director/src/python/director/segmentation.py/findAndFitDrillBarrel/original