Exemple #1
0
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
    # find the shelf plane:
    polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
    polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront,  expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
    vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)

    shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
    shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
    shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
    vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)

    # find the points near to the shelf plane and find objects on it:
    points = vnp.getNumpyFromVtk(polyData, 'Points')
    dist = np.dot(points - origin, normal)
    vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
    shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
    vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)

    data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
    vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')

    # remove the points that we considered from the orginal cloud
    dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
    diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
    vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
    polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])

    vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
    return polyData
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
    # find the shelf plane:
    polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
    polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront,  expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
    vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)

    shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
    shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
    shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
    vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)

    # find the points near to the shelf plane and find objects on it:
    points = vnp.getNumpyFromVtk(polyData, 'Points')
    dist = np.dot(points - origin, normal)
    vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
    shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
    vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)

    data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
    vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')

    # remove the points that we considered from the orginal cloud
    dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
    diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
    vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
    polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])

    vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
    return polyData
Exemple #3
0
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):

    yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
    backY = np.percentile(yvalues, percentile)

    if ( percentile > 50):
        searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
    else:
        searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])

    vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)

    # find the plane of the back wall, remove it and the points behind it:
    _, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)

    points = vnp.getNumpyFromVtk(polyData, 'Points')
    dist = np.dot(points - origin, normal)
    vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')

    backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
    vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
    vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)

    polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
    vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)

    return polyData
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):

    yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
    backY = np.percentile(yvalues, percentile)

    if ( percentile > 50):
        searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
    else:
        searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])

    vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)

    # find the plane of the back wall, remove it and the points behind it:
    _, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)

    points = vnp.getNumpyFromVtk(polyData, 'Points')
    dist = np.dot(points - origin, normal)
    vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')

    backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
    vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
    vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)

    polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
    vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)

    return polyData
Exemple #5
0
    def segmentTable(scenePolyData=None,
                     searchRadius=0.3,
                     visualize=True,
                     thickness=0.01,
                     pointOnTable=None,
                     pointAboveTable=None,
                     computeAboveTablePolyData=False):
        """
        This requires two clicks using measurement panel. One on the table, one above the table on one of the objects. Call them point0, point1. Then we will attempt to fit a plane that passes through point0 with approximate normal point1 - point0
        :param scenePolyData:
        :param searchRadius:
        :return:
        """

        if scenePolyData is None:
            scenePolyData = om.findObjectByName('reconstruction').polyData

        assert scenePolyData is not None
        assert pointOnTable is not None
        assert pointAboveTable is not None

        expectedNormal = pointAboveTable - pointOnTable
        expectedNormal = expectedNormal / np.linalg.norm(expectedNormal)

        polyData, normal = segmentation.applyPlaneFit(
            scenePolyData,
            searchOrigin=pointOnTable,
            searchRadius=searchRadius,
            expectedNormal=expectedNormal)

        # get points above plane
        abovePolyData = None
        belowPolyData = None
        if computeAboveTablePolyData:
            abovePolyData = filterUtils.thresholdPoints(
                polyData, 'dist_to_plane', [thickness / 2.0, np.inf])
            belowPolyData = filterUtils.thresholdPoints(
                polyData, 'dist_to_plane', [-np.inf, -thickness / 2.0])

        # some debugging visualization
        if visualize:
            visFolder = om.getOrCreateContainer('debug')

            if abovePolyData is not None:
                vis.showPolyData(abovePolyData,
                                 'above table segmentation',
                                 color=[0, 1, 0],
                                 parent=visFolder)
            arrowLength = 0.3
            headRadius = 0.02
            d = DebugData()
            visFolder = om.getOrCreateContainer('debug')
            d.addArrow(pointOnTable,
                       pointOnTable + arrowLength * expectedNormal,
                       headRadius=headRadius)
            vis.showPolyData(d.getPolyData(),
                             'expected normal',
                             color=[1, 0, 0],
                             parent=visFolder)

            d = DebugData()
            d.addArrow(pointOnTable,
                       pointOnTable + arrowLength * normal,
                       headRadius=headRadius)
            vis.showPolyData(d.getPolyData(),
                             'computed normal',
                             color=[0, 1, 0],
                             parent=visFolder)

        returnData = dict()
        returnData['abovePolyData'] = abovePolyData
        returnData['polyData'] = polyData
        returnData['normal'] = normal
        returnData['pointOnTable'] = pointOnTable
        return returnData