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 onSegmentTableScene(): data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint) vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
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')
def onSegmentTableScene(): data = segmentation.segmentTableScene(pointCloudObj.polyData, pickedPoint) vis.showClusterObjects(data.clusters, parent='segmentation') segmentation.showTable(data.table, parent='segmentation')
'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()
# 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()
def fit(self, polyData, points): iiwaplanning.fitSupport(pickPoint=points[0]) return pickPoint = points[0] t = vtk.vtkTransform() t.Translate(pickPoint) print 'pick point:', pickPoint print 'crop' polyData = segmentation.cropToBox(polyData, t, [0.3,0.3,0.5]) import colorsys rgb = vnp.getNumpyFromVtk(polyData, 'rgb_colors')/255.0 hsv = np.array([colorsys.rgb_to_hsv(*t) for t in rgb]) vnp.addNumpyToVtk(polyData, hsv[:,0].copy(), 'hue') vnp.addNumpyToVtk(polyData, hsv[:,1].copy(), 'saturation') vnp.addNumpyToVtk(polyData, hsv[:,2].copy(), 'value') vis.updatePolyData(polyData, 'crop region', colorByName='rgb_colors', visible=False) # hide input data #om.findObjectByName(self.pointCloudObjectName).setProperty('Visible', False) #cluster = segmentation.makePolyDataFields(polyData) #vis.showClusterObjects([cluster], parent='segmentation') hueRange = [0.12, 0.14] valueRange = [0.5, 1.0] print 'thresh' points = segmentation.thresholdPoints(segmentation.thresholdPoints(polyData, 'hue', hueRange), 'value', valueRange) #points = segmentation.extractLargestCluster(points, minClusterSize=10, clusterTolerance=0.02) vis.updatePolyData(points, 'pole points', color=[0,0,1], visible=False) maxZ = np.nanmax(vnp.getNumpyFromVtk(points, 'Points')[:,2]) print 'maxZ', maxZ pickPoint = pickPoint[0], pickPoint[1], maxZ+0.2 print pickPoint t = vtk.vtkTransform() t.Translate(pickPoint) print 'crop2' polyData = segmentation.cropToBox(polyData, t, [0.15,0.15,0.4]) vis.updatePolyData(polyData, 'object points', colorByName='rgb_colors', visible=False) if polyData.GetNumberOfPoints() > 5: print 'make fields' cluster = segmentation.makePolyDataFields(polyData) vis.showClusterObjects([cluster], parent='segmentation') print 'done'