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 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 cropToCylinder(polyData, p1, p2, radius): polyData = segmentation.cropToLineSegment(polyData, p1, p2) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine(polyData, p1, p2) polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, radius]) return polyData
def onNewWalkingGoal(self, walkingGoal=None): walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() if self.placer: self.placer.stop() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1]) polyData = segmentation.thresholdPoints( polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax( vnp.getNumpyFromVtk(polyData, 'Points')[:, 2]) frameObj.transform.Translate( pos - np.array(frameObj.transform.GetPosition())) d = DebugData() d.addSphere((0, 0, 0), radius=0.03) handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1, 1, 0]) handle.actor.SetUserTransform(frameObj.transform) self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == 'Edit': if propertySet.getProperty(propertyName): self.placer.start() else: self.placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, 'Edit') frameObj.connectFrameModified(self.onWalkingGoalModified) self.onWalkingGoalModified(frameObj)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None): d = DebugData() self.uid = uid vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view) self.transform = seed_pose d.addSphere((0,0,0), radius=0.02) self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds')) self.seedObj.actor.SetUserTransform(self.transform) self.frameObj = vis.showFrame(self.transform, 'region seed frame', scale=0.2, visible=False, parent=self.seedObj) self.frameObj.setProperty('Edit', True) self.frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: rep = self.frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) pos = np.array(self.frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1]) polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2]) self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition())) self.placer = PlacerWidget(view, self.seedObj, terrain) self.placer.start() else: self.frameObj.setProperty('Edit', True) self.frameObj.setProperty('Visible', True) self.driver = irisDriver self.safe_region = None self.addProperty('Visible', True) self.addProperty('Enabled for Walking', True) self.addProperty('Alpha', 1.0) self.addProperty('Color', QtGui.QColor(200,200,20)) self.frameObj.connectFrameModified(self.onFrameModified) if existing_region is None: self.onFrameModified(self.frameObj) else: self.setRegion(existing_region) self.setProperty('Alpha', 0.5) self.setProperty('Color', QtGui.QColor(220,220,220))
def onNewWalkingGoal(self, walkingGoal=None): walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() if self.placer: self.placer.stop() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1]) polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2]) frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition())) d = DebugData() d.addSphere((0,0,0), radius=0.03) handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0]) handle.actor.SetUserTransform(frameObj.transform) self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == 'Edit': if propertySet.getProperty(propertyName): self.placer.start() else: self.placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, 'Edit') frameObj.connectFrameModified(self.onWalkingGoalModified) self.onWalkingGoalModified(frameObj)
def cropPointCloudToModel(pointCloud, objectPointCloud, distanceThreshold=0.02, visualize=True, applyEuclideanClustering=True): """ Crops pointCloud to just the points withing distanceThreshold of objectPointCloud :param pointCloud: :param objectPointCloud: :param distanceThreshold: :return: cropped pointcloud """ registration = GlobalRegistrationUtils pointCloud = GlobalRegistration.cropPointCloudToModelBoundingBox( pointCloud, objectPointCloud, scaleFactor=1.5) arrayName = 'distance_to_mesh' print "computing point to point distance" dists = registration.computePointToPointDistance( pointCloud, objectPointCloud) vnp.addNumpyToVtk(pointCloud, dists, arrayName) polyData = filterUtils.thresholdPoints(pointCloud, arrayName, [0.0, distanceThreshold]) # this stuff may be unecessary if applyEuclideanClustering: # polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) polyData = segmentation.applyEuclideanClustering( polyData, clusterTolerance=0.04) polyData = segmentation.thresholdPoints(polyData, 'cluster_labels', [1, 1]) if visualize: parent = om.getOrCreateContainer('global registration') vis.updatePolyData(polyData, 'cropped pointcloud', color=[0, 1, 0], parent=parent) return polyData
def fitRunningBoardAtFeet(self): # get stance frame startPose = self.getPlanningStartPose() stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False) stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame) # get pointcloud and extract search region covering the running board polyData = segmentation.getCurrentRevolutionData() polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) _, polyData = segmentation.removeGround(polyData) polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1]) if not polyData.GetNumberOfPoints(): print 'empty search region point cloud' return vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False) # extract maximal points along the stance x axis perpAxis = stanceFrameAxes[0] edgeAxis = stanceFrameAxes[1] edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis) edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints) vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True) # ransac fit a line to the edge points linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints) if np.dot(lineDirection, stanceFrameAxes[1]) < 0: lineDirection = -lineDirection linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0]) dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection) p1 = linePoint + lineDirection*np.min(dists) p2 = linePoint + lineDirection*np.max(dists) vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False) # compute a new frame that is in plane with the stance frame # and matches the orientation and position of the detected edge origin = np.array(stanceFrame.GetPosition()) normal = np.array(stanceFrameAxes[2]) # project stance origin to edge, then back to foot frame originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection) originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal) zaxis = np.array(stanceFrameAxes[2]) yaxis = np.array(lineDirection) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) d = DebugData() d.addSphere(p1, radius=0.005) d.addSphere(p2, radius=0.005) d.addLine(p1, p2) d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0]) d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0]) d.addLine(originProjectedToPlane, origin, color=[0,1,0]) d.addLine(originProjectedToEdge, origin, color=[1,0,0]) vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False) # update the running board box affordance position and orientation to # fit the detected edge box = self.spawnRunningBoardAffordance() boxDimensions = box.getProperty('Dimensions') t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane) t.PreMultiply() t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0) box.getChildFrame().copyFrame(t) self.initialize()
normals = vnp.getNumpyFromVtk(polyData, 'normals') segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1]) normalsValid = np.any(normals, axis=1) vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32), 'normals_valid') vis.showPolyData(polyData, 'scene points', colorByName='normals_valid', visible=False) numPoints = polyData.GetNumberOfPoints() polyData = segmentation.thresholdPoints(polyData, 'normals_valid', [1, 1]) vis.showPolyData(polyData, 'cloud normals', colorByName='curvature', visible=True) print 'number of filtered points:', numPoints - polyData.GetNumberOfPoints( ) if showGlyphs: polyData.GetPointData().SetNormals( polyData.GetPointData().GetArray('normals')) arrows = segmentation.applyArrowGlyphs(polyData, computeNormals=False) disks = segmentation.applyDiskGlyphs(polyData, computeNormals=False) polyData.GetPointData().SetNormals(None)
def fitRunningBoardAtFeet(self): # get stance frame startPose = self.getPlanningStartPose() stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint( self.robotSystem.robotStateModel, useWorldZ=False) stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame) # get pointcloud and extract search region covering the running board polyData = segmentation.getCurrentRevolutionData() polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) _, polyData = segmentation.removeGround(polyData) polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1]) if not polyData.GetNumberOfPoints(): print('empty search region point cloud') return vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0, 1, 0], visible=False) # extract maximal points along the stance x axis perpAxis = stanceFrameAxes[0] edgeAxis = stanceFrameAxes[1] edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis) edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints) vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True) # ransac fit a line to the edge points linePoint, lineDirection, fitPoints = segmentation.applyLineFit( edgePoints) if np.dot(lineDirection, stanceFrameAxes[1]) < 0: lineDirection = -lineDirection linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0]) dists = np.dot( vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint, lineDirection) p1 = linePoint + lineDirection * np.min(dists) p2 = linePoint + lineDirection * np.max(dists) vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False) # compute a new frame that is in plane with the stance frame # and matches the orientation and position of the detected edge origin = np.array(stanceFrame.GetPosition()) normal = np.array(stanceFrameAxes[2]) # project stance origin to edge, then back to foot frame originProjectedToEdge = linePoint + lineDirection * np.dot( origin - linePoint, lineDirection) originProjectedToPlane = segmentation.projectPointToPlane( originProjectedToEdge, origin, normal) zaxis = np.array(stanceFrameAxes[2]) yaxis = np.array(lineDirection) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) d = DebugData() d.addSphere(p1, radius=0.005) d.addSphere(p2, radius=0.005) d.addLine(p1, p2) d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0]) d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0]) d.addLine(originProjectedToPlane, origin, color=[0, 1, 0]) d.addLine(originProjectedToEdge, origin, color=[1, 0, 0]) vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False) # update the running board box affordance position and orientation to # fit the detected edge box = self.spawnRunningBoardAffordance() boxDimensions = box.getProperty('Dimensions') t = transformUtils.getTransformFromAxesAndOrigin( xaxis, yaxis, zaxis, originProjectedToPlane) t.PreMultiply() t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0) box.getChildFrame().copyFrame(t) self.initialize()
segmentationroutines.SegmentationContext.initWithUser(groundHeight, viewFrame) # load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData( os.path.join(dataDir, 'tabletop/table-sparse-stereo.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot original', colorByName='rgb_colors') polyData = segmentationroutines.sparsifyStereoCloud(polyData) vis.showPolyData(polyData, 'pointcloud snapshot') # Use only returns near the robot: polyData = segmentation.addCoordArraysToPolyData(polyData) polyData = segmentation.thresholdPoints(polyData, 'distance_along_view_x', [0, 1.3]) segmentation.segmentTableThenFindDrills(polyData, [1.2864902, -0.93351376, 1.10208917]) if app.getTestingInteractiveEnabled(): v = applogic.getCurrentView() v.camera().SetPosition([3, 3, 3]) v.camera().SetFocalPoint([0, 0, 0]) view.show() app.showObjectModel() app.start()
from director import transformUtils from director import segmentation from director.debugVis import DebugData from director import vtkAll as vtk from director import vtkNumpy as vnp import numpy as np polyData = ioUtils.readPolyData('snapshot_2.vtp') polyData = segmentation.cropToBounds(polyData, vtk.vtkTransform(), [[-10, 10], [-10, 10], [0.15, 10]]) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) polyData = segmentation.applyEuclideanClustering(polyData, clusterTolerance=0.04) polyData = segmentation.thresholdPoints(polyData, 'cluster_labels', [1, 1]) vis.showPolyData(polyData, 'snapshot_2', colorByName='rgb_colors', visible=True) robotMesh = ioUtils.readPolyData('robot_mesh_2.vtp') vis.showPolyData(robotMesh, 'robot_mesh_2', visible=True) t = segmentation.applyICP(polyData, robotMesh) print t.GetPosition() print t.GetOrientation() polyData = filterUtils.transformPolyData(polyData, t)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None): d = DebugData() self.uid = uid vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view) self.transform = seed_pose d.addSphere((0, 0, 0), radius=0.02) self.seedObj = vis.showPolyData( d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds')) self.seedObj.actor.SetUserTransform(self.transform) self.frameObj = vis.showFrame(self.transform, 'region seed frame', scale=0.2, visible=False, parent=self.seedObj) self.frameObj.setProperty('Edit', True) self.frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: rep = self.frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) pos = np.array(self.frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1]) polyData = segmentation.thresholdPoints( polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax( vnp.getNumpyFromVtk(polyData, 'Points')[:, 2]) self.frameObj.transform.Translate( pos - np.array(self.frameObj.transform.GetPosition())) self.placer = PlacerWidget(view, self.seedObj, terrain) self.placer.start() else: self.frameObj.setProperty('Edit', True) self.frameObj.setProperty('Visible', True) self.driver = irisDriver self.safe_region = None self.addProperty('Visible', True) self.addProperty('Enabled for Walking', True) self.addProperty('Alpha', 1.0) self.addProperty('Color', QtGui.QColor(200, 200, 20)) self.frameObj.connectFrameModified(self.onFrameModified) if existing_region is None: self.onFrameModified(self.frameObj) else: self.setRegion(existing_region) self.setProperty('Alpha', 0.5) self.setProperty('Color', QtGui.QColor(220, 220, 220))
view = app.createView() segmentation._defaultSegmentationView = view robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True) segmentationroutines.SegmentationContext.initWithRobot(robotStateModel) # load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData(os.path.join(dataDir, 'amazon-pod/01-small-changes.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot', visible=False) # remove ground and clip to just the pod: groundPoints, polyData = segmentation.removeGround(polyData) vis.showPolyData(polyData, 'scene', visible=False) polyData = segmentation.addCoordArraysToPolyData(polyData) polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6]) polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5]) vis.showPolyData(polyData, 'clipped', visible=False) # remove outliers polyData = segmentation.labelOutliers(polyData, searchRadius=0.03, neighborsInSearchRadius=40) polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0]) vis.showPolyData(polyData, 'inliers', visible=False) # remove walls, and points behind temp: polyData = removePlaneAndBeyond(polyData, expectedNormal=[0,1,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95) polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=0, whichAxisLetter='x', percentile = 95) polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[0.03, np.inf], whichAxis=0, whichAxisLetter='x', percentile = 5) vis.updatePolyData(polyData, 'only shelves', parent='segmentation', visible=False)
robotStateJointController.push() ''' groundHeight = 0.0 viewFrame = segmentation.transformUtils.frameFromPositionAndRPY([1, -1, groundHeight + 1.5], [0, 0, -35]) segmentationroutines.SegmentationContext.initWithUser(groundHeight, viewFrame) # load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-sparse-stereo.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot original', colorByName='rgb_colors') polyData = segmentationroutines.sparsifyStereoCloud( polyData ) vis.showPolyData(polyData, 'pointcloud snapshot') # Use only returns near the robot: polyData = segmentation.addCoordArraysToPolyData(polyData) polyData = segmentation.thresholdPoints(polyData, 'distance_along_view_x', [0, 1.3]) segmentation.segmentTableThenFindDrills(polyData, [1.2864902, -0.93351376, 1.10208917]) if app.getTestingInteractiveEnabled(): v = applogic.getCurrentView() v.camera().SetPosition([3,3,3]) v.camera().SetFocalPoint([0,0,0]) view.show() app.showObjectModel() app.start()
robot_config = drcargs.getRobotConfig() urdf_path = os.path.join(robot_config.dirname, robot_config['urdfConfig']['default']) robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', urdfFile = urdf_path, color=roboturdf.getRobotGrayColor(), visible=True) segmentationroutines.SegmentationContext.initWithRobot(robotStateModel) # load poly data dataDir = app.getTestingDataDirectory() polyData = ioutils.readPolyData(os.path.join(dataDir, 'misc/amazon-pod/01-small-changes.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot', visible=False) # remove ground and clip to just the pod: groundPoints, polyData = segmentation.removeGround(polyData) vis.showPolyData(polyData, 'scene', visible=False) polyData = segmentation.addCoordArraysToPolyData(polyData) polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6]) polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5]) vis.showPolyData(polyData, 'clipped', visible=False) # remove outliers polyData = segmentation.labelOutliers(polyData, searchRadius=0.03, neighborsInSearchRadius=40) polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0]) vis.showPolyData(polyData, 'inliers', visible=False) # remove walls, and points behind temp: polyData = removePlaneAndBeyond(polyData, expectedNormal=[0,1,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95) polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=0, whichAxisLetter='x', percentile = 95) polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[0.03, np.inf], whichAxis=0, whichAxisLetter='x', percentile = 5) vis.updatePolyData(polyData, 'only shelves', parent='segmentation', visible=False)
def newWalkingGoal(self, displayPoint, view): # put walking goal at robot's base mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"] footFrame = self.robotModel.getLinkFrame(mainLink) if not footFrame: print( "ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in " "the robot's URDF. Cannot place walking goal.".format(mainLink) ) return worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint) groundOrigin = footFrame.GetPosition() groundNormal = [0.0, 0.0, 1.0] selectedGroundPoint = [0.0, 0.0, 0.0] t = vtk.mutable(0.0) vtk.vtkPlane.IntersectWithLine( worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint ) walkingTarget = transformUtils.frameFromPositionAndRPY( selectedGroundPoint, np.array(footFrame.GetOrientation()) ) frameObj = vis.updateFrame( walkingTarget, self.robotName + " walking goal", parent="planning", scale=0.25, ) frameObj.setProperty("Edit", True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() terrain = om.findObjectByName("HEIGHT_MAP_SCENE") if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1] ) polyData = segmentation.thresholdPoints( polyData, "distance_to_line", [0.0, 0.1] ) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2]) frameObj.transform.Translate( pos - np.array(frameObj.transform.GetPosition()) ) d = DebugData() d.addSphere((0, 0, 0), radius=0.03) handle = vis.showPolyData( d.getPolyData(), "walking goal terrain handle " + self.robotName, parent=frameObj, visible=True, color=[1, 1, 0], ) handle.actor.SetUserTransform(frameObj.transform) placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == "Edit": if propertySet.getProperty(propertyName): placer.start() else: placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, "Edit") frameObj.connectFrameModified(self.onWalkingGoalModified)
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'
print 'done.' # filter points without normals normals = vnp.getNumpyFromVtk(polyData, 'normals') segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1]) normalsValid = np.any(normals, axis=1) vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32), 'normals_valid') vis.showPolyData(polyData, 'scene points', colorByName='normals_valid', visible=False) numPoints = polyData.GetNumberOfPoints() polyData = segmentation.thresholdPoints(polyData, 'normals_valid', [1, 1]) vis.showPolyData(polyData, 'cloud normals', colorByName='curvature', visible=True) print 'number of filtered points:', numPoints - polyData.GetNumberOfPoints() if showGlyphs: polyData.GetPointData().SetNormals(polyData.GetPointData().GetArray('normals')) arrows = segmentation.applyArrowGlyphs(polyData, computeNormals=False) disks = segmentation.applyDiskGlyphs(polyData, computeNormals=False) polyData.GetPointData().SetNormals(None) vis.showPolyData(arrows, 'arrows') vis.showPolyData(disks, 'disks') #arrows2 = segmentation.applyArrowGlyphs(polyData) #vis.showPolyData(arrows2, 'arrows pcl normals')