def testICP(self, objectName, scenePointCloud=None, applyVoxelGrid=True):
print "running ICP"
visObj = om.findObjectByName(objectName)
if scenePointCloud is None:
scenePointCloud = om.findObjectByName(
'cropped pointcloud').polyData
modelPointcloud = filterUtils.transformPolyData(
visObj.polyData, visObj.actor.GetUserTransform())
if applyVoxelGrid:
modelPointcloud = segmentation.applyVoxelGrid(modelPointcloud,
leafSize=0.0005)
sceneToModelTransform = segmentation.applyICP(scenePointCloud,
modelPointcloud)
modelToSceneTransform = sceneToModelTransform.GetLinearInverse()
concatenatedTransform = transformUtils.concatenateTransforms(
[visObj.actor.GetUserTransform(), modelToSceneTransform])
visObj.getChildFrame().copyFrame(concatenatedTransform)
print "ICP finished"
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)
import numpy as np
polyData = ioUtils.readPolyData('snapshot_2.vtp')
polyData = segmentation.cropToBounds(polyData, vtk.vtkTransform(),
[[-10, 10], [-10, 10], [0.15, 10]])
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,
'snapshot_2',
colorByName='rgb_colors',
visible=True)
robotMesh = ioUtils.readPolyData('robot_mesh_2.vtp')
vis.showPolyData(robotMesh, 'robot_mesh_2', visible=True)
t = segmentation.applyICP(polyData, robotMesh)
print t.GetPosition()
print t.GetOrientation()
polyData = filterUtils.transformPolyData(polyData, t)
vis.showPolyData(polyData,
'transformed pointcloud',
colorByName='rgb_colors',
visible=False)