def computeDepthImageAndPointCloud(depthBuffer, colorBuffer, camera): """ Returns depth image and pointcloud as vtkImageData and vtkPolyData :param depthBuffer: OpenGL depth buffer :type depthBuffer: :param colorBuffer: OpenGL color buffer :type colorBuffer: :param camera: vtkCamera instance that was used to render the scene :type camera: vtkCamera instance :return: :rtype: vtkImageData, vtkPolyData, numpy array """ depthImage = vtk.vtkImageData() pts = vtk.vtkPoints() ptColors = vtk.vtkUnsignedCharArray() vtk.vtkDepthImageUtils.DepthBufferToDepthImage(depthBuffer, colorBuffer, camera, depthImage, pts, ptColors) pts = vnp.numpy_support.vtk_to_numpy(pts.GetData()) polyData = vnp.numpyToPolyData(pts, createVertexCells=True) ptColors.SetName('rgb') polyData.GetPointData().AddArray(ptColors) return depthImage, polyData, pts
def handle_init_message(self, msg): """Creates the polydata for the deformable mesh specified in msg.""" folder = om.getOrCreateContainer(self._folder_name) for poly_item in self._poly_item_list: om.removeFromObjectModel(poly_item) self._names = [] self._vertex_counts = [] self._poly_data_list = [] self._poly_item_list = [] for mesh in msg.meshes: self._names.append(mesh.name) # Initial vertex positions are garbage; meaningful values will be # set by the update message. points = vtk.vtkPoints() self._vertex_counts.append(mesh.num_vertices) for i in range(mesh.num_vertices): points.InsertNextPoint(0, 0, 0) triangles = vtk.vtkCellArray() for tri in mesh.tris: triangles.InsertNextCell(3) for i in tri.vertices: triangles.InsertCellPoint(i) poly_data = vtk.vtkPolyData() poly_data.SetPoints(points) poly_data.SetPolys(triangles) poly_item = vis.showPolyData(poly_data, mesh.name, parent=folder) poly_item.setProperty("Surface Mode", "Surface with edges") self._poly_data_list.append(poly_data) self._poly_item_list.append(poly_item)
def add_vtk_polygon(vertices): """ Create a 3d polygon data with vtk. :type vertices: vertices data of polygon """ # Setup four points points = vtk.vtkPoints() for point in vertices: points.InsertNextPoint(point[0], point[1], point[2]) # Create the polygon polygon = vtk.vtkPolygon() polygon.GetPointIds().SetNumberOfIds(4) # make a quad polygon.GetPointIds().SetId(0, 0) polygon.GetPointIds().SetId(1, 1) polygon.GetPointIds().SetId(2, 2) polygon.GetPointIds().SetId(3, 3) # Add the polygon to a list of polygons polygons = vtk.vtkCellArray() polygons.InsertNextCell(polygon) # Create a PolyData polygon_poly_data = vtk.vtkPolyData() polygon_poly_data.SetPoints(points) polygon_poly_data.SetPolys(polygons) return polygon_poly_data
def handle_update_message(self, msg): """Updates vertex data for the deformable meshes specified in msg.""" if not len(self._poly_data_list) == msg.num_meshes: print( "Received a deformable mesh update message with '{}' meshes; " "expected {} meshes.".format(msg.num_meshes, len(self._poly_data_list))) return for mesh_id in range(msg.num_meshes): mesh = msg.meshes[mesh_id] if mesh.name != self._names[mesh_id]: print("The deformable mesh update message contains data for " "a mesh named '{}', expected name '{}'.".format( mesh.name, self._names[mesh_id])) return if mesh.num_vertices != self._vertex_counts[mesh_id]: print( "The deformable mesh update message contains data for {} " "vertices; expected {}.".format(mesh.num_vertices, self._vertex_count)) return points = vtk.vtkPoints() for i in range(0, mesh.num_vertices): points.InsertNextPoint(mesh.vertices_W[i][0], mesh.vertices_W[i][1], mesh.vertices_W[i][2]) # TODO(SeanCurtis-TRI): Instead of creating a new set of points and # stomping on the old; can I just update the values? That might # improve performance. self._poly_data_list[mesh_id].SetPoints(points) self._poly_item_list[mesh_id].setPolyData( self._poly_data_list[mesh_id]) self._poly_item_list[mesh_id]._renderAllViews()
def getTraceData(self): t = self.frame.findChild(self.traceName) if not t: pts = vtk.vtkPoints() pts.SetDataTypeToDouble() pts.InsertNextPoint(self.frame.transform.GetPosition()) pd = vtk.vtkPolyData() pd.SetPoints(pts) pd.SetLines(vtk.vtkCellArray()) t = showPolyData(pd, self.traceName, parent=self.frame) return t
def getTraceData(self): t = self.frame.findChild(self.traceName) if not t: pts = vtk.vtkPoints() pts.SetDataTypeToDouble() pts.InsertNextPoint(self.lastPosition) pd = vtk.vtkPolyData() pd.Allocate(1, 1) pd.SetPoints(pts) polyline = vtk.vtkPolyLine() pd.InsertNextCell(polyline.GetCellType(), polyline.GetPointIds()) idArray = pd.GetLines().GetData() idArray.InsertNextValue(0) t = showPolyData(pd, self.traceName, parent=self.frame) return t
def createPolyDataFromMeshArrays(pts, faces): pd = vtk.vtkPolyData() pd.SetPoints(vtk.vtkPoints()) pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy())) cells = vtk.vtkCellArray() for face in faces: assert len(face) == 3, "Non-triangular faces are not supported." tri = vtk.vtkTriangle() tri.GetPointIds().SetId(0, face[0]) tri.GetPointIds().SetId(1, face[1]) tri.GetPointIds().SetId(2, face[2]) cells.InsertNextCell(tri) pd.SetPolys(cells) return pd
def createPolyDataFromMeshArrays(pts, faces): pd = vtk.vtkPolyData() pd.SetPoints(vtk.vtkPoints()) pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy())) assert len(faces) % 3 == 0 cells = vtk.vtkCellArray() for i in xrange(len(faces) / 3): tri = vtk.vtkTriangle() tri.GetPointIds().SetId(0, faces[i * 3 + 0]) tri.GetPointIds().SetId(1, faces[i * 3 + 1]) tri.GetPointIds().SetId(2, faces[i * 3 + 2]) cells.InsertNextCell(tri) pd.SetPolys(cells) return pd
def computeDepthImageAndPointCloud(depthBuffer, colorBuffer, camera): ''' Input args are an OpenGL depth buffer and color buffer as vtkImageData objects, and the vtkCamera instance that was used to render the scene. The function returns returns a depth image and a point cloud as vtkImageData and vtkPolyData. ''' depthImage = vtk.vtkImageData() pts = vtk.vtkPoints() ptColors = vtk.vtkUnsignedCharArray() vtk.vtkDepthImageUtils.DepthBufferToDepthImage(depthBuffer, colorBuffer, camera, depthImage, pts, ptColors) pts = vnp.numpy_support.vtk_to_numpy(pts.GetData()) polyData = vnp.numpyToPolyData(pts, createVertexCells=True) ptColors.SetName('rgb') polyData.GetPointData().AddArray(ptColors) return depthImage, polyData, pts
def computeDepthImageAndPointCloud(depthBuffer, colorBuffer, camera): ''' Input args are an OpenGL depth buffer and color buffer as vtkImageData objects, and the vtkCamera instance that was used to render the scene. The function returns returns a depth image and a point cloud as vtkImageData and vtkPolyData. ''' depthImage = vtk.vtkImageData() pts = vtk.vtkPoints() ptColors = vtk.vtkUnsignedCharArray() vtk.vtkDepthImageUtils.DepthBufferToDepthImage(depthBuffer, colorBuffer, camera, depthImage, pts, ptColors) pts = vnp.numpy_support.vtk_to_numpy(pts.GetData()) polyData = vnp.numpyToPolyData(pts, createVertexCells=True) ptColors.SetName('rgb') polyData.GetPointData().AddArray(ptColors) return depthImage, polyData
def onPlanarLidar(self, msg, channel): linkName = channel.replace('DRAKE_PLANAR_LIDAR_', '', 1) robotNum, linkName = linkName.split('_', 1) robotNum = int(robotNum) try: link = self.getLink(robotNum, linkName) except KeyError: if linkName not in self.linkWarnings: print 'Error locating link name:', linkName self.linkWarnings.add(linkName) else: if len(link.geometry): polyData = link.geometry[0].polyDataItem else: polyData = vtk.vtkPolyData() name = linkName + ' geometry data' visInfo = FieldContainer(color=[1.0, 0.0, 0.0], alpha=1.0, texture=None) g = Geometry(name, polyData, visInfo) link.geometry.append(g) linkFolder = self.getLinkFolder(robotNum, linkName) self.addLinkGeometry(g, linkName, linkFolder) g.polyDataItem.actor.SetUserTransform(link.transform) points = vtk.vtkPoints() verts = vtk.vtkCellArray() t = msg.rad0 for r in msg.ranges: if r >= 0: x = r * math.cos(t) y = r * math.sin(t) pointId = points.InsertNextPoint([x, y, 0]) verts.InsertNextCell(1) verts.InsertCellPoint(pointId) t += msg.radstep polyData.SetPoints(points) polyData.SetVerts(verts)
def onPlanarLidar(self, msg, channel): linkName = channel.replace("DRAKE_PLANAR_LIDAR_", "", 1) robotNum, linkName = linkName.split("_", 1) robotNum = int(robotNum) try: link = self.getLink(robotNum, linkName) except KeyError: if linkName not in self.linkWarnings: print "Error locating link name:", linkName self.linkWarnings.add(linkName) else: if len(link.geometry): polyData = link.geometry[0].polyDataItem else: polyData = vtk.vtkPolyData() name = linkName + " geometry data" geom = type("", (), {}) geom.color = [1.0, 0.0, 0.0, 1.0] g = Geometry(name, geom, polyData, link.transform) link.geometry.append(g) linkFolder = self.getLinkFolder(robotNum, linkName) self.addLinkGeometry(g, linkName, linkFolder) g.polyDataItem.actor.SetUserTransform(link.transform) points = vtk.vtkPoints() verts = vtk.vtkCellArray() t = msg.rad0 for r in msg.ranges: if r >= 0: x = r * math.cos(t) y = r * math.sin(t) pointId = points.InsertNextPoint([x, y, 0]) verts.InsertNextCell(1) verts.InsertCellPoint(pointId) t += msg.radstep polyData.SetPoints(points) polyData.SetVerts(verts)
def onPlanarLidar(self, msg, channel): linkName = channel.replace('DRAKE_PLANAR_LIDAR_', '', 1) robotNum, linkName = linkName.split('_', 1) robotNum = int(robotNum) try: link = self.getLink(robotNum, linkName) except KeyError: if linkName not in self.linkWarnings: print 'Error locating link name:', linkName self.linkWarnings.add(linkName) else: if len(link.geometry): polyData = link.geometry[0].polyDataItem else: polyData = vtk.vtkPolyData() name = linkName + ' geometry data' visInfo = FieldContainer(color=[1.0,0.0,0.0], alpha=1.0, texture=None) g = Geometry(name, polyData, visInfo) link.geometry.append(g) linkFolder = self.getLinkFolder(robotNum, linkName) self.addLinkGeometry(g, linkName, linkFolder) g.polyDataItem.actor.SetUserTransform(link.transform) points = vtk.vtkPoints() verts = vtk.vtkCellArray() t = msg.rad0 for r in msg.ranges: if r >= 0: x = r * math.cos(t) y = r * math.sin(t) pointId = points.InsertNextPoint([x,y,0]) verts.InsertNextCell(1) verts.InsertCellPoint(pointId) t += msg.radstep polyData.SetPoints(points) polyData.SetVerts(verts)
def createPolyDataFromMeshArrays(pts, faces): pd = vtk.vtkPolyData() pd.SetPoints(vtk.vtkPoints()) if pts.size > 0: pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy())) cells = vtk.vtkCellArray() tri = vtk.vtkTriangle() setId = tri.GetPointIds().SetId # bind the method for convenience for face in faces: assert len(face) == 3, "Non-triangular faces are not supported." setId(0, face[0]) setId(1, face[1]) setId(2, face[2]) cells.InsertNextCell(tri) pd.SetPolys(cells) return pd
cameraToCameraStart = poses.getCameraPoseAtUTime(utime) t = cameraToCameraStart common.setCameraTransform(camera, t) common.setCameraTransform(camera1, t) renWin.Render() renSource.Update() #update filters filter1.Modified() filter1.Update() windowToColorBuffer.Modified() windowToColorBuffer.Update() #extract depth image depthImage = vtk.vtkImageData() pts = vtk.vtkPoints() ptColors = vtk.vtkUnsignedCharArray() vtk.vtkDepthImageUtils.DepthBufferToDepthImage( filter1.GetOutput(), windowToColorBuffer.GetOutput(), camera, depthImage, pts, ptColors) scale.SetInputData(depthImage) scale.Update() #source = np.flip(np.reshape(numpy_support.vtk_to_numpy(renSource.GetOutput().GetPointData().GetScalars()),(480,640)),axis=0) #modify this for simulated depth source = np.flip(np.reshape( numpy_support.vtk_to_numpy( renSource.GetOutput().GetPointData().GetScalars()), (480, 640)), axis=0)
def getVtkPointsFromNumpy(numpyArray): points = vtk.vtkPoints() points.SetData(getVtkFromNumpy(numpyArray)) return points
def render_depth(renWin, renderer, camera, data_dir, data_dir_name, num_im, out_dir, use_mesh, object_dir, mesh='meshed_scene.ply', keyword=None): actor = vtk.vtkActor() filter1 = vtk.vtkWindowToImageFilter() imageWriter = vtk.vtkPNGWriter() scale = vtk.vtkImageShiftScale() if use_mesh: #use meshed version of scene if not glob.glob(data_dir + "/" + mesh): out = None if glob.glob(data_dir + "/original_log.lcmlog.ply"): out = "original_log.lcmlog.ply" elif glob.glob(data_dir + "/trimmed_log.lcmlog.ply"): out = "trimmed_log.lcmlog.ply" elif glob.glob('*.ply'): out = glob.glob('*.ply')[0] else: return mesher = mesh_wrapper.Mesh(out_dir=data_dir) status = mesher.mesh_cloud(out) print status #blocks until done mapper = vtk.vtkPolyDataMapper() fileReader = vtk.vtkPLYReader() fileReader.SetFileName(data_dir + "/" + mesh) mapper.SetInputConnection(fileReader.GetOutputPort()) actor.SetMapper(mapper) renderer.AddActor(actor) else: #import just the objects objects = common.Objects(data_dir, object_dir) objects.loadObjectMeshes("/registration_result.yaml", renderer, keyword=keyword) #setup filters filter1.SetInput(renWin) filter1.SetMagnification(1) filter1.SetInputBufferTypeToZBuffer() windowToColorBuffer = vtk.vtkWindowToImageFilter() windowToColorBuffer.SetInput(renWin) windowToColorBuffer.SetInputBufferTypeToRGB() scale.SetOutputScalarTypeToUnsignedShort() scale.SetScale(1000) poses = common.CameraPoses(data_dir + "/posegraph.posegraph") for i in range(1, num_im + 1): try: utimeFile = open( data_dir + "/images/" + str(i).zfill(10) + "_utime.txt", 'r') utime = int(utimeFile.read()) #update camera transform cameraToCameraStart = poses.getCameraPoseAtUTime(utime) t = cameraToCameraStart common.setCameraTransform(camera, t) renWin.Render() #update filters filter1.Modified() filter1.Update() windowToColorBuffer.Modified() windowToColorBuffer.Update() #extract depth image depthImage = vtk.vtkImageData() pts = vtk.vtkPoints() ptColors = vtk.vtkUnsignedCharArray() vtk.vtkDepthImageUtils.DepthBufferToDepthImage( filter1.GetOutput(), windowToColorBuffer.GetOutput(), camera, depthImage, pts, ptColors) scale.SetInputData(depthImage) scale.Update() #write out depth image imageWriter.SetFileName(out_dir + str(i).zfill(10) + "_" + data_dir_name + "_depth_ground_truth.png") imageWriter.SetInputConnection(scale.GetOutputPort()) imageWriter.Write() except (IOError): break renderer.RemoveAllViewProps() renWin.Render()