def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
def findFarRightCorner(self, polyData, linkFrame):
'''
Within a point cloud find the point to the far right from the link
The input is typically the 4 corners of a minimum bounding box
'''
diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
diagonalTransform.Concatenate(linkFrame)
vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
#polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
return points[farRightIndex,:]
def getRecedingTerrainRegion(self, polyData, linkFrame):
''' Find the point cloud in front of the foot frame'''
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
return polyData
def getRecedingTerrainRegion(self, polyData, linkFrame):
''' Find the point cloud in front of the foot frame'''
#polyData = shallowCopy(polyData)
points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
#vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
#vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
#vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')
viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])
vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
return polyData
