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])