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