def segmentBlockByTopPlane(polyData, blockDimensions, expectedNormal, expectedXAxis, edgeSign=1, name='block affordance'):

    polyData, planeOrigin, normal  = applyPlaneFit(polyData, distanceThreshold=0.05, expectedNormal=expectedNormal, returnOrigin=True)

    _, lineDirection, _ = applyLineFit(polyData)

    zaxis = lineDirection
    yaxis = normal
    xaxis = np.cross(yaxis, zaxis)

    if np.dot(xaxis, expectedXAxis) < 0:
        xaxis *= -1

    # make right handed
    zaxis = np.cross(xaxis, yaxis)
    xaxis /= np.linalg.norm(xaxis)
    yaxis /= np.linalg.norm(yaxis)
    zaxis /= np.linalg.norm(zaxis)

    expectedXAxis = np.array(xaxis)

    edgePoints = computeEdge(polyData, zaxis, xaxis*edgeSign)
    edgePoints = vtkNumpy.getVtkPolyDataFromNumpyPoints(edgePoints)

    d = DebugData()
    obj = updatePolyData(edgePoints, 'edge points', parent=getDebugFolder(), visible=False)

    linePoint, lineDirection, _ = applyLineFit(edgePoints)
    zaxis = lineDirection
    xaxis = np.cross(yaxis, zaxis)


    if np.dot(xaxis, expectedXAxis) < 0:
        xaxis *= -1

    # make right handed
    zaxis = np.cross(xaxis, yaxis)
    xaxis /= np.linalg.norm(xaxis)
    yaxis /= np.linalg.norm(yaxis)
    zaxis /= np.linalg.norm(zaxis)

    polyData = labelPointDistanceAlongAxis(polyData, xaxis, resultArrayName='dist_along_line')
    pts = vtkNumpy.getNumpyFromVtk(polyData, 'Points')

    dists = np.dot(pts-linePoint, zaxis)

    p1 = linePoint + zaxis*np.min(dists)
    p2 = linePoint + zaxis*np.max(dists)

    p1 = projectPointToPlane(p1, planeOrigin, normal)
    p2 = projectPointToPlane(p2, planeOrigin, normal)

    xwidth, ywidth = blockDimensions
    zwidth = np.linalg.norm(p2 - p1)

    origin = p1 - edgeSign*xaxis*xwidth/2.0 - yaxis*ywidth/2.0 + zaxis*zwidth/2.0

    d = DebugData()

    #d.addSphere(linePoint, radius=0.02)
    #d.addLine(linePoint, linePoint + yaxis*ywidth)
    #d.addLine(linePoint, linePoint + xaxis*xwidth)
    #d.addLine(linePoint, linePoint + zaxis*zwidth)


    d.addSphere(p1, radius=0.01)
    d.addSphere(p2, radius=0.01)
    d.addLine(p1, p2)

    d.addSphere(origin, radius=0.01)
    #d.addLine(origin - xaxis*xwidth/2.0, origin + xaxis*xwidth/2.0)
    #d.addLine(origin - yaxis*ywidth/2.0, origin + yaxis*ywidth/2.0)
    #d.addLine(origin - zaxis*zwidth/2.0, origin + zaxis*zwidth/2.0)

    d.addLine(origin, origin + xaxis*xwidth/2.0)
    d.addLine(origin, origin + yaxis*ywidth/2.0)
    d.addLine(origin, origin + zaxis*zwidth/2.0)


    #obj = updatePolyData(d.getPolyData(), 'block axes')
    #obj.setProperty('Color', QtGui.QColor(255, 255, 0))
    #obj.setProperty('Visible', False)

    obj = createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name)
    obj.setProperty('Color', [222/255.0, 184/255.0, 135/255.0])

    computeDebrisGraspSeed(obj)
    t = computeDebrisStanceFrame(obj)
    if t:
        showFrame(t, 'debris stance frame', parent=obj)
        obj.publishCallback = functools.partial(publishDebrisStanceFrame, obj)

    return obj
dataset/ETHPy150Open RobotLocomotion/director/src/python/director/segmentation.py/segmentBlockByTopPlane/original