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)