def segmentDrillInHand(p1, p2):

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

    distanceToLineThreshold = 0.05

    polyData = labelDistanceToLine(polyData, p1, p2)
    polyData = thresholdPoints(polyData, 'distance_to_line', [0.0, distanceToLineThreshold])

    lineSegment = p2 - p1
    lineLength = np.linalg.norm(lineSegment)

    cropped, polyData = cropToPlane(polyData, p1, lineSegment/lineLength, [-0.03, lineLength + 0.03])

    updatePolyData(cropped, 'drill cluster', parent=getDebugFolder(), visible=False)


    drillPoints = cropped
    normal = lineSegment/lineLength

    centroids = computeCentroids(drillPoints, axis=normal)

    centroidsPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(centroids)
    d = DebugData()
    updatePolyData(centroidsPolyData, 'cluster centroids', parent=getDebugFolder(), visible=False)

    drillToTopPoint = np.array([-0.002904, -0.010029, 0.153182])

    zaxis = normal
    yaxis = centroids[0] - centroids[-1]
    yaxis /= np.linalg.norm(yaxis)
    xaxis = np.cross(yaxis, zaxis)
    xaxis /= np.linalg.norm(xaxis)
    yaxis = np.cross(zaxis, xaxis)

    t = getTransformFromAxes(xaxis, yaxis, zaxis)
    t.PreMultiply()
    t.Translate(-drillToTopPoint)
    t.PostMultiply()
    t.Translate(p2)

    drillMesh = getDrillMesh()

    aff = showPolyData(drillMesh, 'drill', cls=FrameAffordanceItem, visible=True)
    aff.actor.SetUserTransform(t)
    showFrame(t, 'drill frame', parent=aff, visible=False).addToView(app.getDRCView())

    params = getDrillAffordanceParams(np.array(t.GetPosition()), xaxis, yaxis, zaxis)
    aff.setAffordanceParams(params)
    aff.updateParamsFromActorTransform()
    aff.addToView(app.getDRCView())
dataset/ETHPy150Open RobotLocomotion/director/src/python/director/segmentation.py/segmentDrillInHand/original