Пример #1
0
 def onSegmentGround():
     groundPoints, scenePoints = segmentation.removeGround(
         pointCloudObj.polyData)
     vis.showPolyData(groundPoints,
                      'ground points',
                      color=[0, 1, 0],
                      parent='segmentation')
     vis.showPolyData(scenePoints,
                      'scene points',
                      color=[1, 0, 1],
                      parent='segmentation')
     pickedObj.setProperty('Visible', False)
Пример #2
0
 def onSegmentGround():
     groundPoints, scenePoints =  segmentation.removeGround(pointCloudObj.polyData)
     vis.showPolyData(groundPoints, 'ground points', color=[0,1,0], parent='segmentation')
     vis.showPolyData(scenePoints, 'scene points', color=[1,0,1], parent='segmentation')
     pickedObj.setProperty('Visible', False)
Пример #3
0
app = ConsoleApp()

# create a view
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)
Пример #4
0
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,
    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()
Пример #6
0
    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()