6、经典匹配代价计算方法python

---

1、AD

AD算法将左右图像灰度值之差做为匹配代价，差值最小的认为就是匹配点。

def AD(self, imgL, imgR):
        t1 = time.time()
        size=np.shape(imgL)[0:2]
        h=size[0]
        w=size[1]
        cost_ad = np.zeros((h, w-self.max_disp, self.max_disp),dtype=np.int64)
        # 方法一
        for x in range(self.max_disp,w):
            for n in range(self.max_disp):
                cost_ad[:,x-self.max_disp,n] = np.abs(imgL[:,x] - imgR[:,x-n])
        
        # 方法二
        # for m in range(self.max_disp):
        #     cost_ad[:,:,m] = np.abs(imgL[:,self.max_disp:w] - imgR[:,m:w-self.max_disp+m])

        print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        return cost_ad
        # 直接输出视差图
        # dispmap=np.zeros((cost_ad.shape[0], cost_ad.shape[1]),dtype=np.int64)
        # for x in range(0,cost_ad.shape[1]):
        #     for y in range(0,cost_ad.shape[0]):
        #         dispmap[y,x]=cost_ad[y,x,:].argsort()[-1]
        # return dispmap

2、SAD

SAD算法将左图窗口与右图窗口对应像素点灰度差的绝对值之和做为匹配代价，值最小的认为就是匹配点。

def SAD(self, imgL, imgR):
        t1 = time.time()
        size=np.shape(imgL)[0:2]
        h=size[0]
        w=size[1]
        #cp是变换中心点的坐标，0，0点作为左上角的窗口中，中心点就是(same,same)
        offsets = [(u, v) for v in range(self.block) for u in range(self.block)]
        block_cost = np.zeros((h-2*self.radius, w-2*(self.max_disp+self.radius), self.max_disp, len(offsets)), dtype=np.int64)

        cost_sad = np.zeros((h-2*self.radius, w-2*(self.max_disp+self.radius), self.max_disp),dtype=np.int64)
        
        for m in range(self.max_disp):
            n = 0
            for u,v in offsets:
                block_cost[:,:,m,n] = imgL[v:v+h-2*self.radius, self.max_disp+u:u+w-self.max_disp-2*self.radius] - \
                                    imgR[v:v+h-2*self.radius, u+m:u+m+w-2*(self.radius+self.max_disp)]
                n += 1
        cost_sad[:,:,:] = np.sum(np.abs(block_cost), axis=3)
        

        print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        
        return cost_sad

3、Census

Census算法在图像区域定义一个矩形窗口，用这个矩形窗口遍历整幅图像。选取中心像素作为参考像素，将矩形窗口中每个像素的灰度值与参考像素的灰度值进行比较，灰度值小于或等于参考值的像素标记为0，大于参考值的像素标记为1，最后再将它们按位连接，得到变换后的结果，变换后的结果是由0和1组成的二进制码流。

def Census(self, imgL, imgR):
        #t1 = time.time()
        size=np.shape(imgL)[0:2]
        h=size[0]
        w=size[1]
        
        #cp是变换中心点的坐标，0，0点作为左上角的窗口中，中心点就是(same,same)
        offsets = [(u, v) for v in range(self.block) for u in range(self.block) if not u == self.radius == v]
        #offset是分别衡量某个点作为窗口左上角是否匹配成功，作为窗口左上角向左平移一个匹配是否成功，一直到作为窗口右下角匹配是否成功，都有衡量
        censusL = np.zeros((h-2*self.radius, w-2*self.radius, len(offsets)), dtype=np.int64)
        censusR = np.zeros((h-2*self.radius, w-2*self.radius, len(offsets)), dtype=np.int64)
        #census就是经过变换后的图像

        
        cpointL = imgL[self.radius:h-self.radius, self.radius:w-self.radius]
        cpointR = imgR[self.radius:h-self.radius, self.radius:w-self.radius]
        n = 0
        for u,v in offsets:
                censusL[:,:,n] = (imgL[v:v+h-2*self.radius, u:u+w-2*self.radius] >= cpointL)
                censusR[:,:,n] = (imgR[v:v+h-2*self.radius, u:u+w-2*self.radius] >= cpointR)
                n += 1

        c_h, c_w = np.shape(censusL)[0:2]
        cost_census = np.zeros((c_h, c_w-self.max_disp, self.max_disp),dtype=np.int64)


        for x in range(self.max_disp,c_w):
            for n in range(self.max_disp):
                cost_census[:,x-self.max_disp,n] = np.sum(np.logical_or(censusL[:,x,:], censusR[:,x-n,:]),axis=1)
               
         
        #print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        
        return cost

4、CensusAD

CensunAD就是AD和Census的结合体。由于二者的尺度不一致（Census为0 ~ maxdisparty，AD为0 ~ 255），所以要对二者进行归一化再相加，得到匹配代价。

def CensusAD(self, imgL, imgR):

        #t1 = time.time()
        size=np.shape(imgL)[0:2]
        h=size[0]
        w=size[1]
        
        #cp是变换中心点的坐标，0，0点作为左上角的窗口中，中心点就是(same,same)
        offsets = [(u, v) for v in range(self.block) for u in range(self.block) if not u == self.radius == v]
        #offset是分别衡量某个点作为窗口左上角是否匹配成功，作为窗口左上角向左平移一个匹配是否成功，一直到作为窗口右下角匹配是否成功，都有衡量
        censusL = np.zeros((h-2*self.radius, w-2*self.radius, len(offsets)), dtype=np.int64)
        censusR = np.zeros((h-2*self.radius, w-2*self.radius, len(offsets)), dtype=np.int64)
        #census就是经过变换后的图像

        
        cpointL = imgL[self.radius:h-self.radius, self.radius:w-self.radius]
        cpointR = imgR[self.radius:h-self.radius, self.radius:w-self.radius]
        n = 0
        for u,v in offsets:
                censusL[:,:,n] = (imgL[v:v+h-2*self.radius, u:u+w-2*self.radius] >= cpointL)
                censusR[:,:,n] = (imgR[v:v+h-2*self.radius, u:u+w-2*self.radius] >= cpointR)
                n += 1

        c_h, c_w = np.shape(censusL)[0:2]
        cost_census = np.zeros((c_h, c_w-self.max_disp, self.max_disp),dtype=np.int64)
        cost_ad = np.zeros((c_h, c_w-self.max_disp, self.max_disp),dtype=np.int64)


        for x in range(self.max_disp,c_w):
            for n in range(self.max_disp):
                cost_census[:,x-self.max_disp,n] = np.sum(np.logical_or(censusL[:,x,:], censusR[:,x-n,:]),axis=1)
                cost_ad[:,x-self.max_disp,n] = np.abs(imgL[self.radius:h-self.radius,x+self.radius] - imgR[self.radius:h-self.radius,x+self.radius-n])
        
         
        cost_census = 1 - np.exp(-(cost_census))
        cost_ad     = 1 - np.exp(-(cost_ad/self.lamda))
        cost        = cost_census + cost_ad

        #print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        
        return cost

5、代价计算类

import time
import cv2
import numpy as np


class Compute_Costs(object):
    def __init__(self, h, w, max_disp, block, radius, lamda):
        self.w        = w
        self.h        = h
        self.max_disp = max_disp
        self.block    = block
        self.radius   = radius
        self.lamda    = lamda
        # -----------------AD------------------------- #
        self.cost_ad = np.zeros((h, w-max_disp, max_disp),dtype=np.int64)
        # -----------------SAD------------------------- #
        self.SAD_Grids = [(u, v) for v in range(block) for u in range(block)]
        self.block_cost = np.zeros((h-2*radius, w-2*(max_disp+radius), max_disp, len(self.SAD_Grids)), dtype=np.int64)
        self.cost_sad = np.zeros((h-2*radius, w-2*(max_disp+radius), max_disp),dtype=np.int64)
        # ------------------Census------------------------ #
        self.Census_Grids = [(u, v) for v in range(self.block) for u in range(self.block) if not u == self.radius == v]
        self.censusL = np.zeros((h-2*radius, w-2*radius, len(self.Census_Grids)), dtype=np.int64)
        self.censusR = np.zeros((h-2*radius, w-2*radius, len(self.Census_Grids)), dtype=np.int64)
        self.cost_census = np.zeros((h-2*radius, w-2*radius-self.max_disp, self.max_disp),dtype=np.int64)
        # ------------------CensusAD------------------------ #
        
        self.cost_censusad_ad = np.zeros((h-2*radius, w-2*radius-self.max_disp, self.max_disp),dtype=np.int64)
        self.cost_censusad_census = np.zeros((h-2*radius, w-2*radius-self.max_disp, self.max_disp),dtype=np.int64)


    # ================ AD ======================= # 
    def AD(self, imgL, imgR):
        t1 = time.time()
        # 方法一
        for x in range(self.max_disp,self.w):
            for n in range(self.max_disp):
                self.cost_ad[:,x-self.max_disp,n] = np.abs(imgL[:,x] - imgR[:,x-n])
        
        # 方法二
        # for m in range(self.max_disp):
        #     self.cost_ad[:,:,m] = np.abs(imgL[:,self.max_disp:w] - imgR[:,m:w-self.max_disp+m])

        print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        
        return self.cost_ad

    # ============== SAD ========================= #
    def SAD(self, imgL, imgR):
        t1 = time.time()

        for m in range(self.max_disp):
            n = 0
            for u,v in self.SAD_Grids:
                self.block_cost[:,:,m,n] = imgL[v:v+self.h-2*self.radius, self.max_disp+u:u+self.w-self.max_disp-2*self.radius] - \
                                    imgR[v:v+self.h-2*self.radius, u+m:u+m+self.w-2*(self.radius+self.max_disp)]
                n += 1
        self.cost_sad[:,:,:] = np.sum(np.abs(self.block_cost), axis=3)
        
        print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        
        return self.cost_sad

    # ================ Census ======================= #
    def Census(self, imgL, imgR):
        t1 = time.time()
        cpointL = imgL[self.radius:self.h-self.radius, self.radius:self.w-self.radius]
        cpointR = imgR[self.radius:self.h-self.radius, self.radius:self.w-self.radius]
        n = 0
        for u,v in self.Census_Grids:
                self.censusL[:,:,n] = (imgL[v:v+self.h-2*self.radius, u:u+self.w-2*self.radius] >= cpointL)
                self.censusR[:,:,n] = (imgR[v:v+self.h-2*self.radius, u:u+self.w-2*self.radius] >= cpointR)
                n += 1

        for x in range(self.max_disp,self.censusL.shape[1]):
            for n in range(self.max_disp):
                self.cost_census[:,x-self.max_disp,n] = np.sum(np.logical_or(self.censusL[:,x,:], self.censusR[:,x-n,:]),axis=1)
               
        print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))
        
        return self.cost_census

    # ================ CensusAD ======================= #
    def CensusAD(self, imgL, imgR):

        t1 = time.time()
        cpointL = imgL[self.radius:self.h-self.radius, self.radius:self.w-self.radius]
        cpointR = imgR[self.radius:self.h-self.radius, self.radius:self.w-self.radius]
        n = 0
        for u,v in self.Census_Grids:
                self.censusL[:,:,n] = (imgL[v:v+self.h-2*self.radius, u:u+self.w-2*self.radius] >= cpointL)
                self.censusR[:,:,n] = (imgR[v:v+self.h-2*self.radius, u:u+self.w-2*self.radius] >= cpointR)
                n += 1

        for x in range(self.max_disp,self.censusL.shape[1]):
            for n in range(self.max_disp):
                self.cost_censusad_census[:,x-self.max_disp,n] = np.sum(np.logical_or(self.censusL[:,x,:], self.censusR[:,x-n,:]),axis=1)
                self.cost_censusad_ad[:,x-self.max_disp,n] = np.abs(imgL[self.radius:self.h-self.radius,x+self.radius] - imgR[self.radius:self.h-self.radius,x+self.radius-n])
        
         
        self.cost_censusad_census = 1 - np.exp(-( self.cost_censusad_census))
        self.cost_censusad_ad = 1 - np.exp(-(self.cost_censusad_ad/self.lamda))
        cost        = self.cost_censusad_census + self.cost_censusad_ad

        print('\t(computed costs in {:.2f}s)'.format(time.time()-t1))

        return cost
    # ======================================= #

if __name__ == '__main__':

    imgL=np.asanyarray(cv2.imread('./data/test/left_000.png',0),dtype=np.int64)
    imgR=np.asanyarray(cv2.imread('./data/test/right_000.png',0),dtype=np.int64)

    tic1=time.time()

    maxDisparity=12 # 最大视差，也就是搜索范围，搜索范围越大耗时越长
                    # 可以根据需求的有效测距范围来大致计算出最小和最大视差

    window_size = 3               #滑动窗口大小，窗口越大匹配成功难度越高
    radius=np.int64(window_size/2)#窗口大小的一半向下取整，比如用来衡量窗口左上角与窗口中点的距离的x与y坐标
    lamda = 20

    h,w = imgL.shape[0:2]
    
    compute_costs = Compute_Costs(h,w,maxDisparity, window_size, radius, lamda)

    # 代价计算
    #costs = compute_costs.AD(imgL, imgR)
    #costs = compute_costs.SAD(imgL, imgR)
    #costs = compute_costs.Census(imgL, imgR)
    costs = compute_costs.CensusAD(imgL, imgR)

    # costs的第三维是按视差0到maxDisparity排列的，所以其最小值的索引值就是视差
    disparity = np.argsort(costs,axis=2)[:,:,0] 


    # 生成深度图（颜色图）
    dis_color = disparity # disparity
    dis_color = cv2.normalize(dis_color, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
    dis_color = cv2.applyColorMap(dis_color, 2)

    print('用时:',time.time()-tic1)

    cv2.imshow('dispmap',dis_color)
    cv2.waitKey()