def processSnippet(): obj = om.getOrCreateContainer("continuous") om.getOrCreateContainer("cont debug", obj) if continuouswalkingDemo.processContinuousStereo: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet_stereo.vtp")) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet.vtp")) vis.updatePolyData(polyData, "walking snapshot trimmed", parent="continuous") standingFootName = "l_foot" standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if clusters is None: print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def fitObjectToPointcloud(self, objectName, pointCloud=None, downsampleObject=True, objectPolyData=None, filename=None, visualize=True, algorithm="GoICP"): if objectPolyData is None: if filename is None: filename = utils.getObjectMeshFilename(objectName) objectPolyData = ioUtils.readPolyData(filename) # downsample the object poly data objectPolyDataForAlgorithm = objectPolyData if downsampleObject: objectPolyDataForAlgorithm = segmentation.applyVoxelGrid( objectPolyData, leafSize=0.002) if pointCloud is None: pointCloud = om.findObjectByName('reconstruction').polyData sceneToModelTransform = GlobalRegistration.runRegistration( algorithm, pointCloud, objectPolyDataForAlgorithm) objectToWorld = sceneToModelTransform.GetLinearInverse() self.objectToWorldTransform[objectName] = objectToWorld if visualize: alignedObj = vis.updatePolyData(objectPolyData, objectName + ' aligned') alignedObj.actor.SetUserTransform(objectToWorld) return objectToWorld
def testAlign(modelName, sceneName): removeFile('model_features.bin') removeFile('scene_features.bin') removeFile('features.bin') for objType, objName in [('model', modelName), ('scene', sceneName)]: print 'process', objName pd = om.findObjectByName(objName).polyData pd = segmentation.applyVoxelGrid(pd, leafSize=0.005) print 'compute normals...' pd = segmentation.normalEstimation(pd, searchRadius=0.05, useVoxelGrid=False, voxelGridLeafSize=0.01) print 'compute features...' computePointFeatureHistograms(pd, searchRadius=0.10) renameFeaturesFile(objType + '_features.bin') print 'run registration...' subprocess.check_call(['bash', 'runFGR.sh']) showTransformedData(sceneName) print 'done.'
def processSnippet(): obj = om.getOrCreateContainer('continuous') om.getOrCreateContainer('cont debug', obj) if (continuouswalkingDemo.processContinuousStereo): polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp')) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp')) vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous') standingFootName = cwdemo.ikPlanner.leftFootLink standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if (clusters is None): print("No cluster found, stop walking now!") return # Step 3: find the corners of the minimum bounding rectangles blocks,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def computeFeatures(polyData, name): pd = segmentation.applyVoxelGrid(pd, leafSize=0.005) print 'compute normals...' pd = segmentation.normalEstimation(pd, searchRadius=0.05, useVoxelGrid=False, voxelGridLeafSize=0.01) print 'compute features...' FastGlobalRegistration.computePointFeatureHistograms(pd, searchRadius=0.10) newName = name + '_features.bin' FastGlobalRegistration.renameFeaturesFile(name)
def test(self, algorithm="GoICP"): """ Runs a defaul registration test with a kuka arm mesh :param algorithm: :return: """ assert algorithm in ["GoICP", "Super4PCS"] baseName = os.path.join(utils.getLabelFusionDataDir(), 'registration-output/robot-scene') pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp') robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp') robotMeshPointcloudFile = os.path.join(baseName, 'robot_mesh_pointcloud.vtp') pointCloud = ioUtils.readPolyData(pointCloudFile) robotMesh = ioUtils.readPolyData(robotMeshFile) robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile) pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01) # rotate the scene 90 degrees sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 90]) # sceneTransform = transformUtils.frameFromPositionAndRPY([-1, 0, 0], [0, 0, 0]) pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform) print pointCloud.GetNumberOfPoints() print robotMeshPointcloud.GetNumberOfPoints() sceneName = 'scene pointcloud' modelName = 'model pointcloud' vis.showPolyData(robotMeshPointcloud, modelName) vis.showPolyData(pointCloud, sceneName) self.view.resetCamera() self.view.forceRender() sceneToModelTransform = GlobalRegistration.runRegistration( algorithm, pointCloud, robotMeshPointcloud) modelToSceneTransform = sceneToModelTransform.GetLinearInverse() GlobalRegistration.showAlignedPointcloud(robotMeshPointcloud, modelToSceneTransform, modelName + " aligned", color=[1, 0, 0])
def testSuperPCS4(self): """ Test the SuperPCS4 algorithm on some default data :return: """ baseName = os.path.join(CorlUtils.getCorlDataDir(), 'registration-output/robot-scene') pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp') robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp') robotMeshPointcloudFile = os.path.join(baseName, 'robot_mesh_pointcloud.vtp') pointCloud = ioUtils.readPolyData(pointCloudFile) robotMesh = ioUtils.readPolyData(robotMeshFile) robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile) # PCS4 algorithm performs very differently if you remove the origin point # pointCloud = removeOriginPoints(pointCloud) pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01) sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 90]) pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform) # robotMeshPointcloud = shuffleAndShiftPoints(robotMeshPointcloud) # pointCloud = shuffleAndShiftPoints(pointCloud) print pointCloud.GetNumberOfPoints() print robotMeshPointcloud.GetNumberOfPoints() sceneName = 'scene pointcloud' modelName = 'model pointcloud' vis.showPolyData(robotMeshPointcloud, modelName) vis.showPolyData(pointCloud, sceneName) self.view.resetCamera() self.view.forceRender() sceneToModelTransform = SuperPCS4.run(pointCloud, robotMeshPointcloud) GlobalRegistration.showAlignedPointcloud(pointCloud, sceneToModelTransform, sceneName + " aligned")
def testICP(self, objectName, scenePointCloud=None, applyVoxelGrid=True): print "running ICP" visObj = om.findObjectByName(objectName) if scenePointCloud is None: scenePointCloud = om.findObjectByName( 'cropped pointcloud').polyData modelPointcloud = filterUtils.transformPolyData( visObj.polyData, visObj.actor.GetUserTransform()) if applyVoxelGrid: modelPointcloud = segmentation.applyVoxelGrid(modelPointcloud, leafSize=0.0005) sceneToModelTransform = segmentation.applyICP(scenePointCloud, modelPointcloud) modelToSceneTransform = sceneToModelTransform.GetLinearInverse() concatenatedTransform = transformUtils.concatenateTransforms( [visObj.actor.GetUserTransform(), modelToSceneTransform]) visObj.getChildFrame().copyFrame(concatenatedTransform) print "ICP finished"
def loadCube(subdivisions=30): d = DebugData() dim = np.array([0.11,0.11,0.13]) center = np.array([0,0,0]) d.addCube(dim, center, subdivisions=subdivisions) polyData = d.getPolyData() # set vertex colors of top face to green points = vnp.getNumpyFromVtk(polyData, 'Points') colors = vnp.getNumpyFromVtk(polyData, 'RGB255') maxZ = points[:,2].max() inds = points[:,2] > (maxZ - 0.0001) colors[inds] = [0, 255, 0] visObj = vis.showPolyData(polyData, 'tissue_box_subdivision', colorByName='RGB255') print "number of points = ", polyData.GetNumberOfPoints() sampledPolyData = segmentation.applyVoxelGrid(polyData, leafSize=0.0001) visObj2 = vis.showPolyData(sampledPolyData, 'voxel grid', color=[0,1,0]) print "voxel number of points ", sampledPolyData.GetNumberOfPoints() return (visObj, visObj2)
baseName = os.path.join(labelfusion.utils.getLabelFusionDataDir(), 'registration-output/robot-scene') pointCloudFile = os.path.join(baseName, 'robot_mesh.vtp') robotMeshFile = os.path.join(baseName, 'robot_mesh.vtp') robotMeshPointcloudFile = os.path.join(baseName, 'robot_mesh_pointcloud.vtp') pointCloud = ioUtils.readPolyData(pointCloudFile) robotMesh = ioUtils.readPolyData(robotMeshFile) robotMeshPointcloud = ioUtils.readPolyData(robotMeshPointcloudFile) # PCS4 algorithm performs very differently if you remove the origin point #pointCloud = removeOriginPoints(pointCloud) pointCloud = segmentation.applyVoxelGrid(pointCloud, leafSize=0.01) sceneTransform = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 90]) pointCloud = filterUtils.transformPolyData(pointCloud, sceneTransform) #robotMeshPointcloud = shuffleAndShiftPoints(robotMeshPointcloud) #pointCloud = shuffleAndShiftPoints(pointCloud) print pointCloud.GetNumberOfPoints() print robotMeshPointcloud.GetNumberOfPoints() sceneName = 'scene pointcloud' modelName = 'model pointcloud' vis.showPolyData(robotMeshPointcloud, modelName) vis.showPolyData(pointCloud, sceneName)
useVoxelGrid = False reorientNormals = True testNormals = False showGlyphs = False testPlaneSegmentation = True if reorientNormals: polyData = filterUtils.computeNormals(polyData) if removeGround: groundPoints, scenePoints = segmentation.removeGround(polyData) vis.showPolyData(groundPoints, 'ground', visible=False) polyData = scenePoints if useVoxelGrid: polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02) if testNormals: print 'computing normals...' f = vtk.vtkRobustNormalEstimator() f.SetInput(polyData) f.SetMaxIterations(100) f.SetMaxEstimationError(0.01) f.SetMaxCenterError(0.02) f.SetComputeCurvature(True) f.SetRadius(0.1) f.Update() polyData = shallowCopy(f.GetOutput()) print 'done.'
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()
robotMesh = ioUtils.readPolyData('robot-mesh.vtp') t = transformUtils.frameFromPositionAndRPY([0.0, 0.0, 2.0], [90, 0, 0]) robotMesh = filterUtils.transformPolyData(robotMesh, t) robotMesh.GetPointData().SetNormals(None) obj = vis.showPolyData(robotMesh, 'robot mesh', visible=False) subd = vtk.vtkLoopSubdivisionFilter() subd.SetInput(robotMesh) subd.SetNumberOfSubdivisions(3) subd.Update() subdividedMesh = subd.GetOutput() modelPoints = segmentation.applyVoxelGrid(subdividedMesh, leafSize=0.005) vis.showPolyData(modelPoints, 'model points') print 'model pts:', modelPoints.GetNumberOfPoints() pointCloud = ioUtils.readPolyData('pointcloud.vtp') obj = vis.showPolyData(pointCloud, 'pointcloud original') ''' t = transformUtils.frameFromPositionAndRPY([0.2, 0.3, 0.4], [10,14,15]) pointCloud = filterUtils.transformPolyData(pointCloud, t) scenePoints = segmentation.applyVoxelGrid(pointCloud, leafSize=0.005) vis.showPolyData(scenePoints, 'scene points') print 'scene pts:', scenePoints.GetNumberOfPoints() ''' testAlign('model points', 'pointcloud original')
showGlyphs = False testPlaneSegmentation = True if reorientNormals: polyData = filterUtils.computeNormals(polyData) if removeGround: groundPoints, scenePoints = segmentation.removeGround(polyData) vis.showPolyData(groundPoints, 'ground', visible=False) polyData = scenePoints if useVoxelGrid: polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02) if testNormals: print 'computing normals...' f = vtk.vtkRobustNormalEstimator() f.SetInput(polyData) f.SetMaxIterations(100) f.SetMaxEstimationError(0.01) f.SetMaxCenterError(0.02) f.SetComputeCurvature(True) f.SetRadius(0.1) f.Update() polyData = shallowCopy(f.GetOutput()) print 'done.'
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()