Python segmentTableScene Examples

Example #1

File: testAmazonPod.py Project: ori-drs/director

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

Example #2

File: testAmazonPod.py Project: RobotLocomotion/director

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

Example #3

File: viewbehaviors.py Project: tkoolen/director

 def onSegmentTableScene():
     data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint)
     vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')

Example #4

File: testAffordanceItems.py Project: caomw/director

def segmentTableTopPointCloud():
    polyData = om.findObjectByName('pointcloud snapshot').polyData
    pickPoint = [-0.70420271,  0.64272028,  1.07856214]
    data = segmentation.segmentTableScene(polyData, pickPoint)
    vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')

Example #5

File: robotviewbehaviors.py Project: openhumanoids/director

 def onSegmentTableScene():
     data = segmentation.segmentTableScene(pointCloudObj.polyData,
                                           pickedPoint)
     vis.showClusterObjects(data.clusters, parent='segmentation')
     segmentation.showTable(data.table, parent='segmentation')

Example #6

    'robot state model',
    view,
    parent='sensors',
    urdfFile=urdf_path,
    color=roboturdf.getRobotGrayColor(),
    visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)

# Move robot to near to table:
robotStateJointController.q[5] = math.radians(120)
robotStateJointController.q[0] = -1.5
robotStateJointController.q[1] = 2
robotStateJointController.q[2] = 0.95
robotStateJointController.push()

# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioutils.readPolyData(
    os.path.join(dataDir, 'misc/tabletop/table-and-bin-scene.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot')

p1 = [-1.58661389, 2.91242337, 0.79958105]
data = segmentation.segmentTableScene(polyData, p1)
vis.showClusterObjects(data.clusters, parent='segmentation')
segmentation.showTable(data.table, parent='segmentation')

if app.getTestingInteractiveEnabled():
    view.show()
    app.showObjectModel()
    app.start()

Example #7

File: testAffordanceItems.py Project: jediofgever/director

def segmentTableTopPointCloud():
    polyData = om.findObjectByName('pointcloud snapshot').polyData
    pickPoint = [-0.70420271, 0.64272028, 1.07856214]
    data = segmentation.segmentTableScene(polyData, pickPoint)
    vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')

Example #8

File: testTableFit.py Project: RobotLocomotion/director

# create a view
view = app.createView()
segmentation._defaultSegmentationView = view
segmentation.initAffordanceManager(view)

robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)

# Move robot to near to table:
robotStateJointController.q [5] = math.radians(120)
robotStateJointController.q[0] = -1.5
robotStateJointController.q[1] = 2
robotStateJointController.q[2] = 0.95
robotStateJointController.push()

# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-and-bin-scene.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot')

p1 = [-1.58661389,  2.91242337,  0.79958105]
data = segmentation.segmentTableScene(polyData, p1)
vis.showClusterObjects(data.clusters, parent='segmentation')
segmentation.showTable(data.table, parent='segmentation')

if app.getTestingInteractiveEnabled():
    view.show()
    app.showObjectModel()
    app.start()