def fitSwitchBox(self, polyData, points):
boxPosition = points[0]
wallPoint = points[1]
# find a frame that is aligned with wall
searchRadius = 0.2
planePoints, normal = segmentation.applyLocalPlaneFit(polyData, points[0], searchRadius=np.linalg.norm(points[1] - points[0]), searchRadiusEnd=1.0)
obj = vis.updatePolyData(planePoints, 'wall plane points', color=[0,1,0], visible=False)
obj.setProperty('Point Size', 7)
viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
if np.dot(normal, viewDirection) < 0:
normal = -normal
origin = segmentation.computeCentroid(planePoints)
zaxis = [0,0,1]
xaxis = normal
yaxis = np.cross(zaxis, xaxis)
yaxis /= np.linalg.norm(yaxis)
zaxis = np.cross(xaxis, yaxis)
zaxis /= np.linalg.norm(zaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
# translate that frame to the box position
t.PostMultiply()
t.Translate(boxPosition)
boxFrame = transformUtils.copyFrame(t)
self.switchPlanner.spawnBoxAffordanceAtFrame(boxFrame)
def fitSupport(pickPoint=[0.92858565, 0.00213802, 0.30315629]):
om.removeFromObjectModel(om.findObjectByName('cylinder'))
polyData = getPointCloud()
t = vtk.vtkTransform()
t.Translate(pickPoint)
polyData = segmentation.cropToBox(polyData, t, [0.3, 0.3, 0.5])
addHSVArrays(polyData)
vis.updatePolyData(polyData,
'crop region',
colorByName='rgb_colors',
visible=False)
zMax = getMaxZCoordinate(polyData)
cyl = makeCylinder()
cyl.setProperty('Radius', 0.03)
cyl.setProperty('Length', zMax)
origin = segmentation.computeCentroid(polyData)
origin[2] = zMax / 2.0
t = transformUtils.frameFromPositionAndRPY(origin, [0, 0, 0])
cyl.getChildFrame().copyFrame(t)
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 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 run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('shelf item'))
obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
segmentation.makeMovable(obj, t)
def run(self):
polyData = self.getPointCloud()
annotation = self.getAnnotationInput()
annotationPoint = annotation.annotationPoints[0]
mesh = segmentation.fitShelfItem(polyData, annotationPoint, clusterTolerance=self.properties.getProperty('Cluster tolerance'))
annotation.setProperty('Visible', False)
om.removeFromObjectModel(om.findObjectByName('shelf item'))
obj = vis.showPolyData(mesh, 'shelf item', color=[0,1,0])
t = transformUtils.frameFromPositionAndRPY(segmentation.computeCentroid(mesh), [0,0,0])
segmentation.makeMovable(obj, t)
def fitObjectFromPolyData(polyData):
origin, edges, wireframe = segmentation.getOrientedBoundingBox(polyData)
edgeLengths = [float(np.linalg.norm(edge)) for edge in edges]
axes = [edge / np.linalg.norm(edge) for edge in edges]
boxCenter = segmentation.computeCentroid(wireframe)
t = transformUtils.getTransformFromAxes(axes[0], axes[1], axes[2])
zAxisIndex, zAxis, zAxisSign = transformUtils.findTransformAxis(
t, [0, 0, 1.0])
xAxisIndex, xAxis, xAxisSign = transformUtils.findTransformAxis(
t, [1.0, 0, 0])
assert zAxisIndex != xAxisIndex
#zAxis = zAxis*zAxisSign
zAxis = [0, 0, 1.0]
xAxis = xAxis * xAxisSign
yAxis = np.cross(zAxis, xAxis)
xAxis = np.cross(yAxis, zAxis)
zLength = edgeLengths[zAxisIndex]
xLength = edgeLengths[xAxisIndex]
ids = [0, 1, 2]
ids.remove(zAxisIndex)
ids.remove(xAxisIndex)
yLength = edgeLengths[ids[0]]
t = transformUtils.getTransformFromAxes(xAxis, yAxis, zAxis)
t.PostMultiply()
t.Translate(boxCenter)
om.removeFromObjectModel(om.findObjectByName('box'))
obj = makeBox()
obj.setProperty('Dimensions', [xLength, yLength, zLength])
obj.getChildFrame().copyFrame(t)
#obj.getChildFrame().setProperty('Visible', True)
obj.setProperty('Surface Mode', 'Wireframe')
obj.setProperty('Color', [1, 0, 0])
return obj
def getOrientedBoundingBox(pd):
mesh = pd
origin, edges, wireframe = segmentation.getOrientedBoundingBox(mesh)
edgeLengths = np.array([np.linalg.norm(edge) for edge in edges])
axes = [edge / np.linalg.norm(edge) for edge in edges]
# find axis nearest to the +/- up vector
upVector = [0, 0, 1]
dotProducts = [np.abs(np.dot(axe, upVector)) for axe in axes]
zAxisIndex = np.argmax(dotProducts)
# re-index axes and edge lengths so that the found axis is the z axis
axesInds = [(zAxisIndex + 1) % 3, (zAxisIndex + 2) % 3, zAxisIndex]
axes = [axes[i] for i in axesInds]
edgeLengths = [edgeLengths[i] for i in axesInds]
# flip if necessary so that z axis points toward up
if np.dot(axes[2], upVector) < 0:
axes[1] = -axes[1]
axes[2] = -axes[2]
boxCenter = segmentation.computeCentroid(wireframe)
t = transformUtils.getTransformFromAxes(axes[0], axes[1], axes[2])
t.PostMultiply()
t.Translate(boxCenter)
pd = filterUtils.transformPolyData(pd, t.GetLinearInverse())
wireframe = filterUtils.transformPolyData(wireframe,
t.GetLinearInverse())
return FieldContainer(points=pd,
box=wireframe,
frame=t,
dims=edgeLengths,
axes=axes)
chulls.append(
computePlanarConvexHull(groundPoints, expectedNormal=[0, 0, 1]))
d = DebugData()
for i, result in enumerate(chulls):
planePoints, chull, plane = result.points, result.convexHull, result.plane
c = segmentation.getRandomColor()
vis.showPolyData(planePoints, 'plane %d' % i, color=c)
chull = vis.showPolyData(chull, 'convex hull %d' % i, color=c)
chull.setProperty('Surface Mode', 'Surface with edges')
chull.actor.GetProperty().SetLineWidth(3)
center = segmentation.computeCentroid(chull.polyData)
chullPoints = vnp.getNumpyFromVtk(chull.polyData, 'Points')
d.addLine(plane.GetOrigin(),
np.array(plane.GetOrigin()) +
0.005 * np.array(plane.GetNormal()),
radius=0.0001,
color=[0, 0, 0])
#d.addArrow(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.01 * np.array(plane.GetNormal()), headRadius=0.001, tubeRadius=0.0002)
#d.addSphere(chullPoints[0], radius=0.001, color=[1,0,0])
#d.addSphere(chullPoints[1], radius=0.001, color=[0,1,0])
vis.showPolyData(d.getPolyData(), 'plane normals', colorByName='RGB255')
#saveConvexHulls(chulls, name)
#vis.showPolyData(ioUtils.readPolyData(os.path.join(name, 'merged_planes.ply')), 'merged_planes')
chulls.append(computePlanarConvexHull(groundPoints, expectedNormal=[0,0,1]))
d = DebugData()
for i, result in enumerate(chulls):
planePoints, chull, plane = result.points, result.convexHull, result.plane
c = segmentation.getRandomColor()
vis.showPolyData(planePoints, 'plane %d' % i, color=c)
chull = vis.showPolyData(chull, 'convex hull %d' % i, color=c)
chull.setProperty('Surface Mode', 'Surface with edges')
chull.actor.GetProperty().SetLineWidth(3)
center = segmentation.computeCentroid(chull.polyData)
chullPoints = vnp.getNumpyFromVtk(chull.polyData, 'Points')
d.addLine(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.005 * np.array(plane.GetNormal()), radius=0.0001, color=[0,0,0])
#d.addArrow(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.01 * np.array(plane.GetNormal()), headRadius=0.001, tubeRadius=0.0002)
#d.addSphere(chullPoints[0], radius=0.001, color=[1,0,0])
#d.addSphere(chullPoints[1], radius=0.001, color=[0,1,0])
vis.showPolyData(d.getPolyData(), 'plane normals', colorByName='RGB255')
#saveConvexHulls(chulls, name)
#vis.showPolyData(ioUtils.readPolyData(os.path.join(name, 'merged_planes.ply')), 'merged_planes')
applogic.resetCamera([1,1,0])
applogic.setBackgroundColor([1,1,1])
