def applyCameraTexture(obj, imageManager, imageName="CAMERA_LEFT"):
imageUtime = imageManager.getUtime(imageName)
if not imageUtime:
return
cameraToLocal = vtk.vtkTransform()
imageManager.queue.getTransform(imageName, "local", imageUtime, cameraToLocal)
pd = filterUtils.transformPolyData(obj.polyData, obj.actor.GetUserTransform())
pd = filterUtils.transformPolyData(pd, cameraToLocal.GetLinearInverse())
imageManager.queue.computeTextureCoords(imageName, pd)
tcoordsArrayName = "tcoords_%s" % imageName
tcoords = pd.GetPointData().GetArray(tcoordsArrayName)
assert tcoords
obj.polyData.GetPointData().SetTCoords(None)
obj.polyData.GetPointData().SetTCoords(tcoords)
obj._updateColorByProperty()
obj.actor.SetTexture(imageManager.getTexture(imageName))
obj.actor.GetProperty().LightingOff()
obj.actor.GetProperty().SetColor([1, 1, 1])
def applyCameraTexture(obj, imageManager, imageName='CAMERA_LEFT'):
imageUtime = imageManager.getUtime(imageName)
if not imageUtime:
return
cameraToLocal = vtk.vtkTransform()
imageManager.queue.getTransform(imageName, 'local', imageUtime,
cameraToLocal)
pd = filterUtils.transformPolyData(obj.polyData,
obj.actor.GetUserTransform())
pd = filterUtils.transformPolyData(pd, cameraToLocal.GetLinearInverse())
imageManager.queue.computeTextureCoords(imageName, pd)
tcoordsArrayName = 'tcoords_%s' % imageName
tcoords = pd.GetPointData().GetArray(tcoordsArrayName)
assert tcoords
obj.polyData.GetPointData().SetTCoords(None)
obj.polyData.GetPointData().SetTCoords(tcoords)
obj._updateColorByProperty()
obj.actor.SetTexture(imageManager.getTexture(imageName))
obj.actor.GetProperty().LightingOff()
obj.actor.GetProperty().SetColor([1, 1, 1])
def align(self):
if self.meshPoints is None or self.imagePoints is None:
return
t1 = computeLandmarkTransform(self.imagePoints, self.meshPoints)
polyData = filterUtils.transformPolyData(self.pointCloud, t1)
vis.showPolyData(polyData, 'transformed pointcloud', view=self.view, colorByName='rgb_colors', visible=True)
vis.showPolyData(filterUtils.transformPolyData(makeDebugPoints(self.imagePoints), t1), 'transformed image pick points', color=[0,0,1], view=self.view, parent=self.parent)
sceneToModelTransform = t1
modelToSceneTransform = t1.GetLinearInverse()
alignedModel = filterUtils.transformPolyData(self.modelPolyData, modelToSceneTransform)
vis.showPolyData(alignedModel, 'aligned model', view=self.sceneView, parent=self.parent)
#
# boxBounds = [[-0.5,0.50], [-0.3,0.3], [0.15,1.5]] #xmin,xmax, ymin,ymax, zmin,zmax
#
# polyData = segmentation.cropToBounds(polyData, self.robotBaseFrame, [[-0.5,0.50],[-0.3,0.3],[0.15,1.5]])
#
# #arrayName = 'distance_to_mesh'
# #dists = computePointToSurfaceDistance(polyData, self.robotMesh)
# #vnp.addNumpyToVtk(polyData, dists, arrayName)
# #polyData = filterUtils.thresholdPoints(polyData, arrayName, [0.0, distanceToMeshThreshold])
#
#
# #polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
# #polyData = segmentation.applyEuclideanClustering(polyData, clusterTolerance=0.04)
# #polyData = segmentation.thresholdPoints(polyData, 'cluster_labels', [1,1])
#
# vis.showPolyData(polyData, 'filtered points for icp', color=[0,1,0], view=self.view, visible=True)
#
# t2 = segmentation.applyICP(polyData, self.robotMesh)
#
# vis.showPolyData(filterUtils.transformPolyData(polyData, t2), 'filtered points after icp', color=[0,1,0], view=self.view, visible=False)
#
#
# cameraToWorld = transformUtils.concatenateTransforms([t1, t2])
# polyData = filterUtils.transformPolyData(self.pointCloud, cameraToWorld)
# vis.showPolyData(polyData, 'aligned pointcloud', colorByName='rgb_colors', view=self.view, visible=True)
#
# # cameraToWorldMsg = lcmframe.rigidTransformMessageFromFrame(cameraToWorld)
# # lcmUtils.publish('OPENNI_FRAME_LEFT_TO_LOCAL', cameraToWorldMsg)
#
#
if self.resultsDict is not None:
self.resultsDict['modelToSceneTransform'] = modelToSceneTransform
self.resultsDict['alignedModel'] = alignedModel
if self.callback is not None:
self.callback()
def drawFrameInCamera(t, frameName='new frame',visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
om.removeFromObjectModel(res)
pd = res.polyData
pd = filterUtils.transformPolyData(pd, t)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd )
vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
def drawFrameInCamera(t, frameName='new frame',visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
res = vis.showFrame( vtk.vtkTransform() , 'temp',view=v, visible=True, scale = 0.2)
om.removeFromObjectModel(res)
pd = res.polyData
pd = filterUtils.transformPolyData(pd, t)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd )
vis.showPolyData(pd, ('overlay ' + frameName), view=v, colorByName='Axes',parent='camera overlay',visible=visible)
def buildAccelSphere(center, a_max, color=[1,0.3,0.0], alpha=0.5):
d = DebugData()
d.addSphere([0,0,0], radius=1)
polyData = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(a_max, a_max, a_max)
polyData = filterUtils.transformPolyData(polyData, t)
t = vtk.vtkTransform()
t.Translate(center)
polyData = filterUtils.transformPolyData(polyData, t)
obj = vis.updatePolyData(polyData, 'accel_sphere', color=color, alpha=alpha)
return polyData
def buildEllipse(i, center, x_scale, y_scale, z_scale, color=[1,1,1], alpha=1.0):
d = DebugData()
d.addSphere([0,0,0], radius=1)
polyData = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(x_scale, y_scale, z_scale)
polyData = filterUtils.transformPolyData(polyData, t)
t = vtk.vtkTransform()
t.Translate(center)
polyData = filterUtils.transformPolyData(polyData, t)
obj = vis.updatePolyData(polyData, str(i), color=color, alpha=alpha)
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 transformGeometry(polyDataList, geom):
t = transformUtils.transformFromPose(geom.position, geom.quaternion)
polyDataList = [
filterUtils.transformPolyData(polyData, t)
for polyData in polyDataList
]
return polyDataList
def _handleRigidBodies(self, bodies):
if not bodies:
return
folder = self.getRootFolder()
for body in bodies:
name = 'Body ' + str(body.id)
x,y,z,w = body.quat
quat = (w,x,y,z)
bodyToOptitrack = transformUtils.transformFromPose(body.xyz, quat)
bodyToWorld = transformUtils.concatenateTransforms((bodyToOptitrack, self.optitrackToWorld))
obj = folder.findChild(name)
if not obj:
geometry = self._makeMarkers(body.marker_xyz)
geometry = filterUtils.transformPolyData(geometry, bodyToOptitrack.GetLinearInverse())
obj = vis.showPolyData(geometry, name, parent=folder, color=[1,0,0])
frameObj = vis.addChildFrame(obj)
frameObj.setProperty('Scale', 0.2)
frameObj.setProperty('Visible', True)
obj.getChildFrame().copyFrame(bodyToWorld)
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName('stereo'))
om.getOrCreateContainer('stereo')
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime('CAMERA_TSDF')
q.getTransform('CAMERA_LEFT', 'local', utime, cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform('CAMERA_LEFT_ALT', 'local', utime,
cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate(cameraToLocalNow)
fusedNowToLocalNow.Concatenate(
cameraToLocalFusedNow.GetLinearInverse())
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate(fusedNowToLocalNow)
fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i])
pd = filterUtils.transformPolyData(self.pointClouds[i],
fusedTransform)
vis.showFrame(fusedTransform, ('cloud frame ' + str(i)),
visible=True,
scale=0.2,
parent='stereo')
vis.showPolyData(pd, ('stereo ' + str(i)),
parent='stereo',
colorByName='rgb_colors')
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 drawObjectInCamera(objectName,visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
obj = om.findObjectByName(objectName)
if obj is None:
return
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
objPolyDataOriginal = obj.polyData
pd = objPolyDataOriginal
pd = filterUtils.transformPolyData(pd, objToLocalT)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd)
vis.showPolyData(pd, ('overlay ' + objectName), view=v, color=[0,1,0],parent='camera overlay',visible=visible)
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 saveAboveTablePolyData(self):
aboveTablePolyDataRaw = filterUtils.transformPolyData(
self.aboveTablePolyData,
self.firstFrameToWorldTransform.GetLinearInverse())
ioUtils.writePolyData(aboveTablePolyDataRaw,
self.pathDict['aboveTablePointcloud'])
def addChildFrame(obj, initialTransform=None):
'''
Adds a child frame to the given PolyDataItem. If initialTransform is given,
the object's polydata is transformed using the inverse of initialTransform
and then a child frame is assigned to the object to maintain its original
position.
'''
if obj.getChildFrame():
return obj.getChildFrame()
if initialTransform:
pd = filterUtils.transformPolyData(obj.polyData,
initialTransform.GetLinearInverse())
obj.setPolyData(pd)
t = initialTransform
else:
t = obj.actor.GetUserTransform()
if t is None:
t = vtk.vtkTransform()
t.PostMultiply()
frame = showFrame(t,
obj.getProperty('Name') + ' frame',
parent=obj,
scale=0.2,
visible=False)
obj.actor.SetUserTransform(t)
return frame
def centerObject(visObj):
polyData = filterUtils.transformPolyData(visObj.polyData, visObj.actor.GetUserTransform())
name = visObj.getProperty('Name')
om.removeFromObjectModel(visObj)
newVisObj = vis.showPolyData(polyData, name)
vis.addChildFrame(newVisObj)
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 loadStoredObjects(self):
if not os.path.isfile(self.pathDict['registrationResult']):
return
stream = file(self.pathDict['registrationResult'])
registrationResult = yaml.load(stream)
for objName, data in registrationResult.iteritems():
objectMeshFilename = data[
'filename'] # should be relative to getLabelFusionDataDir()
if len(objectMeshFilename) == 0:
objectMeshFilename = utils.getObjectMeshFilename(
self.objectData, objName)
else:
objectMeshFilename = os.path.join(
utils.getLabelFusionDataDir(), objectMeshFilename)
objectToFirstFrame = transformUtils.transformFromPose(
data['pose'][0], data['pose'][1])
objectToWorld = transformUtils.concatenateTransforms(
[objectToFirstFrame, self.firstFrameToWorldTransform])
polyData = ioUtils.readPolyData(objectMeshFilename)
polyDataWorld = filterUtils.transformPolyData(
polyData, objectToWorld)
d = dict()
d['transform'] = objectToWorld
d['polyData'] = polyDataWorld
self.storedObjects[objName] = d
def saveObjectPolyData(objectName):
visObj = om.findObjectByName(objectName)
filename = os.path.join(getLabelFusionDataDir(), 'object-meshes',
objectName + '_aligned.vtp')
polyData = filterUtils.transformPolyData(visObj.polyData,
visObj.actor.GetUserTransform())
ioUtils.writePolyData(polyData, filename)
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 addChildFrame(obj, initialTransform=None):
'''
Adds a child frame to the given PolyDataItem. If initialTransform is given,
the object's polydata is transformed using the inverse of initialTransform
and then a child frame is assigned to the object to maintain its original
position.
'''
if obj.getChildFrame():
return frame
if initialTransform:
pd = filterUtils.transformPolyData(obj.polyData, initialTransform.GetLinearInverse())
obj.setPolyData(pd)
t = initialTransform
else:
t = obj.actor.GetUserTransform()
if t is None:
t = vtk.vtkTransform()
t.PostMultiply()
frame = showFrame(t, obj.getProperty('Name') + ' frame', parent=obj, scale=0.2, visible=False)
obj.actor.SetUserTransform(t)
return frame
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform('local', imageView.imageName, localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
'''
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
'''
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
def updateObjectInCamera(self, obj, cameraObj):
imageView = self.imageView
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform()
or vtk.vtkTransform())
localToCameraT = vtk.vtkTransform()
self.imageQueue.getTransform("local", imageView.imageName,
localToCameraT)
t = vtk.vtkTransform()
t.PostMultiply()
t.Concatenate(objToLocalT)
t.Concatenate(localToCameraT)
pd = filterUtils.transformPolyData(obj.polyData, t)
"""
normals = pd.GetPointData().GetNormals()
cameraToImageT = vtk.vtkTransform()
imageQueue.getCameraProjectionTransform(imageView.imageName, cameraToImageT)
pd = filterUtils.transformPolyData(pd, cameraToImageT)
pts = vnp.getNumpyFromVtk(pd, 'Points')
pts[:,0] /= pts[:,2]
pts[:,1] /= pts[:,2]
pd.GetPointData().SetNormals(normals)
"""
self.imageQueue.projectPoints(imageView.imageName, pd)
cameraObj.setPolyData(pd)
self.addActorToImageOverlay(cameraObj, imageView)
def drawObjectInCamera(objectName,visible=True):
v = imageView.view
q = cameraview.imageManager.queue
localToCameraT = vtk.vtkTransform()
q.getTransform('local', 'CAMERA_LEFT', localToCameraT)
obj = om.findObjectByName(objectName)
if obj is None:
return
objToLocalT = transformUtils.copyFrame(obj.actor.GetUserTransform() or vtk.vtkTransform())
objPolyDataOriginal = obj.polyData
pd = objPolyDataOriginal
pd = filterUtils.transformPolyData(pd, objToLocalT)
pd = filterUtils.transformPolyData(pd, localToCameraT)
q.projectPoints('CAMERA_LEFT', pd)
vis.showPolyData(pd, ('overlay ' + objectName), view=v, color=[0,1,0],parent='camera overlay',visible=visible)
def showFusedMaps(self):
om.removeFromObjectModel(om.findObjectByName("stereo"))
om.getOrCreateContainer("stereo")
q = imageManager.queue
cameraToLocalNow = vtk.vtkTransform()
utime = q.getCurrentImageTime("CAMERA_TSDF")
q.getTransform("CAMERA_LEFT", "local", utime, cameraToLocalNow)
cameraToLocalFusedNow = vtk.vtkTransform()
q.getTransform("CAMERA_LEFT_ALT", "local", utime, cameraToLocalFusedNow)
for i in range(len(self.pointClouds)):
fusedNowToLocalNow = vtk.vtkTransform()
fusedNowToLocalNow.PreMultiply()
fusedNowToLocalNow.Concatenate(cameraToLocalNow)
fusedNowToLocalNow.Concatenate(cameraToLocalFusedNow.GetLinearInverse())
fusedTransform = vtk.vtkTransform()
fusedTransform.PreMultiply()
fusedTransform.Concatenate(fusedNowToLocalNow)
fusedTransform.Concatenate(self.cameraToLocalFusedTransforms[i])
pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform)
vis.showFrame(fusedTransform, ("cloud frame " + str(i)), visible=True, scale=0.2, parent="stereo")
vis.showPolyData(pd, ("stereo " + str(i)), parent="stereo", colorByName="rgb_colors")
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData,
t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
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 transformGeometry(polyDataList, geom):
if "transform" in geom:
t = transformFromDict(geom["transform"])
polyDataList = [
filterUtils.transformPolyData(polyData, t)
for polyData in polyDataList
]
return polyDataList
def setPolyData(self, polyData):
if polyData.GetNumberOfPoints():
originPose = self.getProperty('Origin')
pos, quat = originPose[:3], originPose[3:]
t = transformUtils.transformFromPose(pos, quat)
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
PolyDataItem.setPolyData(self, polyData)
def setPolyData(self, polyData):
if polyData.GetNumberOfPoints():
originPose = self.getProperty('Origin')
pos, quat = originPose[:3], originPose[3:]
t = transformUtils.transformFromPose(pos, quat)
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
PolyDataItem.setPolyData(self, polyData)
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 loadElasticFustionReconstruction(filename):
"""
Loads reconstructed pointcloud into director view
:param filename:
:return:
"""
polyData = ioUtils.readPolyData(filename)
polyData = filterUtils.transformPolyData(polyData,
getDefaultCameraToWorld())
obj = vis.showPolyData(polyData, 'reconstruction', colorByName='RGB')
return obj
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 _update_state(self, next_state):
"""Updates the moving object's state.
Args:
next_state: New state.
"""
t = vtk.vtkTransform()
t.Translate([next_state[0], next_state[1], 0.])
t.RotateZ(np.degrees(next_state[2]))
self._polydata = filterUtils.transformPolyData(self._raw_polydata, t)
self._state = next_state
list(map(lambda s: s.update(*self._state), self._sensors))
def _update_state(self, next_state):
"""Updates the moving object's state.
Args:
next_state: New state.
"""
t = vtk.vtkTransform()
t.Translate([next_state[0], next_state[1], 0.])
t.RotateZ(np.degrees(next_state[2]))
self._polydata = filterUtils.transformPolyData(self._raw_polydata, t)
self._state = next_state
list(map(lambda s: s.update(*self._state), self._sensors))
def scaleGeometry(polyDataList, scale):
if len(scale) == 1:
scale_x = scale_y = scale_z = scale
elif len(geom.float_data) == 3:
scale_x, scale_y, scale_z = scale
if scale_x != 1.0 or scale_y != 1.0 or scale_z != 1.0:
t = vtk.vtkTransform()
t.Scale(scale_x, scale_y, scale_z)
polyDataList = [filterUtils.transformPolyData(polyData, t) for polyData in polyDataList]
return polyDataList
def makeAlignmentTool(cameraView, pathDict, objectName='phone'):
# check if we have already figured out a transform for this or not
firstFrameToWorldTransform = CorlUtils.getFirstFrameToWorldTransform(pathDict['transforms'])
pointCloud = ioUtils.readPolyData(pathDict['reconstruction'])
pointCloud = filterUtils.transformPolyData(pointCloud, firstFrameToWorldTransform)
objectMeshFilename = CorlUtils.getObjectMeshFilename(objectName)
objectMesh = ioUtils.readPolyData(objectMeshFilename)
alignmentTool = ObjectAlignmentTool(cameraView, modelPolyData=objectMesh, pointCloud=pointCloud)
return alignmentTool
def loadFootMeshes():
meshes = []
for i in range(0,2):
d = DebugData()
for footMeshFile in _footMeshFiles[i]:
d.addPolyData(ioUtils.readPolyData( footMeshFile , computeNormals=True))
t = vtk.vtkTransform()
t.Scale(0.98, 0.98, 0.98)
pd = filterUtils.transformPolyData(d.getPolyData(), t)
meshes.append(pd)
return meshes
def scaleGeometry(polyDataList, scale):
if len(scale) == 1:
scale_x = scale_y = scale_z = scale
else:
assert len(scale) == 3
scale_x, scale_y, scale_z = scale
if scale_x != 1.0 or scale_y != 1.0 or scale_z != 1.0:
t = vtk.vtkTransform()
t.Scale(scale_x, scale_y, scale_z)
polyDataList = [filterUtils.transformPolyData(polyData, t) for polyData in polyDataList]
return polyDataList
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 _update_state(self, next_state, altitude_change=0.):
"""
Updates the moving object's state.
Args:
next_state: New state.
"""
t = vtk.vtkTransform()
t.Translate([next_state[0], next_state[1], altitude_change])
t.RotateZ(np.degrees(next_state[2]))
self._polydata = filterUtils.transformPolyData(self._raw_polydata, t)
self._state = next_state
self.altitude += altitude_change
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 _update_state(self, next_state, altitude_change=0.):
"""Updates the moving object's state.
Args:
next_state: New state.
"""
t = vtk.vtkTransform()
t.Translate([next_state[0], next_state[1], altitude_change])
t.RotateZ(np.degrees(next_state[2]))
self._polydata = filterUtils.transformPolyData(self._raw_polydata, t)
self.total_motion_distance += np.sqrt(np.square(next_state[0] - self._state[0]) + np.square(next_state[1] - self._state[1]))
self._state = next_state
self.altitude += altitude_change
list(map(lambda s: s.update(*self._state), self._sensors))
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 getOrientedBoundingBox(pd):
mesh = pd
origin, edges, wireframe = segmentation.getOrientedBoundingBox(mesh)
edgeLengths = np.array([np.linalg.norm(edge) for edge in edges])
axes = [edge / np.linalg.norm(edge) for edge in edges]
# find axis nearest to the +/- up vector
upVector = [0, 0, 1]
dotProducts = [np.abs(np.dot(axe, upVector)) for axe in axes]
zAxisIndex = np.argmax(dotProducts)
# re-index axes and edge lengths so that the found axis is the z axis
axesInds = [(zAxisIndex + 1) % 3, (zAxisIndex + 2) % 3, zAxisIndex]
axes = [axes[i] for i in axesInds]
edgeLengths = [edgeLengths[i] for i in axesInds]
# flip if necessary so that z axis points toward up
if np.dot(axes[2], upVector) < 0:
axes[1] = -axes[1]
axes[2] = -axes[2]
boxCenter = segmentation.computeCentroid(wireframe)
t = transformUtils.getTransformFromAxes(axes[0], axes[1], axes[2])
t.PostMultiply()
t.Translate(boxCenter)
pd = filterUtils.transformPolyData(pd, t.GetLinearInverse())
wireframe = filterUtils.transformPolyData(wireframe,
t.GetLinearInverse())
return FieldContainer(points=pd,
box=wireframe,
frame=t,
dims=edgeLengths,
axes=axes)
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 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 buildRobot(x=0,y=0):
#print "building robot"
polyData = ioUtils.readPolyData('crazyflie.obj')
scale = 0.005
t = vtk.vtkTransform()
t.RotateX(90)
t.Scale(scale, scale, scale)
polyData = filterUtils.transformPolyData(polyData, t)
#d = DebugData()
#d.addCone((x,y,0), (1,0,0), height=0.2, radius=0.1)
#polyData = d.getPolyData()
obj = vis.showPolyData(polyData, 'robot')
robotFrame = vis.addChildFrame(obj)
return obj, robotFrame
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 __init__(self, velocity=25.0, scale=0.15, exploration=0.5,
model="A10.obj"):
"""Constructs a Robot.
Args:
velocity: Velocity of the robot in the forward direction.
scale: Scale of the model.
exploration: Exploration rate.
model: Object model to use.
"""
self._target = (0, 0)
self._exploration = exploration
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
polydata = ioUtils.readPolyData(model)
polydata = filterUtils.transformPolyData(polydata, t)
super(Robot, self).__init__(velocity, polydata)
self._ctrl = Controller()
def createTextureGround(imageFilename, view):
pd = createTexturedPlane()
texture = createTexture(imageFilename)
texture.RepeatOn()
tcoords = vnp.getNumpyFromVtk(pd, 'Texture Coordinates')
tcoords *= 60
t = vtk.vtkTransform()
t.PostMultiply()
t.Scale(200,200,200)
t.Translate(0,0,-0.005)
pd = filterUtils.transformPolyData(pd, t)
obj = vis.showPolyData(pd, 'ground', view=view, alpha=1.0, parent='skybox')
obj.actor.SetTexture(texture)
obj.actor.GetProperty().LightingOff()
def createSkyboxPlane(side):
pd = createTexturedPlane()
t = vtk.vtkTransform()
t.PostMultiply()
if side == 'top':
t.Translate(0,0,0.5)
t.RotateZ(180)
elif side == 'bottom':
t.RotateX(180)
t.RotateY(180)
t.RotateZ(-270)
t.Translate(0,0,-0.5)
elif side == 'front':
t.RotateY(90)
t.RotateX(90)
t.RotateZ(180)
t.Translate(0.5,0.0,0.0)
elif side == 'back':
t.RotateY(90)
t.RotateX(90)
t.RotateZ(0)
t.Translate(-0.5,0.0,0.0)
elif side == 'left':
t.RotateY(90)
t.RotateX(90)
t.RotateZ(-90)
t.Translate(0.0,0.5,0.0)
elif side == 'right':
t.RotateY(90)
t.RotateX(90)
t.RotateZ(90)
t.Translate(0.0,-0.5,0.0)
pd = filterUtils.transformPolyData(pd, t)
return pd
def transformGeometry(polyDataList, geom):
if "transform" in geom:
t = transformFromDict(geom["transform"])
polyDataList = [filterUtils.transformPolyData(polyData, t) for polyData in polyDataList]
return polyDataList
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 transformGeometry(polyDataList, geom):
t = transformUtils.transformFromPose(geom.position, geom.quaternion)
polyDataList = [filterUtils.transformPolyData(polyData, t) for polyData in polyDataList]
return polyDataList
def load_poly_data(self):
self.poly_data_raw = ioUtils.readPolyData(self.reconstruction_filename)
self.poly_data = filterUtils.transformPolyData(self.poly_data_raw, self._reconstruction_to_world)
self.crop_poly_data()
