<s> import os <EOL> import sys <EOL> import math <EOL> import vtk <EOL> import time <EOL> import functools <EOL> import traceback <EOL> import PythonQt <EOL> from PythonQt import QtCore , QtGui <EOL> import director . applogic as app <EOL> from director import objectmodel as om <EOL> from director import perception <EOL> from director import lcmUtils <EOL> from director import roboturdf <EOL> from director import transformUtils <EOL> from director import visualization as vis <EOL> from director . transformUtils import getTransformFromAxes <EOL> from director . timercallback import TimerCallback <EOL> from director import mapsregistrar <EOL> from director import affordancemanager <EOL> from director . affordanceitems import * <EOL> from director . visualization import * <EOL> from director . filterUtils import * <EOL> from director . fieldcontainer import FieldContainer <EOL> from director . segmentationroutines import * <EOL> from director import cameraview <EOL> from thirdparty import qhull_2d <EOL> from thirdparty import min_bounding_rect <EOL> import numpy as np <EOL> import vtkNumpy <EOL> from debugVis import DebugData <EOL> from shallowCopy import shallowCopy <EOL> import ioUtils <EOL> from director . uuidutil import newUUID <EOL> import drc as lcmdrc <EOL> import bot_core as lcmbotcore <EOL> import vs as lcmvs <EOL> from director import lcmUtils <EOL> DRILL_TRIANGLE_BOTTOM_LEFT = '<STR_LIT>' <EOL> DRILL_TRIANGLE_BOTTOM_RIGHT = '<STR_LIT>' <EOL> DRILL_TRIANGLE_TOP_LEFT = '<STR_LIT>' <EOL> DRILL_TRIANGLE_TOP_RIGHT = '<STR_LIT>' <EOL> planeSegmentationFilter = vtk . vtkPlaneSegmentation <EOL> _defaultSegmentationView = None <EOL> def getSegmentationView ( ) : <EOL> return _defaultSegmentationView or app . getViewManager ( ) . findView ( '<STR_LIT>' ) <EOL> def getDRCView ( ) : <EOL> return app . getDRCView ( ) <EOL> def switchToView ( viewName ) : <EOL> app . getViewManager ( ) . switchToView ( viewName ) <EOL> def getCurrentView ( ) : <EOL> return app . getCurrentRenderView ( ) <EOL> def initAffordanceManager ( view ) : <EOL> '''<STR_LIT>''' <EOL> global affordanceManager <EOL> affordanceManager = affordancemanager . AffordanceObjectModelManager ( view ) <EOL> def cropToLineSegment ( polyData , point1 , point2 ) : <EOL> line = np . array ( point2 ) - np . array ( point1 ) <EOL> length = np . linalg . norm ( line ) <EOL> axis = line / length <EOL> polyData = labelPointDistanceAlongAxis ( polyData , axis , origin = point1 , resultArrayName = '<STR_LIT>' ) <EOL> return thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0.0> , length ] ) <EOL> '''<STR_LIT>''' <EOL> def computeAToB ( a , b ) : <EOL> t = vtk . vtkTransform ( ) <EOL> t . PostMultiply ( ) <EOL> t . Concatenate ( b ) <EOL> t . Concatenate ( a . GetLinearInverse ( ) ) <EOL> tt = vtk . vtkTransform ( ) <EOL> tt . SetMatrix ( t . GetMatrix ( ) ) <EOL> return tt <EOL> def lockAffordanceToHand ( aff , hand = '<STR_LIT>' ) : <EOL> linkFrame = getLinkFrame ( hand ) <EOL> affT = aff . actor . GetUserTransform ( ) <EOL> if not hasattr ( aff , '<STR_LIT>' ) or not aff . handToAffT : <EOL> aff . handToAffT = computeAToB ( linkFrame , affT ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . PostMultiply ( ) <EOL> t . Concatenate ( aff . handToAffT ) <EOL> t . Concatenate ( linkFrame ) <EOL> aff . actor . GetUserTransform ( ) . SetMatrix ( t . GetMatrix ( ) ) <EOL> handAffUpdater = None <EOL> def lockToHandOn ( ) : <EOL> aff = getDefaultAffordanceObject ( ) <EOL> if not aff : <EOL> return <EOL> global handAffUpdater <EOL> if handAffUpdater is None : <EOL> handAffUpdater = TimerCallback ( ) <EOL> handAffUpdater . targetFps = <NUM_LIT:30> <EOL> handAffUpdater . callback = functools . partial ( lockAffordanceToHand , aff ) <EOL> handAffUpdater . start ( ) <EOL> def lockToHandOff ( ) : <EOL> aff = getDefaultAffordanceObject ( ) <EOL> if not aff : <EOL> return <EOL> handAffUpdater . stop ( ) <EOL> aff . handToAffT = None <EOL> class DisparityPointCloudItem ( vis . PolyDataItem ) : <EOL> def __init__ ( self , name , imagesChannel , cameraName , imageManager ) : <EOL> vis . PolyDataItem . __init__ ( self , name , vtk . vtkPolyData ( ) , view = None ) <EOL> self . addProperty ( '<STR_LIT>' , imagesChannel ) <EOL> self . addProperty ( '<STR_LIT>' , cameraName ) <EOL> self . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = om . PropertyAttributes ( enumNames = [ '<STR_LIT:1>' , '<STR_LIT:2>' , '<STR_LIT:4>' , '<STR_LIT>' , '<STR_LIT>' ] ) ) <EOL> self . addProperty ( '<STR_LIT>' , <NUM_LIT:1000> , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:0> , minimum = <NUM_LIT:0> , maximum = <NUM_LIT> , singleStep = <NUM_LIT:1000> ) ) <EOL> self . addProperty ( '<STR_LIT>' , <NUM_LIT:1.0> , attributes = om . PropertyAttributes ( decimals = <NUM_LIT:1> , minimum = <NUM_LIT:0.1> , maximum = <NUM_LIT> , singleStep = <NUM_LIT:0.1> ) ) <EOL> self . timer = TimerCallback ( ) <EOL> self . timer . callback = self . update <EOL> self . lastUtime = <NUM_LIT:0> <EOL> self . imageManager = imageManager <EOL> self . cameraName = cameraName <EOL> self . setProperty ( '<STR_LIT>' , False ) <EOL> def _onPropertyChanged ( self , propertySet , propertyName ) : <EOL> vis . PolyDataItem . _onPropertyChanged ( self , propertySet , propertyName ) <EOL> if propertyName == '<STR_LIT>' : <EOL> if self . getProperty ( propertyName ) : <EOL> self . timer . start ( ) <EOL> else : <EOL> self . timer . stop ( ) <EOL> elif propertyName in ( '<STR_LIT>' , '<STR_LIT>' ) : <EOL> self . lastUtime = <NUM_LIT:0> <EOL> def onRemoveFromObjectModel ( self ) : <EOL> vis . PolyDataItem . onRemoveFromObjectModel ( self ) <EOL> self . timer . stop ( ) <EOL> def update ( self ) : <EOL> utime = self . imageManager . queue . getCurrentImageTime ( self . cameraName ) <EOL> if utime == self . lastUtime : <EOL> return <EOL> if ( utime < self . lastUtime ) : <EOL> temp = <NUM_LIT:0> <EOL> elif ( utime - self . lastUtime < <NUM_LIT> / self . getProperty ( '<STR_LIT>' ) ) : <EOL> return <EOL> decimation = int ( self . properties . getPropertyEnumValue ( '<STR_LIT>' ) ) <EOL> removeSize = int ( self . properties . getProperty ( '<STR_LIT>' ) ) <EOL> polyData = getDisparityPointCloud ( decimation , imagesChannel = self . getProperty ( '<STR_LIT>' ) , cameraName = self . getProperty ( '<STR_LIT>' ) , <EOL> removeOutliers = False , removeSize = removeSize ) <EOL> self . setPolyData ( polyData ) <EOL> if not self . lastUtime : <EOL> self . setProperty ( '<STR_LIT>' , '<STR_LIT>' ) <EOL> self . lastUtime = utime <EOL> def extractLargestCluster ( polyData , minClusterSize = <NUM_LIT:100> ) : <EOL> polyData = applyEuclideanClustering ( polyData , minClusterSize = minClusterSize ) <EOL> return thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:1> , <NUM_LIT:1> ] ) <EOL> def segmentGround ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) : <EOL> '''<STR_LIT>''' <EOL> searchRegionThickness = <NUM_LIT:0.5> <EOL> zvalues = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) [ : , <NUM_LIT:2> ] <EOL> groundHeight = np . percentile ( zvalues , <NUM_LIT:5> ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , zvalues . copy ( ) , '<STR_LIT:z>' ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT:z>' , [ groundHeight - searchRegionThickness / <NUM_LIT> , groundHeight + searchRegionThickness / <NUM_LIT> ] ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT:z>' , visible = False ) <EOL> _ , origin , normal = applyPlaneFit ( searchRegion , distanceThreshold = <NUM_LIT> , expectedNormal = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] , perpendicularAxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] , returnOrigin = True ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dist = np . dot ( points - origin , normal ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dist , '<STR_LIT>' ) <EOL> groundPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - groundThickness / <NUM_LIT> , groundThickness / <NUM_LIT> ] ) <EOL> scenePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ sceneHeightFromGround , <NUM_LIT:100> ] ) <EOL> return origin , normal , groundPoints , scenePoints <EOL> def segmentGroundPlane ( ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> inputObj . setProperty ( '<STR_LIT>' , False ) <EOL> polyData = shallowCopy ( inputObj . polyData ) <EOL> zvalues = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) [ : , <NUM_LIT:2> ] <EOL> groundHeight = np . percentile ( zvalues , <NUM_LIT:5> ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT:z>' , [ groundHeight - <NUM_LIT> , groundHeight + <NUM_LIT> ] ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT:z>' , visible = False ) <EOL> _ , origin , normal = applyPlaneFit ( searchRegion , distanceThreshold = <NUM_LIT> , expectedNormal = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] , perpendicularAxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] , returnOrigin = True ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dist = np . dot ( points - origin , normal ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dist , '<STR_LIT>' ) <EOL> groundPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> scenePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT:10> ] ) <EOL> updatePolyData ( groundPoints , '<STR_LIT>' , alpha = <NUM_LIT> ) <EOL> updatePolyData ( scenePoints , '<STR_LIT>' , alpha = <NUM_LIT> ) <EOL> def applyLocalPlaneFit ( polyData , searchPoint , searchRadius , searchRadiusEnd = None , removeGroundFirst = True ) : <EOL> useVoxelGrid = True <EOL> voxelGridSize = <NUM_LIT> <EOL> distanceToPlaneThreshold = <NUM_LIT> <EOL> if useVoxelGrid : <EOL> polyData = applyVoxelGrid ( polyData , leafSize = voxelGridSize ) <EOL> if removeGroundFirst : <EOL> _ , polyData = removeGround ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) <EOL> cropped = cropToSphere ( polyData , searchPoint , searchRadius ) <EOL> updatePolyData ( cropped , '<STR_LIT>' , visible = False , colorByName = '<STR_LIT>' ) <EOL> polyData , normal = applyPlaneFit ( polyData , distanceToPlaneThreshold , searchOrigin = searchPoint , searchRadius = searchRadius ) <EOL> if searchRadiusEnd is not None : <EOL> polyData , normal = applyPlaneFit ( polyData , distanceToPlaneThreshold , perpendicularAxis = normal , angleEpsilon = math . radians ( <NUM_LIT:30> ) , searchOrigin = searchPoint , searchRadius = searchRadiusEnd ) <EOL> fitPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - distanceToPlaneThreshold , distanceToPlaneThreshold ] ) <EOL> updatePolyData ( fitPoints , '<STR_LIT>' , visible = False ) <EOL> fitPoints = labelDistanceToPoint ( fitPoints , searchPoint ) <EOL> clusters = extractClusters ( fitPoints , clusterTolerance = <NUM_LIT> , minClusterSize = <NUM_LIT:3> ) <EOL> clusters . sort ( key = lambda x : vtkNumpy . getNumpyFromVtk ( x , '<STR_LIT>' ) . min ( ) ) <EOL> fitPoints = clusters [ <NUM_LIT:0> ] <EOL> return fitPoints , normal <EOL> normalEstimationSearchRadius = <NUM_LIT> <EOL> f = vtk . vtkPCLNormalEstimation ( ) <EOL> f . SetSearchRadius ( normalEstimationSearchRadius ) <EOL> f . SetInput ( polyData ) <EOL> f . Update ( ) <EOL> scenePoints = shallowCopy ( f . GetOutput ( ) ) <EOL> normals = vtkNumpy . getNumpyFromVtk ( scenePoints , '<STR_LIT>' ) <EOL> normalsDotPlaneNormal = np . abs ( np . dot ( normals , normal ) ) <EOL> vtkNumpy . addNumpyToVtk ( scenePoints , normalsDotPlaneNormal , '<STR_LIT>' ) <EOL> showPolyData ( scenePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' ) <EOL> surfaces = thresholdPoints ( scenePoints , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT:1.0> ] ) <EOL> clusters = extractClusters ( surfaces , clusterTolerance = <NUM_LIT:0.1> , minClusterSize = <NUM_LIT:5> ) <EOL> clusters = clusters [ : <NUM_LIT:10> ] <EOL> for i , cluster in enumerate ( clusters ) : <EOL> showPolyData ( cluster , '<STR_LIT>' % i , parent = getDebugFolder ( ) , visible = False ) <EOL> return fitPoints <EOL> def orientToMajorPlane ( polyData , pickedPoint ) : <EOL> '''<STR_LIT>''' <EOL> distanceToPlaneThreshold = <NUM_LIT> <EOL> searchRadius = <NUM_LIT:0.5> <EOL> planePoints , origin , normal = applyPlaneFit ( polyData , distanceToPlaneThreshold , searchOrigin = pickedPoint , searchRadius = searchRadius , returnOrigin = True ) <EOL> vis . updatePolyData ( planePoints , '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = False ) <EOL> planeFrame = transformUtils . getTransformFromOriginAndNormal ( pickedPoint , normal ) <EOL> vis . updateFrame ( planeFrame , '<STR_LIT>' , scale = <NUM_LIT> , parent = getDebugFolder ( ) , visible = False ) <EOL> polyData = transformPolyData ( polyData , planeFrame . GetLinearInverse ( ) ) <EOL> zvalues = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) [ : , <NUM_LIT:2> ] <EOL> midCloudHeight = np . mean ( zvalues ) <EOL> if ( midCloudHeight < <NUM_LIT:0> ) : <EOL> flipTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> polyData = transformPolyData ( polyData , flipTransform ) <EOL> return polyData , planeFrame <EOL> def getMajorPlanes ( polyData , useVoxelGrid = True ) : <EOL> voxelGridSize = <NUM_LIT> <EOL> distanceToPlaneThreshold = <NUM_LIT> <EOL> if useVoxelGrid : <EOL> polyData = applyVoxelGrid ( polyData , leafSize = voxelGridSize ) <EOL> polyDataList = [ ] <EOL> minClusterSize = <NUM_LIT:100> <EOL> while len ( polyDataList ) < <NUM_LIT> : <EOL> f = planeSegmentationFilter ( ) <EOL> f . SetInput ( polyData ) <EOL> f . SetDistanceThreshold ( distanceToPlaneThreshold ) <EOL> f . Update ( ) <EOL> polyData = shallowCopy ( f . GetOutput ( ) ) <EOL> outliers = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> inliers = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:1> , <NUM_LIT:1> ] ) <EOL> largestCluster = extractLargestCluster ( inliers ) <EOL> if largestCluster . GetNumberOfPoints ( ) > minClusterSize : <EOL> polyDataList . append ( largestCluster ) <EOL> polyData = outliers <EOL> else : <EOL> break <EOL> return polyDataList <EOL> def showMajorPlanes ( polyData = None ) : <EOL> if not polyData : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> inputObj . setProperty ( '<STR_LIT>' , False ) <EOL> polyData = inputObj . polyData <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> folderObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> folderObj = om . getOrCreateContainer ( '<STR_LIT>' , folderObj ) <EOL> origin = SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) . GetPosition ( ) <EOL> polyData = labelDistanceToPoint ( polyData , origin ) <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:1> , <NUM_LIT:4> ] ) <EOL> polyDataList = getMajorPlanes ( polyData ) <EOL> for i , polyData in enumerate ( polyDataList ) : <EOL> obj = showPolyData ( polyData , '<STR_LIT>' % i , color = getRandomColor ( ) , visible = True , parent = '<STR_LIT>' ) <EOL> obj . setProperty ( '<STR_LIT>' , <NUM_LIT:3> ) <EOL> def cropToBox ( polyData , transform , dimensions ) : <EOL> '''<STR_LIT>''' <EOL> origin = np . array ( transform . GetPosition ( ) ) <EOL> axes = transformUtils . getAxesFromTransform ( transform ) <EOL> for axis , length in zip ( axes , dimensions ) : <EOL> cropAxis = np . array ( axis ) * ( length / <NUM_LIT> ) <EOL> polyData = cropToLineSegment ( polyData , origin - cropAxis , origin + cropAxis ) <EOL> return polyData <EOL> def cropToBounds ( polyData , transform , bounds ) : <EOL> '''<STR_LIT>''' <EOL> origin = np . array ( transform . GetPosition ( ) ) <EOL> axes = transformUtils . getAxesFromTransform ( transform ) <EOL> for axis , bound in zip ( axes , bounds ) : <EOL> axis = np . array ( axis ) / np . linalg . norm ( axis ) <EOL> polyData = cropToLineSegment ( polyData , origin + axis * bound [ <NUM_LIT:0> ] , origin + axis * bound [ <NUM_LIT:1> ] ) <EOL> return polyData <EOL> def cropToSphere ( polyData , origin , radius ) : <EOL> polyData = labelDistanceToPoint ( polyData , origin ) <EOL> return thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0> , radius ] ) <EOL> def applyPlaneFit ( polyData , distanceThreshold = <NUM_LIT> , expectedNormal = None , perpendicularAxis = None , angleEpsilon = <NUM_LIT> , returnOrigin = False , searchOrigin = None , searchRadius = None ) : <EOL> expectedNormal = expectedNormal if expectedNormal is not None else [ - <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> fitInput = polyData <EOL> if searchOrigin is not None : <EOL> assert searchRadius <EOL> fitInput = cropToSphere ( fitInput , searchOrigin , searchRadius ) <EOL> f = planeSegmentationFilter ( ) <EOL> f . SetInput ( fitInput ) <EOL> f . SetDistanceThreshold ( distanceThreshold ) <EOL> if perpendicularAxis is not None : <EOL> f . SetPerpendicularConstraintEnabled ( True ) <EOL> f . SetPerpendicularAxis ( perpendicularAxis ) <EOL> f . SetAngleEpsilon ( angleEpsilon ) <EOL> f . Update ( ) <EOL> origin = f . GetPlaneOrigin ( ) <EOL> normal = np . array ( f . GetPlaneNormal ( ) ) <EOL> if np . dot ( normal , expectedNormal ) < <NUM_LIT:0> : <EOL> normal = - normal <EOL> polyData = shallowCopy ( polyData ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dist = np . dot ( points - origin , normal ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dist , '<STR_LIT>' ) <EOL> if returnOrigin : <EOL> return polyData , origin , normal <EOL> else : <EOL> return polyData , normal <EOL> def flipNormalsWithViewDirection ( polyData , viewDirection ) : <EOL> normals = vnp . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> normals [ np . dot ( normals , viewDirection ) > <NUM_LIT:0> ] *= - <NUM_LIT:1> <EOL> def normalEstimation ( dataObj , searchCloud = None , searchRadius = <NUM_LIT> , useVoxelGrid = False , voxelGridLeafSize = <NUM_LIT> ) : <EOL> f = vtk . vtkPCLNormalEstimation ( ) <EOL> f . SetSearchRadius ( searchRadius ) <EOL> f . SetInput ( dataObj ) <EOL> if searchCloud : <EOL> f . SetInput ( <NUM_LIT:1> , searchCloud ) <EOL> elif useVoxelGrid : <EOL> f . SetInput ( <NUM_LIT:1> , applyVoxelGrid ( dataObj , voxelGridLeafSize ) ) <EOL> f . Update ( ) <EOL> dataObj = shallowCopy ( f . GetOutput ( ) ) <EOL> dataObj . GetPointData ( ) . SetNormals ( dataObj . GetPointData ( ) . GetArray ( '<STR_LIT>' ) ) <EOL> return dataObj <EOL> def addCoordArraysToPolyData ( polyData ) : <EOL> polyData = shallowCopy ( polyData ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , points [ : , <NUM_LIT:0> ] . copy ( ) , '<STR_LIT:x>' ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , points [ : , <NUM_LIT:1> ] . copy ( ) , '<STR_LIT:y>' ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , points [ : , <NUM_LIT:2> ] . copy ( ) , '<STR_LIT:z>' ) <EOL> viewFrame = SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) <EOL> viewOrigin = viewFrame . TransformPoint ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> viewX = viewFrame . TransformVector ( [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> viewY = viewFrame . TransformVector ( [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] ) <EOL> viewZ = viewFrame . TransformVector ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , viewX , origin = viewOrigin , resultArrayName = '<STR_LIT>' ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , viewY , origin = viewOrigin , resultArrayName = '<STR_LIT>' ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , viewZ , origin = viewOrigin , resultArrayName = '<STR_LIT>' ) <EOL> return polyData <EOL> def getDebugRevolutionData ( ) : <EOL> filename = os . path . expanduser ( '<STR_LIT>' ) <EOL> return addCoordArraysToPolyData ( ioUtils . readPolyData ( filename ) ) <EOL> def getCurrentScanBundle ( ) : <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if not obj : <EOL> return None <EOL> revPolyData = obj . polyData <EOL> if not revPolyData or not revPolyData . GetNumberOfPoints ( ) : <EOL> return None <EOL> if useVoxelGrid : <EOL> revPolyData = applyVoxelGrid ( revPolyData , leafSize = <NUM_LIT> ) <EOL> return addCoordArraysToPolyData ( revPolyData ) <EOL> def getCurrentRevolutionData ( ) : <EOL> revPolyData = perception . _multisenseItem . model . revPolyData <EOL> if not revPolyData or not revPolyData . GetNumberOfPoints ( ) : <EOL> return getCurrentScanBundle ( ) <EOL> if useVoxelGrid : <EOL> revPolyData = applyVoxelGrid ( revPolyData , leafSize = <NUM_LIT> ) <EOL> return addCoordArraysToPolyData ( revPolyData ) <EOL> def getDisparityPointCloud ( decimation = <NUM_LIT:4> , removeOutliers = True , removeSize = <NUM_LIT:0> , imagesChannel = '<STR_LIT>' , cameraName = '<STR_LIT>' ) : <EOL> p = cameraview . getStereoPointCloud ( decimation , imagesChannel = imagesChannel , cameraName = cameraName , removeSize = removeSize ) <EOL> if not p : <EOL> return None <EOL> if removeOutliers : <EOL> scaling = ( <NUM_LIT:10> * <NUM_LIT:16> ) / ( decimation * decimation ) <EOL> p = labelOutliers ( p , searchRadius = <NUM_LIT> , neighborsInSearchRadius = scaling ) <EOL> p = thresholdPoints ( p , '<STR_LIT>' , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> return p <EOL> def getCurrentMapServerData ( ) : <EOL> mapServer = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = None <EOL> if mapServer and mapServer . getProperty ( '<STR_LIT>' ) : <EOL> polyData = mapServer . source . polyData <EOL> if not polyData or not polyData . GetNumberOfPoints ( ) : <EOL> return None <EOL> return addCoordArraysToPolyData ( polyData ) <EOL> useVoxelGrid = False <EOL> def segmentGroundPlanes ( ) : <EOL> objs = [ ] <EOL> for obj in om . getObjects ( ) : <EOL> name = obj . getProperty ( '<STR_LIT:Name>' ) <EOL> if name . startswith ( '<STR_LIT>' ) : <EOL> objs . append ( obj ) <EOL> objs = sorted ( objs , key = lambda x : x . getProperty ( '<STR_LIT:Name>' ) ) <EOL> d = DebugData ( ) <EOL> prevHeadAxis = None <EOL> for obj in objs : <EOL> name = obj . getProperty ( '<STR_LIT:Name>' ) <EOL> print '<STR_LIT>' % name <EOL> print '<STR_LIT>' , obj . headAxis <EOL> origin , normal , groundPoints , _ = segmentGround ( obj . polyData ) <EOL> print '<STR_LIT>' , normal <EOL> showPolyData ( groundPoints , name + '<STR_LIT>' , visible = False ) <EOL> a = np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) <EOL> b = np . array ( normal ) <EOL> diff = math . degrees ( math . acos ( np . dot ( a , b ) / ( np . linalg . norm ( a ) * np . linalg . norm ( b ) ) ) ) <EOL> if diff > <NUM_LIT> : <EOL> print <NUM_LIT> - diff <EOL> else : <EOL> print diff <EOL> if prevHeadAxis is not None : <EOL> a = prevHeadAxis <EOL> b = np . array ( obj . headAxis ) <EOL> diff = math . degrees ( math . acos ( np . dot ( a , b ) / ( np . linalg . norm ( a ) * np . linalg . norm ( b ) ) ) ) <EOL> if diff > <NUM_LIT> : <EOL> print <NUM_LIT> - diff <EOL> else : <EOL> print diff <EOL> prevHeadAxis = np . array ( obj . headAxis ) <EOL> d . addLine ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , normal ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' ) <EOL> def extractCircle ( polyData , distanceThreshold = <NUM_LIT> , radiusLimit = None ) : <EOL> circleFit = vtk . vtkPCLSACSegmentationCircle ( ) <EOL> circleFit . SetDistanceThreshold ( distanceThreshold ) <EOL> circleFit . SetInput ( polyData ) <EOL> if radiusLimit is not None : <EOL> circleFit . SetRadiusLimit ( radiusLimit ) <EOL> circleFit . SetRadiusConstraintEnabled ( True ) <EOL> circleFit . Update ( ) <EOL> polyData = thresholdPoints ( circleFit . GetOutput ( ) , '<STR_LIT>' , [ <NUM_LIT:1.0> , <NUM_LIT:1.0> ] ) <EOL> return polyData , circleFit <EOL> def removeMajorPlane ( polyData , distanceThreshold = <NUM_LIT> ) : <EOL> f = planeSegmentationFilter ( ) <EOL> f . SetInput ( polyData ) <EOL> f . SetDistanceThreshold ( distanceThreshold ) <EOL> f . Update ( ) <EOL> polyData = thresholdPoints ( f . GetOutput ( ) , '<STR_LIT>' , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> return polyData , f <EOL> def removeGroundSimple ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) : <EOL> '''<STR_LIT>''' <EOL> groundHeight = SegmentationContext . getGlobalInstance ( ) . getGroundHeight ( ) <EOL> origin = [ <NUM_LIT:0> , <NUM_LIT:0> , groundHeight ] <EOL> normal = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dist = np . dot ( points - origin , normal ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dist , '<STR_LIT>' ) <EOL> groundPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - groundThickness / <NUM_LIT> , groundThickness / <NUM_LIT> ] ) <EOL> scenePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ sceneHeightFromGround , <NUM_LIT:100> ] ) <EOL> return groundPoints , scenePoints <EOL> def removeGround ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) : <EOL> origin , normal , groundPoints , scenePoints = segmentGround ( polyData , groundThickness , sceneHeightFromGround ) <EOL> return groundPoints , scenePoints <EOL> def generateFeetForValve ( ) : <EOL> aff = om . findObjectByName ( '<STR_LIT>' ) <EOL> assert aff <EOL> params = aff . params <EOL> origin = np . array ( params [ '<STR_LIT>' ] ) <EOL> origin [ <NUM_LIT:2> ] = <NUM_LIT:0.0> <EOL> xaxis = - params [ '<STR_LIT>' ] <EOL> zaxis = np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> stanceWidth = <NUM_LIT> <EOL> stanceRotation = <NUM_LIT> <EOL> stanceOffset = [ - <NUM_LIT:1.0> , - <NUM_LIT:0.5> , <NUM_LIT:0.0> ] <EOL> valveFrame = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> valveFrame . PostMultiply ( ) <EOL> valveFrame . Translate ( origin ) <EOL> stanceFrame , lfootFrame , rfootFrame = getFootFramesFromReferenceFrame ( valveFrame , stanceWidth , stanceRotation , stanceOffset ) <EOL> showFrame ( boardFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> , visible = False ) <EOL> showFrame ( lfootFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> ) <EOL> showFrame ( rfootFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> ) <EOL> def generateFeetForDebris ( ) : <EOL> aff = om . findObjectByName ( '<STR_LIT>' ) <EOL> if not aff : <EOL> return <EOL> params = aff . params <EOL> origin = np . array ( params [ '<STR_LIT>' ] ) <EOL> origin = origin + params [ '<STR_LIT>' ] * params [ '<STR_LIT>' ] / <NUM_LIT> - params [ '<STR_LIT>' ] * params [ '<STR_LIT>' ] / <NUM_LIT> <EOL> origin [ <NUM_LIT:2> ] = <NUM_LIT:0.0> <EOL> yaxis = params [ '<STR_LIT>' ] <EOL> zaxis = np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> stanceWidth = <NUM_LIT> <EOL> stanceRotation = <NUM_LIT:0.0> <EOL> stanceOffset = [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT:0> ] <EOL> boardFrame = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> boardFrame . PostMultiply ( ) <EOL> boardFrame . Translate ( origin ) <EOL> stanceFrame , lfootFrame , rfootFrame = getFootFramesFromReferenceFrame ( boardFrame , stanceWidth , stanceRotation , stanceOffset ) <EOL> showFrame ( boardFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> , visible = False ) <EOL> lfoot = showFrame ( lfootFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> ) <EOL> rfoot = showFrame ( rfootFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> ) <EOL> for obj in [ lfoot , rfoot ] : <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> def generateFeetForWye ( ) : <EOL> aff = om . findObjectByName ( '<STR_LIT>' ) <EOL> if not aff : <EOL> return <EOL> params = aff . params <EOL> origin = np . array ( params [ '<STR_LIT>' ] ) <EOL> origin [ <NUM_LIT:2> ] = <NUM_LIT:0.0> <EOL> yaxis = params [ '<STR_LIT>' ] <EOL> xaxis = - params [ '<STR_LIT>' ] <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> stanceWidth = <NUM_LIT> <EOL> stanceRotation = <NUM_LIT:0.0> <EOL> stanceOffset = [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT:0> ] <EOL> affGroundFrame = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> affGroundFrame . PostMultiply ( ) <EOL> affGroundFrame . Translate ( origin ) <EOL> stanceFrame , lfootFrame , rfootFrame = getFootFramesFromReferenceFrame ( affGroundFrame , stanceWidth , stanceRotation , stanceOffset ) <EOL> showFrame ( affGroundFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> , visible = False ) <EOL> lfoot = showFrame ( lfootFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> ) <EOL> rfoot = showFrame ( rfootFrame , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> ) <EOL> for obj in [ lfoot , rfoot ] : <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> def getFootFramesFromReferenceFrame ( referenceFrame , stanceWidth , stanceRotation , stanceOffset ) : <EOL> footHeight = <NUM_LIT> <EOL> ref = vtk . vtkTransform ( ) <EOL> ref . SetMatrix ( referenceFrame . GetMatrix ( ) ) <EOL> stanceFrame = vtk . vtkTransform ( ) <EOL> stanceFrame . PostMultiply ( ) <EOL> stanceFrame . RotateZ ( stanceRotation ) <EOL> stanceFrame . Translate ( stanceOffset ) <EOL> stanceFrame . Concatenate ( ref ) <EOL> lfootFrame = vtk . vtkTransform ( ) <EOL> lfootFrame . PostMultiply ( ) <EOL> lfootFrame . Translate ( <NUM_LIT:0> , stanceWidth / <NUM_LIT> , footHeight ) <EOL> lfootFrame . Concatenate ( stanceFrame ) <EOL> rfootFrame = vtk . vtkTransform ( ) <EOL> rfootFrame . PostMultiply ( ) <EOL> rfootFrame . Translate ( <NUM_LIT:0> , - stanceWidth / <NUM_LIT> , footHeight ) <EOL> rfootFrame . Concatenate ( stanceFrame ) <EOL> return stanceFrame , lfootFrame , rfootFrame <EOL> def poseFromFrame ( frame ) : <EOL> trans = lcmbotcore . vector_3d_t ( ) <EOL> trans . x , trans . y , trans . z = frame . GetPosition ( ) <EOL> wxyz = range ( <NUM_LIT:4> ) <EOL> perception . drc . vtkMultisenseSource . GetBotQuaternion ( frame , wxyz ) <EOL> quat = lcmbotcore . quaternion_t ( ) <EOL> quat . w , quat . x , quat . y , quat . z = wxyz <EOL> pose = lcmbotcore . position_3d_t ( ) <EOL> pose . translation = trans <EOL> pose . rotation = quat <EOL> return pose <EOL> def cropToPlane ( polyData , origin , normal , threshold ) : <EOL> polyData = shallowCopy ( polyData ) <EOL> normal = normal / np . linalg . norm ( normal ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dist = np . dot ( points - origin , normal ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dist , '<STR_LIT>' ) <EOL> cropped = thresholdPoints ( polyData , '<STR_LIT>' , threshold ) <EOL> return cropped , polyData <EOL> def createLine ( blockDimensions , p1 , p2 ) : <EOL> sliceWidth = np . array ( blockDimensions ) . max ( ) / <NUM_LIT> + <NUM_LIT> <EOL> sliceThreshold = [ - sliceWidth , sliceWidth ] <EOL> if p1 [ <NUM_LIT:0> ] > p2 [ <NUM_LIT:0> ] : <EOL> p1 , p2 = p2 , p1 <EOL> _ , worldPt1 = getRayFromDisplayPoint ( app . getCurrentRenderView ( ) , p1 ) <EOL> _ , worldPt2 = getRayFromDisplayPoint ( app . getCurrentRenderView ( ) , p2 ) <EOL> cameraPt = np . array ( app . getCurrentRenderView ( ) . camera ( ) . GetPosition ( ) ) <EOL> leftRay = worldPt1 - cameraPt <EOL> rightRay = worldPt2 - cameraPt <EOL> middleRay = ( leftRay + rightRay ) / <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addLine ( cameraPt , worldPt1 ) <EOL> d . addLine ( cameraPt , worldPt2 ) <EOL> d . addLine ( worldPt1 , worldPt2 ) <EOL> d . addLine ( cameraPt , cameraPt + middleRay ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if inputObj : <EOL> polyData = shallowCopy ( inputObj . polyData ) <EOL> else : <EOL> polyData = getCurrentRevolutionData ( ) <EOL> origin = cameraPt <EOL> normal = np . cross ( rightRay , leftRay ) <EOL> leftNormal = np . cross ( normal , leftRay ) <EOL> rightNormal = np . cross ( rightRay , normal ) <EOL> normal /= np . linalg . norm ( normal ) <EOL> leftNormal /= np . linalg . norm ( leftNormal ) <EOL> rightNormal /= np . linalg . norm ( rightNormal ) <EOL> middleRay /= np . linalg . norm ( middleRay ) <EOL> cropped , polyData = cropToPlane ( polyData , origin , normal , sliceThreshold ) <EOL> updatePolyData ( polyData , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , colorByRange = [ - <NUM_LIT:0.5> , <NUM_LIT:0.5> ] , visible = False ) <EOL> updatePolyData ( cropped , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> cropped , _ = cropToPlane ( cropped , origin , leftNormal , [ - <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> cropped , _ = cropToPlane ( cropped , origin , rightNormal , [ - <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> updatePolyData ( cropped , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> planePoints , planeNormal = applyPlaneFit ( cropped , distanceThreshold = <NUM_LIT> , perpendicularAxis = middleRay , angleEpsilon = math . radians ( <NUM_LIT> ) ) <EOL> planePoints = thresholdPoints ( planePoints , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , color = getRandomColor ( ) , visible = False ) <EOL> '''<STR_LIT>''' <EOL> name = '<STR_LIT>' <EOL> segmentBlockByTopPlane ( planePoints , blockDimensions , expectedNormal = - middleRay , expectedXAxis = middleRay , edgeSign = - <NUM_LIT:1> , name = name ) <EOL> def updateBlockAffordances ( polyData = None ) : <EOL> for obj in om . getObjects ( ) : <EOL> if isinstance ( obj , BoxAffordanceItem ) : <EOL> if '<STR_LIT>' in obj . getProperty ( '<STR_LIT:Name>' ) : <EOL> om . removeFromObjectModel ( obj ) <EOL> for obj in om . getObjects ( ) : <EOL> if isinstance ( obj , BoxAffordanceItem ) : <EOL> updateBlockFit ( obj , polyData ) <EOL> def updateBlockFit ( affordanceObj , polyData = None ) : <EOL> affordanceObj . updateParamsFromActorTransform ( ) <EOL> name = affordanceObj . getProperty ( '<STR_LIT:Name>' ) + '<STR_LIT>' <EOL> origin = affordanceObj . params [ '<STR_LIT>' ] <EOL> normal = affordanceObj . params [ '<STR_LIT>' ] <EOL> edgePerpAxis = affordanceObj . params [ '<STR_LIT>' ] <EOL> blockDimensions = [ affordanceObj . params [ '<STR_LIT>' ] , affordanceObj . params [ '<STR_LIT>' ] ] <EOL> if polyData is None : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = shallowCopy ( inputObj . polyData ) <EOL> cropThreshold = <NUM_LIT:0.1> <EOL> cropped = polyData <EOL> cropped , _ = cropToPlane ( cropped , origin , normal , [ - cropThreshold , cropThreshold ] ) <EOL> cropped , _ = cropToPlane ( cropped , origin , edgePerpAxis , [ - cropThreshold , cropThreshold ] ) <EOL> updatePolyData ( cropped , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> cropped = extractLargestCluster ( cropped ) <EOL> planePoints , planeNormal = applyPlaneFit ( cropped , distanceThreshold = <NUM_LIT> , perpendicularAxis = normal , angleEpsilon = math . radians ( <NUM_LIT:10> ) ) <EOL> planePoints = thresholdPoints ( planePoints , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> refitObj = segmentBlockByTopPlane ( planePoints , blockDimensions , expectedNormal = normal , expectedXAxis = edgePerpAxis , edgeSign = - <NUM_LIT:1> , name = name ) <EOL> refitOrigin = np . array ( refitObj . params [ '<STR_LIT>' ] ) <EOL> refitLength = refitObj . params [ '<STR_LIT>' ] <EOL> refitZAxis = refitObj . params [ '<STR_LIT>' ] <EOL> refitEndPoint1 = refitOrigin + refitZAxis * refitLength / <NUM_LIT> <EOL> originalLength = affordanceObj . params [ '<STR_LIT>' ] <EOL> correctedOrigin = refitEndPoint1 - refitZAxis * originalLength / <NUM_LIT> <EOL> originDelta = correctedOrigin - refitOrigin <EOL> refitObj . params [ '<STR_LIT>' ] = originalLength <EOL> refitObj . polyData . DeepCopy ( affordanceObj . polyData ) <EOL> refitObj . actor . GetUserTransform ( ) . Translate ( originDelta ) <EOL> refitObj . updateParamsFromActorTransform ( ) <EOL> def startInteractiveLineDraw ( blockDimensions ) : <EOL> picker = LineDraw ( app . getCurrentRenderView ( ) ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( createLine , blockDimensions ) <EOL> def startLeverValveSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentLeverValve ) <EOL> def refitValveAffordance ( aff , point1 , origin , normal ) : <EOL> xaxis = aff . params [ '<STR_LIT>' ] <EOL> yaxis = aff . params [ '<STR_LIT>' ] <EOL> zaxis = aff . params [ '<STR_LIT>' ] <EOL> origin = aff . params [ '<STR_LIT>' ] <EOL> zaxis = normal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> aff . actor . GetUserTransform ( ) . SetMatrix ( t . GetMatrix ( ) ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> def segmentValve ( expectedValveRadius , point1 , point2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , _ , wallNormal = applyPlaneFit ( polyData , expectedNormal = viewPlaneNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> polyData , _ , _ = applyPlaneFit ( polyData , expectedNormal = wallNormal , searchOrigin = point2 , searchRadius = expectedValveRadius , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> valveCluster = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> valveCluster = cropToSphere ( valveCluster , point2 , expectedValveRadius * <NUM_LIT:2> ) <EOL> valveCluster = extractLargestCluster ( valveCluster , minClusterSize = <NUM_LIT:1> ) <EOL> updatePolyData ( valveCluster , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> origin = np . average ( vtkNumpy . getNumpyFromVtk ( valveCluster , '<STR_LIT>' ) , axis = <NUM_LIT:0> ) <EOL> zaxis = wallNormal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> zwidth = <NUM_LIT> <EOL> radius = expectedValveRadius <EOL> d = DebugData ( ) <EOL> d . addLine ( np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , - zwidth / <NUM_LIT> ] ) , np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , zwidth / <NUM_LIT> ] ) , radius = radius ) <EOL> name = '<STR_LIT>' <EOL> obj = showPolyData ( d . getPolyData ( ) , name , cls = FrameAffordanceItem , parent = '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> refitWallCallbacks . append ( functools . partial ( refitValveAffordance , obj ) ) <EOL> params = dict ( axis = zaxis , radius = radius , length = zwidth , origin = origin , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , <EOL> xwidth = radius , ywidth = radius , zwidth = zwidth , <EOL> otdf_type = '<STR_LIT>' , friendly_name = '<STR_LIT>' ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> frameObj = showFrame ( obj . actor . GetUserTransform ( ) , name + '<STR_LIT>' , parent = obj , scale = radius , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> def segmentValveByBoundingBox ( polyData , searchPoint ) : <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> polyData = cropToSphere ( polyData , searchPoint , radius = <NUM_LIT> ) <EOL> polyData = applyVoxelGrid ( polyData , leafSize = <NUM_LIT> ) <EOL> polyData = labelDistanceToLine ( polyData , searchPoint , np . array ( searchPoint ) + np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0.0> , <NUM_LIT> ] ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , visible = False ) <EOL> _ , origin , normal = applyPlaneFit ( searchRegion , distanceThreshold = <NUM_LIT> , perpendicularAxis = viewDirection , angleEpsilon = math . radians ( <NUM_LIT:30> ) , expectedNormal = - viewDirection , returnOrigin = True ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , normal , origin ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> valvePoints = extractLargestCluster ( searchRegion , minClusterSize = <NUM_LIT:1> ) <EOL> updatePolyData ( valvePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> valvePoints , _ = applyPlaneFit ( valvePoints , expectedNormal = normal , perpendicularAxis = normal , distanceThreshold = <NUM_LIT> ) <EOL> valveFit = thresholdPoints ( valvePoints , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( valveFit , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> points = vtkNumpy . getNumpyFromVtk ( valveFit , '<STR_LIT>' ) <EOL> zvalues = points [ : , <NUM_LIT:2> ] . copy ( ) <EOL> minZ = np . min ( zvalues ) <EOL> maxZ = np . max ( zvalues ) <EOL> tubeRadius = <NUM_LIT> <EOL> radius = float ( ( maxZ - minZ ) / <NUM_LIT> ) - tubeRadius <EOL> fields = makePolyDataFields ( valveFit ) <EOL> origin = np . array ( fields . frame . GetPosition ( ) ) <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> xaxis = normal <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> pose = transformUtils . poseFromTransform ( t ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , uuid = newUUID ( ) , pose = pose , Color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , Radius = radius , Segments = <NUM_LIT:20> ) <EOL> desc [ '<STR_LIT>' ] = tubeRadius <EOL> obj = affordanceManager . newAffordanceFromDescription ( desc ) <EOL> obj . params = dict ( radius = radius ) <EOL> return obj <EOL> def segmentDoorPlane ( polyData , doorPoint , stanceFrame ) : <EOL> doorPoint = np . array ( doorPoint ) <EOL> doorBand = <NUM_LIT> <EOL> polyData = cropToLineSegment ( polyData , doorPoint + [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , doorBand / <NUM_LIT:2> ] , doorPoint - [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , doorBand / <NUM_LIT:2> ] ) <EOL> fitPoints , normal = applyLocalPlaneFit ( polyData , doorPoint , searchRadius = <NUM_LIT> , searchRadiusEnd = <NUM_LIT:1.0> , removeGroundFirst = False ) <EOL> updatePolyData ( fitPoints , '<STR_LIT>' , visible = False , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> if np . dot ( normal , viewDirection ) > <NUM_LIT:0> : <EOL> normal = - normal <EOL> origin = computeCentroid ( fitPoints ) <EOL> groundHeight = stanceFrame . GetPosition ( ) [ <NUM_LIT:2> ] <EOL> origin = [ origin [ <NUM_LIT:0> ] , origin [ <NUM_LIT:1> ] , groundHeight ] <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> return t <EOL> def segmentValveByRim ( polyData , rimPoint1 , rimPoint2 ) : <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> yaxis = np . array ( rimPoint2 ) - np . array ( rimPoint1 ) <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> if np . dot ( xaxis , viewDirection ) < <NUM_LIT:0> : <EOL> xaxis = - xaxis <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> origin = ( np . array ( rimPoint2 ) + np . array ( rimPoint1 ) ) / <NUM_LIT> <EOL> polyData = labelPointDistanceAlongAxis ( polyData , xaxis , origin ) <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( polyData , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> polyData = cropToSphere ( polyData , origin , radius = <NUM_LIT> ) <EOL> polyData = applyVoxelGrid ( polyData , leafSize = <NUM_LIT> ) <EOL> updatePolyData ( polyData , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , visible = False ) <EOL> valveFit = extractLargestCluster ( polyData , minClusterSize = <NUM_LIT:1> ) <EOL> updatePolyData ( valveFit , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> points = vtkNumpy . getNumpyFromVtk ( valveFit , '<STR_LIT>' ) <EOL> zvalues = points [ : , <NUM_LIT:2> ] . copy ( ) <EOL> minZ = np . min ( zvalues ) <EOL> maxZ = np . max ( zvalues ) <EOL> tubeRadius = <NUM_LIT> <EOL> radius = float ( ( maxZ - minZ ) / <NUM_LIT> ) - tubeRadius <EOL> fields = makePolyDataFields ( valveFit ) <EOL> origin = np . array ( fields . frame . GetPosition ( ) ) <EOL> vis . updatePolyData ( transformPolyData ( fields . box , fields . frame ) , '<STR_LIT>' , visible = False ) <EOL> '''<STR_LIT>''' <EOL> radius = np . max ( fields . dims ) / <NUM_LIT> - tubeRadius <EOL> proj = [ np . abs ( np . dot ( xaxis , axis ) ) for axis in fields . axes ] <EOL> xaxisNew = fields . axes [ np . argmax ( proj ) ] <EOL> if np . dot ( xaxisNew , xaxis ) < <NUM_LIT:0> : <EOL> xaxisNew = - xaxisNew <EOL> xaxis = xaxisNew <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> pose = transformUtils . poseFromTransform ( t ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , uuid = newUUID ( ) , pose = pose , Color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , Radius = float ( radius ) , Segments = <NUM_LIT:20> ) <EOL> desc [ '<STR_LIT>' ] = tubeRadius <EOL> obj = affordanceManager . newAffordanceFromDescription ( desc ) <EOL> obj . params = dict ( radius = radius ) <EOL> return obj <EOL> def segmentValveByWallPlane ( expectedValveRadius , point1 , point2 ) : <EOL> centerPoint = ( point1 + point2 ) / <NUM_LIT> <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> _ , polyData = removeGround ( polyData ) <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = - viewDirection , returnOrigin = True ) <EOL> perpLine = np . cross ( point2 - point1 , normal ) <EOL> point3 , point4 = centerPoint + perpLine / <NUM_LIT> , centerPoint - perpLine / <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addLine ( point1 , point2 ) <EOL> d . addLine ( point3 , point4 ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT:0.5> ] ) <EOL> searchRegion = cropToLineSegment ( searchRegion , point1 , point2 ) <EOL> searchRegion = cropToLineSegment ( searchRegion , point3 , point4 ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , visible = False ) <EOL> searchRegionSpokes = shallowCopy ( searchRegion ) <EOL> searchRegion , origin , _ = applyPlaneFit ( searchRegion , expectedNormal = normal , perpendicularAxis = normal , returnOrigin = True ) <EOL> searchRegion = thresholdPoints ( searchRegion , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> largestCluster = extractLargestCluster ( searchRegion , minClusterSize = <NUM_LIT:1> ) <EOL> updatePolyData ( largestCluster , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> radiusLimit = [ expectedValveRadius - <NUM_LIT> , expectedValveRadius + <NUM_LIT> ] if expectedValveRadius else None <EOL> polyData , circleFit = extractCircle ( largestCluster , distanceThreshold = <NUM_LIT> , radiusLimit = radiusLimit ) <EOL> updatePolyData ( polyData , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> radius = circleFit . GetCircleRadius ( ) <EOL> origin = np . array ( circleFit . GetCircleOrigin ( ) ) <EOL> circleNormal = np . array ( circleFit . GetCircleNormal ( ) ) <EOL> circleNormal = circleNormal / np . linalg . norm ( circleNormal ) <EOL> if np . dot ( circleNormal , normal ) < <NUM_LIT:0> : <EOL> circleNormal *= - <NUM_LIT:1> <EOL> circleNormal = normal <EOL> radius = expectedValveRadius <EOL> d = DebugData ( ) <EOL> d . addLine ( origin - normal * radius , origin + normal * radius ) <EOL> d . addCircle ( origin , circleNormal , radius ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> zaxis = - circleNormal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( zaxis , - yaxis , xaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> if ( <NUM_LIT:1> == <NUM_LIT:0> ) : <EOL> searchRegionSpokes = labelDistanceToLine ( searchRegionSpokes , origin , [ origin + circleNormal ] ) <EOL> searchRegionSpokes = thresholdPoints ( searchRegionSpokes , '<STR_LIT>' , [ <NUM_LIT> , radius - <NUM_LIT> ] ) <EOL> updatePolyData ( searchRegionSpokes , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> searchRegionSpokesLocal = transformPolyData ( searchRegionSpokes , t . GetLinearInverse ( ) ) <EOL> points = vtkNumpy . getNumpyFromVtk ( searchRegionSpokesLocal , '<STR_LIT>' ) <EOL> spoke_angle = findValveSpokeAngle ( points ) <EOL> else : <EOL> spoke_angle = <NUM_LIT:0> <EOL> spokeAngleTransform = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , spoke_angle ] ) <EOL> spokeTransform = transformUtils . copyFrame ( t ) <EOL> spokeAngleTransform . Concatenate ( spokeTransform ) <EOL> spokeObj = showFrame ( spokeAngleTransform , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False , scale = radius ) <EOL> spokeObj . addToView ( app . getDRCView ( ) ) <EOL> t = spokeAngleTransform <EOL> tubeRadius = <NUM_LIT> <EOL> pose = transformUtils . poseFromTransform ( t ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , uuid = newUUID ( ) , pose = pose , Color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , Radius = float ( radius ) , Segments = <NUM_LIT:20> ) <EOL> desc [ '<STR_LIT>' ] = tubeRadius <EOL> obj = affordanceManager . newAffordanceFromDescription ( desc ) <EOL> obj . params = dict ( radius = radius ) <EOL> def showHistogram ( polyData , arrayName , numberOfBins = <NUM_LIT:100> ) : <EOL> import matplotlib . pyplot as plt <EOL> x = vnp . getNumpyFromVtk ( polyData , arrayName ) <EOL> hist , bins = np . histogram ( x , bins = numberOfBins ) <EOL> width = <NUM_LIT> * ( bins [ <NUM_LIT:1> ] - bins [ <NUM_LIT:0> ] ) <EOL> center = ( bins [ : - <NUM_LIT:1> ] + bins [ <NUM_LIT:1> : ] ) / <NUM_LIT:2> <EOL> plt . bar ( center , hist , align = '<STR_LIT>' , width = width ) <EOL> plt . show ( ) <EOL> return bins [ np . argmax ( hist ) ] + ( bins [ <NUM_LIT:1> ] - bins [ <NUM_LIT:0> ] ) / <NUM_LIT> <EOL> def showTable ( table , parent ) : <EOL> '''<STR_LIT>''' <EOL> pose = transformUtils . poseFromTransform ( table . frame ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' , Color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , pose = pose ) <EOL> aff = affordanceManager . newAffordanceFromDescription ( desc ) <EOL> aff . setPolyData ( table . mesh ) <EOL> tableBox = vis . showPolyData ( table . box , '<STR_LIT>' , parent = aff , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = False ) <EOL> tableBox . actor . SetUserTransform ( table . frame ) <EOL> def applyKmeansLabel ( polyData , arrayName , numberOfClusters , whiten = False ) : <EOL> import scipy . cluster <EOL> ar = vnp . getNumpyFromVtk ( polyData , arrayName ) . copy ( ) <EOL> if whiten : <EOL> scipy . cluster . vq . whiten ( ar ) <EOL> codes , disturbances = scipy . cluster . vq . kmeans ( ar , numberOfClusters ) <EOL> if arrayName == '<STR_LIT>' and numberOfClusters == <NUM_LIT:2> : <EOL> v1 = codes [ <NUM_LIT:0> ] <EOL> v2 = codes [ <NUM_LIT:1> ] <EOL> v1 /= np . linalg . norm ( v1 ) <EOL> v2 /= np . linalg . norm ( v2 ) <EOL> angle = np . arccos ( np . dot ( v1 , v2 ) ) <EOL> print '<STR_LIT>' , np . degrees ( angle ) <EOL> code , distance = scipy . cluster . vq . vq ( ar , codes ) <EOL> polyData = shallowCopy ( polyData ) <EOL> vnp . addNumpyToVtk ( polyData , code , '<STR_LIT>' % arrayName ) <EOL> return polyData <EOL> def findValveSpokeAngle ( points ) : <EOL> '''<STR_LIT>''' <EOL> angle = np . degrees ( np . arctan2 ( points [ : , <NUM_LIT:1> ] , points [ : , <NUM_LIT:0> ] ) ) <EOL> qq = np . where ( angle < <NUM_LIT:0> ) [ <NUM_LIT:0> ] <EOL> angle [ qq ] += <NUM_LIT> <EOL> angle = np . mod ( angle , <NUM_LIT> ) <EOL> bins = range ( <NUM_LIT:0> , <NUM_LIT> , <NUM_LIT:10> ) <EOL> freq , bins = np . histogram ( angle , bins ) <EOL> amax = np . argmax ( freq ) <EOL> spoke_angle = bins [ amax ] + <NUM_LIT:5> <EOL> return spoke_angle <EOL> def findWallCenter ( polyData , removeGroundMethod = removeGround ) : <EOL> '''<STR_LIT>''' <EOL> _ , polyData = removeGroundMethod ( polyData ) <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = - viewDirection , returnOrigin = True ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> wallPoints = applyVoxelGrid ( wallPoints , leafSize = <NUM_LIT> ) <EOL> wallPoints = extractLargestCluster ( wallPoints , minClusterSize = <NUM_LIT:100> ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> xvalues = vtkNumpy . getNumpyFromVtk ( wallPoints , '<STR_LIT>' ) [ : , <NUM_LIT:0> ] <EOL> yvalues = vtkNumpy . getNumpyFromVtk ( wallPoints , '<STR_LIT>' ) [ : , <NUM_LIT:1> ] <EOL> xcenter = ( np . max ( xvalues ) + np . min ( xvalues ) ) / <NUM_LIT:2> <EOL> ycenter = ( np . max ( yvalues ) + np . min ( yvalues ) ) / <NUM_LIT:2> <EOL> zcenter = SegmentationContext . getGlobalInstance ( ) . getGroundHeight ( ) + <NUM_LIT> <EOL> point1 = np . array ( [ xcenter , ycenter , zcenter ] ) <EOL> zaxis = - normal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point1 ) <EOL> normalObj = showFrame ( t , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> normalObj . addToView ( app . getDRCView ( ) ) <EOL> return t <EOL> def segmentValveWallAuto ( expectedValveRadius = <NUM_LIT> , mode = '<STR_LIT>' , removeGroundMethod = removeGround ) : <EOL> '''<STR_LIT>''' <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> t = findWallCenter ( polyData , removeGroundMethod ) <EOL> valve_point1 = [ <NUM_LIT:0> , <NUM_LIT> , <NUM_LIT:0> ] <EOL> valveTransform1 = transformUtils . frameFromPositionAndRPY ( valve_point1 , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> valveTransform1 . Concatenate ( t ) <EOL> point1 = np . array ( valveTransform1 . GetPosition ( ) ) <EOL> valve_point2 = [ <NUM_LIT:0> , - <NUM_LIT> , <NUM_LIT:0> ] <EOL> valveTransform2 = transformUtils . frameFromPositionAndRPY ( valve_point2 , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> valveTransform2 . Concatenate ( t ) <EOL> point2 = np . array ( valveTransform2 . GetPosition ( ) ) <EOL> valve_point3 = [ <NUM_LIT:0> , <NUM_LIT:1.0> , <NUM_LIT:0> ] <EOL> valveTransform3 = transformUtils . frameFromPositionAndRPY ( valve_point3 , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> valveTransform3 . Concatenate ( t ) <EOL> point3 = valveTransform3 . GetPosition ( ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( point2 , radius = <NUM_LIT> ) <EOL> d . addSphere ( point1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( point3 , radius = <NUM_LIT> ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> if ( mode == '<STR_LIT>' ) : <EOL> segmentValveByWallPlane ( expectedValveRadius , point1 , point2 ) <EOL> elif ( mode == '<STR_LIT>' ) : <EOL> segmentLeverByWallPlane ( point1 , point3 ) <EOL> elif ( mode == '<STR_LIT>' ) : <EOL> segmentValveByWallPlane ( expectedValveRadius , point1 , point2 ) <EOL> segmentLeverByWallPlane ( point1 , point3 ) <EOL> else : <EOL> raise Exception ( '<STR_LIT>' + mode ) <EOL> def segmentLeverByWallPlane ( point1 , point2 ) : <EOL> '''<STR_LIT>''' <EOL> centerPoint = ( point1 + point2 ) / <NUM_LIT> <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = - viewDirection , returnOrigin = True ) <EOL> perpLine = np . cross ( point2 - point1 , - normal ) <EOL> point3 , point4 = centerPoint + perpLine / <NUM_LIT> , centerPoint - perpLine / <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addLine ( point1 , point2 ) <EOL> d . addLine ( point3 , point4 ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT> ] ) <EOL> searchRegion = cropToLineSegment ( searchRegion , point1 , point2 ) <EOL> searchRegion = cropToLineSegment ( searchRegion , point3 , point4 ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , visible = False ) <EOL> linePoint , lineDirection , _ = applyLineFit ( searchRegion , distanceThreshold = <NUM_LIT> ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( linePoint , radius = <NUM_LIT> ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> pts = vtkNumpy . getNumpyFromVtk ( searchRegion , '<STR_LIT>' ) <EOL> dists = np . dot ( pts - linePoint , lineDirection ) <EOL> lever_center = linePoint + lineDirection * np . min ( dists ) <EOL> lever_tip = linePoint + lineDirection * np . max ( dists ) <EOL> zaxis = - normal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point1 ) <EOL> wall_point_lower_left = [ - <NUM_LIT:20> , - <NUM_LIT> , <NUM_LIT:0> ] <EOL> wall_point_lower_left_Transform = transformUtils . frameFromPositionAndRPY ( wall_point_lower_left , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> wall_point_lower_left_Transform . Concatenate ( t ) <EOL> wall_point_lower_left = wall_point_lower_left_Transform . GetPosition ( ) <EOL> d1 = np . sqrt ( np . sum ( ( wall_point_lower_left - projectPointToPlane ( lever_center , origin , normal ) ) ** <NUM_LIT:2> ) ) <EOL> d2 = np . sqrt ( np . sum ( ( wall_point_lower_left - projectPointToPlane ( lever_tip , origin , normal ) ) ** <NUM_LIT:2> ) ) <EOL> if ( d2 < d1 ) : <EOL> p_temp = lever_center <EOL> lever_center = lever_tip <EOL> lever_tip = p_temp <EOL> lineDirection = - lineDirection <EOL> zaxis = - normal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( lever_center ) <EOL> rotationAngle = - computeSignedAngleBetweenVectors ( lineDirection , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] , - normal ) <EOL> t_lever = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , math . degrees ( rotationAngle ) ] ) <EOL> t_lever . PostMultiply ( ) <EOL> t_lever . Concatenate ( t ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( wall_point_lower_left , radius = <NUM_LIT:0.1> ) <EOL> d . addSphere ( lever_center , radius = <NUM_LIT> ) <EOL> d . addSphere ( lever_tip , radius = <NUM_LIT> ) <EOL> d . addLine ( lever_center , lever_tip ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = False ) <EOL> radius = <NUM_LIT> <EOL> length = np . sqrt ( np . sum ( ( lever_tip - lever_center ) ** <NUM_LIT:2> ) ) <EOL> d = DebugData ( ) <EOL> d . addLine ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ length , <NUM_LIT:0> , <NUM_LIT:0> ] , radius = radius ) <EOL> d . addSphere ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , <NUM_LIT> ) <EOL> geometry = d . getPolyData ( ) <EOL> obj = showPolyData ( geometry , '<STR_LIT>' , cls = FrameAffordanceItem , parent = '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = True ) <EOL> obj . actor . SetUserTransform ( t_lever ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> frameObj = showFrame ( t_lever , '<STR_LIT>' , parent = obj , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> otdfType = '<STR_LIT>' <EOL> params = dict ( origin = np . array ( t_lever . GetPosition ( ) ) , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , radius = radius , length = length , friendly_name = otdfType , otdf_type = otdfType ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> def applyICP ( source , target ) : <EOL> icp = vtk . vtkIterativeClosestPointTransform ( ) <EOL> icp . SetSource ( source ) <EOL> icp . SetTarget ( target ) <EOL> icp . GetLandmarkTransform ( ) . SetModeToRigidBody ( ) <EOL> icp . Update ( ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . SetMatrix ( icp . GetMatrix ( ) ) <EOL> return t <EOL> def applyDiskGlyphs ( polyData , computeNormals = True ) : <EOL> voxelGridLeafSize = <NUM_LIT> <EOL> normalEstimationSearchRadius = <NUM_LIT> <EOL> diskRadius = <NUM_LIT> <EOL> diskResolution = <NUM_LIT:12> <EOL> if computeNormals : <EOL> scanInput = polyData <EOL> pd = applyVoxelGrid ( scanInput , leafSize = voxelGridLeafSize ) <EOL> pd = labelOutliers ( pd , searchRadius = normalEstimationSearchRadius , neighborsInSearchRadius = <NUM_LIT:3> ) <EOL> pd = thresholdPoints ( pd , '<STR_LIT>' , [ <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> pd = normalEstimation ( pd , searchRadius = normalEstimationSearchRadius , searchCloud = scanInput ) <EOL> else : <EOL> pd = polyData <EOL> assert polyData . GetPointData ( ) . GetNormals ( ) <EOL> disk = vtk . vtkDiskSource ( ) <EOL> disk . SetOuterRadius ( diskRadius ) <EOL> disk . SetInnerRadius ( <NUM_LIT:0.0> ) <EOL> disk . SetRadialResolution ( <NUM_LIT:0> ) <EOL> disk . SetCircumferentialResolution ( diskResolution ) <EOL> disk . Update ( ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . RotateY ( <NUM_LIT> ) <EOL> disk = transformPolyData ( disk . GetOutput ( ) , t ) <EOL> glyph = vtk . vtkGlyph3D ( ) <EOL> glyph . ScalingOff ( ) <EOL> glyph . OrientOn ( ) <EOL> glyph . SetSource ( disk ) <EOL> glyph . SetInput ( pd ) <EOL> glyph . SetVectorModeToUseNormal ( ) <EOL> glyph . Update ( ) <EOL> return shallowCopy ( glyph . GetOutput ( ) ) <EOL> def applyArrowGlyphs ( polyData , computeNormals = True , voxelGridLeafSize = <NUM_LIT> , normalEstimationSearchRadius = <NUM_LIT> , arrowSize = <NUM_LIT> ) : <EOL> if computeNormals : <EOL> polyData = applyVoxelGrid ( polyData , leafSize = <NUM_LIT> ) <EOL> voxelData = applyVoxelGrid ( polyData , leafSize = voxelGridLeafSize ) <EOL> polyData = normalEstimation ( polyData , searchRadius = normalEstimationSearchRadius , searchCloud = voxelData ) <EOL> polyData = removeNonFinitePoints ( polyData , '<STR_LIT>' ) <EOL> flipNormalsWithViewDirection ( polyData , SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) ) <EOL> assert polyData . GetPointData ( ) . GetNormals ( ) <EOL> arrow = vtk . vtkArrowSource ( ) <EOL> arrow . Update ( ) <EOL> glyph = vtk . vtkGlyph3D ( ) <EOL> glyph . SetScaleFactor ( arrowSize ) <EOL> glyph . SetSource ( arrow . GetOutput ( ) ) <EOL> glyph . SetInput ( polyData ) <EOL> glyph . SetVectorModeToUseNormal ( ) <EOL> glyph . Update ( ) <EOL> return shallowCopy ( glyph . GetOutput ( ) ) <EOL> def segmentLeverValve ( point1 , point2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = viewPlaneNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> radius = <NUM_LIT> <EOL> length = <NUM_LIT> <EOL> normal = - normal <EOL> zaxis = normal <EOL> xaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point2 ) <EOL> leverP1 = point2 <EOL> leverP2 = point2 + xaxis * length <EOL> d = DebugData ( ) <EOL> d . addLine ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ length , <NUM_LIT:0> , <NUM_LIT:0> ] , radius = radius ) <EOL> d . addSphere ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , <NUM_LIT> ) <EOL> geometry = d . getPolyData ( ) <EOL> obj = showPolyData ( geometry , '<STR_LIT>' , cls = FrameAffordanceItem , parent = '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = True ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> frameObj = showFrame ( t , '<STR_LIT>' , parent = obj , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> otdfType = '<STR_LIT>' <EOL> params = dict ( origin = np . array ( t . GetPosition ( ) ) , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , radius = radius , length = length , friendly_name = otdfType , otdf_type = otdfType ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> def segmentWye ( point1 , point2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = viewPlaneNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> wyeMesh = ioUtils . readPolyData ( os . path . join ( app . getDRCBase ( ) , '<STR_LIT>' ) ) <EOL> wyeMeshPoint = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT> ] ) <EOL> wyeMeshLeftHandle = np . array ( [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] ) <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PreMultiply ( ) <EOL> t . Translate ( - wyeMeshPoint ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point2 ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( point2 , radius = <NUM_LIT> ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> wyeObj = showPolyData ( wyeMesh , '<STR_LIT>' , cls = FrameAffordanceItem , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = True ) <EOL> wyeObj . actor . SetUserTransform ( t ) <EOL> wyeObj . addToView ( app . getDRCView ( ) ) <EOL> frameObj = showFrame ( t , '<STR_LIT>' , parent = wyeObj , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = np . array ( t . GetPosition ( ) ) , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , friendly_name = '<STR_LIT>' , otdf_type = '<STR_LIT>' ) <EOL> wyeObj . setAffordanceParams ( params ) <EOL> wyeObj . updateParamsFromActorTransform ( ) <EOL> def segmentDoorHandle ( otdfType , point1 , point2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = viewPlaneNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> handlePoint = np . array ( [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] ) <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> xwidth = <NUM_LIT> <EOL> ywidth = <NUM_LIT> <EOL> zwidth = <NUM_LIT> <EOL> cube = vtk . vtkCubeSource ( ) <EOL> cube . SetXLength ( xwidth ) <EOL> cube . SetYLength ( ywidth ) <EOL> cube . SetZLength ( zwidth ) <EOL> cube . Update ( ) <EOL> cube = shallowCopy ( cube . GetOutput ( ) ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point2 ) <EOL> name = '<STR_LIT>' <EOL> obj = showPolyData ( cube , name , cls = FrameAffordanceItem , parent = '<STR_LIT>' ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = origin , xwidth = xwidth , ywidth = ywidth , zwidth = zwidth , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , friendly_name = name , otdf_type = otdfType ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> frameObj = showFrame ( obj . actor . GetUserTransform ( ) , name + '<STR_LIT>' , parent = obj , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> def segmentTruss ( point1 , point2 ) : <EOL> edge = point2 - point1 <EOL> edgeLength = np . linalg . norm ( edge ) <EOL> stanceOffset = [ - <NUM_LIT> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] <EOL> stanceYaw = <NUM_LIT:0.0> <EOL> d = DebugData ( ) <EOL> p1 = [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] <EOL> p2 = - np . array ( [ <NUM_LIT:0.0> , - <NUM_LIT:1.0> , <NUM_LIT:0.0> ] ) * edgeLength <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> stanceTransform = vtk . vtkTransform ( ) <EOL> stanceTransform . PostMultiply ( ) <EOL> stanceTransform . Translate ( stanceOffset ) <EOL> geometry = transformPolyData ( d . getPolyData ( ) , stanceTransform . GetLinearInverse ( ) ) <EOL> yaxis = edge / edgeLength <EOL> zaxis = [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xwidth = <NUM_LIT:0.1> <EOL> ywidth = edgeLength <EOL> zwidth = <NUM_LIT:0.1> <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PreMultiply ( ) <EOL> t . Concatenate ( stanceTransform ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point1 ) <EOL> name = '<STR_LIT>' <EOL> otdfType = '<STR_LIT>' <EOL> obj = showPolyData ( geometry , name , cls = FrameAffordanceItem , parent = '<STR_LIT>' ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = t . GetPosition ( ) , xwidth = xwidth , ywidth = ywidth , zwidth = zwidth , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , friendly_name = name , otdf_type = otdfType ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> frameObj = showFrame ( obj . actor . GetUserTransform ( ) , name + '<STR_LIT>' , parent = obj , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> def segmentHoseNozzle ( point1 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> searchRegion = cropToSphere ( polyData , point1 , <NUM_LIT> ) <EOL> updatePolyData ( searchRegion , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> xaxis = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> yaxis = [ <NUM_LIT:0> , - <NUM_LIT:1> , <NUM_LIT:0> ] <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , - <NUM_LIT:1> ] <EOL> origin = point1 <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point1 ) <EOL> nozzleRadius = <NUM_LIT> <EOL> nozzleLength = <NUM_LIT> <EOL> nozzleTipRadius = <NUM_LIT> <EOL> nozzleTipLength = <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addLine ( np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , - nozzleLength / <NUM_LIT> ] ) , np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , nozzleLength / <NUM_LIT> ] ) , radius = nozzleRadius ) <EOL> d . addLine ( np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , nozzleLength / <NUM_LIT> ] ) , np . array ( [ <NUM_LIT:0> , <NUM_LIT:0> , nozzleLength / <NUM_LIT> + nozzleTipLength ] ) , radius = nozzleTipRadius ) <EOL> obj = showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , cls = FrameAffordanceItem , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = True ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> frameObj = showFrame ( t , '<STR_LIT>' , parent = obj , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = origin , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , friendly_name = '<STR_LIT>' , otdf_type = '<STR_LIT>' ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> def segmentDrillWall ( point1 , point2 , point3 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> points = [ point1 , point2 , point3 ] <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> expectedNormal = np . cross ( point2 - point1 , point3 - point1 ) <EOL> expectedNormal /= np . linalg . norm ( expectedNormal ) <EOL> if np . dot ( expectedNormal , viewPlaneNormal ) < <NUM_LIT:0> : <EOL> expectedNormal *= - <NUM_LIT:1.0> <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = expectedNormal , searchOrigin = ( point1 + point2 + point3 ) / <NUM_LIT> , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> points = [ projectPointToPlane ( point , origin , normal ) for point in points ] <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( points [ <NUM_LIT:0> ] ) <EOL> d = DebugData ( ) <EOL> pointsInWallFrame = [ ] <EOL> for p in points : <EOL> pp = np . zeros ( <NUM_LIT:3> ) <EOL> t . GetLinearInverse ( ) . TransformPoint ( p , pp ) <EOL> pointsInWallFrame . append ( pp ) <EOL> d . addSphere ( pp , radius = <NUM_LIT> ) <EOL> for a , b in zip ( pointsInWallFrame , pointsInWallFrame [ <NUM_LIT:1> : ] + [ pointsInWallFrame [ <NUM_LIT:0> ] ] ) : <EOL> d . addLine ( a , b , radius = <NUM_LIT> ) <EOL> aff = showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , cls = FrameAffordanceItem , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = True ) <EOL> aff . actor . SetUserTransform ( t ) <EOL> showFrame ( t , '<STR_LIT>' , parent = aff , visible = False ) <EOL> refitWallCallbacks . append ( functools . partial ( refitDrillWall , aff ) ) <EOL> params = dict ( origin = points [ <NUM_LIT:0> ] , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , <EOL> p1y = pointsInWallFrame [ <NUM_LIT:0> ] [ <NUM_LIT:1> ] , p1z = pointsInWallFrame [ <NUM_LIT:0> ] [ <NUM_LIT:2> ] , <EOL> p2y = pointsInWallFrame [ <NUM_LIT:1> ] [ <NUM_LIT:1> ] , p2z = pointsInWallFrame [ <NUM_LIT:1> ] [ <NUM_LIT:2> ] , <EOL> p3y = pointsInWallFrame [ <NUM_LIT:2> ] [ <NUM_LIT:1> ] , p3z = pointsInWallFrame [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] , <EOL> friendly_name = '<STR_LIT>' , otdf_type = '<STR_LIT>' ) <EOL> aff . setAffordanceParams ( params ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> refitWallCallbacks = [ ] <EOL> def refitWall ( point1 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = viewPlaneNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> wallPoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( wallPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> for func in refitWallCallbacks : <EOL> func ( point1 , origin , normal ) <EOL> def refitDrillWall ( aff , point1 , origin , normal ) : <EOL> t = aff . actor . GetUserTransform ( ) <EOL> targetOrigin = np . array ( t . GetPosition ( ) ) <EOL> projectedOrigin = projectPointToPlane ( targetOrigin , origin , normal ) <EOL> projectedOrigin [ <NUM_LIT:2> ] = targetOrigin [ <NUM_LIT:2> ] <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( projectedOrigin ) <EOL> aff . actor . GetUserTransform ( ) . SetMatrix ( t . GetMatrix ( ) ) <EOL> def getGroundHeightFromFeet ( ) : <EOL> rfoot = getLinkFrame ( drcargs . getDirectorConfig ( ) [ '<STR_LIT>' ] ) <EOL> return np . array ( rfoot . GetPosition ( ) ) [ <NUM_LIT:2> ] - <NUM_LIT> <EOL> def getTranslationRelativeToFoot ( t ) : <EOL> rfoot = getLinkFrame ( drcargs . getDirectorConfig ( ) [ '<STR_LIT>' ] ) <EOL> def segmentDrillWallConstrained ( rightAngleLocation , point1 , point2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> expectedNormal = np . cross ( point2 - point1 , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> expectedNormal /= np . linalg . norm ( expectedNormal ) <EOL> if np . dot ( expectedNormal , viewPlaneNormal ) < <NUM_LIT:0> : <EOL> expectedNormal *= - <NUM_LIT:1.0> <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = expectedNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> triangleOrigin = projectPointToPlane ( point2 , origin , normal ) <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( triangleOrigin ) <EOL> createDrillWall ( rightAngleLocation , t ) <EOL> def createDrillWall ( rightAngleLocation , trianglePose ) : <EOL> triangleOrigin = trianglePose . GetPosition ( ) <EOL> xaxis , yaxis , zaxis = transformUtils . getAxesFromTransform ( trianglePose ) <EOL> edgeRight = np . array ( [ <NUM_LIT:0.0> , - <NUM_LIT:1.0> , <NUM_LIT:0.0> ] ) * ( <NUM_LIT> ) <EOL> edgeUp = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) * ( <NUM_LIT> ) <EOL> pointsInWallFrame = np . zeros ( ( <NUM_LIT:3> , <NUM_LIT:3> ) ) <EOL> if rightAngleLocation == DRILL_TRIANGLE_BOTTOM_LEFT : <EOL> pointsInWallFrame [ <NUM_LIT:1> ] = edgeUp <EOL> pointsInWallFrame [ <NUM_LIT:2> ] = edgeRight <EOL> elif rightAngleLocation == DRILL_TRIANGLE_BOTTOM_RIGHT : <EOL> pointsInWallFrame [ <NUM_LIT:1> ] = edgeUp <EOL> pointsInWallFrame [ <NUM_LIT:2> ] = - edgeRight <EOL> elif rightAngleLocation == DRILL_TRIANGLE_TOP_LEFT : <EOL> pointsInWallFrame [ <NUM_LIT:1> ] = edgeRight <EOL> pointsInWallFrame [ <NUM_LIT:2> ] = - edgeUp <EOL> elif rightAngleLocation == DRILL_TRIANGLE_TOP_RIGHT : <EOL> pointsInWallFrame [ <NUM_LIT:1> ] = edgeRight <EOL> pointsInWallFrame [ <NUM_LIT:2> ] = edgeRight - edgeUp <EOL> else : <EOL> raise Exception ( '<STR_LIT>' , + rightAngleLocation ) <EOL> center = pointsInWallFrame . sum ( axis = <NUM_LIT:0> ) / <NUM_LIT> <EOL> shrinkFactor = <NUM_LIT:1> <EOL> shrinkPoints = ( pointsInWallFrame - center ) * shrinkFactor + center <EOL> d = DebugData ( ) <EOL> for p in pointsInWallFrame : <EOL> d . addSphere ( p , radius = <NUM_LIT> ) <EOL> for a , b in zip ( pointsInWallFrame , np . vstack ( ( pointsInWallFrame [ <NUM_LIT:1> : ] , pointsInWallFrame [ <NUM_LIT:0> ] ) ) ) : <EOL> d . addLine ( a , b , radius = <NUM_LIT> ) <EOL> for a , b in zip ( shrinkPoints , np . vstack ( ( shrinkPoints [ <NUM_LIT:1> : ] , shrinkPoints [ <NUM_LIT:0> ] ) ) ) : <EOL> d . addLine ( a , b , radius = <NUM_LIT> ) <EOL> folder = om . getOrCreateContainer ( '<STR_LIT>' ) <EOL> wall = om . findObjectByName ( '<STR_LIT>' ) <EOL> om . removeFromObjectModel ( wall ) <EOL> aff = showPolyData ( d . getPolyData ( ) , '<STR_LIT>' , cls = FrameAffordanceItem , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , visible = True , parent = folder ) <EOL> aff . actor . SetUserTransform ( trianglePose ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> refitWallCallbacks . append ( functools . partial ( refitDrillWall , aff ) ) <EOL> frameObj = showFrame ( trianglePose , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = triangleOrigin , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , <EOL> p1y = shrinkPoints [ <NUM_LIT:0> ] [ <NUM_LIT:1> ] , p1z = shrinkPoints [ <NUM_LIT:0> ] [ <NUM_LIT:2> ] , <EOL> p2y = shrinkPoints [ <NUM_LIT:1> ] [ <NUM_LIT:1> ] , p2z = shrinkPoints [ <NUM_LIT:1> ] [ <NUM_LIT:2> ] , <EOL> p3y = shrinkPoints [ <NUM_LIT:2> ] [ <NUM_LIT:1> ] , p3z = shrinkPoints [ <NUM_LIT:2> ] [ <NUM_LIT:2> ] , <EOL> friendly_name = '<STR_LIT>' , otdf_type = '<STR_LIT>' ) <EOL> aff . setAffordanceParams ( params ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> '''<STR_LIT>''' <EOL> def getDrillAffordanceParams ( origin , xaxis , yaxis , zaxis , drillType = "<STR_LIT>" ) : <EOL> if ( drillType == "<STR_LIT>" ) : <EOL> params = dict ( origin = origin , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , <EOL> button_x = <NUM_LIT> , <EOL> button_y = - <NUM_LIT> , <EOL> button_z = - <NUM_LIT> , <EOL> button_roll = - <NUM_LIT> , <EOL> button_pitch = - <NUM_LIT> , <EOL> button_yaw = <NUM_LIT:0.0> , <EOL> bit_x = - <NUM_LIT> , <EOL> bit_y = <NUM_LIT:0.0> , <EOL> bit_z = <NUM_LIT> , <EOL> bit_roll = <NUM_LIT:0> , <EOL> bit_pitch = - <NUM_LIT> , <EOL> bit_yaw = <NUM_LIT:0> , <EOL> friendly_name = '<STR_LIT>' , otdf_type = '<STR_LIT>' ) <EOL> else : <EOL> params = dict ( origin = origin , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis , xwidth = <NUM_LIT:0.1> , ywidth = <NUM_LIT:0.1> , zwidth = <NUM_LIT:0.1> , <EOL> button_x = <NUM_LIT> , <EOL> button_y = - <NUM_LIT> , <EOL> button_z = - <NUM_LIT> , <EOL> button_roll = <NUM_LIT:0.0> , <EOL> button_pitch = <NUM_LIT:0.0> , <EOL> button_yaw = <NUM_LIT:0.0> , <EOL> bit_x = <NUM_LIT> , <EOL> bit_y = <NUM_LIT:0.0> , <EOL> bit_z = <NUM_LIT> , <EOL> bit_roll = <NUM_LIT:0> , <EOL> bit_pitch = <NUM_LIT:0> , <EOL> bit_yaw = <NUM_LIT:0> , <EOL> friendly_name = '<STR_LIT>' , otdf_type = '<STR_LIT>' ) <EOL> return params <EOL> def getDrillMesh ( applyBitOffset = False ) : <EOL> button = np . array ( [ <NUM_LIT> , - <NUM_LIT> , - <NUM_LIT> ] ) <EOL> drillMesh = ioUtils . readPolyData ( os . path . join ( app . getDRCBase ( ) , '<STR_LIT>' ) ) <EOL> if applyBitOffset : <EOL> t = vtk . vtkTransform ( ) <EOL> t . Translate ( <NUM_LIT> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ) <EOL> drillMesh = transformPolyData ( drillMesh , t ) <EOL> d = DebugData ( ) <EOL> d . addPolyData ( drillMesh ) <EOL> d . addSphere ( button , radius = <NUM_LIT> , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> d . addLine ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT> ] , [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT> ] , radius = <NUM_LIT> , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> return shallowCopy ( d . getPolyData ( ) ) <EOL> def getDrillBarrelMesh ( ) : <EOL> return ioUtils . readPolyData ( os . path . join ( app . getDRCBase ( ) , '<STR_LIT>' ) , computeNormals = True ) <EOL> def segmentDrill ( point1 , point2 , point3 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = viewPlaneNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> tablePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT> ] ) <EOL> searchRegion = cropToSphere ( searchRegion , point2 , <NUM_LIT> ) <EOL> drillPoints = extractLargestCluster ( searchRegion ) <EOL> drillToTopPoint = np . array ( [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> ] ) <EOL> zaxis = normal <EOL> yaxis = point3 - point2 <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PreMultiply ( ) <EOL> t . Translate ( - drillToTopPoint ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( point2 ) <EOL> drillMesh = getDrillMesh ( ) <EOL> aff = showPolyData ( drillMesh , '<STR_LIT>' , cls = FrameAffordanceItem , visible = True ) <EOL> aff . actor . SetUserTransform ( t ) <EOL> showFrame ( t , '<STR_LIT>' , parent = aff , visible = False ) . addToView ( app . getDRCView ( ) ) <EOL> params = getDrillAffordanceParams ( origin , xaxis , yaxis , zaxis ) <EOL> aff . setAffordanceParams ( params ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> def makePolyDataFields ( pd ) : <EOL> mesh = computeDelaunay3D ( pd ) <EOL> if not mesh . GetNumberOfPoints ( ) : <EOL> return None <EOL> origin , edges , wireframe = getOrientedBoundingBox ( mesh ) <EOL> edgeLengths = np . array ( [ np . linalg . norm ( edge ) for edge in edges ] ) <EOL> axes = [ edge / np . linalg . norm ( edge ) for edge in edges ] <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> dot_products = [ np . dot ( axe , zaxis ) for axe in axes ] <EOL> axes = [ axes [ i ] for i in np . argsort ( dot_products ) ] <EOL> edgeLengths = [ edgeLengths [ i ] for i in np . argsort ( dot_products ) ] <EOL> boxCenter = computeCentroid ( wireframe ) <EOL> t = getTransformFromAxes ( axes [ <NUM_LIT:0> ] , axes [ <NUM_LIT:1> ] , axes [ <NUM_LIT:2> ] ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( boxCenter ) <EOL> pd = transformPolyData ( pd , t . GetLinearInverse ( ) ) <EOL> wireframe = transformPolyData ( wireframe , t . GetLinearInverse ( ) ) <EOL> mesh = transformPolyData ( mesh , t . GetLinearInverse ( ) ) <EOL> return FieldContainer ( points = pd , box = wireframe , mesh = mesh , frame = t , dims = edgeLengths , axes = axes ) <EOL> def makeMovable ( obj , initialTransform = None ) : <EOL> '''<STR_LIT>''' <EOL> pd = obj . polyData <EOL> t = initialTransform <EOL> if t is None : <EOL> origin , edges , wireframe = getOrientedBoundingBox ( pd ) <EOL> edgeLengths = np . array ( [ np . linalg . norm ( edge ) for edge in edges ] ) <EOL> axes = [ edge / np . linalg . norm ( edge ) for edge in edges ] <EOL> boxCenter = computeCentroid ( wireframe ) <EOL> t = getTransformFromAxes ( axes [ <NUM_LIT:0> ] , axes [ <NUM_LIT:1> ] , axes [ <NUM_LIT:2> ] ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( boxCenter ) <EOL> pd = transformPolyData ( pd , t . GetLinearInverse ( ) ) <EOL> obj . setPolyData ( pd ) <EOL> frame = obj . getChildFrame ( ) <EOL> if frame : <EOL> frame . copyFrame ( t ) <EOL> else : <EOL> frame = vis . showFrame ( t , obj . getProperty ( '<STR_LIT:Name>' ) + '<STR_LIT>' , parent = obj , scale = <NUM_LIT> , visible = False ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> def segmentTable ( polyData , searchPoint ) : <EOL> '''<STR_LIT>''' <EOL> expectedNormal = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> tableNormalEpsilon = <NUM_LIT> <EOL> polyData = applyVoxelGrid ( polyData , leafSize = <NUM_LIT> ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = expectedNormal , perpendicularAxis = expectedNormal , searchOrigin = searchPoint , searchRadius = <NUM_LIT> , angleEpsilon = tableNormalEpsilon , returnOrigin = True ) <EOL> tablePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> tablePoints = labelDistanceToPoint ( tablePoints , searchPoint ) <EOL> tablePointsClusters = extractClusters ( tablePoints , minClusterSize = <NUM_LIT:10> , clusterTolerance = <NUM_LIT:0.1> ) <EOL> tablePointsClusters . sort ( key = lambda x : vtkNumpy . getNumpyFromVtk ( x , '<STR_LIT>' ) . min ( ) ) <EOL> tablePoints = tablePointsClusters [ <NUM_LIT:0> ] <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> return polyData , tablePoints , origin , normal <EOL> def filterClusterObjects ( clusters ) : <EOL> result = [ ] <EOL> for cluster in clusters : <EOL> if np . abs ( np . dot ( cluster . axes [ <NUM_LIT:2> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) ) < <NUM_LIT:0.5> : <EOL> continue <EOL> if cluster . dims [ <NUM_LIT:2> ] < <NUM_LIT:0.1> : <EOL> continue <EOL> result . append ( cluster ) <EOL> return result <EOL> def segmentTableScene ( polyData , searchPoint , filterClustering = True ) : <EOL> objectClusters , tableData = segmentTableSceneClusters ( polyData , searchPoint ) <EOL> clusters = [ makePolyDataFields ( cluster ) for cluster in objectClusters ] <EOL> clusters = [ cluster for cluster in clusters if cluster is not None ] <EOL> table_axes = transformUtils . getAxesFromTransform ( tableData . frame ) <EOL> for cluster in clusters : <EOL> cluster_axes = transformUtils . getAxesFromTransform ( cluster . frame ) <EOL> zaxis = cluster_axes [ <NUM_LIT:2> ] <EOL> xaxis = table_axes [ <NUM_LIT:0> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> orientedFrame = transformUtils . getTransformFromAxesAndOrigin ( xaxis , yaxis , zaxis , cluster . frame . GetPosition ( ) ) <EOL> cluster . _add_fields ( oriented_frame = orientedFrame ) <EOL> if ( filterClustering ) : <EOL> clusters = filterClusterObjects ( clusters ) <EOL> return FieldContainer ( table = tableData , clusters = clusters ) <EOL> def segmentTableSceneClusters ( polyData , searchPoint , clusterInXY = False ) : <EOL> '''<STR_LIT>''' <EOL> tableData , polyData = segmentTableAndFrame ( polyData , searchPoint ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT:0.5> ] ) <EOL> searchRegion = cropToSphere ( searchRegion , tableData . frame . GetPosition ( ) , <NUM_LIT:0.5> ) <EOL> showFrame ( tableData . frame , '<STR_LIT>' , visible = False , parent = getDebugFolder ( ) , scale = <NUM_LIT> ) <EOL> showPolyData ( searchRegion , '<STR_LIT>' , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , visible = False , parent = getDebugFolder ( ) ) <EOL> objectClusters = extractClusters ( searchRegion , clusterInXY , clusterTolerance = <NUM_LIT> , minClusterSize = <NUM_LIT:10> ) <EOL> for i , c in enumerate ( objectClusters ) : <EOL> name = "<STR_LIT>" % i <EOL> showPolyData ( c , name , color = getRandomColor ( ) , visible = False , parent = getDebugFolder ( ) ) <EOL> return objectClusters , tableData <EOL> def segmentTableAndFrame ( polyData , searchPoint ) : <EOL> '''<STR_LIT>''' <EOL> polyData , tablePoints , _ , _ = segmentTable ( polyData , searchPoint ) <EOL> tableMesh = computeDelaunay3D ( tablePoints ) <EOL> viewFrame = SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> robotYaw = math . atan2 ( viewDirection [ <NUM_LIT:1> ] , viewDirection [ <NUM_LIT:0> ] ) * <NUM_LIT> / np . pi <EOL> linkFrame = transformUtils . frameFromPositionAndRPY ( viewFrame . GetPosition ( ) , [ <NUM_LIT:0> , <NUM_LIT:0> , robotYaw ] ) <EOL> cornerTransform , rectDepth , rectWidth , _ = findMinimumBoundingRectangle ( tablePoints , linkFrame ) <EOL> rectHeight = <NUM_LIT> <EOL> t = transformUtils . copyFrame ( cornerTransform ) <EOL> t . PreMultiply ( ) <EOL> table_center = [ - rectDepth / <NUM_LIT:2> , rectWidth / <NUM_LIT:2> , <NUM_LIT:0> ] <EOL> t3 = transformUtils . frameFromPositionAndRPY ( table_center , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> t . Concatenate ( t3 ) <EOL> edgeLengths = [ rectDepth , rectWidth , rectHeight ] <EOL> tableXAxis , tableYAxis , tableZAxis = transformUtils . getAxesFromTransform ( t ) <EOL> axes = tableXAxis , tableYAxis , tableZAxis <EOL> wf = vtk . vtkOutlineSource ( ) <EOL> wf . SetBounds ( [ - rectDepth / <NUM_LIT:2> , rectDepth / <NUM_LIT:2> , - rectWidth / <NUM_LIT:2> , rectWidth / <NUM_LIT:2> , - rectHeight / <NUM_LIT:2> , rectHeight / <NUM_LIT:2> ] ) <EOL> wireframe = wf . GetOutput ( ) <EOL> tablePoints = transformPolyData ( tablePoints , t . GetLinearInverse ( ) ) <EOL> tableMesh = transformPolyData ( tableMesh , t . GetLinearInverse ( ) ) <EOL> return FieldContainer ( points = tablePoints , box = wireframe , mesh = tableMesh , frame = t , dims = edgeLengths , axes = axes ) , polyData <EOL> def segmentDrillAuto ( point1 , polyData = None ) : <EOL> if polyData is None : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> expectedNormal = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , expectedNormal = expectedNormal , perpendicularAxis = expectedNormal , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> tablePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> tablePoints = labelDistanceToPoint ( tablePoints , point1 ) <EOL> tablePointsClusters = extractClusters ( tablePoints ) <EOL> tablePointsClusters . sort ( key = lambda x : vtkNumpy . getNumpyFromVtk ( x , '<STR_LIT>' ) . min ( ) ) <EOL> tablePoints = tablePointsClusters [ <NUM_LIT:0> ] <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT> ] ) <EOL> searchRegion = cropToSphere ( searchRegion , point1 , <NUM_LIT> ) <EOL> drillPoints = extractLargestCluster ( searchRegion , minClusterSize = <NUM_LIT:1> ) <EOL> centroids = computeCentroids ( drillPoints , axis = normal ) <EOL> centroidsPolyData = vtkNumpy . getVtkPolyDataFromNumpyPoints ( centroids ) <EOL> d = DebugData ( ) <EOL> updatePolyData ( centroidsPolyData , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> drillToTopPoint = np . array ( [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> ] ) <EOL> zaxis = normal <EOL> yaxis = centroids [ <NUM_LIT:0> ] - centroids [ - <NUM_LIT:1> ] <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> t = getTransformFromAxes ( yaxis , - xaxis , zaxis ) <EOL> t . PreMultiply ( ) <EOL> t . Translate ( - drillToTopPoint ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( centroids [ - <NUM_LIT:1> ] ) <EOL> drillMesh = getDrillMesh ( ) <EOL> aff = showPolyData ( drillMesh , '<STR_LIT>' , cls = FrameAffordanceItem , visible = True ) <EOL> aff . actor . SetUserTransform ( t ) <EOL> showFrame ( t , '<STR_LIT>' , parent = aff , visible = False , scale = <NUM_LIT> ) . addToView ( app . getDRCView ( ) ) <EOL> params = getDrillAffordanceParams ( origin , xaxis , yaxis , zaxis ) <EOL> aff . setAffordanceParams ( params ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> def segmentDrillButton ( point1 ) : <EOL> d = DebugData ( ) <EOL> d . addSphere ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , radius = <NUM_LIT> ) <EOL> obj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:0.5> , <NUM_LIT:0.5> ] , visible = True ) <EOL> pointerTipFrame = transformUtils . frameFromPositionAndRPY ( point1 , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> obj . actor . SetUserTransform ( pointerTipFrame ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> frameObj = updateFrame ( obj . actor . GetUserTransform ( ) , '<STR_LIT>' , parent = obj , scale = <NUM_LIT> , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> def segmentPointerTip ( point1 ) : <EOL> d = DebugData ( ) <EOL> d . addSphere ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , radius = <NUM_LIT> ) <EOL> obj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , color = [ <NUM_LIT:0.5> , <NUM_LIT:0.5> , <NUM_LIT:0.0> ] , visible = True ) <EOL> pointerTipFrame = transformUtils . frameFromPositionAndRPY ( point1 , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> obj . actor . SetUserTransform ( pointerTipFrame ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> frameObj = updateFrame ( obj . actor . GetUserTransform ( ) , '<STR_LIT>' , parent = obj , scale = <NUM_LIT> , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> def fitGroundObject ( polyData = None , expectedDimensionsMin = [ <NUM_LIT> , <NUM_LIT> ] , expectedDimensionsMax = [ <NUM_LIT> , <NUM_LIT:0.1> ] ) : <EOL> removeGroundFunc = removeGroundSimple <EOL> polyData = polyData or getCurrentRevolutionData ( ) <EOL> groundPoints , scenePoints = removeGroundFunc ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) <EOL> searchRegion = thresholdPoints ( scenePoints , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT> ] ) <EOL> clusters = extractClusters ( searchRegion , clusterTolerance = <NUM_LIT> , minClusterSize = <NUM_LIT:4> ) <EOL> candidates = [ ] <EOL> for clusterId , cluster in enumerate ( clusters ) : <EOL> origin , edges , _ = getOrientedBoundingBox ( cluster ) <EOL> edgeLengths = [ np . linalg . norm ( edge ) for edge in edges [ : <NUM_LIT:2> ] ] <EOL> found = ( expectedDimensionsMin [ <NUM_LIT:0> ] <= edgeLengths [ <NUM_LIT:0> ] < expectedDimensionsMax [ <NUM_LIT:0> ] <EOL> and expectedDimensionsMin [ <NUM_LIT:1> ] <= edgeLengths [ <NUM_LIT:1> ] < expectedDimensionsMax [ <NUM_LIT:1> ] ) <EOL> if not found : <EOL> updatePolyData ( cluster , '<STR_LIT>' % clusterId , color = [ <NUM_LIT:1> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = False ) <EOL> continue <EOL> updatePolyData ( cluster , '<STR_LIT>' % clusterId , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = False ) <EOL> candidates . append ( cluster ) <EOL> if not candidates : <EOL> return None <EOL> viewFrame = SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) <EOL> viewOrigin = np . array ( viewFrame . GetPosition ( ) ) <EOL> dists = [ np . linalg . norm ( viewOrigin - computeCentroid ( cluster ) ) for cluster in candidates ] <EOL> candidates = [ candidates [ i ] for i in np . argsort ( dists ) ] <EOL> cluster = candidates [ <NUM_LIT:0> ] <EOL> obj = makePolyDataFields ( cluster ) <EOL> return vis . showClusterObjects ( [ obj ] , parent = '<STR_LIT>' ) [ <NUM_LIT:0> ] <EOL> def findHorizontalSurfaces ( polyData , removeGroundFirst = False , normalEstimationSearchRadius = <NUM_LIT> , <EOL> clusterTolerance = <NUM_LIT> , minClusterSize = <NUM_LIT> , distanceToPlaneThreshold = <NUM_LIT> , normalsDotUpRange = [ <NUM_LIT> , <NUM_LIT:1.0> ] , showClusters = False ) : <EOL> '''<STR_LIT>''' <EOL> searchZ = [ <NUM_LIT:0.0> , <NUM_LIT> ] <EOL> voxelGridLeafSize = <NUM_LIT> <EOL> verboseFlag = False <EOL> if ( removeGroundFirst ) : <EOL> groundPoints , scenePoints = removeGround ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) <EOL> scenePoints = thresholdPoints ( scenePoints , '<STR_LIT>' , searchZ ) <EOL> updatePolyData ( groundPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = verboseFlag ) <EOL> else : <EOL> scenePoints = polyData <EOL> if not scenePoints . GetNumberOfPoints ( ) : <EOL> return <EOL> f = vtk . vtkPCLNormalEstimation ( ) <EOL> f . SetSearchRadius ( normalEstimationSearchRadius ) <EOL> f . SetInput ( scenePoints ) <EOL> f . SetInput ( <NUM_LIT:1> , applyVoxelGrid ( scenePoints , voxelGridLeafSize ) ) <EOL> f . Update ( ) <EOL> scenePoints = shallowCopy ( f . GetOutput ( ) ) <EOL> normals = vtkNumpy . getNumpyFromVtk ( scenePoints , '<STR_LIT>' ) <EOL> normalsDotUp = np . abs ( np . dot ( normals , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) ) <EOL> vtkNumpy . addNumpyToVtk ( scenePoints , normalsDotUp , '<STR_LIT>' ) <EOL> surfaces = thresholdPoints ( scenePoints , '<STR_LIT>' , normalsDotUpRange ) <EOL> updatePolyData ( scenePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = verboseFlag ) <EOL> updatePolyData ( surfaces , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = verboseFlag ) <EOL> clusters = extractClusters ( surfaces , clusterTolerance = clusterTolerance , minClusterSize = minClusterSize ) <EOL> planeClusters = [ ] <EOL> clustersLarge = [ ] <EOL> om . removeFromObjectModel ( om . findObjectByName ( '<STR_LIT>' ) ) <EOL> folder = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = getDebugFolder ( ) ) <EOL> for i , cluster in enumerate ( clusters ) : <EOL> updatePolyData ( cluster , '<STR_LIT>' % i , parent = folder , color = getRandomColor ( ) , visible = verboseFlag ) <EOL> planePoints , _ = applyPlaneFit ( cluster , distanceToPlaneThreshold ) <EOL> planePoints = thresholdPoints ( planePoints , '<STR_LIT>' , [ - distanceToPlaneThreshold , distanceToPlaneThreshold ] ) <EOL> if planePoints . GetNumberOfPoints ( ) > minClusterSize : <EOL> clustersLarge . append ( cluster ) <EOL> obj = makePolyDataFields ( planePoints ) <EOL> if obj is not None : <EOL> planeClusters . append ( obj ) <EOL> folder = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = getDebugFolder ( ) ) <EOL> if showClusters : <EOL> vis . showClusterObjects ( planeClusters , parent = folder ) <EOL> return clustersLarge <EOL> def fitVerticalPosts ( polyData ) : <EOL> groundPoints , scenePoints = removeGround ( polyData ) <EOL> scenePoints = thresholdPoints ( scenePoints , '<STR_LIT>' , [ <NUM_LIT:0.1> , <NUM_LIT> ] ) <EOL> if not scenePoints . GetNumberOfPoints ( ) : <EOL> return <EOL> scenePoints = applyVoxelGrid ( scenePoints , leafSize = <NUM_LIT> ) <EOL> clusters = extractClusters ( scenePoints , clusterTolerance = <NUM_LIT> , minClusterSize = <NUM_LIT:10> ) <EOL> def isPostCluster ( cluster , lineDirection ) : <EOL> up = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> minPostLength = <NUM_LIT:1.0> <EOL> maxRadius = <NUM_LIT> <EOL> angle = math . degrees ( math . acos ( np . dot ( up , lineDirection ) / ( np . linalg . norm ( up ) * np . linalg . norm ( lineDirection ) ) ) ) <EOL> if angle > <NUM_LIT:15> : <EOL> return False <EOL> origin , edges , _ = getOrientedBoundingBox ( cluster ) <EOL> edgeLengths = [ np . linalg . norm ( edge ) for edge in edges ] <EOL> if edgeLengths [ <NUM_LIT:0> ] < minPostLength : <EOL> return False <EOL> zvalues = vtkNumpy . getNumpyFromVtk ( cluster , '<STR_LIT>' ) [ : , <NUM_LIT:2> ] . copy ( ) <EOL> vtkNumpy . addNumpyToVtk ( cluster , zvalues , '<STR_LIT:z>' ) <EOL> minZ = np . min ( zvalues ) <EOL> maxZ = np . max ( zvalues ) <EOL> cluster = thresholdPoints ( cluster , '<STR_LIT:z>' , [ ( minZ + maxZ ) / <NUM_LIT> , maxZ ] ) <EOL> origin , edges , _ = getOrientedBoundingBox ( cluster ) <EOL> edgeLengths = [ np . linalg . norm ( edge ) for edge in edges ] <EOL> if edgeLengths [ <NUM_LIT:1> ] > maxRadius or edgeLengths [ <NUM_LIT:2> ] > maxRadius : <EOL> return False <EOL> return True <EOL> def makeCylinderAffordance ( linePoints , lineDirection , lineOrigin , postId ) : <EOL> pts = vtkNumpy . getNumpyFromVtk ( linePoints , '<STR_LIT>' ) <EOL> dists = np . dot ( pts - lineOrigin , lineDirection ) <EOL> p1 = lineOrigin + lineDirection * np . min ( dists ) <EOL> p2 = lineOrigin + lineDirection * np . max ( dists ) <EOL> origin = ( p1 + p2 ) / <NUM_LIT> <EOL> lineLength = np . linalg . norm ( p2 - p1 ) <EOL> t = transformUtils . getTransformFromOriginAndNormal ( origin , lineDirection ) <EOL> pose = transformUtils . poseFromTransform ( t ) <EOL> desc = dict ( classname = '<STR_LIT>' , Name = '<STR_LIT>' % postId , <EOL> uuid = newUUID ( ) , pose = pose , Radius = <NUM_LIT> , Length = float ( lineLength ) , Color = [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] ) <EOL> desc [ '<STR_LIT>' ] = True <EOL> return affordanceManager . newAffordanceFromDescription ( desc ) <EOL> rejectFolder = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = getDebugFolder ( ) ) <EOL> keepFolder = om . getOrCreateContainer ( '<STR_LIT>' , parentObj = getDebugFolder ( ) ) <EOL> for i , cluster in enumerate ( clusters ) : <EOL> linePoint , lineDirection , linePoints = applyLineFit ( cluster , distanceThreshold = <NUM_LIT:0.1> ) <EOL> if isPostCluster ( cluster , lineDirection ) : <EOL> vis . showPolyData ( cluster , '<STR_LIT>' % i , visible = False , color = getRandomColor ( ) , alpha = <NUM_LIT:0.5> , parent = keepFolder ) <EOL> makeCylinderAffordance ( linePoints , lineDirection , linePoint , i ) <EOL> else : <EOL> vis . showPolyData ( cluster , '<STR_LIT>' % i , visible = False , color = getRandomColor ( ) , alpha = <NUM_LIT:0.5> , parent = rejectFolder ) <EOL> def findAndFitDrillBarrel ( polyData = None ) : <EOL> '''<STR_LIT>''' <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = polyData or inputObj . polyData <EOL> groundPoints , scenePoints = removeGround ( polyData , groundThickness = <NUM_LIT> , sceneHeightFromGround = <NUM_LIT> ) <EOL> scenePoints = thresholdPoints ( scenePoints , '<STR_LIT>' , [ <NUM_LIT:0.5> , <NUM_LIT> ] ) <EOL> if not scenePoints . GetNumberOfPoints ( ) : <EOL> return <EOL> normalEstimationSearchRadius = <NUM_LIT> <EOL> f = vtk . vtkPCLNormalEstimation ( ) <EOL> f . SetSearchRadius ( normalEstimationSearchRadius ) <EOL> f . SetInput ( scenePoints ) <EOL> f . Update ( ) <EOL> scenePoints = shallowCopy ( f . GetOutput ( ) ) <EOL> normals = vtkNumpy . getNumpyFromVtk ( scenePoints , '<STR_LIT>' ) <EOL> normalsDotUp = np . abs ( np . dot ( normals , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) ) <EOL> vtkNumpy . addNumpyToVtk ( scenePoints , normalsDotUp , '<STR_LIT>' ) <EOL> surfaces = thresholdPoints ( scenePoints , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT:1.0> ] ) <EOL> updatePolyData ( groundPoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> updatePolyData ( scenePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> updatePolyData ( surfaces , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> clusters = extractClusters ( surfaces , clusterTolerance = <NUM_LIT> , minClusterSize = <NUM_LIT:50> ) <EOL> fitResults = [ ] <EOL> viewFrame = SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) <EOL> forwardDirection = np . array ( [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> viewFrame . TransformVector ( forwardDirection , forwardDirection ) <EOL> robotOrigin = viewFrame . GetPosition ( ) <EOL> robotForward = forwardDirection <EOL> centroid = [ ] <EOL> for clusterId , cluster in enumerate ( clusters ) : <EOL> clusterObj = updatePolyData ( cluster , '<STR_LIT>' % clusterId , color = [ <NUM_LIT:1> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = False ) <EOL> origin , edges , _ = getOrientedBoundingBox ( cluster ) <EOL> edgeLengths = [ np . linalg . norm ( edge ) for edge in edges [ : <NUM_LIT:2> ] ] <EOL> skipCluster = False <EOL> for edgeLength in edgeLengths : <EOL> if edgeLength < <NUM_LIT> or edgeLength > <NUM_LIT> : <EOL> skipCluster = True <EOL> if skipCluster : <EOL> continue <EOL> clusterObj . setSolidColor ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) <EOL> centroid = np . average ( vtkNumpy . getNumpyFromVtk ( cluster , '<STR_LIT>' ) , axis = <NUM_LIT:0> ) <EOL> try : <EOL> drillFrame = segmentDrillBarrelFrame ( centroid , polyData = scenePoints , forwardDirection = robotForward ) <EOL> if drillFrame is not None : <EOL> fitResults . append ( ( clusterObj , drillFrame ) ) <EOL> except : <EOL> print traceback . format_exc ( ) <EOL> print '<STR_LIT>' , clusterId <EOL> if not fitResults : <EOL> return <EOL> sortFittedDrills ( fitResults , robotOrigin , robotForward ) <EOL> return centroid <EOL> def sortFittedDrills ( fitResults , robotOrigin , robotForward ) : <EOL> angleToFitResults = [ ] <EOL> for fitResult in fitResults : <EOL> cluster , drillFrame = fitResult <EOL> drillOrigin = np . array ( drillFrame . GetPosition ( ) ) <EOL> angleToDrill = np . abs ( computeSignedAngleBetweenVectors ( robotForward , drillOrigin - robotOrigin , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) ) <EOL> angleToFitResults . append ( ( angleToDrill , cluster , drillFrame ) ) <EOL> angleToFitResults . sort ( key = lambda x : x [ <NUM_LIT:0> ] ) <EOL> drillMesh = getDrillBarrelMesh ( ) <EOL> for i , fitResult in enumerate ( angleToFitResults ) : <EOL> angleToDrill , cluster , drillFrame = fitResult <EOL> if i == <NUM_LIT:0> : <EOL> drill = om . findObjectByName ( '<STR_LIT>' ) <EOL> drill = updatePolyData ( drillMesh , '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , cls = FrameAffordanceItem , visible = True ) <EOL> drillFrame = updateFrame ( drillFrame , '<STR_LIT>' , parent = drill , visible = False ) <EOL> drill . actor . SetUserTransform ( drillFrame . transform ) <EOL> drill . setAffordanceParams ( dict ( otdf_type = '<STR_LIT>' , friendly_name = '<STR_LIT>' ) ) <EOL> drill . updateParamsFromActorTransform ( ) <EOL> drill . setSolidColor ( [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> else : <EOL> drill = showPolyData ( drillMesh , '<STR_LIT>' , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , visible = False , parent = getDebugFolder ( ) ) <EOL> drill . actor . SetUserTransform ( drillFrame ) <EOL> om . addToObjectModel ( drill , parentObj = getDebugFolder ( ) ) <EOL> def computeSignedAngleBetweenVectors ( v1 , v2 , perpendicularVector ) : <EOL> '''<STR_LIT>''' <EOL> v1 = np . array ( v1 ) <EOL> v2 = np . array ( v2 ) <EOL> perpendicularVector = np . array ( perpendicularVector ) <EOL> v1 /= np . linalg . norm ( v1 ) <EOL> v2 /= np . linalg . norm ( v2 ) <EOL> perpendicularVector /= np . linalg . norm ( perpendicularVector ) <EOL> return math . atan2 ( np . dot ( perpendicularVector , np . cross ( v1 , v2 ) ) , np . dot ( v1 , v2 ) ) <EOL> def segmentDrillBarrelFrame ( point1 , polyData , forwardDirection ) : <EOL> tableClusterSearchRadius = <NUM_LIT> <EOL> drillClusterSearchRadius = <NUM_LIT:0.5> <EOL> expectedNormal = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> if not polyData . GetNumberOfPoints ( ) : <EOL> return <EOL> polyData , plane_origin , plane_normal = applyPlaneFit ( polyData , expectedNormal = expectedNormal , <EOL> perpendicularAxis = expectedNormal , searchOrigin = point1 , <EOL> searchRadius = tableClusterSearchRadius , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> if not polyData . GetNumberOfPoints ( ) : <EOL> return <EOL> tablePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> tablePoints = labelDistanceToPoint ( tablePoints , point1 ) <EOL> tablePointsClusters = extractClusters ( tablePoints ) <EOL> tablePointsClusters . sort ( key = lambda x : vtkNumpy . getNumpyFromVtk ( x , '<STR_LIT>' ) . min ( ) ) <EOL> if not tablePointsClusters : <EOL> return <EOL> tablePoints = tablePointsClusters [ <NUM_LIT:0> ] <EOL> updatePolyData ( tablePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> searchRegion = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT> ] ) <EOL> if not searchRegion . GetNumberOfPoints ( ) : <EOL> return <EOL> searchRegion = cropToSphere ( searchRegion , point1 , drillClusterSearchRadius ) <EOL> t = fitDrillBarrel ( searchRegion , forwardDirection , plane_origin , plane_normal ) <EOL> return t <EOL> def segmentDrillBarrel ( point1 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> forwardDirection = - np . array ( getCurrentView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> t = segmentDrillBarrel ( point1 , polyData , forwardDirection ) <EOL> assert t is not None <EOL> drillMesh = getDrillBarrelMesh ( ) <EOL> aff = showPolyData ( drillMesh , '<STR_LIT>' , visible = True ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> aff . actor . SetUserTransform ( t ) <EOL> drillFrame = showFrame ( t , '<STR_LIT>' , parent = aff , visible = False ) <EOL> drillFrame . addToView ( app . getDRCView ( ) ) <EOL> return aff , drillFrame <EOL> def segmentDrillAlignedWithTable ( point , polyData = None ) : <EOL> '''<STR_LIT>''' <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = polyData or inputObj . polyData <EOL> polyDataOut , tablePoints , origin , normal = segmentTable ( polyData , point ) <EOL> OBBorigin , edges , _ = getOrientedBoundingBox ( tablePoints ) <EOL> edgeLengths = np . array ( [ np . linalg . norm ( edge ) for edge in edges ] ) <EOL> axes = [ edge / np . linalg . norm ( edge ) for edge in edges ] <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> if np . dot ( axes [ <NUM_LIT:1> ] , viewDirection ) < <NUM_LIT:0> : <EOL> axes [ <NUM_LIT:1> ] = - axes [ <NUM_LIT:1> ] <EOL> xaxis = axes [ <NUM_LIT:1> ] <EOL> xaxis = np . array ( xaxis ) <EOL> zaxis = np . array ( normal ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> tableOrientation = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> data = segmentTableScene ( polyData , point ) <EOL> drillTransform = transformUtils . frameFromPositionAndRPY ( data . clusters [ <NUM_LIT:0> ] . frame . GetPosition ( ) , tableOrientation . GetOrientation ( ) ) <EOL> drillMesh = getDrillMesh ( ) <EOL> drill = om . findObjectByName ( '<STR_LIT>' ) <EOL> om . removeFromObjectModel ( drill ) <EOL> aff = showPolyData ( drillMesh , '<STR_LIT>' , color = [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] , cls = FrameAffordanceItem , visible = True ) <EOL> aff . actor . SetUserTransform ( drillTransform ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> frameObj = updateFrame ( drillTransform , '<STR_LIT>' , parent = aff , scale = <NUM_LIT> , visible = False ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> params = getDrillAffordanceParams ( np . array ( drillTransform . GetPosition ( ) ) , [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] , drillType = "<STR_LIT>" ) <EOL> aff . setAffordanceParams ( params ) <EOL> def segmentDrillInHand ( p1 , p2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> distanceToLineThreshold = <NUM_LIT> <EOL> polyData = labelDistanceToLine ( polyData , p1 , p2 ) <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0.0> , distanceToLineThreshold ] ) <EOL> lineSegment = p2 - p1 <EOL> lineLength = np . linalg . norm ( lineSegment ) <EOL> cropped , polyData = cropToPlane ( polyData , p1 , lineSegment / lineLength , [ - <NUM_LIT> , lineLength + <NUM_LIT> ] ) <EOL> updatePolyData ( cropped , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> drillPoints = cropped <EOL> normal = lineSegment / lineLength <EOL> centroids = computeCentroids ( drillPoints , axis = normal ) <EOL> centroidsPolyData = vtkNumpy . getVtkPolyDataFromNumpyPoints ( centroids ) <EOL> d = DebugData ( ) <EOL> updatePolyData ( centroidsPolyData , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> drillToTopPoint = np . array ( [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> ] ) <EOL> zaxis = normal <EOL> yaxis = centroids [ <NUM_LIT:0> ] - centroids [ - <NUM_LIT:1> ] <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PreMultiply ( ) <EOL> t . Translate ( - drillToTopPoint ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( p2 ) <EOL> drillMesh = getDrillMesh ( ) <EOL> aff = showPolyData ( drillMesh , '<STR_LIT>' , cls = FrameAffordanceItem , visible = True ) <EOL> aff . actor . SetUserTransform ( t ) <EOL> showFrame ( t , '<STR_LIT>' , parent = aff , visible = False ) . addToView ( app . getDRCView ( ) ) <EOL> params = getDrillAffordanceParams ( np . array ( t . GetPosition ( ) ) , xaxis , yaxis , zaxis ) <EOL> aff . setAffordanceParams ( params ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> def addDrillAffordance ( ) : <EOL> drillMesh = getDrillMesh ( ) <EOL> aff = showPolyData ( drillMesh , '<STR_LIT>' , cls = FrameAffordanceItem , visible = True ) <EOL> t = vtk . vtkTransform ( ) <EOL> t . PostMultiply ( ) <EOL> aff . actor . SetUserTransform ( t ) <EOL> showFrame ( t , '<STR_LIT>' , parent = aff , visible = False ) . addToView ( app . getDRCView ( ) ) <EOL> params = getDrillAffordanceParams ( np . array ( t . GetPosition ( ) ) , [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) <EOL> aff . setAffordanceParams ( params ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . addToView ( app . getDRCView ( ) ) <EOL> return aff <EOL> def getLinkFrame ( linkName ) : <EOL> robotStateModel = om . findObjectByName ( '<STR_LIT>' ) <EOL> robotStateModel = robotStateModel or getVisibleRobotModel ( ) <EOL> assert robotStateModel <EOL> t = vtk . vtkTransform ( ) <EOL> robotStateModel . model . getLinkToWorld ( linkName , t ) <EOL> return t <EOL> def getDrillInHandOffset ( zRotation = <NUM_LIT:0.0> , zTranslation = <NUM_LIT:0.0> , xTranslation = <NUM_LIT:0.0> , yTranslation = <NUM_LIT:0.0> , flip = False ) : <EOL> drillOffset = vtk . vtkTransform ( ) <EOL> drillOffset . PostMultiply ( ) <EOL> if flip : <EOL> drillOffset . RotateY ( <NUM_LIT> ) <EOL> drillOffset . RotateZ ( zRotation ) <EOL> drillOffset . RotateY ( - <NUM_LIT> ) <EOL> drillOffset . Translate ( zTranslation , xTranslation , <NUM_LIT:0.0> + yTranslation ) <EOL> return drillOffset <EOL> def moveDrillToHand ( drillOffset , hand = '<STR_LIT:right>' ) : <EOL> drill = om . findObjectByName ( '<STR_LIT>' ) <EOL> if not drill : <EOL> drill = addDrillAffordance ( ) <EOL> assert hand in ( '<STR_LIT:right>' , '<STR_LIT:left>' ) <EOL> drillTransform = drill . actor . GetUserTransform ( ) <EOL> rightBaseLink = getLinkFrame ( '<STR_LIT>' % hand [ <NUM_LIT:0> ] ) <EOL> drillTransform . PostMultiply ( ) <EOL> drillTransform . Identity ( ) <EOL> drillTransform . Concatenate ( drillOffset ) <EOL> drillTransform . Concatenate ( rightBaseLink ) <EOL> drill . _renderAllViews ( ) <EOL> class PointPicker ( TimerCallback ) : <EOL> def __init__ ( self , numberOfPoints = <NUM_LIT:3> ) : <EOL> TimerCallback . __init__ ( self ) <EOL> self . targetFps = <NUM_LIT:30> <EOL> self . enabled = False <EOL> self . numberOfPoints = numberOfPoints <EOL> self . annotationObj = None <EOL> self . drawLines = True <EOL> self . clear ( ) <EOL> def clear ( self ) : <EOL> self . points = [ None for i in xrange ( self . numberOfPoints ) ] <EOL> self . hoverPos = None <EOL> self . annotationFunc = None <EOL> self . lastMovePos = [ <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> def onMouseMove ( self , displayPoint , modifiers = None ) : <EOL> self . lastMovePos = displayPoint <EOL> def onMousePress ( self , displayPoint , modifiers = None ) : <EOL> for i in xrange ( self . numberOfPoints ) : <EOL> if self . points [ i ] is None : <EOL> self . points [ i ] = self . hoverPos <EOL> break <EOL> if self . points [ - <NUM_LIT:1> ] is not None : <EOL> self . finish ( ) <EOL> def finish ( self ) : <EOL> self . enabled = False <EOL> om . removeFromObjectModel ( self . annotationObj ) <EOL> points = [ p . copy ( ) for p in self . points ] <EOL> if self . annotationFunc is not None : <EOL> self . annotationFunc ( * points ) <EOL> removeViewPicker ( self ) <EOL> def handleRelease ( self , displayPoint ) : <EOL> pass <EOL> def draw ( self ) : <EOL> d = DebugData ( ) <EOL> points = [ p if p is not None else self . hoverPos for p in self . points ] <EOL> for p in points : <EOL> if p is not None : <EOL> d . addSphere ( p , radius = <NUM_LIT> ) <EOL> if self . drawLines : <EOL> for a , b in zip ( points , points [ <NUM_LIT:1> : ] ) : <EOL> if b is not None : <EOL> d . addLine ( a , b ) <EOL> if points [ - <NUM_LIT:1> ] is not None : <EOL> d . addLine ( points [ <NUM_LIT:0> ] , points [ - <NUM_LIT:1> ] ) <EOL> self . annotationObj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) ) <EOL> self . annotationObj . setProperty ( '<STR_LIT>' , QtGui . QColor ( <NUM_LIT:0> , <NUM_LIT:255> , <NUM_LIT:0> ) ) <EOL> self . annotationObj . actor . SetPickable ( False ) <EOL> def tick ( self ) : <EOL> if not self . enabled : <EOL> return <EOL> if not om . findObjectByName ( '<STR_LIT>' ) : <EOL> self . annotationFunc = None <EOL> self . finish ( ) <EOL> return <EOL> self . hoverPos = pickPoint ( self . lastMovePos , getSegmentationView ( ) , obj = '<STR_LIT>' ) <EOL> self . draw ( ) <EOL> class LineDraw ( TimerCallback ) : <EOL> def __init__ ( self , view ) : <EOL> TimerCallback . __init__ ( self ) <EOL> self . targetFps = <NUM_LIT:30> <EOL> self . enabled = False <EOL> self . view = view <EOL> self . renderer = view . renderer ( ) <EOL> self . line = vtk . vtkLeaderActor2D ( ) <EOL> self . line . SetArrowPlacementToNone ( ) <EOL> self . line . GetPositionCoordinate ( ) . SetCoordinateSystemToViewport ( ) <EOL> self . line . GetPosition2Coordinate ( ) . SetCoordinateSystemToViewport ( ) <EOL> self . line . GetProperty ( ) . SetLineWidth ( <NUM_LIT:4> ) <EOL> self . line . SetPosition ( <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> self . line . SetPosition2 ( <NUM_LIT:0> , <NUM_LIT:0> ) <EOL> self . clear ( ) <EOL> def clear ( self ) : <EOL> self . p1 = None <EOL> self . p2 = None <EOL> self . annotationFunc = None <EOL> self . lastMovePos = [ <NUM_LIT:0> , <NUM_LIT:0> ] <EOL> self . renderer . RemoveActor2D ( self . line ) <EOL> def onMouseMove ( self , displayPoint , modifiers = None ) : <EOL> self . lastMovePos = displayPoint <EOL> def onMousePress ( self , displayPoint , modifiers = None ) : <EOL> if self . p1 is None : <EOL> self . p1 = list ( self . lastMovePos ) <EOL> if self . p1 is not None : <EOL> self . renderer . AddActor2D ( self . line ) <EOL> else : <EOL> self . p2 = self . lastMovePos <EOL> self . finish ( ) <EOL> def finish ( self ) : <EOL> self . enabled = False <EOL> self . renderer . RemoveActor2D ( self . line ) <EOL> if self . annotationFunc is not None : <EOL> self . annotationFunc ( self . p1 , self . p2 ) <EOL> def handleRelease ( self , displayPoint ) : <EOL> pass <EOL> def tick ( self ) : <EOL> if not self . enabled : <EOL> return <EOL> if self . p1 : <EOL> self . line . SetPosition ( self . p1 ) <EOL> self . line . SetPosition2 ( self . lastMovePos ) <EOL> self . view . render ( ) <EOL> viewPickers = [ ] <EOL> def addViewPicker ( picker ) : <EOL> global viewPickers <EOL> viewPickers . append ( picker ) <EOL> def removeViewPicker ( picker ) : <EOL> global viewPickers <EOL> viewPickers . remove ( picker ) <EOL> def distanceToLine ( x0 , x1 , x2 ) : <EOL> numerator = np . sqrt ( np . sum ( np . cross ( ( x0 - x1 ) , ( x0 - x2 ) ) ** <NUM_LIT:2> ) ) <EOL> denom = np . linalg . norm ( x2 - x1 ) <EOL> return numerator / denom <EOL> def labelDistanceToLine ( polyData , linePoint1 , linePoint2 , resultArrayName = '<STR_LIT>' ) : <EOL> x0 = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> x1 = np . array ( linePoint1 ) <EOL> x2 = np . array ( linePoint2 ) <EOL> numerator = np . sqrt ( np . sum ( np . cross ( ( x0 - x1 ) , ( x0 - x2 ) ) ** <NUM_LIT:2> , axis = <NUM_LIT:1> ) ) <EOL> denom = np . linalg . norm ( x2 - x1 ) <EOL> dists = numerator / denom <EOL> polyData = shallowCopy ( polyData ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dists , resultArrayName ) <EOL> return polyData <EOL> def labelDistanceToPoint ( polyData , point , resultArrayName = '<STR_LIT>' ) : <EOL> assert polyData . GetNumberOfPoints ( ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> points = points - point <EOL> dists = np . sqrt ( np . sum ( points ** <NUM_LIT:2> , axis = <NUM_LIT:1> ) ) <EOL> polyData = shallowCopy ( polyData ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , dists , resultArrayName ) <EOL> return polyData <EOL> def getPlaneEquationFromPolyData ( polyData , expectedNormal ) : <EOL> _ , origin , normal = applyPlaneFit ( polyData , expectedNormal = expectedNormal , returnOrigin = True ) <EOL> return origin , normal , np . hstack ( ( normal , [ np . dot ( origin , normal ) ] ) ) <EOL> def computeEdge ( polyData , edgeAxis , perpAxis , binWidth = <NUM_LIT> ) : <EOL> polyData = labelPointDistanceAlongAxis ( polyData , edgeAxis , resultArrayName = '<STR_LIT>' ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , perpAxis , resultArrayName = '<STR_LIT>' ) <EOL> polyData , bins = binByScalar ( polyData , '<STR_LIT>' , binWidth ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> binLabels = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> distToEdge = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> numberOfBins = len ( bins ) - <NUM_LIT:1> <EOL> edgePoints = [ ] <EOL> for i in xrange ( numberOfBins ) : <EOL> binPoints = points [ binLabels == i ] <EOL> binDists = distToEdge [ binLabels == i ] <EOL> if len ( binDists ) : <EOL> edgePoints . append ( binPoints [ binDists . argmax ( ) ] ) <EOL> return np . array ( edgePoints ) <EOL> def computeCentroids ( polyData , axis , binWidth = <NUM_LIT> ) : <EOL> polyData = labelPointDistanceAlongAxis ( polyData , axis , resultArrayName = '<STR_LIT>' ) <EOL> polyData , bins = binByScalar ( polyData , '<STR_LIT>' , binWidth ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> binLabels = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> numberOfBins = len ( bins ) - <NUM_LIT:1> <EOL> centroids = [ ] <EOL> for i in xrange ( numberOfBins ) : <EOL> binPoints = points [ binLabels == i ] <EOL> if len ( binPoints ) : <EOL> centroids . append ( np . average ( binPoints , axis = <NUM_LIT:0> ) ) <EOL> return np . array ( centroids ) <EOL> def computePointCountsAlongAxis ( polyData , axis , binWidth = <NUM_LIT> ) : <EOL> polyData = labelPointDistanceAlongAxis ( polyData , axis , resultArrayName = '<STR_LIT>' ) <EOL> polyData , bins = binByScalar ( polyData , '<STR_LIT>' , binWidth ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> binLabels = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> numberOfBins = len ( bins ) - <NUM_LIT:1> <EOL> binCount = [ ] <EOL> for i in xrange ( numberOfBins ) : <EOL> binPoints = points [ binLabels == i ] <EOL> binCount . append ( len ( binPoints ) ) <EOL> return np . array ( binCount ) <EOL> def binByScalar ( lidarData , scalarArrayName , binWidth , binLabelsArrayName = '<STR_LIT>' ) : <EOL> '''<STR_LIT>''' <EOL> scalars = vtkNumpy . getNumpyFromVtk ( lidarData , scalarArrayName ) <EOL> bins = np . arange ( scalars . min ( ) , scalars . max ( ) + binWidth , binWidth ) <EOL> binLabels = np . digitize ( scalars , bins ) - <NUM_LIT:1> <EOL> assert ( len ( binLabels ) == len ( scalars ) ) <EOL> newData = shallowCopy ( lidarData ) <EOL> vtkNumpy . addNumpyToVtk ( newData , binLabels , binLabelsArrayName ) <EOL> return newData , bins <EOL> def showObbs ( polyData ) : <EOL> labelsArrayName = '<STR_LIT>' <EOL> assert polyData . GetPointData ( ) . GetArray ( labelsArrayName ) <EOL> f = vtk . vtkAnnotateOBBs ( ) <EOL> f . SetInputArrayToProcess ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , vtk . vtkDataObject . FIELD_ASSOCIATION_POINTS , labelsArrayName ) <EOL> f . SetInput ( polyData ) <EOL> f . Update ( ) <EOL> showPolyData ( f . GetOutput ( ) , '<STR_LIT>' ) <EOL> def getOrientedBoundingBox ( polyData ) : <EOL> '''<STR_LIT>''' <EOL> nPoints = polyData . GetNumberOfPoints ( ) <EOL> assert nPoints <EOL> polyData = shallowCopy ( polyData ) <EOL> labelsArrayName = '<STR_LIT>' <EOL> labels = np . ones ( nPoints ) <EOL> vtkNumpy . addNumpyToVtk ( polyData , labels , labelsArrayName ) <EOL> f = vtk . vtkAnnotateOBBs ( ) <EOL> f . SetInputArrayToProcess ( <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> , vtk . vtkDataObject . FIELD_ASSOCIATION_POINTS , labelsArrayName ) <EOL> f . SetInput ( polyData ) <EOL> f . Update ( ) <EOL> assert f . GetNumberOfBoundingBoxes ( ) == <NUM_LIT:1> <EOL> origin = np . zeros ( <NUM_LIT:3> ) <EOL> edges = [ np . zeros ( <NUM_LIT:3> ) for i in xrange ( <NUM_LIT:3> ) ] <EOL> f . GetBoundingBoxOrigin ( <NUM_LIT:0> , origin ) <EOL> for i in xrange ( <NUM_LIT:3> ) : <EOL> f . GetBoundingBoxEdge ( <NUM_LIT:0> , i , edges [ i ] ) <EOL> return origin , edges , shallowCopy ( f . GetOutput ( ) ) <EOL> def segmentBlockByAnnotation ( blockDimensions , p1 , p2 , p3 ) : <EOL> segmentationObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> segmentationObj . mapper . ScalarVisibilityOff ( ) <EOL> segmentationObj . setProperty ( '<STR_LIT>' , <NUM_LIT:2> ) <EOL> segmentationObj . setProperty ( '<STR_LIT>' , <NUM_LIT> ) <EOL> zedge = p2 - p1 <EOL> zaxis = zedge / np . linalg . norm ( zedge ) <EOL> xwidth , ywidth = blockDimensions <EOL> zwidth = np . linalg . norm ( zedge ) <EOL> yaxis = np . cross ( p2 - p1 , p3 - p1 ) <EOL> yaxis = yaxis / np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> viewPlaneNormal = getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) <EOL> if np . dot ( yaxis , viewPlaneNormal ) < <NUM_LIT:0> : <EOL> yaxis *= - <NUM_LIT:1> <EOL> if np . dot ( xaxis , p3 - p1 ) < <NUM_LIT:0> : <EOL> xaxis *= - <NUM_LIT:1> <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> origin = ( ( p1 + p2 ) / <NUM_LIT> ) + xaxis * xwidth / <NUM_LIT> + yaxis * ywidth / <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addSphere ( origin , radius = <NUM_LIT> ) <EOL> d . addLine ( origin - xaxis * xwidth / <NUM_LIT> , origin + xaxis * xwidth / <NUM_LIT> ) <EOL> d . addLine ( origin - yaxis * ywidth / <NUM_LIT> , origin + yaxis * ywidth / <NUM_LIT> ) <EOL> d . addLine ( origin - zaxis * zwidth / <NUM_LIT> , origin + zaxis * zwidth / <NUM_LIT> ) <EOL> obj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' ) <EOL> obj . setProperty ( '<STR_LIT>' , QtGui . QColor ( <NUM_LIT:255> , <NUM_LIT:255> , <NUM_LIT:0> ) ) <EOL> obj . setProperty ( '<STR_LIT>' , False ) <EOL> om . findObjectByName ( '<STR_LIT>' ) . setProperty ( '<STR_LIT>' , False ) <EOL> cube = vtk . vtkCubeSource ( ) <EOL> cube . SetXLength ( xwidth ) <EOL> cube . SetYLength ( ywidth ) <EOL> cube . SetZLength ( zwidth ) <EOL> cube . Update ( ) <EOL> cube = shallowCopy ( cube . GetOutput ( ) ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> obj = updatePolyData ( cube , '<STR_LIT>' , cls = BlockAffordanceItem , parent = '<STR_LIT>' ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = origin , xwidth = xwidth , ywidth = ywidth , zwidth = zwidth , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> def getBoardCorners ( params ) : <EOL> axes = [ np . array ( params [ axis ] ) for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> widths = [ np . array ( params [ axis ] ) / <NUM_LIT> for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> edges = [ axes [ i ] * widths [ i ] for i in xrange ( <NUM_LIT:3> ) ] <EOL> origin = np . array ( params [ '<STR_LIT>' ] ) <EOL> return [ <EOL> origin + edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin + edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] + edges [ <NUM_LIT:2> ] , <EOL> origin + edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] + edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> origin - edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> origin + edges [ <NUM_LIT:0> ] - edges [ <NUM_LIT:1> ] - edges [ <NUM_LIT:2> ] , <EOL> ] <EOL> def getPointDistances ( target , points ) : <EOL> return np . array ( [ np . linalg . norm ( target - p ) for p in points ] ) <EOL> def computeClosestCorner ( aff , referenceFrame ) : <EOL> corners = getBoardCorners ( aff . params ) <EOL> dists = getPointDistances ( np . array ( referenceFrame . GetPosition ( ) ) , corners ) <EOL> return corners [ dists . argmin ( ) ] <EOL> def computeGroundFrame ( aff , referenceFrame ) : <EOL> refAxis = [ <NUM_LIT:0.0> , - <NUM_LIT:1.0> , <NUM_LIT:0.0> ] <EOL> referenceFrame . TransformVector ( refAxis , refAxis ) <EOL> refAxis = np . array ( refAxis ) <EOL> axes = [ np . array ( aff . params [ axis ] ) for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> axisProjections = np . array ( [ np . abs ( np . dot ( axis , refAxis ) ) for axis in axes ] ) <EOL> boardAxis = axes [ axisProjections . argmax ( ) ] <EOL> if np . dot ( boardAxis , refAxis ) < <NUM_LIT:0> : <EOL> boardAxis = - boardAxis <EOL> xaxis = boardAxis <EOL> zaxis = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> closestCorner = computeClosestCorner ( aff , referenceFrame ) <EOL> groundFrame = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> groundFrame . PostMultiply ( ) <EOL> groundFrame . Translate ( closestCorner [ <NUM_LIT:0> ] , closestCorner [ <NUM_LIT:1> ] , <NUM_LIT:0.0> ) <EOL> return groundFrame <EOL> def computeCornerFrame ( aff , referenceFrame ) : <EOL> refAxis = [ <NUM_LIT:0.0> , - <NUM_LIT:1.0> , <NUM_LIT:0.0> ] <EOL> referenceFrame . TransformVector ( refAxis , refAxis ) <EOL> refAxis = np . array ( refAxis ) <EOL> axes = [ np . array ( aff . params [ axis ] ) for axis in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> edgeLengths = [ edgeLength for edgeLength in [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] ] <EOL> axisProjections = np . array ( [ np . abs ( np . dot ( axis , refAxis ) ) for axis in axes ] ) <EOL> boardAxis = axes [ axisProjections . argmax ( ) ] <EOL> if np . dot ( boardAxis , refAxis ) < <NUM_LIT:0> : <EOL> boardAxis = - boardAxis <EOL> longAxis = axes [ np . argmax ( edgeLengths ) ] <EOL> xaxis = boardAxis <EOL> yaxis = axes [ <NUM_LIT:2> ] <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> closestCorner = computeClosestCorner ( aff , referenceFrame ) <EOL> cornerFrame = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> cornerFrame . PostMultiply ( ) <EOL> cornerFrame . Translate ( closestCorner ) <EOL> return cornerFrame <EOL> def publishTriad ( transform , collectionId = <NUM_LIT> ) : <EOL> o = lcmvs . obj_t ( ) <EOL> xyz = transform . GetPosition ( ) <EOL> rpy = transformUtils . rollPitchYawFromTransform ( transform ) <EOL> o . roll , o . pitch , o . yaw = rpy <EOL> o . x , o . y , o . z = xyz <EOL> o . id = <NUM_LIT:1> <EOL> m = lcmvs . obj_collection_t ( ) <EOL> m . id = collectionId <EOL> m . name = '<STR_LIT>' <EOL> m . type = lcmvs . obj_collection_t . AXIS3D <EOL> m . nobjs = <NUM_LIT:1> <EOL> m . reset = False <EOL> m . objs = [ o ] <EOL> lcmUtils . publish ( '<STR_LIT>' , m ) <EOL> def createBlockAffordance ( origin , xaxis , yaxis , zaxis , xwidth , ywidth , zwidth , name , parent = '<STR_LIT>' ) : <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> obj = BoxAffordanceItem ( name , view = app . getCurrentRenderView ( ) ) <EOL> obj . setProperty ( '<STR_LIT>' , [ float ( v ) for v in [ xwidth , ywidth , zwidth ] ] ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> om . addToObjectModel ( obj , parentObj = om . getOrCreateContainer ( parent ) ) <EOL> frameObj = vis . showFrame ( t , name + '<STR_LIT>' , scale = <NUM_LIT> , visible = False , parent = obj ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> frameObj . addToView ( app . getDRCView ( ) ) <EOL> affordanceManager . registerAffordance ( obj ) <EOL> return obj <EOL> def segmentBlockByTopPlane ( polyData , blockDimensions , expectedNormal , expectedXAxis , edgeSign = <NUM_LIT:1> , name = '<STR_LIT>' ) : <EOL> polyData , planeOrigin , normal = applyPlaneFit ( polyData , distanceThreshold = <NUM_LIT> , expectedNormal = expectedNormal , returnOrigin = True ) <EOL> _ , lineDirection , _ = applyLineFit ( polyData ) <EOL> zaxis = lineDirection <EOL> yaxis = normal <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> if np . dot ( xaxis , expectedXAxis ) < <NUM_LIT:0> : <EOL> xaxis *= - <NUM_LIT:1> <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis /= np . linalg . norm ( zaxis ) <EOL> expectedXAxis = np . array ( xaxis ) <EOL> edgePoints = computeEdge ( polyData , zaxis , xaxis * edgeSign ) <EOL> edgePoints = vtkNumpy . getVtkPolyDataFromNumpyPoints ( edgePoints ) <EOL> d = DebugData ( ) <EOL> obj = updatePolyData ( edgePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> linePoint , lineDirection , _ = applyLineFit ( edgePoints ) <EOL> zaxis = lineDirection <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> if np . dot ( xaxis , expectedXAxis ) < <NUM_LIT:0> : <EOL> xaxis *= - <NUM_LIT:1> <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis /= np . linalg . norm ( zaxis ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , xaxis , resultArrayName = '<STR_LIT>' ) <EOL> pts = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dists = np . dot ( pts - linePoint , zaxis ) <EOL> p1 = linePoint + zaxis * np . min ( dists ) <EOL> p2 = linePoint + zaxis * np . max ( dists ) <EOL> p1 = projectPointToPlane ( p1 , planeOrigin , normal ) <EOL> p2 = projectPointToPlane ( p2 , planeOrigin , normal ) <EOL> xwidth , ywidth = blockDimensions <EOL> zwidth = np . linalg . norm ( p2 - p1 ) <EOL> origin = p1 - edgeSign * xaxis * xwidth / <NUM_LIT> - yaxis * ywidth / <NUM_LIT> + zaxis * zwidth / <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> d . addSphere ( origin , radius = <NUM_LIT> ) <EOL> d . addLine ( origin , origin + xaxis * xwidth / <NUM_LIT> ) <EOL> d . addLine ( origin , origin + yaxis * ywidth / <NUM_LIT> ) <EOL> d . addLine ( origin , origin + zaxis * zwidth / <NUM_LIT> ) <EOL> obj = createBlockAffordance ( origin , xaxis , yaxis , zaxis , xwidth , ywidth , zwidth , name ) <EOL> obj . setProperty ( '<STR_LIT>' , [ <NUM_LIT> / <NUM_LIT> , <NUM_LIT> / <NUM_LIT> , <NUM_LIT> / <NUM_LIT> ] ) <EOL> computeDebrisGraspSeed ( obj ) <EOL> t = computeDebrisStanceFrame ( obj ) <EOL> if t : <EOL> showFrame ( t , '<STR_LIT>' , parent = obj ) <EOL> obj . publishCallback = functools . partial ( publishDebrisStanceFrame , obj ) <EOL> return obj <EOL> def computeDebrisGraspSeed ( aff ) : <EOL> debrisReferenceFrame = om . findObjectByName ( '<STR_LIT>' ) <EOL> if debrisReferenceFrame : <EOL> debrisReferenceFrame = debrisReferenceFrame . transform <EOL> affCornerFrame = computeCornerFrame ( aff , debrisReferenceFrame ) <EOL> showFrame ( affCornerFrame , '<STR_LIT>' , parent = aff , visible = False ) <EOL> def computeDebrisStanceFrame ( aff ) : <EOL> debrisReferenceFrame = om . findObjectByName ( '<STR_LIT>' ) <EOL> debrisWallEdge = om . findObjectByName ( '<STR_LIT>' ) <EOL> if debrisReferenceFrame and debrisWallEdge : <EOL> debrisReferenceFrame = debrisReferenceFrame . transform <EOL> affGroundFrame = computeGroundFrame ( aff , debrisReferenceFrame ) <EOL> updateFrame ( affGroundFrame , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> affWallEdge = computeGroundFrame ( aff , debrisReferenceFrame ) <EOL> framePos = np . array ( affGroundFrame . GetPosition ( ) ) <EOL> p1 , p2 = debrisWallEdge . points <EOL> edgeAxis = p2 - p1 <EOL> edgeAxis /= np . linalg . norm ( edgeAxis ) <EOL> projectedPos = p1 + edgeAxis * np . dot ( framePos - p1 , edgeAxis ) <EOL> affWallFrame = vtk . vtkTransform ( ) <EOL> affWallFrame . PostMultiply ( ) <EOL> useWallFrameForRotation = True <EOL> if useWallFrameForRotation : <EOL> affWallFrame . SetMatrix ( debrisReferenceFrame . GetMatrix ( ) ) <EOL> affWallFrame . Translate ( projectedPos - np . array ( debrisReferenceFrame . GetPosition ( ) ) ) <EOL> stanceWidth = <NUM_LIT> <EOL> stanceOffsetX = - <NUM_LIT> <EOL> stanceOffsetY = <NUM_LIT> <EOL> stanceRotation = <NUM_LIT:0.0> <EOL> else : <EOL> affWallFrame . SetMatrix ( affGroundFrame . GetMatrix ( ) ) <EOL> affWallFrame . Translate ( projectedPos - framePos ) <EOL> stanceWidth = <NUM_LIT> <EOL> stanceOffsetX = - <NUM_LIT> <EOL> stanceOffsetY = - <NUM_LIT> <EOL> stanceRotation = math . pi / <NUM_LIT> <EOL> stanceFrame , _ , _ = getFootFramesFromReferenceFrame ( affWallFrame , stanceWidth , math . degrees ( stanceRotation ) , [ stanceOffsetX , stanceOffsetY , <NUM_LIT:0.0> ] ) <EOL> return stanceFrame <EOL> def publishDebrisStanceFrame ( aff ) : <EOL> frame = computeDebrisStanceFrame ( aff ) <EOL> publishTriad ( frame ) <EOL> def segmentBlockByPlanes ( blockDimensions ) : <EOL> planes = om . findObjectByName ( '<STR_LIT>' ) . children ( ) [ : <NUM_LIT:2> ] <EOL> viewPlaneNormal = getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) <EOL> origin1 , normal1 , plane1 = getPlaneEquationFromPolyData ( planes [ <NUM_LIT:0> ] . polyData , expectedNormal = viewPlaneNormal ) <EOL> origin2 , normal2 , plane2 = getPlaneEquationFromPolyData ( planes [ <NUM_LIT:1> ] . polyData , expectedNormal = viewPlaneNormal ) <EOL> xaxis = normal2 <EOL> yaxis = normal1 <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> pts1 = vtkNumpy . getNumpyFromVtk ( planes [ <NUM_LIT:0> ] . polyData , '<STR_LIT>' ) <EOL> pts2 = vtkNumpy . getNumpyFromVtk ( planes [ <NUM_LIT:1> ] . polyData , '<STR_LIT>' ) <EOL> linePoint = np . zeros ( <NUM_LIT:3> ) <EOL> centroid2 = np . sum ( pts2 , axis = <NUM_LIT:0> ) / len ( pts2 ) <EOL> vtk . vtkPlane . ProjectPoint ( centroid2 , origin1 , normal1 , linePoint ) <EOL> dists = np . dot ( pts1 - linePoint , zaxis ) <EOL> p1 = linePoint + zaxis * np . min ( dists ) <EOL> p2 = linePoint + zaxis * np . max ( dists ) <EOL> xwidth , ywidth = blockDimensions <EOL> zwidth = np . linalg . norm ( p2 - p1 ) <EOL> origin = p1 + xaxis * xwidth / <NUM_LIT> + yaxis * ywidth / <NUM_LIT> + zaxis * zwidth / <NUM_LIT> <EOL> d = DebugData ( ) <EOL> d . addSphere ( linePoint , radius = <NUM_LIT> ) <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> d . addSphere ( origin , radius = <NUM_LIT> ) <EOL> d . addLine ( origin - xaxis * xwidth / <NUM_LIT> , origin + xaxis * xwidth / <NUM_LIT> ) <EOL> d . addLine ( origin - yaxis * ywidth / <NUM_LIT> , origin + yaxis * ywidth / <NUM_LIT> ) <EOL> d . addLine ( origin - zaxis * zwidth / <NUM_LIT> , origin + zaxis * zwidth / <NUM_LIT> ) <EOL> obj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' ) <EOL> obj . setProperty ( '<STR_LIT>' , QtGui . QColor ( <NUM_LIT:255> , <NUM_LIT:255> , <NUM_LIT:0> ) ) <EOL> obj . setProperty ( '<STR_LIT>' , False ) <EOL> cube = vtk . vtkCubeSource ( ) <EOL> cube . SetXLength ( xwidth ) <EOL> cube . SetYLength ( ywidth ) <EOL> cube . SetZLength ( zwidth ) <EOL> cube . Update ( ) <EOL> cube = shallowCopy ( cube . GetOutput ( ) ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( origin ) <EOL> obj = updatePolyData ( cube , '<STR_LIT>' , cls = BlockAffordanceItem , parent = '<STR_LIT>' ) <EOL> obj . actor . SetUserTransform ( t ) <EOL> obj . addToView ( app . getDRCView ( ) ) <EOL> params = dict ( origin = origin , xwidth = xwidth , ywidth = ywidth , zwidth = zwidth , xaxis = xaxis , yaxis = yaxis , zaxis = zaxis ) <EOL> obj . setAffordanceParams ( params ) <EOL> obj . updateParamsFromActorTransform ( ) <EOL> def estimatePointerTip ( robotModel , polyData ) : <EOL> '''<STR_LIT>''' <EOL> palmFrame = robotModel . getLinkFrame ( '<STR_LIT>' ) <EOL> p1 = [ <NUM_LIT:0.0> , <NUM_LIT> , - <NUM_LIT> ] <EOL> p2 = [ <NUM_LIT:0.0> , <NUM_LIT> , - <NUM_LIT> ] <EOL> palmFrame . TransformPoint ( p1 , p1 ) <EOL> palmFrame . TransformPoint ( p2 , p2 ) <EOL> p1 = np . array ( p1 ) <EOL> p2 = np . array ( p2 ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> vis . updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = False ) <EOL> polyData = cropToLineSegment ( polyData , p1 , p2 ) <EOL> if not polyData . GetNumberOfPoints ( ) : <EOL> return None <EOL> vis . updatePolyData ( polyData , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> polyData = labelDistanceToLine ( polyData , p1 , p2 ) <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0.0> , <NUM_LIT> ] ) <EOL> if polyData . GetNumberOfPoints ( ) < <NUM_LIT:2> : <EOL> return None <EOL> updatePolyData ( polyData , '<STR_LIT>' , colorByName = '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> ransacDistanceThreshold = <NUM_LIT> <EOL> lineOrigin , lineDirection , polyData = applyLineFit ( polyData , distanceThreshold = ransacDistanceThreshold ) <EOL> updatePolyData ( polyData , '<STR_LIT>' , colorByName = '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> lineDirection = np . array ( lineDirection ) <EOL> lineDirection /= np . linalg . norm ( lineDirection ) <EOL> if np . dot ( lineDirection , ( p2 - p1 ) ) < <NUM_LIT:0> : <EOL> lineDirection *= - <NUM_LIT:1> <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:1.0> , <NUM_LIT:1.0> ] ) <EOL> if polyData . GetNumberOfPoints ( ) < <NUM_LIT:2> : <EOL> return None <EOL> obj = updatePolyData ( polyData , '<STR_LIT>' , colorByName = '<STR_LIT>' , parent = getDebugFolder ( ) , visible = True ) <EOL> obj . setProperty ( '<STR_LIT>' , <NUM_LIT:5> ) <EOL> pts = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> dists = np . dot ( pts - lineOrigin , lineDirection ) <EOL> p1 = lineOrigin + lineDirection * np . min ( dists ) <EOL> p2 = lineOrigin + lineDirection * np . max ( dists ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> vis . updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) , visible = True ) <EOL> return p2 <EOL> def startBoundedPlaneSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentBoundedPlaneByAnnotation ) <EOL> def startValveSegmentationByWallPlane ( expectedValveRadius ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentValveByWallPlane , expectedValveRadius ) <EOL> def startValveSegmentationManual ( expectedValveRadius ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentValve , expectedValveRadius ) <EOL> def startRefitWall ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = refitWall <EOL> def startWyeSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentWye ) <EOL> def startDoorHandleSegmentation ( otdfType ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDoorHandle , otdfType ) <EOL> def startTrussSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentTruss ) <EOL> def startHoseNozzleSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentHoseNozzle ) <EOL> def storePoint ( p ) : <EOL> global _pickPoint <EOL> _pickPoint = p <EOL> def getPickPoint ( ) : <EOL> global _pickPoint <EOL> return _pickPoint <EOL> def startPickPoint ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = storePoint <EOL> def startSelectToolTip ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = selectToolTip <EOL> def startDrillSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:3> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrill ) <EOL> def startDrillAutoSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillAuto ) <EOL> def startDrillButtonSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillButton ) <EOL> def startPointerTipSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentPointerTip ) <EOL> def startDrillAutoSegmentationAlignedWithTable ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillAlignedWithTable ) <EOL> def startDrillBarrelSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillBarrel ) <EOL> def startDrillWallSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:3> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillWall ) <EOL> def startDrillWallSegmentationConstrained ( rightAngleLocation ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = False <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillWallConstrained , rightAngleLocation ) <EOL> def startDrillInHandSegmentation ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . drawLines = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDrillInHand ) <EOL> def startSegmentDebrisWall ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:1> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDebrisWall ) <EOL> def startSegmentDebrisWallManual ( ) : <EOL> picker = PointPicker ( numberOfPoints = <NUM_LIT:2> ) <EOL> addViewPicker ( picker ) <EOL> picker . enabled = True <EOL> picker . start ( ) <EOL> picker . annotationFunc = functools . partial ( segmentDebrisWallManual ) <EOL> def selectToolTip ( point1 ) : <EOL> print point1 <EOL> def segmentDebrisWallManual ( point1 , point2 ) : <EOL> p1 , p2 = point1 , point2 <EOL> d = DebugData ( ) <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> edgeObj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , visible = True ) <EOL> edgeObj . points = [ p1 , p2 ] <EOL> xaxis = p2 - p1 <EOL> xaxis /= np . linalg . norm ( xaxis ) <EOL> zaxis = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( p1 ) <EOL> updateFrame ( t , '<STR_LIT>' , parent = edgeObj , visible = False ) <EOL> refFrame = vtk . vtkTransform ( ) <EOL> refFrame . PostMultiply ( ) <EOL> refFrame . SetMatrix ( t . GetMatrix ( ) ) <EOL> refFrame . Translate ( - xaxis + yaxis + zaxis * <NUM_LIT> ) <EOL> updateFrame ( refFrame , '<STR_LIT>' , parent = edgeObj , visible = False ) <EOL> def segmentDebrisWall ( point1 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = shallowCopy ( inputObj . polyData ) <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , distanceThreshold = <NUM_LIT> , expectedNormal = viewPlaneNormal , perpendicularAxis = viewPlaneNormal , <EOL> searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> planePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> planePoints = applyVoxelGrid ( planePoints , leafSize = <NUM_LIT> ) <EOL> planePoints = labelOutliers ( planePoints , searchRadius = <NUM_LIT> , neighborsInSearchRadius = <NUM_LIT:10> ) <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> planePoints = thresholdPoints ( planePoints , '<STR_LIT>' , [ <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> planePoints = labelDistanceToPoint ( planePoints , point1 ) <EOL> clusters = extractClusters ( planePoints , clusterTolerance = <NUM_LIT> ) <EOL> clusters . sort ( key = lambda x : vtkNumpy . getNumpyFromVtk ( x , '<STR_LIT>' ) . min ( ) ) <EOL> planePoints = clusters [ <NUM_LIT:0> ] <EOL> planeObj = updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> perpAxis = [ <NUM_LIT:0> , <NUM_LIT:0> , - <NUM_LIT:1> ] <EOL> perpAxis /= np . linalg . norm ( perpAxis ) <EOL> edgeAxis = np . cross ( normal , perpAxis ) <EOL> edgePoints = computeEdge ( planePoints , edgeAxis , perpAxis ) <EOL> edgePoints = vtkNumpy . getVtkPolyDataFromNumpyPoints ( edgePoints ) <EOL> updatePolyData ( edgePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> linePoint , lineDirection , _ = applyLineFit ( edgePoints ) <EOL> xaxis = lineDirection <EOL> yaxis = normal <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> if np . dot ( zaxis , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] ) < <NUM_LIT:0> : <EOL> zaxis *= - <NUM_LIT:1> <EOL> xaxis *= - <NUM_LIT:1> <EOL> pts = vtkNumpy . getNumpyFromVtk ( planePoints , '<STR_LIT>' ) <EOL> dists = np . dot ( pts - linePoint , xaxis ) <EOL> p1 = linePoint + xaxis * np . min ( dists ) <EOL> p2 = linePoint + xaxis * np . max ( dists ) <EOL> p1 = projectPointToPlane ( p1 , origin , normal ) <EOL> p2 = projectPointToPlane ( p2 , origin , normal ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> edgeObj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = planeObj , visible = True ) <EOL> edgeObj . points = [ p1 , p2 ] <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( p1 ) <EOL> updateFrame ( t , '<STR_LIT>' , parent = planeObj , visible = False ) <EOL> refFrame = vtk . vtkTransform ( ) <EOL> refFrame . PostMultiply ( ) <EOL> refFrame . SetMatrix ( t . GetMatrix ( ) ) <EOL> refFrame . Translate ( - xaxis + yaxis + zaxis * <NUM_LIT> ) <EOL> updateFrame ( refFrame , '<STR_LIT>' , parent = planeObj , visible = False ) <EOL> def segmentBoundedPlaneByAnnotation ( point1 , point2 ) : <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = shallowCopy ( inputObj . polyData ) <EOL> viewPlaneNormal = np . array ( getSegmentationView ( ) . camera ( ) . GetViewPlaneNormal ( ) ) <EOL> polyData , origin , normal = applyPlaneFit ( polyData , distanceThreshold = <NUM_LIT> , expectedNormal = viewPlaneNormal , perpendicularAxis = viewPlaneNormal , <EOL> searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> planePoints = thresholdPoints ( polyData , '<STR_LIT>' , [ - <NUM_LIT> , <NUM_LIT> ] ) <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> planePoints = applyVoxelGrid ( planePoints , leafSize = <NUM_LIT> ) <EOL> planePoints = labelOutliers ( planePoints , searchRadius = <NUM_LIT> , neighborsInSearchRadius = <NUM_LIT:12> ) <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , colorByName = '<STR_LIT>' , visible = False ) <EOL> planePoints = thresholdPoints ( planePoints , '<STR_LIT>' , [ <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> planePoints = labelDistanceToPoint ( planePoints , point1 ) <EOL> clusters = extractClusters ( planePoints , clusterTolerance = <NUM_LIT> ) <EOL> clusters . sort ( key = lambda x : vtkNumpy . getNumpyFromVtk ( x , '<STR_LIT>' ) . min ( ) ) <EOL> planePoints = clusters [ <NUM_LIT:0> ] <EOL> updatePolyData ( planePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> perpAxis = point2 - point1 <EOL> perpAxis /= np . linalg . norm ( perpAxis ) <EOL> edgeAxis = np . cross ( normal , perpAxis ) <EOL> edgePoints = computeEdge ( planePoints , edgeAxis , perpAxis ) <EOL> edgePoints = vtkNumpy . getVtkPolyDataFromNumpyPoints ( edgePoints ) <EOL> updatePolyData ( edgePoints , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> linePoint , lineDirection , _ = applyLineFit ( edgePoints ) <EOL> zaxis = normal <EOL> yaxis = lineDirection <EOL> xaxis = np . cross ( yaxis , zaxis ) <EOL> if np . dot ( xaxis , perpAxis ) < <NUM_LIT:0> : <EOL> xaxis *= - <NUM_LIT:1> <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> pts = vtkNumpy . getNumpyFromVtk ( planePoints , '<STR_LIT>' ) <EOL> dists = np . dot ( pts - linePoint , yaxis ) <EOL> p1 = linePoint + yaxis * np . min ( dists ) <EOL> p2 = linePoint + yaxis * np . max ( dists ) <EOL> p1 = projectPointToPlane ( p1 , origin , normal ) <EOL> p2 = projectPointToPlane ( p2 , origin , normal ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( p1 , radius = <NUM_LIT> ) <EOL> d . addSphere ( p2 , radius = <NUM_LIT> ) <EOL> d . addLine ( p1 , p2 ) <EOL> updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> t = getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( ( p1 + p2 ) / <NUM_LIT> ) <EOL> updateFrame ( t , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> savedCameraParams = None <EOL> def perspective ( ) : <EOL> global savedCameraParams <EOL> if savedCameraParams is None : <EOL> return <EOL> aff = getDefaultAffordanceObject ( ) <EOL> if aff : <EOL> aff . setProperty ( '<STR_LIT>' , <NUM_LIT:1.0> ) <EOL> obj = om . findObjectByName ( '<STR_LIT>' ) <EOL> if obj is not None : <EOL> obj . actor . SetPickable ( <NUM_LIT:1> ) <EOL> view = getSegmentationView ( ) <EOL> c = view . camera ( ) <EOL> c . ParallelProjectionOff ( ) <EOL> c . SetPosition ( savedCameraParams [ '<STR_LIT>' ] ) <EOL> c . SetFocalPoint ( savedCameraParams [ '<STR_LIT>' ] ) <EOL> c . SetViewUp ( savedCameraParams [ '<STR_LIT>' ] ) <EOL> view . setCameraManipulationStyle ( ) <EOL> view . render ( ) <EOL> def saveCameraParams ( overwrite = False ) : <EOL> global savedCameraParams <EOL> if overwrite or ( savedCameraParams is None ) : <EOL> view = getSegmentationView ( ) <EOL> c = view . camera ( ) <EOL> savedCameraParams = dict ( Position = c . GetPosition ( ) , FocalPoint = c . GetFocalPoint ( ) , ViewUp = c . GetViewUp ( ) ) <EOL> def getDefaultAffordanceObject ( ) : <EOL> obj = om . getActiveObject ( ) <EOL> if isinstance ( obj , AffordanceItem ) : <EOL> return obj <EOL> for obj in om . getObjects ( ) : <EOL> if isinstance ( obj , AffordanceItem ) : <EOL> return obj <EOL> def getVisibleRobotModel ( ) : <EOL> for obj in om . getObjects ( ) : <EOL> if isinstance ( obj , roboturdf . RobotModelItem ) and obj . getProperty ( '<STR_LIT>' ) : <EOL> return obj <EOL> def orthoX ( ) : <EOL> aff = getDefaultAffordanceObject ( ) <EOL> if not aff : <EOL> return <EOL> saveCameraParams ( ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . setProperty ( '<STR_LIT>' , <NUM_LIT> ) <EOL> om . findObjectByName ( '<STR_LIT>' ) . actor . SetPickable ( <NUM_LIT:0> ) <EOL> view = getSegmentationView ( ) <EOL> c = view . camera ( ) <EOL> c . ParallelProjectionOn ( ) <EOL> origin = aff . params [ '<STR_LIT>' ] <EOL> viewDirection = aff . params [ '<STR_LIT>' ] <EOL> viewUp = - aff . params [ '<STR_LIT>' ] <EOL> viewDistance = aff . params [ '<STR_LIT>' ] * <NUM_LIT:3> <EOL> scale = aff . params [ '<STR_LIT>' ] <EOL> c . SetFocalPoint ( origin ) <EOL> c . SetPosition ( origin - viewDirection * viewDistance ) <EOL> c . SetViewUp ( viewUp ) <EOL> c . SetParallelScale ( scale ) <EOL> view . setActorManipulationStyle ( ) <EOL> view . render ( ) <EOL> def orthoY ( ) : <EOL> aff = getDefaultAffordanceObject ( ) <EOL> if not aff : <EOL> return <EOL> saveCameraParams ( ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . setProperty ( '<STR_LIT>' , <NUM_LIT> ) <EOL> om . findObjectByName ( '<STR_LIT>' ) . actor . SetPickable ( <NUM_LIT:0> ) <EOL> view = getSegmentationView ( ) <EOL> c = view . camera ( ) <EOL> c . ParallelProjectionOn ( ) <EOL> origin = aff . params [ '<STR_LIT>' ] <EOL> viewDirection = aff . params [ '<STR_LIT>' ] <EOL> viewUp = - aff . params [ '<STR_LIT>' ] <EOL> viewDistance = aff . params [ '<STR_LIT>' ] * <NUM_LIT:4> <EOL> scale = aff . params [ '<STR_LIT>' ] <EOL> c . SetFocalPoint ( origin ) <EOL> c . SetPosition ( origin - viewDirection * viewDistance ) <EOL> c . SetViewUp ( viewUp ) <EOL> c . SetParallelScale ( scale ) <EOL> view . setActorManipulationStyle ( ) <EOL> view . render ( ) <EOL> def orthoZ ( ) : <EOL> aff = getDefaultAffordanceObject ( ) <EOL> if not aff : <EOL> return <EOL> saveCameraParams ( ) <EOL> aff . updateParamsFromActorTransform ( ) <EOL> aff . setProperty ( '<STR_LIT>' , <NUM_LIT> ) <EOL> om . findObjectByName ( '<STR_LIT>' ) . actor . SetPickable ( <NUM_LIT:0> ) <EOL> view = getSegmentationView ( ) <EOL> c = view . camera ( ) <EOL> c . ParallelProjectionOn ( ) <EOL> origin = aff . params [ '<STR_LIT>' ] <EOL> viewDirection = aff . params [ '<STR_LIT>' ] <EOL> viewUp = - aff . params [ '<STR_LIT>' ] <EOL> viewDistance = aff . params [ '<STR_LIT>' ] <EOL> scale = aff . params [ '<STR_LIT>' ] * <NUM_LIT:6> <EOL> c . SetFocalPoint ( origin ) <EOL> c . SetPosition ( origin - viewDirection * viewDistance ) <EOL> c . SetViewUp ( viewUp ) <EOL> c . SetParallelScale ( scale ) <EOL> view . setActorManipulationStyle ( ) <EOL> view . render ( ) <EOL> def zoomToDisplayPoint ( displayPoint , boundsRadius = <NUM_LIT:0.5> , view = None ) : <EOL> pickedPoint = pickPoint ( displayPoint , getSegmentationView ( ) , obj = '<STR_LIT>' ) <EOL> if pickedPoint is None : <EOL> return <EOL> view = view or app . getCurrentRenderView ( ) <EOL> worldPt1 , worldPt2 = getRayFromDisplayPoint ( getSegmentationView ( ) , displayPoint ) <EOL> diagonal = np . array ( [ boundsRadius , boundsRadius , boundsRadius ] ) <EOL> bounds = np . hstack ( [ pickedPoint - diagonal , pickedPoint + diagonal ] ) <EOL> bounds = [ bounds [ <NUM_LIT:0> ] , bounds [ <NUM_LIT:3> ] , bounds [ <NUM_LIT:1> ] , bounds [ <NUM_LIT:4> ] , bounds [ <NUM_LIT:2> ] , bounds [ <NUM_LIT:5> ] ] <EOL> view . renderer ( ) . ResetCamera ( bounds ) <EOL> view . camera ( ) . SetFocalPoint ( pickedPoint ) <EOL> view . render ( ) <EOL> def extractPointsAlongClickRay ( position , ray , polyData = None , distanceToLineThreshold = <NUM_LIT> , nearestToCamera = False ) : <EOL> if polyData is None : <EOL> polyData = getCurrentRevolutionData ( ) <EOL> if not polyData or not polyData . GetNumberOfPoints ( ) : <EOL> return None <EOL> polyData = labelDistanceToLine ( polyData , position , position + ray ) <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT:0.0> , distanceToLineThreshold ] ) <EOL> if not polyData . GetNumberOfPoints ( ) : <EOL> return None <EOL> polyData = labelPointDistanceAlongAxis ( polyData , ray , origin = position , resultArrayName = '<STR_LIT>' ) <EOL> polyData = thresholdPoints ( polyData , '<STR_LIT>' , [ <NUM_LIT> , <NUM_LIT> ] ) <EOL> if not polyData . GetNumberOfPoints ( ) : <EOL> return None <EOL> updatePolyData ( polyData , '<STR_LIT>' , colorByName = '<STR_LIT>' , visible = False , parent = getDebugFolder ( ) ) <EOL> if nearestToCamera : <EOL> dists = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> else : <EOL> dists = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> intersectionPoint = points [ dists . argmin ( ) ] <EOL> d = DebugData ( ) <EOL> d . addSphere ( intersectionPoint , radius = <NUM_LIT> ) <EOL> d . addLine ( position , intersectionPoint ) <EOL> obj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , visible = False , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) ) <EOL> obj . actor . GetProperty ( ) . SetLineWidth ( <NUM_LIT:2> ) <EOL> d2 = DebugData ( ) <EOL> end_of_ray = position + <NUM_LIT:2> * ray <EOL> d2 . addLine ( position , end_of_ray ) <EOL> obj2 = updatePolyData ( d2 . getPolyData ( ) , '<STR_LIT>' , visible = False , color = [ <NUM_LIT:1> , <NUM_LIT:0> , <NUM_LIT:0> ] , parent = getDebugFolder ( ) ) <EOL> obj2 . actor . GetProperty ( ) . SetLineWidth ( <NUM_LIT:2> ) <EOL> return intersectionPoint <EOL> def segmentDrillWallFromTag ( position , ray ) : <EOL> '''<STR_LIT>''' <EOL> polyData = getCurrentRevolutionData ( ) <EOL> if ( polyData is None ) : <EOL> print "<STR_LIT>" <EOL> return False <EOL> point1 = extractPointsAlongClickRay ( position , ray , polyData ) <EOL> viewDirection = - <NUM_LIT:1> * SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> polyDataOut , origin , normal = applyPlaneFit ( polyData , expectedNormal = viewDirection , searchOrigin = point1 , searchRadius = <NUM_LIT> , angleEpsilon = <NUM_LIT> , returnOrigin = True ) <EOL> intersection_point = intersectLineWithPlane ( position , ray , origin , normal ) <EOL> xaxis = - normal <EOL> zaxis = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:1> ] <EOL> yaxis = np . cross ( zaxis , xaxis ) <EOL> yaxis /= np . linalg . norm ( yaxis ) <EOL> zaxis = np . cross ( xaxis , yaxis ) <EOL> t = transformUtils . getTransformFromAxes ( xaxis , yaxis , zaxis ) <EOL> t . PostMultiply ( ) <EOL> t . Translate ( intersection_point ) <EOL> t2 = transformUtils . copyFrame ( t ) <EOL> t2 . PreMultiply ( ) <EOL> t3 = transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT> , - <NUM_LIT> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> t2 . Concatenate ( t3 ) <EOL> rightAngleLocation = '<STR_LIT>' <EOL> createDrillWall ( rightAngleLocation , t2 ) <EOL> wall = om . findObjectByName ( '<STR_LIT>' ) <EOL> vis . updateFrame ( t , '<STR_LIT>' , parent = wall , visible = False , scale = <NUM_LIT> ) <EOL> d = DebugData ( ) <EOL> d . addSphere ( intersection_point , radius = <NUM_LIT> ) <EOL> obj = updatePolyData ( d . getPolyData ( ) , '<STR_LIT>' , parent = wall , visible = False , color = [ <NUM_LIT:0> , <NUM_LIT:1> , <NUM_LIT:0> ] ) <EOL> obj . actor . GetProperty ( ) . SetLineWidth ( <NUM_LIT:1> ) <EOL> return True <EOL> def segmentDrillWallFromWallCenter ( ) : <EOL> '''<STR_LIT>''' <EOL> inputObj = om . findObjectByName ( '<STR_LIT>' ) <EOL> polyData = inputObj . polyData <EOL> wallFrame = transformUtils . copyFrame ( findWallCenter ( polyData ) ) <EOL> wallFrame . PreMultiply ( ) <EOL> t3 = transformUtils . frameFromPositionAndRPY ( [ - <NUM_LIT> , - <NUM_LIT> , <NUM_LIT:0> ] , [ <NUM_LIT> , - <NUM_LIT> , <NUM_LIT:0> ] ) <EOL> wallFrame . Concatenate ( t3 ) <EOL> rightAngleLocation = '<STR_LIT>' <EOL> createDrillWall ( rightAngleLocation , wallFrame ) <EOL> wall = om . findObjectByName ( '<STR_LIT>' ) <EOL> vis . updateFrame ( wallFrame , '<STR_LIT>' , parent = wall , visible = False , scale = <NUM_LIT> ) <EOL> def findFarRightCorner ( polyData , linkFrame ) : <EOL> '''<STR_LIT>''' <EOL> diagonalTransform = transformUtils . copyFrame ( linkFrame ) <EOL> diagonalTransform . PreMultiply ( ) <EOL> diagonalTransform . Concatenate ( transformUtils . frameFromPositionAndRPY ( [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT:0> ] , [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT> ] ) ) <EOL> vis . updateFrame ( diagonalTransform , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> viewOrigin = diagonalTransform . TransformPoint ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> viewX = diagonalTransform . TransformVector ( [ <NUM_LIT:1.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> viewY = diagonalTransform . TransformVector ( [ <NUM_LIT:0.0> , <NUM_LIT:1.0> , <NUM_LIT:0.0> ] ) <EOL> viewZ = diagonalTransform . TransformVector ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> polyData = labelPointDistanceAlongAxis ( polyData , viewY , origin = viewOrigin , resultArrayName = '<STR_LIT>' ) <EOL> vis . updatePolyData ( polyData , '<STR_LIT>' , parent = '<STR_LIT>' , visible = False ) <EOL> farRightIndex = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) . argmin ( ) <EOL> points = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> return points [ farRightIndex , : ] <EOL> def findMinimumBoundingRectangle ( polyData , linkFrame ) : <EOL> '''<STR_LIT>''' <EOL> polyData = applyVoxelGrid ( polyData , leafSize = <NUM_LIT> ) <EOL> def get2DAsPolyData ( xy_points ) : <EOL> '''<STR_LIT>''' <EOL> d = np . vstack ( ( xy_points . T , np . zeros ( xy_points . shape [ <NUM_LIT:0> ] ) ) ) . T <EOL> d2 = d . copy ( ) <EOL> return vtkNumpy . getVtkPolyDataFromNumpyPoints ( d2 ) <EOL> pts = vtkNumpy . getNumpyFromVtk ( polyData , '<STR_LIT>' ) <EOL> xy_points = pts [ : , [ <NUM_LIT:0> , <NUM_LIT:1> ] ] <EOL> vis . updatePolyData ( get2DAsPolyData ( xy_points ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> hull_points = qhull_2d . qhull2D ( xy_points ) <EOL> vis . updatePolyData ( get2DAsPolyData ( hull_points ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> hull_points = hull_points [ : : - <NUM_LIT:1> ] <EOL> ( rot_angle , rectArea , rectDepth , rectWidth , center_point , corner_points_ground ) = min_bounding_rect . minBoundingRect ( hull_points ) <EOL> vis . updatePolyData ( get2DAsPolyData ( corner_points_ground ) , '<STR_LIT>' , parent = getDebugFolder ( ) , visible = False ) <EOL> polyDataCentroid = computeCentroid ( polyData ) <EOL> cornerPoints = np . vstack ( ( corner_points_ground . T , polyDataCentroid [ <NUM_LIT:2> ] * np . ones ( corner_points_ground . shape [ <NUM_LIT:0> ] ) ) ) . T <EOL> cornerPolyData = vtkNumpy . getVtkPolyDataFromNumpyPoints ( cornerPoints ) <EOL> farRightCorner = findFarRightCorner ( cornerPolyData , linkFrame ) <EOL> viewDirection = SegmentationContext . getGlobalInstance ( ) . getViewDirection ( ) <EOL> viewFrame = SegmentationContext . getGlobalInstance ( ) . getViewFrame ( ) <EOL> robotYaw = math . atan2 ( viewDirection [ <NUM_LIT:1> ] , viewDirection [ <NUM_LIT:0> ] ) * <NUM_LIT> / np . pi <EOL> blockAngle = rot_angle * ( <NUM_LIT> / math . pi ) <EOL> blockAngleAll = np . array ( [ blockAngle , blockAngle + <NUM_LIT> , blockAngle + <NUM_LIT> , blockAngle + <NUM_LIT> ] ) <EOL> values = blockAngleAll - robotYaw <EOL> for i in range ( <NUM_LIT:0> , <NUM_LIT:4> ) : <EOL> if ( values [ i ] > <NUM_LIT> ) : <EOL> values [ i ] = values [ i ] - <NUM_LIT> <EOL> values = abs ( values ) <EOL> min_idx = np . argmin ( values ) <EOL> if ( ( min_idx == <NUM_LIT:1> ) or ( min_idx == <NUM_LIT:3> ) ) : <EOL> temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp <EOL> rot_angle = blockAngleAll [ min_idx ] * math . pi / <NUM_LIT> <EOL> cornerTransform = transformUtils . frameFromPositionAndRPY ( farRightCorner , [ <NUM_LIT:0> , <NUM_LIT:0> , np . rad2deg ( rot_angle ) ] ) <EOL> vis . showFrame ( cornerTransform , "<STR_LIT>" , parent = getDebugFolder ( ) , visible = False ) <EOL> return cornerTransform , rectDepth , rectWidth , rectArea </s>
