使用numpy快速获取赛道边线

lzusa 发布于 2022-11-12 0 次阅读


之前在写python上的智能车赛道识别算法的时候,发现寻边线操作在python上非常耗时,同等计算量下速度和C++差了几个量级,就算使用cython编译为c仍然需要消耗很多时间。后面就自己想了一下不使用枚举的方法,直接使用numpy可以快速获得赛道边线。
本质是将canny处理后的边缘信息逐行处理,取出中点左侧的一个点和右侧的一个点就可以了,这个操作可以通过numpy快速实现,这个方法对canny处理后的边缘图像要求很高,需要在全程都有很好的识别效果,实测可以发现只要canny的参数调整够好,且赛道上没有特别明显的明暗变化,这个方法可以非常快的提取出边线信息。

    def searchRow(self, i: int, j: int, isRight: bool, draw: bool = False, color: Tuple[int] = None) -> int:
        "按行搜索左右的黑色"
        cdef int ans = 0
        if isRight:
            s = np.nonzero(self.canny[i,j:M-1])
            if len(s[0]) == 0:
                ans = M - 1
            else:
                ans = s[0][0] + j
        else:
            s = np.nonzero(self.canny[i,0:j])
            if len(s[0]) == 0:
                ans = 0
            else:
                ans = s[0][-1]    
        return ans
    def getEdge(self, draw: bool = False):
        self.checkLeft = 0
        lastside = [0, M - 1]
        left = []
        leftx = []
        right = []
        rightx = []
        cdef int J = 0
        cdef float width = 0
        cdef int[2] side
        cdef float pi
        cdef float[2] pj
        self.roundaboutChecker.reset()
        for u in range(2):
            self.fitter[u].reset()
            self.hillChecker[u].reset()
            self.pointEliminator[u].reset(u ^ 1, self.fitter[u], COLORS[u + 4])
            self.sideForkChecker[u].reset()
        #print(N - 1, self.I - 1)
        tot = 0
        for I in range(N - 1, self.I - 1, -2):
            #start = time.time()
            J = self.calcK(I, self.K)
            if testF1:
                side = [self.searchRow(I, J, u) for u in range(2)]
            else:
                side = [self.searchRow2(I, J, u) for u in range(2)]
            if lastside[0] - side[0] > 10 and self.checkLeft == 0:
                self.checkLeft = I
            lastside = side
            if not side[0] == 0 or not side[1] == M - 1:
                tot = tot + 1
            if I == N - 1:
                self.left = side[0]
                self.right = side[1]
            if not side[0] == 0 and I > FitC:
                left.append(side[0])
                leftx.append(I)
            if not side[1] == M - 1 and I > FitC:
                right.append(side[1])
                rightx.append(I)
            #lastside = side
            #print(side)
            self.canny2[I, side[0], 1] = 100
            self.canny2[I, side[1], 2] = 100
            pj = [0.0] * 2
            nolost = True
            #end = time.time()
            #print("side", end - start)
            #start = time.time()
            for u in range(2):
                if self.checkJ(side[u]):
                    pi, pj[u] = axisTransform(I, side[u], self.PERMAT)
                    self.sideForkChecker[u].update(pi, pj[u])
                    if I > length or (I <= length and tot < totC):
                        self.pointEliminator[u].update(pi, pj[u])
                    if I < HILL_CUT:
                        self.hillChecker[u].update(-pj[u] if u else pj[u])
                else:
                    nolost = False
                    self.sideForkChecker[u].lost()
            if nolost:
                width = pj[1] - pj[0]
                #print(width)
                self.roundaboutChecker.update(width, pi, side[0], -side[1])
            else:
                self.roundaboutChecker.lost()
        reg1 = []
        reg2 = []
        k1 = []
        k2 = []
        if len(left) >= Scheck2:
            k1,reg1,_,_,_=np.polyfit(leftx,left,1,full = True)
        if len(reg1) == 0:
            k1 = [10]
            reg1 = [Scheck * 2]
        if len(right) >= Scheck2:
            k2,reg2,_,_,_=np.polyfit(rightx,right,1,full = True)
        if len(reg2) == 0:
            k2 = [10]
            reg2 = [Scheck * 2]
        #print(reg1)
        #print(reg2)
        self.S = (reg1[0] + reg2[0]) / 2
        self.leftK = abs(k1[0])
        self.rightK = abs(k2[0])
            #end = time.time()
            #print("ot", end - start)
看烟花已落,你我仍是陌路人
最后更新于 2022-11-12