def onEstRobotState(self, msg):
cameraToWorld = self.getCameraToWorld()
# make the message and publish it out
cameraToWorldMsg = lcmframe.rigidTransformMessageFromFrame(
cameraToWorld)
lcmUtils.publish(self.channelName, cameraToWorldMsg)
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)
def align(self):
if self.meshPoints is None or self.imagePoints is None:
return
t1 = computeLandmarkTransform(self.imagePoints, self.meshPoints)
polyData = filterUtils.transformPolyData(
self.imageFitter.getPointCloud(), t1)
vis.showPolyData(polyData,
'transformed pointcloud',
view=self.view,
colorByName='rgb_colors',
visible=False)
vis.showPolyData(filterUtils.transformPolyData(
makeDebugPoints(self.imagePoints), t1),
'transformed image pick points',
color=[0, 0, 1],
view=self.view)
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.imageFitter.getPointCloud(), 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)
def publishTeleopCameraFrame(self):
cameraToWorldMsg = lcmframe.rigidTransformMessageFromFrame(
self.cameraToWorld)
lcmUtils.publish(self.channelName, cameraToWorldMsg)
