def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95):
yvalues = vnp.getNumpyFromVtk(polyData, 'Points')[:, whichAxis]
backY = np.percentile(yvalues, percentile)
if ( percentile > 50):
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [backY - 0.1, np.inf])
else:
searchRegion = segmentation.thresholdPoints(polyData, whichAxisLetter, [-np.inf, backY + 0.1])
vis.updatePolyData(searchRegion, 'search region', parent="segmentation", colorByName=whichAxisLetter, visible=False)
# find the plane of the back wall, remove it and the points behind it:
_, origin, normal = segmentation.applyPlaneFit(searchRegion, distanceThreshold=0.02, expectedNormal=expectedNormal, perpendicularAxis=expectedNormal, returnOrigin=True)
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
backFrame = transformUtils.getTransformFromOriginAndNormal(origin, normal, normalAxis=2)
vis.updateFrame(backFrame, 'back frame', parent='segmentation', scale=0.15 , visible=False)
vis.updatePolyData(polyData, 'dist to back', parent='segmentation', visible=False)
polyData = segmentation.thresholdPoints(polyData, 'dist_to_plane', filterRange)
vis.updatePolyData(polyData, 'back off and all', parent='segmentation', visible=False)
return polyData
def fitObjectsOnShelf(polyData, maxHeight = 0.25):
# find the shelf plane:
polyDataWithoutFront, _ = segmentation.removeMajorPlane(polyData, distanceThreshold=0.02)
polyDataPlaneFit, origin, normal = segmentation.applyPlaneFit(polyDataWithoutFront, expectedNormal=np.array([0.0,0.0,1.0]), perpendicularAxis=np.array([0.0,0.0,1.0]), returnOrigin=True)
vis.updatePolyData(polyDataPlaneFit, 'polyDataPlaneFit', parent='segmentation', visible=False)
shelfSurfacePoints = segmentation.thresholdPoints(polyDataPlaneFit, 'dist_to_plane', [-0.01, 0.01])
shelfCenter = segmentation.computeCentroid(shelfSurfacePoints)
shelfFrame = transformUtils.getTransformFromOriginAndNormal(shelfCenter, normal, normalAxis=2)
vis.showFrame(shelfFrame, 'shelfFrame', parent='segmentation', scale=0.15 , visible=False)
# find the points near to the shelf plane and find objects on it:
points = vnp.getNumpyFromVtk(polyData, 'Points')
dist = np.dot(points - origin, normal)
vnp.addNumpyToVtk(polyData, dist, 'dist_to_plane')
shelfPoints = segmentation.thresholdPoints(polyData, 'dist_to_plane', [-0.01, maxHeight])
vis.updatePolyData(shelfPoints, 'shelf', parent='segmentation', visible=False)
data = segmentation.segmentTableScene(shelfPoints, shelfCenter, filterClustering = False )
vis.showClusterObjects(data.clusters + [data.table], parent='segmentation')
# remove the points that we considered from the orginal cloud
dists = vnp.getNumpyFromVtk(polyData, 'dist_to_plane')
diffShelf = ( ((dists > maxHeight) + (dists < -0.01))) + 0.1 -0.1
vnp.addNumpyToVtk(polyData, diffShelf, 'diff_shelf')
polyData = segmentation.thresholdPoints(polyData, 'diff_shelf', [1, 1])
vis.updatePolyData(polyData, 'rest', parent='segmentation', visible=False)
return polyData
def cropToCylinder(polyData, p1, p2, radius):
polyData = segmentation.cropToLineSegment(polyData, p1, p2)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, p1, p2)
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line',
[0.0, radius])
return polyData
def onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'walking goal',
parent='planning',
scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(),
'walking goal terrain handle',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid), d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0,0,0), radius=0.02)
self.seedObj = vis.showPolyData(d.getPolyData(), 'region seed', parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform, 'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
self.frameObj.transform.Translate(pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200,200,20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220,220,220))
def onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0,0,0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def cropPointCloudToModel(pointCloud,
objectPointCloud,
distanceThreshold=0.02,
visualize=True,
applyEuclideanClustering=True):
"""
Crops pointCloud to just the points withing distanceThreshold of
objectPointCloud
:param pointCloud:
:param objectPointCloud:
:param distanceThreshold:
:return: cropped pointcloud
"""
registration = GlobalRegistrationUtils
pointCloud = GlobalRegistration.cropPointCloudToModelBoundingBox(
pointCloud, objectPointCloud, scaleFactor=1.5)
arrayName = 'distance_to_mesh'
print "computing point to point distance"
dists = registration.computePointToPointDistance(
pointCloud, objectPointCloud)
vnp.addNumpyToVtk(pointCloud, dists, arrayName)
polyData = filterUtils.thresholdPoints(pointCloud, arrayName,
[0.0, distanceThreshold])
# this stuff may be unecessary
if applyEuclideanClustering:
# polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
polyData = segmentation.applyEuclideanClustering(
polyData, clusterTolerance=0.04)
polyData = segmentation.thresholdPoints(polyData, 'cluster_labels',
[1, 1])
if visualize:
parent = om.getOrCreateContainer('global registration')
vis.updatePolyData(polyData,
'cropped pointcloud',
color=[0, 1, 0],
parent=parent)
return polyData
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame, [1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print 'empty search region point cloud'
return
vis.updatePolyData(polyData, 'running board search points', parent=segmentation.getDebugFolder(), color=[0,1,0], visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints, 'edge points', parent=segmentation.getDebugFolder(), visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels', [1.0, 1.0])
dists = np.dot(vnp.getNumpyFromVtk(linePoints, 'Points')-linePoint, lineDirection)
p1 = linePoint + lineDirection*np.min(dists)
p2 = linePoint + lineDirection*np.max(dists)
vis.updatePolyData(fitPoints, 'line fit points', parent=segmentation.getDebugFolder(), colorByName='ransac_labels', visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection*np.dot(origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1,0,0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0,1,0])
d.addLine(originProjectedToPlane, origin, color=[0,1,0])
d.addLine(originProjectedToEdge, origin, color=[1,0,0])
vis.updatePolyData(d.getPolyData(), 'running board edge', parent=segmentation.getDebugFolder(), colorByName='RGB255', visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0]/2.0, 0.0, -boxDimensions[2]/2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
normals = vnp.getNumpyFromVtk(polyData, 'normals')
segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1])
normalsValid = np.any(normals, axis=1)
vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32),
'normals_valid')
vis.showPolyData(polyData,
'scene points',
colorByName='normals_valid',
visible=False)
numPoints = polyData.GetNumberOfPoints()
polyData = segmentation.thresholdPoints(polyData, 'normals_valid', [1, 1])
vis.showPolyData(polyData,
'cloud normals',
colorByName='curvature',
visible=True)
print 'number of filtered points:', numPoints - polyData.GetNumberOfPoints(
)
if showGlyphs:
polyData.GetPointData().SetNormals(
polyData.GetPointData().GetArray('normals'))
arrows = segmentation.applyArrowGlyphs(polyData, computeNormals=False)
disks = segmentation.applyDiskGlyphs(polyData, computeNormals=False)
polyData.GetPointData().SetNormals(None)
def fitRunningBoardAtFeet(self):
# get stance frame
startPose = self.getPlanningStartPose()
stanceFrame = self.robotSystem.footstepsDriver.getFeetMidPoint(
self.robotSystem.robotStateModel, useWorldZ=False)
stanceFrameAxes = transformUtils.getAxesFromTransform(stanceFrame)
# get pointcloud and extract search region covering the running board
polyData = segmentation.getCurrentRevolutionData()
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
_, polyData = segmentation.removeGround(polyData)
polyData = segmentation.cropToBox(polyData, stanceFrame,
[1.0, 1.0, 0.1])
if not polyData.GetNumberOfPoints():
print('empty search region point cloud')
return
vis.updatePolyData(polyData,
'running board search points',
parent=segmentation.getDebugFolder(),
color=[0, 1, 0],
visible=False)
# extract maximal points along the stance x axis
perpAxis = stanceFrameAxes[0]
edgeAxis = stanceFrameAxes[1]
edgePoints = segmentation.computeEdge(polyData, edgeAxis, perpAxis)
edgePoints = vnp.getVtkPolyDataFromNumpyPoints(edgePoints)
vis.updatePolyData(edgePoints,
'edge points',
parent=segmentation.getDebugFolder(),
visible=True)
# ransac fit a line to the edge points
linePoint, lineDirection, fitPoints = segmentation.applyLineFit(
edgePoints)
if np.dot(lineDirection, stanceFrameAxes[1]) < 0:
lineDirection = -lineDirection
linePoints = segmentation.thresholdPoints(fitPoints, 'ransac_labels',
[1.0, 1.0])
dists = np.dot(
vnp.getNumpyFromVtk(linePoints, 'Points') - linePoint,
lineDirection)
p1 = linePoint + lineDirection * np.min(dists)
p2 = linePoint + lineDirection * np.max(dists)
vis.updatePolyData(fitPoints,
'line fit points',
parent=segmentation.getDebugFolder(),
colorByName='ransac_labels',
visible=False)
# compute a new frame that is in plane with the stance frame
# and matches the orientation and position of the detected edge
origin = np.array(stanceFrame.GetPosition())
normal = np.array(stanceFrameAxes[2])
# project stance origin to edge, then back to foot frame
originProjectedToEdge = linePoint + lineDirection * np.dot(
origin - linePoint, lineDirection)
originProjectedToPlane = segmentation.projectPointToPlane(
originProjectedToEdge, origin, normal)
zaxis = np.array(stanceFrameAxes[2])
yaxis = np.array(lineDirection)
xaxis = np.cross(yaxis, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
d = DebugData()
d.addSphere(p1, radius=0.005)
d.addSphere(p2, radius=0.005)
d.addLine(p1, p2)
d.addSphere(originProjectedToEdge, radius=0.001, color=[1, 0, 0])
d.addSphere(originProjectedToPlane, radius=0.001, color=[0, 1, 0])
d.addLine(originProjectedToPlane, origin, color=[0, 1, 0])
d.addLine(originProjectedToEdge, origin, color=[1, 0, 0])
vis.updatePolyData(d.getPolyData(),
'running board edge',
parent=segmentation.getDebugFolder(),
colorByName='RGB255',
visible=False)
# update the running board box affordance position and orientation to
# fit the detected edge
box = self.spawnRunningBoardAffordance()
boxDimensions = box.getProperty('Dimensions')
t = transformUtils.getTransformFromAxesAndOrigin(
xaxis, yaxis, zaxis, originProjectedToPlane)
t.PreMultiply()
t.Translate(-boxDimensions[0] / 2.0, 0.0, -boxDimensions[2] / 2.0)
box.getChildFrame().copyFrame(t)
self.initialize()
segmentationroutines.SegmentationContext.initWithUser(groundHeight, viewFrame)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(
os.path.join(dataDir, 'tabletop/table-sparse-stereo.vtp'))
vis.showPolyData(polyData,
'pointcloud snapshot original',
colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud(polyData)
vis.showPolyData(polyData, 'pointcloud snapshot')
# Use only returns near the robot:
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'distance_along_view_x',
[0, 1.3])
segmentation.segmentTableThenFindDrills(polyData,
[1.2864902, -0.93351376, 1.10208917])
if app.getTestingInteractiveEnabled():
v = applogic.getCurrentView()
v.camera().SetPosition([3, 3, 3])
v.camera().SetFocalPoint([0, 0, 0])
view.show()
app.showObjectModel()
app.start()
from director import transformUtils
from director import segmentation
from director.debugVis import DebugData
from director import vtkAll as vtk
from director import vtkNumpy as vnp
import numpy as np
polyData = ioUtils.readPolyData('snapshot_2.vtp')
polyData = segmentation.cropToBounds(polyData, vtk.vtkTransform(),
[[-10, 10], [-10, 10], [0.15, 10]])
polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01)
polyData = segmentation.applyEuclideanClustering(polyData,
clusterTolerance=0.04)
polyData = segmentation.thresholdPoints(polyData, 'cluster_labels', [1, 1])
vis.showPolyData(polyData,
'snapshot_2',
colorByName='rgb_colors',
visible=True)
robotMesh = ioUtils.readPolyData('robot_mesh_2.vtp')
vis.showPolyData(robotMesh, 'robot_mesh_2', visible=True)
t = segmentation.applyICP(polyData, robotMesh)
print t.GetPosition()
print t.GetOrientation()
polyData = filterUtils.transformPolyData(polyData, t)
def __init__(self, uid, view, seed_pose, irisDriver, existing_region=None):
d = DebugData()
self.uid = uid
vis.PolyDataItem.__init__(self, "IRIS region {:d}".format(uid),
d.getPolyData(), view)
self.transform = seed_pose
d.addSphere((0, 0, 0), radius=0.02)
self.seedObj = vis.showPolyData(
d.getPolyData(),
'region seed',
parent=om.getOrCreateContainer('IRIS region seeds'))
self.seedObj.actor.SetUserTransform(self.transform)
self.frameObj = vis.showFrame(self.transform,
'region seed frame',
scale=0.2,
visible=False,
parent=self.seedObj)
self.frameObj.setProperty('Edit', True)
self.frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
rep = self.frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
pos = np.array(self.frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
self.frameObj.transform.Translate(
pos - np.array(self.frameObj.transform.GetPosition()))
self.placer = PlacerWidget(view, self.seedObj, terrain)
self.placer.start()
else:
self.frameObj.setProperty('Edit', True)
self.frameObj.setProperty('Visible', True)
self.driver = irisDriver
self.safe_region = None
self.addProperty('Visible', True)
self.addProperty('Enabled for Walking', True)
self.addProperty('Alpha', 1.0)
self.addProperty('Color', QtGui.QColor(200, 200, 20))
self.frameObj.connectFrameModified(self.onFrameModified)
if existing_region is None:
self.onFrameModified(self.frameObj)
else:
self.setRegion(existing_region)
self.setProperty('Alpha', 0.5)
self.setProperty('Color', QtGui.QColor(220, 220, 220))
view = app.createView()
segmentation._defaultSegmentationView = view
robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'amazon-pod/01-small-changes.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot', visible=False)
# remove ground and clip to just the pod:
groundPoints, polyData = segmentation.removeGround(polyData)
vis.showPolyData(polyData, 'scene', visible=False)
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5])
vis.showPolyData(polyData, 'clipped', visible=False)
# remove outliers
polyData = segmentation.labelOutliers(polyData, searchRadius=0.03, neighborsInSearchRadius=40)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.showPolyData(polyData, 'inliers', visible=False)
# remove walls, and points behind temp:
polyData = removePlaneAndBeyond(polyData, expectedNormal=[0,1,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95)
polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=0, whichAxisLetter='x', percentile = 95)
polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[0.03, np.inf], whichAxis=0, whichAxisLetter='x', percentile = 5)
vis.updatePolyData(polyData, 'only shelves', parent='segmentation', visible=False)
robotStateJointController.push()
'''
groundHeight = 0.0
viewFrame = segmentation.transformUtils.frameFromPositionAndRPY([1, -1, groundHeight + 1.5], [0, 0, -35])
segmentationroutines.SegmentationContext.initWithUser(groundHeight, viewFrame)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-sparse-stereo.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot original', colorByName='rgb_colors')
polyData = segmentationroutines.sparsifyStereoCloud( polyData )
vis.showPolyData(polyData, 'pointcloud snapshot')
# Use only returns near the robot:
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'distance_along_view_x', [0, 1.3])
segmentation.segmentTableThenFindDrills(polyData, [1.2864902, -0.93351376, 1.10208917])
if app.getTestingInteractiveEnabled():
v = applogic.getCurrentView()
v.camera().SetPosition([3,3,3])
v.camera().SetFocalPoint([0,0,0])
view.show()
app.showObjectModel()
app.start()
robot_config = drcargs.getRobotConfig()
urdf_path = os.path.join(robot_config.dirname, robot_config['urdfConfig']['default'])
robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', urdfFile = urdf_path, color=roboturdf.getRobotGrayColor(), visible=True)
segmentationroutines.SegmentationContext.initWithRobot(robotStateModel)
# load poly data
dataDir = app.getTestingDataDirectory()
polyData = ioutils.readPolyData(os.path.join(dataDir, 'misc/amazon-pod/01-small-changes.vtp'))
vis.showPolyData(polyData, 'pointcloud snapshot', visible=False)
# remove ground and clip to just the pod:
groundPoints, polyData = segmentation.removeGround(polyData)
vis.showPolyData(polyData, 'scene', visible=False)
polyData = segmentation.addCoordArraysToPolyData(polyData)
polyData = segmentation.thresholdPoints(polyData, 'y', [1, 1.6])
polyData = segmentation.thresholdPoints(polyData, 'x', [-1.2, 0.5])
vis.showPolyData(polyData, 'clipped', visible=False)
# remove outliers
polyData = segmentation.labelOutliers(polyData, searchRadius=0.03, neighborsInSearchRadius=40)
polyData = segmentation.thresholdPoints(polyData, 'is_outlier', [0, 0])
vis.showPolyData(polyData, 'inliers', visible=False)
# remove walls, and points behind temp:
polyData = removePlaneAndBeyond(polyData, expectedNormal=[0,1,0], filterRange=[-np.inf, -0.03], whichAxis=1, whichAxisLetter='y', percentile = 95)
polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[-np.inf, -0.03], whichAxis=0, whichAxisLetter='x', percentile = 95)
polyData = removePlaneAndBeyond(polyData, expectedNormal=[1,0,0], filterRange=[0.03, np.inf], whichAxis=0, whichAxisLetter='x', percentile = 5)
vis.updatePolyData(polyData, 'only shelves', parent='segmentation', visible=False)
def newWalkingGoal(self, displayPoint, view):
# put walking goal at robot's base
mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"]
footFrame = self.robotModel.getLinkFrame(mainLink)
if not footFrame:
print(
"ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in "
"the robot's URDF. Cannot place walking goal.".format(mainLink)
)
return
worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
groundOrigin = footFrame.GetPosition()
groundNormal = [0.0, 0.0, 1.0]
selectedGroundPoint = [0.0, 0.0, 0.0]
t = vtk.mutable(0.0)
vtk.vtkPlane.IntersectWithLine(
worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint
)
walkingTarget = transformUtils.frameFromPositionAndRPY(
selectedGroundPoint, np.array(footFrame.GetOrientation())
)
frameObj = vis.updateFrame(
walkingTarget,
self.robotName + " walking goal",
parent="planning",
scale=0.25,
)
frameObj.setProperty("Edit", True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName("HEIGHT_MAP_SCENE")
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1]
)
polyData = segmentation.thresholdPoints(
polyData, "distance_to_line", [0.0, 0.1]
)
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition())
)
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(
d.getPolyData(),
"walking goal terrain handle " + self.robotName,
parent=frameObj,
visible=True,
color=[1, 1, 0],
)
handle.actor.SetUserTransform(frameObj.transform)
placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == "Edit":
if propertySet.getProperty(propertyName):
placer.start()
else:
placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, "Edit")
frameObj.connectFrameModified(self.onWalkingGoalModified)
def fit(self, polyData, points):
iiwaplanning.fitSupport(pickPoint=points[0])
return
pickPoint = points[0]
t = vtk.vtkTransform()
t.Translate(pickPoint)
print 'pick point:', pickPoint
print 'crop'
polyData = segmentation.cropToBox(polyData, t, [0.3,0.3,0.5])
import colorsys
rgb = vnp.getNumpyFromVtk(polyData, 'rgb_colors')/255.0
hsv = np.array([colorsys.rgb_to_hsv(*t) for t in rgb])
vnp.addNumpyToVtk(polyData, hsv[:,0].copy(), 'hue')
vnp.addNumpyToVtk(polyData, hsv[:,1].copy(), 'saturation')
vnp.addNumpyToVtk(polyData, hsv[:,2].copy(), 'value')
vis.updatePolyData(polyData, 'crop region', colorByName='rgb_colors', visible=False)
# hide input data
#om.findObjectByName(self.pointCloudObjectName).setProperty('Visible', False)
#cluster = segmentation.makePolyDataFields(polyData)
#vis.showClusterObjects([cluster], parent='segmentation')
hueRange = [0.12, 0.14]
valueRange = [0.5, 1.0]
print 'thresh'
points = segmentation.thresholdPoints(segmentation.thresholdPoints(polyData, 'hue', hueRange), 'value', valueRange)
#points = segmentation.extractLargestCluster(points, minClusterSize=10, clusterTolerance=0.02)
vis.updatePolyData(points, 'pole points', color=[0,0,1], visible=False)
maxZ = np.nanmax(vnp.getNumpyFromVtk(points, 'Points')[:,2])
print 'maxZ', maxZ
pickPoint = pickPoint[0], pickPoint[1], maxZ+0.2
print pickPoint
t = vtk.vtkTransform()
t.Translate(pickPoint)
print 'crop2'
polyData = segmentation.cropToBox(polyData, t, [0.15,0.15,0.4])
vis.updatePolyData(polyData, 'object points', colorByName='rgb_colors', visible=False)
if polyData.GetNumberOfPoints() > 5:
print 'make fields'
cluster = segmentation.makePolyDataFields(polyData)
vis.showClusterObjects([cluster], parent='segmentation')
print 'done'
print 'done.'
# filter points without normals
normals = vnp.getNumpyFromVtk(polyData, 'normals')
segmentation.flipNormalsWithViewDirection(polyData, [1, -1, -1])
normalsValid = np.any(normals, axis=1)
vnp.addNumpyToVtk(polyData, np.array(normalsValid, dtype=np.int32), 'normals_valid')
vis.showPolyData(polyData, 'scene points', colorByName='normals_valid', visible=False)
numPoints = polyData.GetNumberOfPoints()
polyData = segmentation.thresholdPoints(polyData, 'normals_valid', [1, 1])
vis.showPolyData(polyData, 'cloud normals', colorByName='curvature', visible=True)
print 'number of filtered points:', numPoints - polyData.GetNumberOfPoints()
if showGlyphs:
polyData.GetPointData().SetNormals(polyData.GetPointData().GetArray('normals'))
arrows = segmentation.applyArrowGlyphs(polyData, computeNormals=False)
disks = segmentation.applyDiskGlyphs(polyData, computeNormals=False)
polyData.GetPointData().SetNormals(None)
vis.showPolyData(arrows, 'arrows')
vis.showPolyData(disks, 'disks')
#arrows2 = segmentation.applyArrowGlyphs(polyData)
#vis.showPolyData(arrows2, 'arrows pcl normals')
