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.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 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 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()