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 onMergeIntoPointCloud():
allPointClouds = om.findObjectByName("point clouds")
if allPointClouds:
allPointClouds = [
i.getProperty("Name") for i in allPointClouds.children()
]
sel = QtGui.QInputDialog.getItem(
None,
"Point Cloud Merging",
"Pick point cloud to merge into:",
allPointClouds,
current=0,
editable=False,
)
sel = om.findObjectByName(sel)
# Make a copy of each in same frame
polyDataInto = vtk.vtkPolyData()
polyDataInto.ShallowCopy(sel.polyData)
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(
polyDataInto,
sel.getChildFrame().transform)
polyDataFrom = vtk.vtkPolyData()
polyDataFrom.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyDataFrom = segmentation.transformPolyData(
polyDataFrom,
pointCloudObj.getChildFrame().transform)
# Actual merge
append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(
polyDataInto,
sel.getChildFrame().transform.GetInverse())
# resample
append = segmentationroutines.applyVoxelGrid(append, 0.01)
append = segmentation.addCoordArraysToPolyData(append)
# Recenter the frame
sel.setPolyData(append)
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(append))
segmentation.makeMovable(sel, t)
# Hide the old one
if pointCloudObj.getProperty("Name") in allPointClouds:
pointCloudObj.setProperty("Visible", False)
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 getStereoPointCloudElapsed(self,decimation=4, imagesChannel='CAMERA', cameraName='CAMERA_LEFT', removeSize=0):
q = imageManager.queue
utime = q.getCurrentImageTime(cameraName)
if utime == 0:
return None, None, None
if (utime - self.lastUtime < 1E6):
return None, None, None
p = vtk.vtkPolyData()
cameraToLocalFused = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT', 'local', utime, cameraToLocalFused)
cameraToLocal = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocal)
prevToCurrentCameraTransform = vtk.vtkTransform()
prevToCurrentCameraTransform.PostMultiply()
prevToCurrentCameraTransform.Concatenate( cameraToLocal )
prevToCurrentCameraTransform.Concatenate( self.lastCameraToLocal.GetLinearInverse() )
distTravelled = np.linalg.norm( prevToCurrentCameraTransform.GetPosition() )
# 0.2 heavy overlap
# 0.5 quite a bit of overlap
# 1.0 is good
if (distTravelled < 0.2 ):
return None, None, None
q.getPointCloudFromImages(imagesChannel, p, decimation, removeSize, removeThreshold = -1)
self.lastCameraToLocal = cameraToLocal
self.lastUtime = utime
return p, cameraToLocalFused, cameraToLocal
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.addProperty('Radius', 0.03, attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.addProperty('Length', 0.5, attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.properties.setPropertyIndex('Radius', 0)
self.properties.setPropertyIndex('Length', 1)
self.updateGeometryFromProperties()
def __init__(self, name, transform, view):
PolyDataItem.__init__(self, name, vtk.vtkPolyData(), view)
self.transform = transform
self._blockSignals = False
self.actor.SetUserTransform(transform)
self.widget = vtk.vtkFrameWidget()
self.widget.CreateDefaultRepresentation()
self.widget.EnabledOff()
self.rep = self.widget.GetRepresentation()
self.rep.SetTransform(transform)
self.traceData = None
self._frameSync = None
self.addProperty('Scale', 1.0, attributes=om.PropertyAttributes(decimals=2, minimum=0.01, maximum=100, singleStep=0.1, hidden=False))
self.addProperty('Edit', False)
self.addProperty('Trace', False)
self.addProperty('Tube', False)
self.addProperty('Tube Width', 0.002, attributes=om.PropertyAttributes(decimals=3, minimum=0.001, maximum=10, singleStep=0.01, hidden=True))
self.properties.setPropertyIndex('Edit', 0)
self.properties.setPropertyIndex('Trace', 1)
self.properties.setPropertyIndex('Tube', 2)
self.callbacks.addSignal('FrameModified')
self.onTransformModifiedCallback = None
self.observerTag = self.transform.AddObserver('ModifiedEvent', self.onTransformModified)
self._updateAxesGeometry()
self.setProperty('Color By', 'Axes')
self.setProperty('Icon', om.Icons.Axes)
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.addProperty('Dimensions', [0.25, 0.25, 0.25], attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.addProperty('Subdivisions', 0, attributes=om.PropertyAttributes(minimum=0, maximum=1000))
self.properties.setPropertyIndex('Dimensions', 0)
self.properties.setPropertyIndex('Subdivisions', 1)
self.updateGeometryFromProperties()
def onCopyPointCloud():
global lastRandomColor
polyData = vtk.vtkPolyData()
polyData.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyData = segmentation.transformPolyData(
polyData,
pointCloudObj.getChildFrame().transform)
polyData = segmentation.addCoordArraysToPolyData(polyData)
# generate random color, and average with a common color to make them generally similar
lastRandomColor = lastRandomColor + 0.1 + 0.1 * random.random()
rgb = colorsys.hls_to_rgb(lastRandomColor, 0.7, 1.0)
obj = vis.showPolyData(polyData,
pointCloudObj.getProperty('Name') + ' copy',
color=rgb,
parent='point clouds')
#t = vtk.vtkTransform()
#t.PostMultiply()
#t.Translate(filterUtils.computeCentroid(polyData))
#segmentation.makeMovable(obj, t)
om.setActiveObject(obj)
pickedObj.setProperty('Visible', False)
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 getNewHandPolyData(self):
self.handModel.model.setJointPositions(
np.zeros(self.handModel.model.numberOfJoints())
)
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
return filterUtils.transformPolyData(polyData, self.modelToPalm)
def extractClusters(polyData, clusterInXY=False, **kwargs):
''' Segment a single point cloud into smaller clusters
using Euclidean Clustering
'''
if not polyData.GetNumberOfPoints():
return []
if (clusterInXY == True):
''' If Points are seperated in X&Y, then cluster outside this '''
polyDataXY = vtk.vtkPolyData()
polyDataXY.DeepCopy(polyData)
points=vtkNumpy.getNumpyFromVtk(polyDataXY , 'Points') # shared memory
points[:,2] = 0.0
#showPolyData(polyDataXY, 'polyDataXY', visible=False, parent=getDebugFolder())
polyDataXY = applyEuclideanClustering(polyDataXY, **kwargs)
clusterLabels = vtkNumpy.getNumpyFromVtk(polyDataXY, 'cluster_labels')
vtkNumpy.addNumpyToVtk(polyData, clusterLabels, 'cluster_labels')
else:
polyData = applyEuclideanClustering(polyData, **kwargs)
clusterLabels = vtkNumpy.getNumpyFromVtk(polyData, 'cluster_labels')
clusters = []
for i in xrange(1, clusterLabels.max() + 1):
cluster = thresholdPoints(polyData, 'cluster_labels', [i, i])
clusters.append(cluster)
return clusters
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.addProperty('Dimensions', [0.25, 0.25, 0.25], attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.addProperty('Subdivisions', 0, attributes=om.PropertyAttributes(minimum=0, maximum=1000))
self.properties.setPropertyIndex('Dimensions', 0)
self.properties.setPropertyIndex('Subdivisions', 1)
self.updateGeometryFromProperties()
def startCellCaptureMode(self):
self.cellCaptureMode = True
# make sure we start the event filter
self.installEventFilter()
linkNames = self.robotModel.model.getLinkNames()
self.linkDict = {}
for linkName in linkNames:
polyData = vtk.vtkPolyData()
self.robotModel.model.getLinkModelMesh(linkName, polyData)
numCells = int(polyData.GetNumberOfCells())
if numCells == 0:
continue
cellData = polyData.GetCellData()
colorCodeArray = vtk.vtkIdTypeArray()
colorCodeArray.SetNumberOfTuples(numCells)
colorCodeArray.FillComponent(0,0)
colorCodeArray.SetName("colorCodeArray")
cellData.AddArray(colorCodeArray)
cellData.SetActiveScalars("colorCodeArray")
visObj = vis.showPolyData(polyData, linkName)
mapper = visObj.mapper
mapper.SetScalarModeToUseCellData()
mapper.UseLookupTableScalarRangeOn()
mapper.SetLookupTable(visObj._getDefaultColorMap(colorCodeArray, scalarRange=[0.0,2.0]))
mapper.ScalarVisibilityOn()
data = {'polyData': polyData, 'colorCodeArray': colorCodeArray, 'numCells': numCells,
'visObj': visObj, 'mapper': mapper, 'cellData': cellData}
self.linkDict[linkName] = data
def getStereoPointCloudElapsed(self, decimation=4, imagesChannel="CAMERA", cameraName="CAMERA_LEFT", removeSize=0):
q = imageManager.queue
utime = q.getCurrentImageTime(cameraName)
if utime == 0:
return None, None, None
if utime - self.lastUtime < 1e6:
return None, None, None
p = vtk.vtkPolyData()
cameraToLocalFused = vtk.vtkTransform()
q.getTransform("CAMERA_LEFT_ALT", "local", utime, cameraToLocalFused)
cameraToLocal = vtk.vtkTransform()
q.getTransform("CAMERA_LEFT", "local", utime, cameraToLocal)
prevToCurrentCameraTransform = vtk.vtkTransform()
prevToCurrentCameraTransform.PostMultiply()
prevToCurrentCameraTransform.Concatenate(cameraToLocal)
prevToCurrentCameraTransform.Concatenate(self.lastCameraToLocal.GetLinearInverse())
distTravelled = np.linalg.norm(prevToCurrentCameraTransform.GetPosition())
# 0.2 heavy overlap
# 0.5 quite a bit of overlap
# 1.0 is good
if distTravelled < 0.2:
return None, None, None
q.getPointCloudFromImages(imagesChannel, p, decimation, removeSize, removeThreshold=-1)
self.lastCameraToLocal = cameraToLocal
self.lastUtime = utime
return p, cameraToLocalFused, cameraToLocal
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.addProperty('Radius', 0.03, attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.addProperty('Length', 0.5, attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.properties.setPropertyIndex('Radius', 0)
self.properties.setPropertyIndex('Length', 1)
self.updateGeometryFromProperties()
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(director.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
polyData = filterUtils.transformPolyData(
polyData, transform)
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def vtkICP(self):
object_name = str(self.ui.alignObject.text)
if object_name == "" or not om.findObjectByName(object_name):
print "object: " + object_name + " not found"
return
model = vtk.vtkPolyData()
om.findObjectByName(object_name + ".urdf").model.getModelMesh(model)
scene = om.findObjectByName(object_name).polyData
icp = vtk.vtkIterativeClosestPointTransform()
icp.SetMaximumNumberOfIterations(100)
#need to shift centroid to center of abject after clicking on it
icp.StartByMatchingCentroidsOn()
icp.SetSource(model)
icp.SetTarget(scene)
icp.GetLandmarkTransform().SetModeToRigidBody()
icp.Modified()
icp.Update()
t = vtk.vtkTransformPolyDataFilter()
t.SetInput(model)
t.SetTransform(icp)
t.Update()
transformedObject = t.GetOutput()
print transformedObject
vis.showPolyData(transformedObject,
object_name + "_transform",
color=[0, 1, 0],
parent=self.getTransformedObjectsFolder())
self.packageAlignmentResult(object_name, icp.GetMatrix())
def createCellLocators(self, dataDict=None):
self.locatorData = {}
for linkName, cellCodeArray in dataDict.iteritems():
polyData = vtk.vtkPolyData()
self.robotStateModel.model.getLinkModelMesh(linkName, polyData)
cellCodeArrayVTK = numpy_support.numpy_to_vtk(cellCodeArray)
arrayName = "cellCodeArray"
cellCodeArrayVTK.SetName(arrayName)
polyData.GetCellData().AddArray(cellCodeArrayVTK)
thresholdRange = [1,1]
polyData = filterUtils.thresholdCells(polyData, arrayName, thresholdRange)
cellData = polyData.GetCellData()
normals = cellData.GetNormals()
if polyData.GetNumberOfCells() == 0:
continue
locator = vtk.vtkCellLocator()
locator.SetDataSet(polyData)
locator.BuildLocator()
meshToWorld = transformUtils.copyFrame(self.linkFrameContainer.getLinkFrame(linkName))
worldToMesh = meshToWorld.GetLinearInverse()
self.locatorData[linkName] = {'locator':locator, 'polyData': polyData, 'meshToWorld': meshToWorld,
'worldToMesh': worldToMesh, 'cellData': cellData, 'normals': normals}
def __init__(self, name, transform, view):
PolyDataItem.__init__(self, name, vtk.vtkPolyData(), view)
self.transform = transform
self._blockSignals = False
self.actor.SetUserTransform(transform)
self.widget = vtk.vtkFrameWidget()
self.widget.CreateDefaultRepresentation()
self.widget.EnabledOff()
self.rep = self.widget.GetRepresentation()
self.rep.SetTransform(transform)
self.traceData = None
self._frameSync = None
self.addProperty('Scale', 1.0, attributes=om.PropertyAttributes(decimals=2, minimum=0.01, maximum=100, singleStep=0.1, hidden=False))
self.addProperty('Edit', False)
self.addProperty('Trace', False)
self.addProperty('Tube', False)
self.addProperty('Tube Width', 0.002, attributes=om.PropertyAttributes(decimals=3, minimum=0.001, maximum=10, singleStep=0.01, hidden=True))
self.properties.setPropertyIndex('Edit', 0)
self.properties.setPropertyIndex('Trace', 1)
self.properties.setPropertyIndex('Tube', 2)
self.callbacks.addSignal('FrameModified')
self.onTransformModifiedCallback = None
self.observerTag = self.transform.AddObserver('ModifiedEvent', self.onTransformModified)
self._updateAxesGeometry()
self.setProperty('Color By', 'Axes')
self.setProperty('Icon', om.Icons.Axes)
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
scale = self.getProperty('Scale')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(director.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
if not scale == [1, 1, 1]:
transform = vtk.vtkTransform()
transform.Scale(scale)
transformFilter = vtk.vtkTransformPolyDataFilter()
transformFilter.SetInput(polyData)
transformFilter.SetTransform(transform)
transformFilter.Update()
polyData = transformFilter.GetOutput()
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.setProperty('Collision Enabled', False)
self.addProperty('Radius',
0.15,
attributes=om.PropertyAttributes(decimals=3,
singleStep=0.01,
minimum=0.0,
maximum=1e4))
self.addProperty('Tube Radius',
0.02,
attributes=om.PropertyAttributes(decimals=3,
singleStep=0.01,
minimum=0.0,
maximum=1e4))
self.addProperty('Segments',
8,
attributes=om.PropertyAttributes(decimals=3,
singleStep=1,
minimum=3,
maximum=100))
self.properties.setPropertyIndex('Radius', 0)
self.properties.setPropertyIndex('Tube Radius', 1)
self.properties.setPropertyIndex('Segments', 2)
self.updateGeometryFromProperties()
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.setProperty('Collision Enabled', False)
self.addProperty('Filename', '')
self.properties.setPropertyIndex('Filename', 0)
# attempt to reload geometry if it is currently empty
if self.getProperty('Filename') and not self.polyData.GetNumberOfPoints():
self.updateGeometryFromProperties()
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.setProperty("Collision Enabled", False)
self.addProperty("Filename", "")
self.properties.setPropertyIndex("Filename", 0)
# attempt to reload geometry if it is currently empty
if self.getProperty(
"Filename") and not self.polyData.GetNumberOfPoints():
self.updateGeometryFromProperties()
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(),
self.getTransformedName(obj),
view=imageView.view,
color=obj.getProperty('Color'),
parent=self.getFolderName(),
visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj
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 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 initCameraFrustumVisualizer(depthScanner):
cameraObj = vis.showPolyData(vtk.vtkPolyData(), 'camera', parent=depthScanner.parentFolder, view=depthScanner.pointCloudView)
cameraFrame = vis.addChildFrame(cameraObj)
cameraFrame.setProperty('Scale', 50)
cameraObj.setProperty('Visible', False)
pointCloudToCamera = transformUtils.frameFromPositionAndRPY((0,0,0,), (-90, 90, 0)).GetLinearInverse()
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def onCameraModified():
cameraToWorld = getCameraTransform(depthScanner.view.camera())
depthScanner.pointCloudObj.actor.SetUserTransform(transformUtils.concatenateTransforms([pointCloudToCamera, cameraToWorld]))
cameraFrame.copyFrame(cameraToWorld)
def enableFrustum():
updateCameraMesh()
cameraObj.setProperty('Visible', True)
onCameraModified()
depthScanner._updateFunc = onCameraModified
applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True)
applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView)
depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])
def disableFrustum():
cameraObj.setProperty('Visible', False)
depthScanner.pointCloudObj.actor.SetUserTransform(None)
depthScanner._updateFunc = None
applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False)
applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView)
depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])
return FieldContainer(
updateCameraMesh=updateCameraMesh,
onCameraModified=onCameraModified,
enableFrustum=enableFrustum,
disableFrustum=disableFrustum
)
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.setProperty('Collision Enabled', False)
self.addProperty('Radius', 0.15, attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.addProperty('Tube Radius', 0.02, attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.addProperty('Segments', 8, attributes=om.PropertyAttributes(decimals=3, singleStep=1, minimum=3, maximum=100))
self.properties.setPropertyIndex('Radius', 0)
self.properties.setPropertyIndex('Tube Radius', 1)
self.properties.setPropertyIndex('Segments', 2)
self.updateGeometryFromProperties()
def _init(self, size):
items = self._getPolyDataItems()
for item in items:
om.removeFromObjectModel(item)
for i in range(0, size):
name = self.name if i == 0 else (self.name + str(i))
item = PolyDataItem(name, vtk.vtkPolyData(), view=None)
item.setProperty("Visible", self.getProperty("Visible"))
for view in self.views:
item.addToView(view)
om.addToObjectModel(item, self)
def computePlanarConvexHull(polyData, expectedNormal=None):
plane = vtk.vtkPlane()
vtk.vtkSurfaceFitter.ComputePlane(polyData, plane)
if expectedNormal is not None:
planeNormal = plane.GetNormal()
if np.dot(planeNormal, expectedNormal) < 0:
plane.SetNormal(-1*np.array(planeNormal))
chull = vtk.vtkPolyData()
vtk.vtkSurfaceFitter.ComputeConvexHull(polyData, plane, chull)
return FieldContainer(points=polyData, convexHull=chull, plane=plane)
def addPolygon(self, points, color=[1,1,1]):
points = vnp.getVtkPointsFromNumpy(points.copy())
polygon = vtk.vtkPolygon()
polygon.GetPointIds().SetNumberOfIds(points.GetNumberOfPoints())
for i in range(points.GetNumberOfPoints()):
polygon.GetPointIds().SetId(i, i)
polyData = vtk.vtkPolyData()
polyData.SetPoints(points)
polyData.Allocate(1, 1)
polyData.InsertNextCell(polygon.GetCellType(), polygon.GetPointIds())
self.addPolyData(polyData, color)
def addPolygon(self, points, color=[1, 1, 1]):
points = vnp.getVtkPointsFromNumpy(np.array(points, dtype=np.float64))
polygon = vtk.vtkPolygon()
polygon.GetPointIds().SetNumberOfIds(points.GetNumberOfPoints())
for i in range(points.GetNumberOfPoints()):
polygon.GetPointIds().SetId(i, i)
polyData = vtk.vtkPolyData()
polyData.SetPoints(points)
polyData.Allocate(1, 1)
polyData.InsertNextCell(polygon.GetCellType(), polygon.GetPointIds())
self.addPolyData(polyData, color)
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.setProperty('Collision Enabled', False)
self.addProperty('Filename', '')
self.addProperty('Scale', [1.0, 1.0, 1.0], attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.properties.setPropertyIndex('Filename', 0)
self.properties.setPropertyIndex('Scale', 1)
# attempt to reload geometry if it is currently empty
if self.getProperty('Filename') and not self.polyData.GetNumberOfPoints():
self.updateGeometryFromProperties()
def __init__(self, name, view):
AffordanceItem.__init__(self, name, vtk.vtkPolyData(), view)
self.setProperty('Collision Enabled', False)
self.addProperty('Filename', '')
self.addProperty('Scale', [1.0, 1.0, 1.0], attributes=om.PropertyAttributes(decimals=3, singleStep=0.01, minimum=0.0, maximum=1e4))
self.properties.setPropertyIndex('Filename', 0)
self.properties.setPropertyIndex('Scale', 1)
# attempt to reload geometry if it is currently empty
if self.getProperty('Filename') and not self.polyData.GetNumberOfPoints():
self.updateGeometryFromProperties()
def computePlanarConvexHull(polyData, expectedNormal=None):
plane = vtk.vtkPlane()
vtk.vtkSurfaceFitter.ComputePlane(polyData, plane)
if expectedNormal is not None:
planeNormal = plane.GetNormal()
if np.dot(planeNormal, expectedNormal) < 0:
plane.SetNormal(-1 * np.array(planeNormal))
chull = vtk.vtkPolyData()
vtk.vtkSurfaceFitter.ComputeConvexHull(polyData, plane, chull)
return FieldContainer(points=polyData, convexHull=chull, plane=plane)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.PointCloudQueue.getPointCloudFromPointCloud(p)
if not p.GetNumberOfPoints():
return
sensorToLocalFused = vtk.vtkTransform()
self.queue.getTransform('local', 'local', utime, sensorToLocalFused)
p = filterUtils.transformPolyData(p, sensorToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.initialized = True
def getVtkPolyDataFromNumpyPoints(points):
'''
Given an Nx3 array of xyz points
Return a new vtkPolyData containing points and vertex cells.
If the input points is not float64 it will be converted first.
'''
if points.dtype != np.float64:
points = points.astype(np.float64)
polyData = vtk.vtkPolyData()
polyData.SetPoints(getVtkPointsFromNumpy(points))
vtk.vtkPCLConversions.AddVertexCells(polyData)
return polyData
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.PointCloudQueue.getPointCloudFromPointCloud(p)
if not p.GetNumberOfPoints():
return
sensorToLocalFused = vtk.vtkTransform()
self.queue.getTransform('local', 'local', utime, sensorToLocalFused)
p = filterUtils.transformPolyData(p,sensorToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.initialized = True
def onMergeIntoPointCloud():
allPointClouds = om.findObjectByName('point clouds')
if allPointClouds:
allPointClouds = [i.getProperty('Name') for i in allPointClouds.children()]
sel = QtGui.QInputDialog.getItem(None, "Point Cloud Merging", "Pick point cloud to merge into:", allPointClouds, current=0, editable=False)
sel = om.findObjectByName(sel)
# Make a copy of each in same frame
polyDataInto = vtk.vtkPolyData()
polyDataInto.ShallowCopy(sel.polyData)
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform)
polyDataFrom = vtk.vtkPolyData()
polyDataFrom.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyDataFrom = segmentation.transformPolyData(polyDataFrom, pointCloudObj.getChildFrame().transform)
# Actual merge
append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform.GetInverse())
# resample
append = segmentationroutines.applyVoxelGrid(append, 0.01)
append = segmentation.addCoordArraysToPolyData(append)
# Recenter the frame
sel.setPolyData(append)
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(append))
segmentation.makeMovable(sel, t)
# Hide the old one
if pointCloudObj.getProperty('Name') in allPointClouds:
pointCloudObj.setProperty('Visible', False)
def _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, cameraToLocalFused)
p = filterUtils.transformPolyData(p,cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
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 _updateSource(self):
p = vtk.vtkPolyData()
utime = self.KinectQueue.getPointCloudFromKinect(p)
if not p.GetNumberOfPoints():
return
cameraToLocalFused = vtk.vtkTransform()
self.queue.getTransform('KINECT_RGB', 'local', utime, cameraToLocalFused)
p = filterUtils.transformPolyData(p, cameraToLocalFused)
self.polyDataObj.setPolyData(p)
if not self.polyDataObj.initialized:
self.polyDataObj.setProperty('Color By', 'rgb_colors')
self.polyDataObj.initialized = True
def decodePolyData(data):
'''Given a numpy int8 array, deserializes the data to construct a new
vtkPolyData object and returns the result.'''
if not hasattr(vtk, 'vtkCommunicator'):
r = vtk.vtkPolyDataReader()
r.ReadFromInputStringOn()
r.SetInputString(str(data.data))
r.Update()
return shallowCopy(r.GetOutput())
charArray = vnp.getVtkFromNumpy(data)
assert isinstance(charArray, vtk.vtkCharArray)
polyData = vtk.vtkPolyData()
vtk.vtkCommunicator.UnMarshalDataObject(charArray, polyData)
return polyData
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 numpyToPolyData(pts, pointData=None, createVertexCells=True):
pd = vtk.vtkPolyData()
pd.SetPoints(getVtkPointsFromNumpy(pts.copy()))
if pointData is not None:
for key, value in pointData.iteritems():
addNumpyToVtk(pd, value.copy(), key)
if createVertexCells:
f = vtk.vtkVertexGlyphFilter()
f.SetInputData(pd)
f.Update()
pd = shallowCopy(f.GetOutput())
return pd
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 numpyToPolyData(pts, pointData=None, createVertexCells=True):
pd = vtk.vtkPolyData()
pd.SetPoints(getVtkPointsFromNumpy(pts.copy()))
if pointData is not None:
for key, value in pointData.iteritems():
addNumpyToVtk(pd, value.copy(), key)
if createVertexCells:
f = vtk.vtkVertexGlyphFilter()
f.SetInputData(pd)
f.Update()
pd = shallowCopy(f.GetOutput())
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 getStereoPointCloud(decimation=4, imagesChannel='CAMERA', cameraName='CAMERA_LEFT', removeSize=0):
q = imageManager.queue
utime = q.getCurrentImageTime(cameraName)
if utime == 0:
return None
p = vtk.vtkPolyData()
cameraToLocal = vtk.vtkTransform()
q.getPointCloudFromImages(imagesChannel, p, decimation, removeSize)
q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocal)
p = filterUtils.transformPolyData(p, cameraToLocal)
return p
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 getStereoPointCloud(decimation=4, imagesChannel='MULTISENSE_CAMERA', cameraName='MULTISENSE_CAMERA_LEFT', removeSize=0, rangeThreshold = -1):
q = imageManager.queue
utime = q.getCurrentImageTime(cameraName)
if utime == 0:
return None
p = vtk.vtkPolyData()
cameraToLocal = vtk.vtkTransform()
q.getPointCloudFromImages(imagesChannel, p, decimation, removeSize, rangeThreshold)
if (p.GetNumberOfPoints() > 0):
q.getTransform(cameraName, 'local', utime, cameraToLocal)
p = filterUtils.transformPolyData(p, cameraToLocal)
return p
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 assimpMeshToPolyData(mesh):
verts = mesh.vertices
faces = mesh.faces
nfaces = faces.shape[0]
nverts = verts.shape[0]
assert verts.shape[1] == 3
assert faces.shape[1] == 3
points = vnp.getVtkPointsFromNumpy(verts)
cells = vtk.vtkCellArray()
for i in xrange(nfaces):
face = faces[i]
tri = vtk.vtkTriangle()
tri.GetPointIds().SetId(0, face[0])
tri.GetPointIds().SetId(1, face[1])
tri.GetPointIds().SetId(2, face[2])
cells.InsertNextCell(tri)
polyData = vtk.vtkPolyData()
polyData.SetPoints(points)
polyData.SetPolys(cells)
if mesh.normals.shape[0] > 0:
assert mesh.normals.shape[0] == nverts
normals = vnp.getVtkFromNumpy(mesh.normals)
normals.SetName('normals')
polyData.GetPointData().AddArray(normals)
polyData.GetPointData().SetNormals(normals)
for i, tcoords in enumerate(mesh.texturecoords):
tcoordArray = assimpTextureCoordsToArray(tcoords)
tcoordArray.SetName('tcoords_%d' % i)
polyData.GetPointData().AddArray(tcoordArray)
if i == 0:
polyData.GetPointData().SetTCoords(tcoordArray)
return polyData
def onCopyPointCloud():
global lastRandomColor
polyData = vtk.vtkPolyData()
polyData.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyData = segmentation.transformPolyData(polyData, pointCloudObj.getChildFrame().transform)
polyData = segmentation.addCoordArraysToPolyData(polyData)
# generate random color, and average with a common color to make them generally similar
lastRandomColor = lastRandomColor + 0.1 + 0.1*random.random()
rgb = colorsys.hls_to_rgb(lastRandomColor, 0.7, 1.0)
obj = vis.showPolyData(polyData, pointCloudObj.getProperty('Name') + ' copy', color=rgb, parent='point clouds')
#t = vtk.vtkTransform()
#t.PostMultiply()
#t.Translate(filterUtils.computeCentroid(polyData))
#segmentation.makeMovable(obj, t)
om.setActiveObject(obj)
pickedObj.setProperty('Visible', False)
def __init__(self, view, _PointCloudQueue):
self.view = view
self.PointCloudQueue = _PointCloudQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = PointCloudQueue.getPointCloudFromPointCloud(self.p)
self.polyDataObj = vis.PolyDataItem('pointcloud source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(director.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def getNewHandPolyData(self):
self.handModel.model.setJointPositions(np.zeros(self.handModel.model.numberOfJoints()))
polyData = vtk.vtkPolyData()
self.handModel.model.getModelMesh(polyData)
return filterUtils.transformPolyData(polyData, self.modelToPalm)
def setupObjectInCamera(self, obj):
imageView = self.imageView
obj = vis.updatePolyData(vtk.vtkPolyData(), self.getTransformedName(obj), view=imageView.view, color=obj.getProperty('Color'), parent=self.getFolderName(), visible=obj.getProperty('Visible'))
self.addActorToImageOverlay(obj, imageView)
return obj