def onPointCloud(self, msg, channel):
pointcloudName = channel.replace("DRAKE_POINTCLOUD_", "", 1)
polyData = vnp.numpyToPolyData(np.asarray(msg.points), createVertexCells=True)
# If the user provided color channels, then use them to colorize
# the pointcloud.
channels = {msg.channel_names[i]: msg.channels[i] for i in range(msg.n_channels)}
if "r" in channels and "g" in channels and "b" in channels:
colorized = True
colorArray = np.empty((msg.n_points, 3), dtype=np.uint8)
for (colorIndex, color) in enumerate(["r", "g", "b"]):
colorArray[:, colorIndex] = 255 * np.asarray(channels[color])
vnp.addNumpyToVtk(polyData, colorArray, "rgb")
else:
colorized = False
folder = self.getPointCloudFolder()
# If there was an existing point cloud by this name, then just
# set its polyData to the new point cloud.
# This has the effect of preserving all the user-specified properties
# like point size, coloration mode, alpha, etc.
previousPointcloud = folder.findChild(pointcloudName)
if previousPointcloud is not None:
previousPointcloud.setPolyData(polyData)
previousPointcloud._updateColorByProperty()
else:
item = vis.PolyDataItem(pointcloudName, polyData, view=None)
item.addToView(self.view)
if colorized:
item._updateColorByProperty()
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=folder)
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 onPointCloud(self, msg, channel):
pointcloudName = channel.replace('DRAKE_POINTCLOUD_', '', 1)
polyData = vnp.numpyToPolyData(np.asarray(msg.points), createVertexCells=True)
# If the user provided color channels, then use them to colorize
# the pointcloud.
channels = {msg.channel_names[i]: msg.channels[i] for i in range(msg.n_channels)}
if "r" in channels and "g" in channels and "b" in channels:
colorized = True
colorArray = np.empty((msg.n_points, 3), dtype=np.uint8)
for (colorIndex, color) in enumerate(["r", "g", "b"]):
colorArray[:, colorIndex] = 255 * np.asarray(channels[color])
vnp.addNumpyToVtk(polyData, colorArray, "rgb")
else:
colorized = False
folder = self.getPointCloudFolder()
# If there was an existing point cloud by this name, then just
# set its polyData to the new point cloud.
# This has the effect of preserving all the user-specified properties
# like point size, coloration mode, alpha, etc.
previousPointcloud = folder.findChild(pointcloudName)
if previousPointcloud is not None:
previousPointcloud.setPolyData(polyData)
previousPointcloud._updateColorByProperty()
else:
item = vis.PolyDataItem(pointcloudName, polyData, view=None)
item.addToView(self.view)
if colorized:
item._updateColorByProperty()
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=folder)
def _update_cloud(self, xyz, rgb):
poly_data = vnp.numpyToPolyData(xyz)
vnp.addNumpyToVtk(poly_data, rgb, "rgb")
item = self._parent_folder.findChild(self._name)
if item is not None:
item.setPolyData(poly_data)
else:
view = applogic.getCurrentRenderView()
item = vis.PolyDataItem(self._name, poly_data, view=view)
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=self._parent_folder)
item._updateColorByProperty()
def shiftPointsToOrigin(polyData, copy=True, shuffle=True):
points = None
if copy:
points = vnp.getNumpyFromVtk(polyData, 'Points').copy()
else:
points = vnp.getNumpyFromVtk(polyData, 'Points')
if shuffle:
np.random.shuffle(points)
points -= np.average(points, axis=0)
return vnp.numpyToPolyData(points, createVertexCells=True)
def createPlanarLidar(params):
ranges = np.asarray(params["ranges"])
angle_stop = (params["angle_start"] +
len(ranges) * params["angle_step"])
angles = np.arange(
params["angle_start"],
angle_stop,
params["angle_step"])
x = ranges * np.cos(angles)
y = ranges * np.sin(angles)
z = np.zeros(x.shape)
points = np.vstack((x, y, z)).T
return [vnp.numpyToPolyData(points, createVertexCells=True)]
def createPlanarLidar(params):
ranges = np.asarray(params["ranges"])
angle_stop = (params["angle_start"] +
len(ranges) * params["angle_step"])
angles = np.arange(params["angle_start"], angle_stop,
params["angle_step"])
x = ranges * np.cos(angles)
y = ranges * np.sin(angles)
z = np.zeros(x.shape)
points = np.vstack((x, y, z)).T
polyData = vnp.numpyToPolyData(points, createVertexCells=True)
if "channels" in params:
Geometry.addColorChannels(polyData, params["channels"])
return [polyData]
def createPlanarLidar(params):
ranges = np.asarray(params["ranges"])
angle_stop = (params["angle_start"] +
len(ranges) * params["angle_step"])
angles = np.arange(
params["angle_start"],
angle_stop,
params["angle_step"])
x = ranges * np.cos(angles)
y = ranges * np.sin(angles)
z = np.zeros(x.shape)
points = np.vstack((x, y, z)).T
polyData = vnp.numpyToPolyData(points, createVertexCells=True)
if "channels" in params:
Geometry.addColorChannels(polyData, params["channels"])
return [polyData]
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 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 readPolyData(file):
"""
Read poly data while correctly dispatching to special
handler for ply files
"""
extension = os.path.splitext(file)[1]
if extension == ".ply":
from plyfile import PlyData
plydata = PlyData.read(file)
vertex_data = plydata[
'vertex'].data # numpy array with fields ['x', 'y', 'z']
pts = np.zeros([vertex_data.size, 3])
pts[:, 0] = vertex_data['x']
pts[:, 1] = vertex_data['y']
pts[:, 2] = vertex_data['z']
return vnp.numpyToPolyData(pts)
else:
return ioUtils.readPolyData(file)
def createPointcloud(params):
polyData = vnp.numpyToPolyData(np.asarray(params["points"]),
createVertexCells=True)
if "channels" in params:
Geometry.addColorChannels(polyData, params["channels"])
return [polyData]
from director import consoleapp from director import pointselector from director import visualization as vis from director import vtkNumpy as vnp import numpy as np app = consoleapp.ConsoleApp() view = app.createView() randomPoints = np.random.random((1000,3))*3.0 polyData = vnp.numpyToPolyData(randomPoints, createVertexCells=True) vis.showPolyData(polyData, 'pointcloud') view.show() view.forceRender() selector = pointselector.PointSelector(view, polyData) selector.pickArea((200,200), (300,300)) assert selector.getSelectedPoints().GetNumberOfPoints() app.start()
file and re-save it as an ascii ply file.
'''
import os
from director import ioUtils
from director import vtkNumpy as vnp
if __name__ == '__main__':
filename = sys.argv[1]
outputFilename = os.path.splitext(filename)[0] + '.vtp'
print "reading poly data"
polyData = ioUtils.readPolyData(filename)
print "finished reading poly data"
# TODO:
# This should just be fixed in ioUtils.readPolyData, but for now
# there is a workaround for an issue with the ply reader.
# The output of the ply reader has points but not vertex cells,
# so create a new polydata with vertex cells and copy the cells over.
points = vnp.getNumpyFromVtk(polyData, 'Points')
newPolyData = vnp.numpyToPolyData(points, createVertexCells=True)
polyData.SetVerts(newPolyData.GetVerts())
print 'writing:', outputFilename
ioUtils.writePolyData(polyData, outputFilename)
from director import consoleapp from director import pointselector from director import visualization as vis from director import vtkNumpy as vnp import numpy as np app = consoleapp.ConsoleApp() view = app.createView() randomPoints = np.random.random((1000, 3)) * 3.0 polyData = vnp.numpyToPolyData(randomPoints, createVertexCells=True) vis.showPolyData(polyData, 'pointcloud') view.show() view.forceRender() selector = pointselector.PointSelector(view, polyData) selector.pickArea((200, 200), (300, 300)) assert selector.getSelectedPoints().GetNumberOfPoints() app.start()
d = DebugData()
d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0])
show(d, (0, 4, 0))
d = DebugData()
d.addCube(dimensions=[0.8, 0.5, 0.3], center=[0, 0, 0], color=[0, 1, 1])
show(d, (2, 4, 0))
d = DebugData()
d.addPlane(origin=[0, 0, 0], normal=[0, 0, 1], width=0.8, height=0.7, resolution=10, color=[0, 1, 0])
show(d, (4, 4, 0)).setProperty('Surface Mode', 'Surface with edges')
d = DebugData()
d.addCapsule(center=[0, 0, 0], axis=[1, 0, 0], length=1.0, radius=0.1, color=[0.5, 0.5, 1])
show(d, (6, 4, 0))
d = DebugData()
polyData = vnp.numpyToPolyData(np.random.random((1000, 3)))
vnp.addNumpyToVtk(polyData, np.arange(polyData.GetNumberOfPoints()), 'point_ids')
d.addPolyData(polyData)
show(d, (2.5, 5, 0)).setProperty('Color By', 'point_ids')
applogic.resetCamera(viewDirection=[0, 0.1, -1])
app.start()
Note, the ply file needs to be in ascii format and not
binary. The vtk ply reader seems to crash on binary
ply files. You can use meshlab to open a binary ply
file and re-save it as an ascii ply file.
'''
import os
from director import ioUtils
from director import vtkNumpy as vnp
if __name__ == '__main__':
filename = sys.argv[1]
outputFilename = os.path.splitext(filename)[0] + '.vtp'
print("reading poly data")
polyData = ioUtils.readPolyData(filename)
print("finished reading poly data")
# TODO:
# This should just be fixed in ioUtils.readPolyData, but for now
# there is a workaround for an issue with the ply reader.
# The output of the ply reader has points but not vertex cells,
# so create a new polydata with vertex cells and copy the cells over.
points = vnp.getNumpyFromVtk(polyData, 'Points')
newPolyData = vnp.numpyToPolyData(points, createVertexCells=True)
polyData.SetVerts(newPolyData.GetVerts())
print('writing:', outputFilename)
ioUtils.writePolyData(polyData, outputFilename)
def createPointcloud(params):
polyData = vnp.numpyToPolyData(np.asarray(params["points"]),
createVertexCells=True)
if "channels" in params:
Geometry.addColorChannels(polyData, params["channels"])
return [polyData]
def createPointcloud(params):
polyData = vnp.numpyToPolyData(np.asarray(params["points"]),
createVertexCells=True)
return [polyData]
