def onCameraToWorldTransform(self, msg):
# only do this once
if self.cameraToWorldTransform is not None:
return
self.cameraToWorldTransform = lcmframe.frameFromRigidTransformMessage(
msg)
self.pointcloud.getChildFrame().copyFrame(self.cameraToWorldTransform)
def onAprilTagMessage(msg, channel):
tagToCamera = lcmframe.frameFromRigidTransformMessage(msg)
vis.updateFrame(tagToCamera, channel, visible=False)
cameraToTag = tagToCamera.GetLinearInverse()
tagToWorld = getTagToWorld()
cameraToWorld = transformUtils.concatenateTransforms([cameraToTag, tagToWorld])
cameraToWorldMsg = lcmframe.rigidTransformMessageFromFrame(cameraToWorld)
lcmUtils.publish('OPENNI_FRAME_LEFT_TO_LOCAL', cameraToWorldMsg)
vis.updateFrame(vtk.vtkTransform(), 'world', visible=False)
vis.updateFrame(cameraToWorld, 'camera to world', visible=False)
vis.updateFrame(tagToWorld, 'tag to world', visible=False)
