OpenCV雙目稠密匹配BM算法源代碼詳細(xì)解析

template <typename mType>static voidfindStereoCorrespondenceBM( const Mat& left, const Mat& right,
                           Mat& disp, Mat& cost, const StereoBMParams& state,
                           uchar* buf, int _dy0, int _dy1, const int disp_shift ){
    // opencv代碼的特點(diǎn)：1.空間換時(shí)間：申請(qǐng)足夠大的內(nèi)存，預(yù)先計(jì)算出可以復(fù)用的數(shù)據(jù)并保存，后期直接查表使用；    // 		       2.非常好地定義和使用了各種指針和申請(qǐng)的內(nèi)存。
    const int ALIGN = 16;
    int x, y, d;
    int wsz = state.SADWindowSize, wsz2 = wsz/2;
    int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); // dy0, dy1 是滑動(dòng)窗口中心點(diǎn)到窗口第一行和最后一行的距離，							  // 由于一般使用奇數(shù)大小的方形窗口，因此可以認(rèn)為dy0 = dy1 = wsz2    int ndisp = state.numDisparities;
    int mindisp = state.minDisparity;
    int lofs = MAX(ndisp - 1 + mindisp, 0);
    int rofs = -MIN(ndisp - 1 + mindisp, 0);
    int width = left.cols, height = left.rows;
    int width1 = width - rofs - ndisp + 1;
    int ftzero = state.preFilterCap; // 這里是前面預(yù)處理做x方向的sobel濾波時(shí)的截?cái)嘀?，默認(rèn)為31.				     // 預(yù)處理的結(jié)果并不是sobel濾波的直接結(jié)果，而是做了截?cái)啵?/span>				     // 濾波后的值如果小于-preFilterCap，則說說明紋理較強(qiáng)，結(jié)果為0；				     // 如果大于preFilterCap，則說明紋理強(qiáng)，結(jié)果為2*prefilterCap;				     // 如果濾波后結(jié)果在[-prefilterCap, preFilterCap]之間（區(qū)間表示，下同），對(duì)應(yīng)取[0, 2*preFilterCap]。    int textureThreshold = state.textureThreshold;
    int uniquenessRatio = state.uniquenessRatio;
    mType FILTERED = (mType)((mindisp - 1) << disp_shift);

    int *sad, *hsad0, *hsad, *hsad_sub, *htext;
    uchar *cbuf0, *cbuf;
    const uchar* lptr0 = left.ptr() + lofs;
    const uchar* rptr0 = right.ptr() + rofs;
    const uchar *lptr, *lptr_sub, *rptr;
    mType* dptr = disp.ptr<mType>();
    int sstep = (int)left.step;
    int dstep = (int)(disp.step/sizeof(dptr[0]));
    int cstep = (height+dy0+dy1)*ndisp;
    int costbuf = 0;
    int coststep = cost.data ? (int)(cost.step/sizeof(costbuf)) : 0;
    const int TABSZ = 256;
    uchar tab[TABSZ];

    sad = (int*)alignPtr(buf + sizeof(sad[0]), ALIGN); // 注意到sad的前面留了一個(gè)sizeof(sad[0])的位置，函數(shù)最后要用到。    hsad0 = (int*)alignPtr(sad + ndisp + 1 + dy0*ndisp, ALIGN); // 這里額外說一句，opencv每次確定變量的字節(jié)數(shù)時(shí)都直接使用變量而不是int, double等類型，								// 這樣當(dāng)變量類型變化時(shí)可以少修改代碼。    htext = (int*)alignPtr((int*)(hsad0 + (height+dy1)*ndisp) + wsz2 + 2, ALIGN);
    cbuf0 = (uchar*)alignPtr((uchar*)(htext + height + wsz2 + 2) + dy0*ndisp, ALIGN);

    // 建立映射表，方便后面直接引用。以之前的x方向的sobel濾波的截?cái)嘀禐橹行?，距離這個(gè)截?cái)嘀翟竭h(yuǎn)，說明紋理越強(qiáng)。    for( x = 0; x < TABSZ; x++ )
        tab[x] = (uchar)std::abs(x - ftzero);

    // initialize buffers    memset( hsad0 - dy0*ndisp, 0, (height + dy0 + dy1)*ndisp*sizeof(hsad0[0]) );
    memset( htext - wsz2 - 1, 0, (height + wsz + 1)*sizeof(htext[0]) );

    // 首先初始化計(jì)算左圖 x 在[-wsz2 - 1, wsz2), y 在[-dy0, height + dy1) 范圍內(nèi)的各個(gè)像素，    // 右圖視差為[0. ndisp)像素之間的SAD.
    // 注意這里不處理 wsz2 列，并且是從-wsz2 - 1 列開始，（這一列不在第一個(gè)窗口[-wsz2, wsz2]中），    // 這是為了后續(xù)處理時(shí)邏輯統(tǒng)一和代碼簡化的需要。這樣就可以在處理第一個(gè)滑動(dòng)窗口時(shí)和處理之后的窗口一樣，    // 剪掉滑出窗口的第一列的數(shù)據(jù) (-wsz2 - 1)，加上新一列的數(shù)據(jù) (wsz2)。    for( x = -wsz2-1; x < wsz2; x++ )
    {
	// 統(tǒng)一先往上減去半個(gè)窗口乘以ndisp的距離。        hsad = hsad0 - dy0*ndisp; // 結(jié)合下面的循環(huán)代碼和內(nèi)存示意圖，hsad是累加的，每次回退dy0就好。	cbuf = cbuf0 + (x + wsz2 + 1)*cstep - dy0*ndisp; // 而cbuf, lptr, rptr 需要根據(jù)當(dāng)前在不同x列的需要，移動(dòng)指針指向當(dāng)前所處理的列。        lptr = lptr0 + std::min(std::max(x, -lofs), width-lofs-1) - dy0*sstep; // 前面的min, max 是為了防止內(nèi)存越界而進(jìn)行的判斷。        rptr = rptr0 + std::min(std::max(x, -rofs), width-rofs-ndisp) - dy0*sstep;
		
	// 從SAD窗口的第一個(gè)像素開始。	// 循環(huán)都是以當(dāng)前列為主，先處理當(dāng)前列不同行的像素。        for( y = -dy0; y < height + dy1; y++, hsad += ndisp, cbuf += ndisp, lptr += sstep, rptr += sstep )
        {
            int lval = lptr[0];
            d = 0;

	    // 計(jì)算不同視差d 的SAD。            for( ; d < ndisp; d++ )
            {
                int diff = std::abs(lval - rptr[d]); // SAD.                cbuf[d] = (uchar)diff; // 存儲(chǔ)該列所有行各個(gè)像素在所有視差下的sad，所以cbuf的大小為wsz * cstep.                hsad[d] = (int)(hsad[d] + diff); // 累加同一行內(nèi)，[-wsz2 - 1, wsz2) 像素，不同d下的SAD（預(yù)先進(jìn)行一點(diǎn)cost aggregation）。            }
            htext[y] += tab[lval]; // 利用之前的映射表，統(tǒng)計(jì)一行內(nèi)，窗口大小寬度，左圖像素的紋理度。				   // 注意到y(tǒng)是從-dy0開始的，而前面buf分配指針位置、hsad0和htext初始化為0的時(shí)候已經(jīng)考慮到這一點(diǎn)了，				   // 特別是分配各個(gè)指針指向的內(nèi)存大小的時(shí)候，分別都分配了下一個(gè)指針變量要往上減去的對(duì)應(yīng)的內(nèi)存大小。				   // 讀者可以自己回去看alighPtr語句部分和memset部分。        }
    }

    // initialize the left and right borders of the disparity map    // 初始化圖像左右邊界。    for( y = 0; y < height; y++ )
    {
        for( x = 0; x < lofs; x++ )
            dptr[y*dstep + x] = FILTERED;
        for( x = lofs + width1; x < width; x++ )
            dptr[y*dstep + x] = FILTERED;
    }
    dptr += lofs; // 然后就可以跳過初始化的部分了。
    // 進(jìn)入主循環(huán)，滑動(dòng)窗口法進(jìn)行匹配。注意到該循環(huán)很大，包含了很多內(nèi)循環(huán)。    for( x = 0; x < width1; x++, dptr++ )
    {
        int* costptr = cost.data ? cost.ptr<int>() + lofs + x : &costbuf;
        int x0 = x - wsz2 - 1, x1 = x + wsz2; // 窗口的首尾x坐標(biāo)。	// 同上，所有指針從窗口的第一行開始，即-dy0行。	// 由于之前已經(jīng)初始化計(jì)算過了，x從0開始循環(huán)。	// cbuf_sub 從cbuf0 的第0行開始，cbuf在cbuf0的最后一行；下一次循環(huán)是cbuf_sub在第1行，cbuf在第0行，以此類推，存儲(chǔ)了窗口寬度內(nèi)，每一列的SAD.        const uchar* cbuf_sub = cbuf0 + ((x0 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
        cbuf = cbuf0 + ((x1 + wsz2 + 1) % (wsz + 1))*cstep - dy0*ndisp;
        hsad = hsad0 - dy0*ndisp;
	// 這里了同樣地，lptr_sub 從上一個(gè)窗口的最后一列開始，即x - wsz2 - 1，lptr從當(dāng)前窗口的最后一列開始，即x + wsz2.        lptr_sub = lptr0 + MIN(MAX(x0, -lofs), width-1-lofs) - dy0*sstep;
        lptr = lptr0 + MIN(MAX(x1, -lofs), width-1-lofs) - dy0*sstep;
        rptr = rptr0 + MIN(MAX(x1, -rofs), width-ndisp-rofs) - dy0*sstep;

	// 只算x1列，y 從-dy0到height + dy1 的SAD，將之更新到對(duì)應(yīng)的變量中。        for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp,
            hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep )
        {
            int lval = lptr[0];
            d = 0;

            for( ; d < ndisp; d++ )
            {
                int diff = std::abs(lval - rptr[d]); // 當(dāng)前列的SAD.                cbuf[d] = (uchar)diff;
                hsad[d] = hsad[d] + diff - cbuf_sub[d]; // 累加新一列各個(gè)像素不同d下的SAD，減去滑出窗口的那一列對(duì)應(yīng)的SAD.            }
            htext[y] += tab[lval] - tab[lptr_sub[0]]; // 同上。        }

        // fill borders        for( y = dy1; y <= wsz2; y++ )
            htext[height+y] = htext[height+dy1-1];
        for( y = -wsz2-1; y < -dy0; y++ )
            htext[y] = htext[-dy0];

        // initialize sums	// 將hsad0存儲(chǔ)的第-dy0列的數(shù)據(jù)乘以2拷貝給sad.        for( d = 0; d < ndisp; d++ )
            sad[d] = (int)(hsad0[d-ndisp*dy0]*(wsz2 + 2 - dy0));

	// 將hsad指向hsad0的第1-dy0行，循環(huán)也從1-dy0行開始，并且只處理窗口大小內(nèi)的數(shù)據(jù)（到wsz2 - 1為止）。	// 不處理wsz2行和之前不處理wsz2列的原因是一樣的。        hsad = hsad0 + (1 - dy0)*ndisp;
        for( y = 1 - dy0; y < wsz2; y++, hsad += ndisp )
        {
            d = 0;

	    // cost aggregation 步驟	    // 累加不同行、一個(gè)滑動(dòng)窗口內(nèi)各個(gè)像素取相同d 時(shí)的SAD。            for( ; d < ndisp; d++ )
                sad[d] = (int)(sad[d] + hsad[d]);
        }
	// 循環(huán)累加一個(gè)滑動(dòng)窗口內(nèi)的紋理值。        int tsum = 0;
        for( y = -wsz2-1; y < wsz2; y++ )
            tsum += htext[y];

        // finally, start the real processing	// 雖然官方注釋說現(xiàn)在才開始真正的處理，但之前已經(jīng)做了大量的處理工作了。        for( y = 0; y < height; y++ )
        {
            int minsad = INT_MAX, mind = -1;
            hsad = hsad0 + MIN(y + wsz2, height+dy1-1)*ndisp; // 當(dāng)前窗口的最后一行。            hsad_sub = hsad0 + MAX(y - wsz2 - 1, -dy0)*ndisp; // 上個(gè)窗口的最后一行。            d = 0;

	    // 尋找最優(yōu)視差。            for( ; d < ndisp; d++ )
            {
                int currsad = sad[d] + hsad[d] - hsad_sub[d]; // 同上，加上最后一行的SAD，減去滑出那一行的SAD.							      // 之前給sad賦值時(shí)為何要乘以2也就清楚了。一樣是為了使處理第一個(gè)窗口的SAD之和時(shí)和之后的窗口相同，							      // 可以剪掉第一行的SAD，加上新一行的SAD。所以必須乘以2防止計(jì)算第一個(gè)窗口是漏算了第一行。															  
                sad[d] = currsad; // 更新當(dāng)前d下的SAD之和，方便下次計(jì)算使用。                if( currsad < minsad )
                {
                    minsad = currsad;
                    mind = d;
                }
            }

            tsum += htext[y + wsz2] - htext[y - wsz2 - 1]; // 同樣需要更新紋理值。	    // 如果一個(gè)像素附近的紋理太弱，則視差計(jì)算認(rèn)為無效。            if( tsum < textureThreshold )
            {
                dptr[y*dstep] = FILTERED;
                continue;
            }

	    // 唯一性匹配。	    // 對(duì)于前面找到的最優(yōu)視差mind，及其SAD minsad，自適應(yīng)閾值為minsad * (1 + uniquenessRatio).	    // 要求除了mind 前后一個(gè)視差之外，其余的視差的SAD都必須比閾值大，否則認(rèn)為找到的視差無效。            if( uniquenessRatio > 0 )
            {
                int thresh = minsad + (minsad * uniquenessRatio/100);
                for( d = 0; d < ndisp; d++ )
                {
                    if( (d < mind-1 || d > mind+1) && sad[d] <= thresh)
                        break;
                }
                if( d < ndisp )
                {
                    dptr[y*dstep] = FILTERED;
                    continue;
                }
            }

            {
		// 最后，經(jīng)過層層校驗(yàn)，終于確定了當(dāng)前像素的視差。		// 回顧之前sad指針在確定其指針位置和指向的大小時(shí)，前后都留了一個(gè)位置，在這里用到了。                sad[-1] = sad[1];
                sad[ndisp] = sad[ndisp-2];
		// 這里留兩個(gè)位置的作用就很明顯了：防止mind為0或ndis-1時(shí)下面的語句數(shù)組越界。                int p = sad[mind+1], n = sad[mind-1];
                d = p + n - 2*sad[mind] + std::abs(p - n);
		//  注意到前面將dptr的位置加上了lofs位，所以這里下標(biāo)為y * dstep。                dptr[y*dstep] = (mType)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) // 這里如果讀者留心，會(huì)發(fā)現(xiàn)之前計(jì)算視差d時(shí)，計(jì)算結(jié)果是反過來的。													     // 即d=0時(shí)，理論上右圖像素應(yīng)該是和左圖像素相同的x坐標(biāo)，													     // 但其實(shí)之前在設(shè)置rptr是，此時(shí)右圖像素的x坐標(biāo)為x-(ndisp-1)，													     // 因此這里所算的視差要反轉(zhuǎn)過來，為ndisp-mind-1。													     // 常數(shù)15是因?yàn)閛pencv默認(rèn)輸出類型為16位整數(shù)，后面為了獲得真正的視差要除以16，                                                                                                             // 這里加的一個(gè)針對(duì)整數(shù)類型除法截?cái)嗟囊粋€(gè)保護(hù)。													     // 至于為何多了一個(gè)(p-n)/d，我也不太懂，應(yīng)該是針對(duì)所計(jì)算的SAD的變化率的一個(gè)補(bǔ)償，希望有人可以指點(diǎn)下:)                                 >> (DISPARITY_SHIFT_32S - disp_shift));
                costptr[y*coststep] = sad[mind]; // 最后opencv默認(rèn)得到的視差值需要乘以16，所以前面乘以256，后面在右移4位。            }
        } // y
    }// x}