def fitSupport(pickPoint=[0.92858565, 0.00213802, 0.30315629]): om.removeFromObjectModel(om.findObjectByName('cylinder')) polyData = getPointCloud() t = vtk.vtkTransform() t.Translate(pickPoint) polyData = segmentation.cropToBox(polyData, t, [0.3, 0.3, 0.5]) addHSVArrays(polyData) vis.updatePolyData(polyData, 'crop region', colorByName='rgb_colors', visible=False) zMax = getMaxZCoordinate(polyData) cyl = makeCylinder() cyl.setProperty('Radius', 0.03) cyl.setProperty('Length', zMax) origin = segmentation.computeCentroid(polyData) origin[2] = zMax / 2.0 t = transformUtils.frameFromPositionAndRPY(origin, [0, 0, 0]) cyl.getChildFrame().copyFrame(t)
def processSingleBlock(robotStateModel, whichFile=0): if (whichFile == 0): polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/terrain_simple_ihmc.vtp')) vis.updatePolyData( polyData, 'terrain_simple_ihmc.vtp', parent='continuous') else: polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/terrain_stairs_ihmc.vtp')) cwdemo.chosenTerrain = 'stairs' cwdemo.supportContact = lcmdrc.footstep_params_t.SUPPORT_GROUPS_MIDFOOT_TOE vis.updatePolyData( polyData, 'terrain_stairs_ihmc.vtp', parent='continuous') if drcargs.args().directorConfigFile.find('atlas') != -1: standingFootName = 'l_foot' else: standingFootName = 'leftFoot' standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False) # Step 1: filter the data down to a box in front of the robot: polyData = cwdemo.getRecedingTerrainRegion(polyData, footstepsDriver.getFeetMidPoint(robotStateModel)) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if (clusters is None): print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def rotateAndSegmentPointCloud(self): print "rotating and pointcloud" firstFrameToWorld = self.rotateReconstructionToStandardOrientation() self.firstFrameToWorldTransform = firstFrameToWorld pointOnTable = np.array( firstFrameToWorld.TransformPoint( self.measurementPanel.pickPoints[0])) pointAboveTable = np.array( firstFrameToWorld.TransformPoint( self.measurementPanel.pickPoints[1])) print "segmenting table" d = self.segmentTable(pointOnTable=pointOnTable, pointAboveTable=pointAboveTable, visualize=False) vis.updatePolyData(d['abovePolyData'], 'above table segmentation', colorByName='RGB', parent=self.visFolder) print "saving segmentation" self.saveAboveTablePolyData() self.initializeFields()
def updateDrawPolyApprox(self, frame): distances = self.Sensor.raycastAll(frame) polyCoefficients = self.SensorApproximator.polyFitConstrainedLP(distances) if polyCoefficients == None: polyCoefficients = [0,0] d = DebugData() x = self.SensorApproximator.approxThetaVector y = x * 0.0 for index,val in enumerate(y): y[index] = self.horner(x[index],polyCoefficients) origin = np.array(frame.transform.GetPosition()) origin[2] = -0.001 for i in xrange(self.SensorApproximator.numApproxPoints): if y[i] > 0: ray = self.SensorApproximator.approxRays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) intersection = origin + rayTransformed * y[i] intersection[2] = -0.001 d.addLine(origin, intersection, color=[0,0,1]) vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def cropPointCloud(self, point=None, pointCloud=None, radius=0.3, visualize=True): """ Crop pointcloud using sphere around given point and radius :param point: defaults to point chosen using measurement panel :param pointCloud: defaults to 'reconstruction' :return: cropped point cloud, polyData """ if point is None: if len(self.measurementPanel.pickPoints) == 0: print "you haven't selected any points using measurement panel" return point = self.measurementPanel.pickPoints[-1] if pointCloud is None: pointCloud = om.findObjectByName('reconstruction').polyData croppedPointCloud = segmentation.cropToSphere(pointCloud, point, radius=radius) if visualize: vis.updatePolyData(croppedPointCloud, 'cropped pointcloud') return croppedPointCloud
def updateDrawNormals(self, frame): leftDistances = self.raycastLeftOrRight(frame, self.leftRays) rightDistances = self.raycastLeftOrRight(frame, self.rightRays) d = DebugData() x = self.SensorApproximator.approxThetaVector y = x * 0.0 for index,val in enumerate(y): y[index] = self.horner(x[index],polyCoefficients) origin = np.array(frame.transform.GetPosition()) origin[2] = -0.001 for i in xrange(self.numRays): #ray = self.SensorApproximator.approxRays[:,i] #rayTransformed = np.array(frame.transform.TransformNormal(ray)) #intersection = origin + rayTransformed * y[i] #intersection[2] = -0.001 p1 = leftDistances[i] p2 = rightDistances[i] d.addLine(p1, p2, color=[0,0.1,1]) vis.updatePolyData(d.getPolyData(), 'polyApprox', colorByName='RGB255')
def updateDrawIntersection(self, frame, locator="None"): if locator == "None": locator = self.locator origin = np.array(frame.transform.GetPosition()) # print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) # print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255")
def onVtkObject(name): obj = objectMap.take(name) if not obj: return if isinstance(obj, vtk.vtkPolyData): vis.updatePolyData(obj, name)
def processSnippet(): obj = om.getOrCreateContainer("continuous") om.getOrCreateContainer("cont debug", obj) if continuouswalkingDemo.processContinuousStereo: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet_stereo.vtp")) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet.vtp")) vis.updatePolyData(polyData, "walking snapshot trimmed", parent="continuous") standingFootName = "l_foot" standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if clusters is None: print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def updateIntersection(frame): origin = np.array(frame.transform.GetPosition()) rayLength = 5 for i in range(0, numRays): ray = rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) intersection = computeIntersection(locator, origin, origin + rayTransformed * rayLength) name = 'ray intersection ' + str(i) if intersection is not None: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, intersection) color = [1, 0, 0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color) else: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, origin + rayTransformed * rayLength) color = [0, 1, 0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color)
def initializeFields(self): self.visFolder = om.getOrCreateContainer('global registration') if self.logFolder is None: self.logFolder = "logs/scratch" self.pathDict = utils.getFilenames(self.logFolder) self.objectAlignmentResults = dict( ) # stores results of object alignment tool self.objectAlignmentTool = None # load the above table poly data if it exists self.aboveTablePolyData = None if os.path.isfile(self.pathDict['aboveTablePointcloud']): print "loading above table pointcloud" polyData = ioUtils.readPolyData( self.pathDict['aboveTablePointcloud']) self.aboveTablePolyData = filterUtils.transformPolyData( polyData, self.firstFrameToWorldTransform) vis.updatePolyData(self.aboveTablePolyData, 'above table pointcloud', parent=self.visFolder, colorByName='RGB') self.storedObjects = dict() self.loadStoredObjects()
def runRegistration(algorithm, scenePointCloud, modelPointCloud, visualize=True, **kwargs): assert algorithm in ["GoICP", "Super4PCS"] if (algorithm == "GoICP"): sceneToModelTransform = GoICP.run(scenePointCloud, modelPointCloud, **kwargs) elif algorithm == "Super4PCS": sceneToModelTransform = Super4PCS.run(scenePointCloud, modelPointCloud, **kwargs) if visualize: alignedModelPointCloud = filterUtils.transformPolyData( modelPointCloud, sceneToModelTransform.GetLinearInverse()) parent = om.getOrCreateContainer('registration') vis.updatePolyData(scenePointCloud, 'scene pointcloud', parent=parent) vis.updatePolyData(alignedModelPointCloud, 'aligned model pointcloud', parent=parent, color=[1, 0, 0]) return sceneToModelTransform
def processSingleBlock(robotStateModel, whichFile=0): if whichFile == 0: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/terrain_simple_ihmc.vtp")) vis.updatePolyData(polyData, "terrain_simple_ihmc.vtp", parent="continuous") else: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/terrain_flagstones_ihmc.vtp")) cwdemo.chosenTerrain = "stairs" cwdemo.supportContact = lcmdrc.footstep_params_t.SUPPORT_GROUPS_MIDFOOT_TOE vis.updatePolyData(polyData, "terrain_stairs_ihmc.vtp", parent="continuous") if drcargs.args().directorConfigFile.find("atlas") != -1: standingFootName = "l_foot" else: standingFootName = "leftFoot" standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False) # Step 1: filter the data down to a box in front of the robot: polyData = cwdemo.getRecedingTerrainRegion(polyData, footstepsDriver.getFeetMidPoint(robotStateModel)) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if clusters is None: print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def visualize_background(): if not os.path.exists(background_ply_file): return parent = om.getOrCreateContainer("scene") poly_data = ioUtils.readPolyData(background_ply_file) vis.updatePolyData(poly_data, 'table', parent=parent)
def processSnippet(): obj = om.getOrCreateContainer('continuous') om.getOrCreateContainer('cont debug', obj) if (continuouswalkingDemo.processContinuousStereo): polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp')) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp')) vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous') standingFootName = cwdemo.ikPlanner.leftFootLink standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if (clusters is None): print("No cluster found, stop walking now!") return # Step 3: find the corners of the minimum bounding rectangles blocks,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def __init__(self, robotSystem, view): self.robotStateModel = robotSystem.robotStateModel self.robotStateJointController = robotSystem.robotStateJointController self.robotSystem = robotSystem self.lFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.leftFootLink) self.rFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.rightFootLink) self.leftInContact = 0 self.rightInContact = 0 self.view = view self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper() self.warningButton = QtGui.QLabel('COP Warning') self.warningButton.setStyleSheet("background-color:white") self.dialogVisible = False d = DebugData() vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model') om.findObjectByName('measured cop').setProperty('Visible', False) self.robotStateModel.connectModelChanged(self.update)
def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] # if the QValue was empty then color it green if self.emptyQValue[sliderIdx]: colorMaxRange = [1, 1, 0] # this is yellow for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast( self.locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def update(self, robotModel): name = "camera frustum %s" % self.robotModel.getProperty("Name") obj = om.findObjectByName(name) if obj and not obj.getProperty("Visible"): return vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength), name, parent=self.robotModel, visible=False)
def update(self, robotModel): name = 'camera frustum %s' % self.robotModel.getProperty('Name') obj = om.findObjectByName(name) if obj and not obj.getProperty('Visible'): return vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength), name, parent=self.robotModel, visible=False)
def testCreateBackground(self): d = DebugData() d.addCube((1,1,0.1), (0,0,0), color=[1,1,1]) self.backgroundPolyData = d.getPolyData() if self.vis: view = self.views['background'] vis.updatePolyData(self.backgroundPolyData, 'background', view=view, colorByName='RGB255') view.resetCamera()
def _update_sensor(self, sensor, frame_name): """Updates sensor's rays. Args: sensor: Sensor. frame_name: Frame name. """ vis.updatePolyData(sensor.to_polydata(), frame_name, colorByName="RGB255")
def showDebugPoint(p, name='debug point', update=False, visible=True): d = DebugData() d.addSphere(p, radius=0.01, color=[1, 0, 0]) if update: vis.updatePolyData(d.getPolyData(), name) else: vis.showPolyData(d.getPolyData(), name, colorByName='RGB255', visible=visible)
def drawModel(self, model): modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder()) markerFolder = om.getOrCreateContainer('markers', parentObj=modelFolder) modelName = model.name markerObjects = markerFolder.children() if len(markerObjects) != model.nummarkers: for obj in markerObjects: om.removeFromObjectModel(obj) modelColor = vis.getRandomColor() markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor) self.models[modelName] = markerObjects if len(markerObjects): modelColor = markerObjects[0].getProperty('Color') for i, marker in enumerate(model.markers): xyz = np.array(marker.xyz) * self.unitConversion markerFrame = vtk.vtkTransform() markerFrame.Translate(xyz) markerObjects[i].getChildFrame().copyFrame(markerFrame) if self.drawEdges: d = DebugData() for m1 in model.markers: xyz = np.array(m1.xyz) * self.unitConversion for m2 in model.markers: xyz2 = np.array(m2.xyz) * self.unitConversion d.addLine(xyz, xyz2) edges = shallowCopy(d.getPolyData()) vis.updatePolyData(edges, modelName + ' edges', color=modelColor, parent=modelFolder) else: edgesObj = om.findObjectByName(modelName + ' edges') om.removeFromObjectModel(edgesObj) computeModelFrame = True if computeModelFrame: pos = np.array(model.segments[0].T) * self.unitConversion rpy = np.array(model.segments[0].A) modelFrame = transformUtils.frameFromPositionAndRPY( pos, np.degrees(rpy)) vis.updateFrame(modelFrame, modelName + ' frame', parent=modelFolder, scale=0.1)
def spin(): polyData = vtk.vtkPolyData() reader.GetPointCloud(polyData) frame_id = reader.GetFrameId() sec = reader.GetSec() nsec = reader.GetNsec() message = str(polyData.GetNumberOfPoints()) + " points, " message += frame_id + ", " + str(sec) + "." + str(nsec) print(message) vis.updatePolyData(polyData, 'point cloud')
def visualize_reconstruction(self, view, point_size=None, vis_uncropped=False): if point_size is None: point_size = self.config['point_size'] self.reconstruction_vis_obj = vis.updatePolyData(self.poly_data, 'Fusion Reconstruction', view=view, colorByName='RGB') self.reconstruction_vis_obj.setProperty('Point Size', point_size) if vis_uncropped: vis_obj = vis.updatePolyData(self.poly_data_uncropped, 'Uncropped Fusion Reconstruction', view=view, colorByName='RGB') vis_obj.setProperty('Point Size', point_size)
def spin(): polyData = vtk.vtkPolyData() reader.GetMesh(polyData) vis.updatePolyData(polyData, 'mesh') print "Number of points (a)", polyData.GetNumberOfPoints() if (polyData.GetNumberOfPoints() == 0): return polyDataPC = vtk.vtkPolyData() reader.GetPointCloud(polyDataPC) vis.updatePolyData(polyDataPC, 'output') print "Number of points (b)", polyDataPC.GetNumberOfPoints()
def createSpheres(self, sensorValues): d = DebugData() for key in list(sensorValues.keys()): frame, pos, rpy = self.sensorLocations[key] t = transformUtils.frameFromPositionAndRPY(pos, rpy) t.PostMultiply() t.Concatenate(self.frames[frame]) d.addSphere(t.GetPosition(), radius=0.005, color=self.getColor(sensorValues[key], key), resolution=8) vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
def visualize_reconstruction(self, view, vis_uncropped=False, name=None): if name is None: vis_name = "Fusion Reconstruction " + self.name else: vis_name = name self.reconstruction_vis_obj = vis.updatePolyData(self.poly_data, vis_name, view=view, colorByName='RGB') if vis_uncropped: vis_obj = vis.updatePolyData(self.poly_data_uncropped, 'Uncropped Fusion Reconstruction', view=view, colorByName='RGB')
def createSpheres(self, sensorValues): d = DebugData() for key in sensorValues.keys(): frame, pos, rpy = self.sensorLocations[key] t = transformUtils.frameFromPositionAndRPY(pos, rpy) t.PostMultiply() t.Concatenate(self.frames[frame]) d.addSphere(t.GetPosition(), radius=0.005, color=self.getColor(sensorValues[key], key), resolution=8) vis.updatePolyData(d.getPolyData(), self.name, colorByName='RGB255')
def updateDrawIntersectionManual(self, frame): print "I am getting called too" d = DebugData() originHigher = np.array(frame.transform.GetPosition()) originHigher[2] = 1.0 sliderIdx = self.slider.value for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) distance = self.raycastDataManual[sliderIdx,i] d.addLine(originHigher, originHigher+rayTransformed*distance, color=[0,1,1]) vis.updatePolyData(d.getPolyData(), 'raysManual', colorByName='RGB255')
def doFTDraw(self, unusedrobotstate): frames = [] fts = [] vis_names = [] if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and self.robotStateJointController.lastRobotStateMessage): if self.draw_right: rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque) rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis rft[1] = -1.*rft[1] rft[3] = -1.*rft[3] rft[4] = -1.*rft[4] rft -= self.ft_right_bias fts.append(rft) frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque')) vis_names.append('ft_right') if self.draw_left: lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force + self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque) lft -= self.ft_left_bias fts.append(lft) frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque')) vis_names.append('ft_left') for i in range(0, len(frames)): frame = frames[i] ft = fts[i] offset = [0., 0., 0.] p1 = frame.TransformPoint(offset) scale = 1.0/25.0 # todo add slider for this? scalet = 1.0 / 2.5 p2f = frame.TransformPoint(offset + ft[0:3]*scale) p2t = frame.TransformPoint(offset + ft[3:6]) normalt = (np.array(p2t) - np.array(p1)) normalt = normalt / np.linalg.norm(normalt) d = DebugData() # force if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1: d.addLine(p1, p2f, color=[1.0, 0.0, 0.0]) else: d.addArrow(p1, p2f, color=[1.,0.,0.]) # torque if self.draw_torque: d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6])) # frame (largely for debug) vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2) vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def drawCenterOfMass(model): stanceFrame = footstepsDriver.getFeetMidPoint(model) com = list(model.model.getCenterOfMass()) com[2] = stanceFrame.GetPosition()[2] d = DebugData() d.addSphere(com, radius=0.015) obj = vis.updatePolyData(d.getPolyData(), 'COM %s' % model.getProperty('Name'), color=[1,0,0], visible=False, parent=model)
def fitObjectToPointcloud(self, objectName, pointCloud=None, downsampleObject=True, objectPolyData=None, filename=None, visualize=True, algorithm="GoICP"): if objectPolyData is None: if filename is None: filename = utils.getObjectMeshFilename(objectName) objectPolyData = ioUtils.readPolyData(filename) # downsample the object poly data objectPolyDataForAlgorithm = objectPolyData if downsampleObject: objectPolyDataForAlgorithm = segmentation.applyVoxelGrid( objectPolyData, leafSize=0.002) if pointCloud is None: pointCloud = om.findObjectByName('reconstruction').polyData sceneToModelTransform = GlobalRegistration.runRegistration( algorithm, pointCloud, objectPolyDataForAlgorithm) objectToWorld = sceneToModelTransform.GetLinearInverse() self.objectToWorldTransform[objectName] = objectToWorld if visualize: alignedObj = vis.updatePolyData(objectPolyData, objectName + ' aligned') alignedObj.actor.SetUserTransform(objectToWorld) return objectToWorld
def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points for p in points: d.addSphere(p, radius=5) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1,0,0], view=self.view) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes(self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85/255.0, 255/255.0, 255/255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples-1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def fitSwitchBox(self, polyData, points): boxPosition = points[0] wallPoint = points[1] # find a frame that is aligned with wall searchRadius = 0.2 planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0) obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False) obj.setProperty('Point Size', 7) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal origin = segmentation.computeCentroid(planePoints) zaxis = [0,0,1] xaxis = normal yaxis = np.cross(zaxis, xaxis) yaxis /= np.linalg.norm(yaxis) zaxis = np.cross(xaxis, yaxis) zaxis /= np.linalg.norm(zaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) # translate that frame to the box position t.PostMultiply() t.Translate(boxPosition) boxFrame = transformUtils.copyFrame(t) self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def buildGlobalGoal(scale): #print "building circle world" d = DebugData() worldXmin, worldXmax, worldYmin, worldYmax = World.buildBoundaries(d, scale=scale, boundaryType="Square", withData=False) #print "boundaries done" firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin) firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin) firstEndpt = (firstX,firstY,0.2) secondEndpt = (firstX,firstY,-0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=1.0, color=[0.5,1,0]) obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255') world = World() world.visObj = obj world.global_goal_x = firstX world.global_goal_y = firstY print "I should have actually built a goal" return world
def testCreateForeground(self): d = DebugData() dims = np.array([0.2,0.2,0.2]) center = (0,0,dims[2]/2.0) d.addCube(dims, center, color=[0,1,0]) d.addPolyData(self.backgroundPolyData) self.foregroundPolyData = d.getPolyData() if self.vis: view=self.views['foreground'] vis.updatePolyData(self.foregroundPolyData, 'foreground', view=view, colorByName='RGB255') view.resetCamera()
def drawActionSetFull(self, go_nowhere=False): # print "I am drawing the action set" d = DebugData() for index, value in enumerate(self.pos_trajectories): for time_step_index in xrange(2 * self.numPointsToDraw - 1): firstX = value[0, time_step_index] firstY = value[1, time_step_index] firstZ = value[2, time_step_index] secondX = value[0, time_step_index + 1] secondY = value[1, time_step_index + 1] secondZ = value[2, time_step_index + 1] firstEndpt = (firstX, firstY, firstZ) secondEndpt = (secondX, secondY, secondZ) if time_step_index >= 10: color = [0.8, 0, 0.8] else: color = [0.1, 0.1, 1.0] if go_nowhere: firstEndpt = [0, 0, 0] secondEndpt = [0, 0, 0.001] d.addLine(firstEndpt, secondEndpt, radius=0.02, color=color) obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def draw(self): d = DebugData() points = [p if p is not None else self.hoverPos for p in self.points] # draw points for p in points: if p is not None: d.addSphere(p, radius=0.008) if self.drawLines: # draw lines for a, b in zip(points, points[1:]): if b is not None: d.addLine(a, b) # connect end points if points[-1] is not None and self.drawClosedLoop: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData(d.getPolyData(), self.annotationName, parent=self.annotationFolder, view=self.view) self.annotationObj.setProperty('Color', [1,0,0]) self.annotationObj.actor.SetPickable(False)
def buildGlobalGoal(): #print "building circle world" d = DebugData() worldXmin = 25 worldXmax = 50 worldYmin = -20 worldYmax = 20 firstX = worldXmin + np.random.rand()*(worldXmax-worldXmin) firstY = worldYmin + np.random.rand()*(worldYmax-worldYmin) firstEndpt = (firstX,firstY,0.2) secondEndpt = (firstX,firstY,-0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=0.3, color=[0.5,1,0]) obj = vis.updatePolyData(d.getPolyData(), 'global_goal', colorByName='RGB255') world = World() world.visObj = obj world.global_goal_x = firstX world.global_goal_y = firstY return world
def drawForce(self, name, linkName, forceLocation, force, color, key=None): forceDirection = force / np.linalg.norm(force) # om.removeFromObjectModel(om.findObjectByName(name)) linkToWorld = self.robotStateModel.getLinkFrame(linkName) forceLocationInWorld = np.array( linkToWorld.TransformPoint(forceLocation)) forceDirectionInWorld = np.array( linkToWorld.TransformDoubleVector(forceDirection)) # point = forceLocationInWorld - 0.1*forceDirectionInWorld # d = DebugData() # # d.addArrow(point, forceLocationInWorld, headRadius=0.025, tubeRadius=0.005, color=color) # d.addSphere(forceLocationInWorld, radius=0.01) # d.addLine(point, forceLocationInWorld, radius=0.005) transformForVis = transformUtils.getTransformFromOriginAndNormal( forceLocationInWorld, forceDirectionInWorld) obj = vis.updatePolyData(self.plungerPolyData, name, color=color) obj.actor.SetUserTransform(transformForVis) if key is not None and om.findObjectByName(name) is not None: obj.addProperty('magnitude', self.externalForces[key]['forceMagnitude']) obj.addProperty('linkName', linkName) obj.addProperty('key', key) obj.connectRemovedFromObjectModel(self.removeForceFromFrameObject) obj.properties.connectPropertyChanged( functools.partial(self.onPropertyChanged, obj)) return obj
def onMouseMove(self, displayPoint, modifiers=None): om.removeFromObjectModel(om.findObjectByName('link selection')) self.linkName = None self.pickedPoint = None selection = self.getSelection(displayPoint) if selection is None: return pickedPoint, linkName, normal, pickedCellId = selection d = DebugData() d.addSphere(pickedPoint, radius=0.01) d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005) obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0]) obj.actor.SetPickable(False) self.linkName = linkName self.pickedPoint = pickedPoint self.normal = normal self.pickedCellId = pickedCellId modifiers = QtGui.QApplication.keyboardModifiers() if modifiers == QtCore.Qt.ControlModifier and self.cellCaptureMode: self.provisionallyCaptureCell(linkName, pickedCellId)
def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points radius = 5 scale = (2 * self.view.camera().GetParallelScale()) / ( self.view.renderer().GetSize()[1]) for p in points: d.addSphere(p, radius=radius * scale) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData( d.getPolyData(), "annotation", parent="segmentation", color=[1, 0, 0], view=self.view, ) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def draw(self): d = DebugData() points = list(self.points) if self.hoverPos is not None: points.append(self.hoverPos) # draw points for p in points: d.addSphere(p, radius=5) if self.drawLines and len(points) > 1: for a, b in zip(points, points[1:]): d.addLine(a, b) # connect end points # d.addLine(points[0], points[-1]) if self.annotationObj: self.annotationObj.setPolyData(d.getPolyData()) else: self.annotationObj = vis.updatePolyData(d.getPolyData(), 'annotation', parent='segmentation', color=[1, 0, 0], view=self.view) self.annotationObj.addToView(self.view) self.annotationObj.actor.SetPickable(False) self.annotationObj.actor.GetProperty().SetLineWidth(2)
def updatePlanFrames(self): if self.getViewMode() != 'frames': return numberOfSamples = self.getNumberOfSamples() meshes = self.planPlayback.getPlanPoseMeshes( self.plan, self.playbackJointController, self.playbackRobotModel, numberOfSamples) d = DebugData() startColor = [0.8, 0.8, 0.8] endColor = [85 / 255.0, 255 / 255.0, 255 / 255.0] colorFunc = scipy.interpolate.interp1d([0, numberOfSamples - 1], [startColor, endColor], axis=0, kind='slinear') for i, mesh in reversed(list(enumerate(meshes))): d.addPolyData(mesh, color=colorFunc(i)) pd = d.getPolyData() clean = vtk.vtkCleanPolyData() clean.SetInput(pd) clean.Update() pd = clean.GetOutput() self.planFramesObj = vis.updatePolyData(d.getPolyData(), 'robot plan', alpha=1.0, visible=False, colorByName='RGB255', parent='planning') self.showPlanFrames()
def draw(self): d = DebugData() points = [p if p is not None else self.hoverPos for p in self.points] # draw points for p in points: if p is not None: d.addSphere(p, radius=0.008) if self.drawLines: # draw lines for a, b in zip(points, points[1:]): if b is not None: d.addLine(a, b) # connect end points if points[-1] is not None and self.drawClosedLoop: d.addLine(points[0], points[-1]) self.annotationObj = vis.updatePolyData(d.getPolyData(), self.annotationName, parent=self.annotationFolder, view=self.view) self.annotationObj.setProperty('Color', [1, 0, 0]) self.annotationObj.actor.SetPickable(False)
def drawModel(self, model): modelFolder = om.getOrCreateContainer(model.name, parentObj=self.getRootFolder()) markerFolder = om.getOrCreateContainer("markers", parentObj=modelFolder) modelName = model.name markerObjects = markerFolder.children() if len(markerObjects) != model.nummarkers: for obj in markerObjects: om.removeFromObjectModel(obj) modelColor = vis.getRandomColor() markerObjects = self.createMarkerObjects(model.nummarkers, markerFolder, modelName, modelColor) self.models[modelName] = markerObjects if len(markerObjects): modelColor = markerObjects[0].getProperty("Color") for i, marker in enumerate(model.markers): xyz = np.array(marker.xyz) * self.unitConversion markerFrame = vtk.vtkTransform() markerFrame.Translate(xyz) markerObjects[i].getChildFrame().copyFrame(markerFrame) if self.drawEdges: d = DebugData() for m1 in model.markers: xyz = np.array(m1.xyz) * self.unitConversion for m2 in model.markers: xyz2 = np.array(m2.xyz) * self.unitConversion d.addLine(xyz, xyz2) edges = shallowCopy(d.getPolyData()) vis.updatePolyData(edges, modelName + " edges", color=modelColor, parent=modelFolder) else: edgesObj = om.findObjectByName(modelName + " edges") om.removeFromObjectModel(edgesObj) computeModelFrame = True if computeModelFrame: pos = np.array(model.segments[0].T) * self.unitConversion rpy = np.array(model.segments[0].A) modelFrame = transformUtils.frameFromPositionAndRPY(pos, np.degrees(rpy)) vis.updateFrame(modelFrame, modelName + " frame", parent=modelFolder, scale=0.1)
def buildLineSegmentTestWorld(percentObsDensity, nonRandom=False, circleRadius=3, scale=1.0, randomSeed=5, obstaclesInnerFraction=1.0): #print "building circle world" if nonRandom: np.random.seed(randomSeed) d = DebugData() worldXmin = 3 worldXmax = 15 worldYmin = -10 worldYmax = 10 #print "boundaries done" worldArea = (worldXmax-worldXmin)*(worldYmax-worldYmin) #print worldArea obsScalingFactor = 5.0/12.0 maxNumObstacles = obsScalingFactor * worldArea numObstacles = int(obstaclesInnerFraction**2 * percentObsDensity/100.0 * maxNumObstacles) print numObstacles, " is num obstacles" # draw random stick obstacles obsLength = 2.0 obsXmin = worldXmin + (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin) obsXmax = worldXmax - (1-obstaclesInnerFraction)/2.0*(worldXmax - worldXmin) obsYmin = worldYmin + (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin) obsYmax = worldYmax - (1-obstaclesInnerFraction)/2.0*(worldYmax - worldYmin) for i in xrange(numObstacles): firstX = obsXmin + np.random.rand()*(obsXmax-obsXmin) firstY = obsYmin + np.random.rand()*(obsYmax-obsYmin) secondX = obsXmin + np.random.rand()*(obsXmax-obsXmin) secondY = obsYmin + np.random.rand()*(obsYmax-obsYmin) firstEndpt = (firstX,firstY,0.2) secondEndpt = (firstX,firstY,-0.2) #d.addLine(firstEndpt, secondEndpt, radius=2*np.random.randn()) d.addLine(firstEndpt, secondEndpt, radius=0.5) obj = vis.updatePolyData(d.getPolyData(), 'world') world = World() world.visObj = obj world.Xmax = worldXmax world.Xmin = worldXmin world.Ymax = worldYmax world.Ymin = worldYmin world.numObstacles = numObstacles world.percentObsDensity = percentObsDensity return world
def updateDrawIntersection(frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() for i in xrange(numRays): ray = rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*rayLength, color=[0,1,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255')
def drawActionSetEmpty(self): # print "I am drawing the action set" d = DebugData() for index, value in enumerate(self.pos_trajectories): for time_step_index in xrange(2 * self.numPointsToDraw - 1): d.addLine([0, 0, 0], [0, 0, 0], radius=0.01, color=[1, 1, 1]) obj = vis.updatePolyData(d.getPolyData(), "action_set", colorByName="RGB255")
def drawFirstIntersections(self, frame, firstRaycast): origin = np.array(frame.transform.GetPosition()) d = DebugData() firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast) for i in xrange(self.Sensor.numRays): endpoint = firstRaycastLocations[i,:] if firstRaycast[i] == 20.0: d.addLine(origin, endpoint, color=[0,1,0]) else: d.addLine(origin, endpoint, color=[1,0,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.Sensor.setLocator(self.LineSegmentLocator)
def gravitizeAccelSphere(accel_sphere_poly_data, gravity_max, color=[1,0.3,0.0], alpha=0.5): d = DebugData() center = [0, 0, -gravity_max] t = vtk.vtkTransform() t.Translate(center) polyData = filterUtils.transformPolyData(accel_sphere_poly_data, t) obj = vis.updatePolyData(polyData, 'accel_sphere', color=color, alpha=alpha) return polyData
def loadMap(self, filename, transform=None): print filename pd = io.readPolyData(filename) if transform is not None: pd = filterUtils.transformPolyData(pd, self.cameraToLocalInit) pdi = vis.updatePolyData(pd, 'map') try: pdi.setProperty('Color By', 'rgb_colors') except Exception, e: print "Could not set color to RGB - not an element" #raise e
def testDepthImageToPointcloud(self): ds = self.depthScanners['foreground'] camera = ds.view.camera() depth_img = ds.getDepthImage() depth_img_raw, _, _ = ds.getDepthImageAndPointCloud() depth_img_numpy = ds.getDepthImageAsNumpyArray() print "depth min %.3f, depth max = %.3f" %(np.min(depth_img_numpy), np.max(depth_img_numpy)) # self.changeDetectionImageVisualizer.setImage(depth_img) f = vtk.vtkDepthImageToPointCloud() f.SetCamera(camera) f.SetInputData(depth_img) f.Update() polyData = f.GetOutput() print "polyData.GetNumberOfPoints() = ", polyData.GetNumberOfPoints() view = self.changeDetectionPointCloudView vis.updatePolyData(polyData, 'depth_im_to_pointcloud')