def extractClusters(polyData, clusterInXY=False, **kwargs):
    ''' Segment a single point cloud into smaller clusters
        using Euclidean Clustering
     '''

    if not polyData.GetNumberOfPoints():
        return []

    if (clusterInXY == True):
        ''' If Points are seperated in X&Y, then cluster outside this '''
        polyDataXY = vtk.vtkPolyData()
        polyDataXY.DeepCopy(polyData)
        points=vtkNumpy.getNumpyFromVtk(polyDataXY , 'Points') # shared memory
        points[:,2] = 0.0
        #showPolyData(polyDataXY, 'polyDataXY', visible=False, parent=getDebugFolder())
        polyDataXY = applyEuclideanClustering(polyDataXY, **kwargs)
        clusterLabels = vtkNumpy.getNumpyFromVtk(polyDataXY, 'cluster_labels')
        vtkNumpy.addNumpyToVtk(polyData, clusterLabels, 'cluster_labels')

    else:
        polyData = applyEuclideanClustering(polyData, **kwargs)
        clusterLabels = vtkNumpy.getNumpyFromVtk(polyData, 'cluster_labels')


    clusters = []
    for i in xrange(1, clusterLabels.max() + 1):
        cluster = thresholdPoints(polyData, 'cluster_labels', [i, i])
        clusters.append(cluster)
    return clusters
    def findFarRightCorner(self, polyData, linkFrame):
        '''
        Within a point cloud find the point to the far right from the link
        The input is typically the 4 corners of a minimum bounding box
        '''

        diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
        diagonalTransform.Concatenate(linkFrame)
        vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)

        #polyData = shallowCopy(polyData)
        points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')

        viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
        viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
        viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
        viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
        #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
        #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')

        vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
        farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
        points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
        return points[farRightIndex,:]
예제 #3
0
    def findFarRightCorner(self, polyData, linkFrame):
        '''
        Within a point cloud find the point to the far right from the link
        The input is typically the 4 corners of a minimum bounding box
        '''

        diagonalTransform = transformUtils.frameFromPositionAndRPY([0,0,0], [0,0,45])
        diagonalTransform.Concatenate(linkFrame)
        vis.updateFrame(diagonalTransform, 'diagonal frame', parent='cont debug', visible=False)

        #polyData = shallowCopy(polyData)
        points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')

        viewOrigin = diagonalTransform.TransformPoint([0.0, 0.0, 0.0])
        viewX = diagonalTransform.TransformVector([1.0, 0.0, 0.0])
        viewY = diagonalTransform.TransformVector([0.0, 1.0, 0.0])
        viewZ = diagonalTransform.TransformVector([0.0, 0.0, 1.0])
        #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
        #polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')

        vis.updatePolyData( polyData, 'cornerPoints', parent='cont debug', visible=False)
        farRightIndex = vtkNumpy.getNumpyFromVtk(polyData, 'distance_along_foot_y').argmin()
        points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
        return points[farRightIndex,:]
def fitDrillBarrel ( drillPoints, forwardDirection, plane_origin, plane_normal):
    ''' Given a point cloud which ONLY contains points from a barrell drill, standing upright
        and the equations of a table its resting on, and the general direction of the robot
        Fit a barrell drill
     '''

    if not drillPoints.GetNumberOfPoints():
        return

    vis.updatePolyData(drillPoints, 'drill cluster', parent=getDebugFolder(), visible=False)
    drillBarrelPoints = thresholdPoints(drillPoints, 'dist_to_plane', [0.177, 0.30])

    if not drillBarrelPoints.GetNumberOfPoints():
        return


    # fit line to drill barrel points
    linePoint, lineDirection, _ = applyLineFit(drillBarrelPoints, distanceThreshold=0.5)

    if np.dot(lineDirection, forwardDirection) < 0:
        lineDirection = -lineDirection

    vis.updatePolyData(drillBarrelPoints, 'drill barrel points', parent=getDebugFolder(), visible=False)


    pts = vtkNumpy.getNumpyFromVtk(drillBarrelPoints, 'Points')

    dists = np.dot(pts-linePoint, lineDirection)

    p1 = linePoint + lineDirection*np.min(dists)
    p2 = linePoint + lineDirection*np.max(dists)

    p1 = projectPointToPlane(p1, plane_origin, plane_normal)
    p2 = projectPointToPlane(p2, plane_origin, plane_normal)


    d = DebugData()
    d.addSphere(p1, radius=0.01)
    d.addSphere(p2, radius=0.01)
    d.addLine(p1, p2)
    vis.updatePolyData(d.getPolyData(), 'drill debug points', color=[0,1,0], parent=getDebugFolder(), visible=False)


    drillToBasePoint = np.array([-0.07,  0.0  , -0.12])

    zaxis = plane_normal
    xaxis = lineDirection
    xaxis /= np.linalg.norm(xaxis)
    yaxis = np.cross(zaxis, xaxis)
    yaxis /= np.linalg.norm(yaxis)
    xaxis = np.cross(yaxis, zaxis)
    xaxis /= np.linalg.norm(xaxis)

    t = getTransformFromAxes(xaxis, yaxis, zaxis)
    t.PreMultiply()
    t.Translate(-drillToBasePoint)
    t.PostMultiply()
    t.Translate(p1)

    return t
def labelPointDistanceAlongAxis(polyData, axis, origin=None, resultArrayName='distance_along_axis'):

    points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
    if origin is not None:
        points = points - origin
    distanceValues = np.dot(points, axis)
    if origin is None:
        distanceValues -= np.nanmin(distanceValues)
    newData = shallowCopy(polyData)
    vtkNumpy.addNumpyToVtk(newData, distanceValues, resultArrayName)
    return newData
    def getRecedingTerrainRegion(self, polyData, linkFrame):
        ''' Find the point cloud in front of the foot frame'''

        #polyData = shallowCopy(polyData)
        points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')

        viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
        viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
        viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
        viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')

        polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
        polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
        polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])

        vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
        return polyData
예제 #7
0
    def getRecedingTerrainRegion(self, polyData, linkFrame):
        ''' Find the point cloud in front of the foot frame'''

        #polyData = shallowCopy(polyData)
        points = vtkNumpy.getNumpyFromVtk(polyData, 'Points')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,0].copy(), 'x')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,1].copy(), 'y')
        #vtkNumpy.addNumpyToVtk(polyData, points[:,2].copy(), 'z')

        viewOrigin = linkFrame.TransformPoint([0.0, 0.0, 0.0])
        viewX = linkFrame.TransformVector([1.0, 0.0, 0.0])
        viewY = linkFrame.TransformVector([0.0, 1.0, 0.0])
        viewZ = linkFrame.TransformVector([0.0, 0.0, 1.0])
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewX, origin=viewOrigin, resultArrayName='distance_along_foot_x')
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewY, origin=viewOrigin, resultArrayName='distance_along_foot_y')
        polyData = segmentation.labelPointDistanceAlongAxis(polyData, viewZ, origin=viewOrigin, resultArrayName='distance_along_foot_z')

        polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_x', [0.12, 1.6])
        polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_y', [-0.4, 0.4])
        polyData = segmentation.thresholdPoints(polyData, 'distance_along_foot_z', [-0.4, 0.4])

        vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='cont debug', visible=True)
        return polyData
    def findMinimumBoundingRectangle(self, polyData, linkFrame):
        '''
        Find minimum bounding rectangle.
        The input is assumed to be a rectangular point cloud of cinder blocks
        Returns transform of far right corner (pointing away from robot)
        '''
        # TODO: for non-z up surfaces, this needs work
        # TODO: return other parameters

        # Originally From: https://github.com/dbworth/minimum-area-bounding-rectangle
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
        #vis.updatePolyData( polyData, 'block top', parent='cont debug', visible=False)
        polyDataCentroid = segmentation.computeCentroid(polyData)
        pts =vtkNumpy.getNumpyFromVtk( polyData , 'Points' )

        xy_points =  pts[:,[0,1]]
        vis.updatePolyData( get2DAsPolyData(xy_points) , 'xy_points', parent='cont debug', visible=False)
        hull_points = qhull_2d.qhull2D(xy_points)
        vis.updatePolyData( get2DAsPolyData(hull_points) , 'hull_points', parent='cont debug', visible=False)
        # Reverse order of points, to match output from other qhull implementations
        hull_points = hull_points[::-1]
        # print 'Convex hull points: \n', hull_points, "\n"

        # Find minimum area bounding rectangle
        (rot_angle, rectArea, rectDepth, rectWidth, center_point, corner_points_ground) = min_bounding_rect.minBoundingRect(hull_points)
        vis.updatePolyData( get2DAsPolyData(corner_points_ground) , 'corner_points_ground', parent='cont debug', visible=False)
        cornerPoints = np.vstack((corner_points_ground.T, polyDataCentroid[2]*np.ones( corner_points_ground.shape[0]) )).T
        cornerPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(cornerPoints)

        # Create a frame at the far right point - which points away from the robot
        farRightCorner = self.findFarRightCorner(cornerPolyData , linkFrame)
        viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
        robotYaw = math.atan2( viewDirection[1], viewDirection[0] )*180.0/np.pi
        blockAngle =  rot_angle*(180/math.pi)
        #print "robotYaw   ", robotYaw
        #print "blockAngle ", blockAngle
        blockAngleAll = np.array([blockAngle , blockAngle+90 , blockAngle+180, blockAngle+270])
        #print blockAngleAll
        for i in range(0,4):
            if(blockAngleAll[i]>180):
              blockAngleAll[i]=blockAngleAll[i]-360
        #print blockAngleAll
        values = abs(blockAngleAll - robotYaw)
        #print values
        min_idx = np.argmin(values)
        if ( (min_idx==1) or (min_idx==3) ):
            #print "flip rectDepth and rectWidth as angle is not away from robot"
            temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp

        #print "best angle", blockAngleAll[min_idx]
        rot_angle = blockAngleAll[min_idx]*math.pi/180.0


        # Optional: overwrite all of the above with the yaw of the robt when it was standing in front of the blocks:
        if (self.fixBlockYawWithInitial):
            rot_angle = self.initialRobotYaw

        cornerTransform = transformUtils.frameFromPositionAndRPY( farRightCorner , [0,0, np.rad2deg(rot_angle) ] )

        #print "Minimum area bounding box:"
        #print "Rotation angle:", rot_angle, "rad  (", rot_angle*(180/math.pi), "deg )"
        #print "rectDepth:", rectDepth, " rectWidth:", rectWidth, "  Area:", rectArea
        #print "Center point: \n", center_point # numpy array
        #print "Corner points: \n", cornerPoints, "\n"  # numpy array
        return cornerTransform, rectDepth, rectWidth, rectArea
예제 #9
0
    def findMinimumBoundingRectangle(self, polyData, linkFrame):
        '''
        Find minimum bounding rectangle.
        The input is assumed to be a rectangular point cloud of cinder blocks
        Returns transform of far right corner (pointing away from robot)
        '''
        # TODO: for non-z up surfaces, this needs work
        # TODO: return other parameters

        # Originally From: https://github.com/dbworth/minimum-area-bounding-rectangle
        polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.02)
        #vis.updatePolyData( polyData, 'block top', parent='cont debug', visible=False)
        polyDataCentroid = segmentation.computeCentroid(polyData)
        pts =vtkNumpy.getNumpyFromVtk( polyData , 'Points' )

        xy_points =  pts[:,[0,1]]
        vis.updatePolyData( get2DAsPolyData(xy_points) , 'xy_points', parent='cont debug', visible=False)
        hull_points = qhull_2d.qhull2D(xy_points)
        vis.updatePolyData( get2DAsPolyData(hull_points) , 'hull_points', parent='cont debug', visible=False)
        # Reverse order of points, to match output from other qhull implementations
        hull_points = hull_points[::-1]
        # print 'Convex hull points: \n', hull_points, "\n"

        # Find minimum area bounding rectangle
        (rot_angle, rectArea, rectDepth, rectWidth, center_point, corner_points_ground) = min_bounding_rect.minBoundingRect(hull_points)
        vis.updatePolyData( get2DAsPolyData(corner_points_ground) , 'corner_points_ground', parent='cont debug', visible=False)
        cornerPoints = np.vstack((corner_points_ground.T, polyDataCentroid[2]*np.ones( corner_points_ground.shape[0]) )).T
        cornerPolyData = vtkNumpy.getVtkPolyDataFromNumpyPoints(cornerPoints)

        # Create a frame at the far right point - which points away from the robot
        farRightCorner = self.findFarRightCorner(cornerPolyData , linkFrame)
        viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection()
        robotYaw = math.atan2( viewDirection[1], viewDirection[0] )*180.0/np.pi
        blockAngle =  rot_angle*(180/math.pi)
        #print "robotYaw   ", robotYaw
        #print "blockAngle ", blockAngle
        blockAngleAll = np.array([blockAngle , blockAngle+90 , blockAngle+180, blockAngle+270])
        #print blockAngleAll
        for i in range(0,4):
            if(blockAngleAll[i]>180):
              blockAngleAll[i]=blockAngleAll[i]-360
        #print blockAngleAll
        values = abs(blockAngleAll - robotYaw)
        #print values
        min_idx = np.argmin(values)
        if ( (min_idx==1) or (min_idx==3) ):
            #print "flip rectDepth and rectWidth as angle is not away from robot"
            temp = rectWidth ; rectWidth = rectDepth ; rectDepth = temp

        #print "best angle", blockAngleAll[min_idx]
        rot_angle = blockAngleAll[min_idx]*math.pi/180.0


        # Optional: overwrite all of the above with the yaw of the robt when it was standing in front of the blocks:
        if (self.fixBlockYawWithInitial):
            rot_angle = self.initialRobotYaw

        cornerTransform = transformUtils.frameFromPositionAndRPY( farRightCorner , [0,0, np.rad2deg(rot_angle) ] )

        #print "Minimum area bounding box:"
        #print "Rotation angle:", rot_angle, "rad  (", rot_angle*(180/math.pi), "deg )"
        #print "rectDepth:", rectDepth, " rectWidth:", rectWidth, "  Area:", rectArea
        #print "Center point: \n", center_point # numpy array
        #print "Corner points: \n", cornerPoints, "\n"  # numpy array
        return cornerTransform, rectDepth, rectWidth, rectArea