双目立体视觉SGBM

算法原理

bm的局部的代价聚合算法,受到噪声影响比较大,所以bm算法的速度虽然比较快,但是效果比较差,

而sgbm是一个半全局的代价聚合算法,计算代价的时候考虑周围领域的代价值,但不是一次性及算法

整个图像上的代价最小值(这样全局的算法效率比较低),而是利用动态规划的算法,从局部利用贪心的

思想计算全局。

能量代价函数如下:

E(D) = \sum_{p}\left ( C\left ( p,D_{p} \right ) \right ) + \sum_{q=N_{p}}\left ( P1*T[\left \| D_{p}- D_{q}\right \| ==1]\right ) + \sum_{q=N_{p}}\left ( P2*T[\left \| D_{p}- D_{q}\right \| ]\right )

总共包含三项:

  • 第一项就是局部的代价SAD聚合值

  • 第二项表示的是视差小于1的惩罚项

  • 第三项表示的是视差大于1的惩罚项

这样第二项的惩罚项可以适应变化比较小的平滑区域,第三项的惩罚项适应的是变化比较大的边缘区域或者纹理丰富区域。

具体某一点像素p沿着方向r的路径代价函数如下:

上面的公式就是一个BP算法的基本状态变化公式,平滑项就是选择当前方向的路径代价值、视差小于1的路径代价值加上p1、

当前方向范围内的最小值加上p2 这几项的最小值减去当前方向的最小代价聚合值。这样当前方向的代价聚合的上限为:

L \leqslant C_{max}+ P2  ↵

 总的代价聚合函数就是计算各个路径的路径代价之和:

S = \sum_{r} (L_{r}(p, d))

最终就是计算最小的S对应的视差作为最优的视差。

 

代码分析

代价计算

static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y,
                            int minD, int maxD, CostType* cost,
                            PixType* buffer, const PixType* tab,
                            int xrange_min = 0, int xrange_max = DEFAULT_RIGHT_BORDER )
{
    int x, c, width = img1.cols, cn = img1.channels();
    int minX1 = std::max(maxD, 0), maxX1 = width + std::min(minD, 0);
    int D = (int)alignSize(maxD - minD, v_int16::nlanes), width1 = maxX1 - minX1;
    //This minX1 & maxX2 correction is defining which part of calculatable line must be calculated
    //That is needs of parallel algorithm
    xrange_min = (xrange_min < 0) ? 0: xrange_min;
    xrange_max = (xrange_max == DEFAULT_RIGHT_BORDER) || (xrange_max > width1) ? width1 : xrange_max;
    maxX1 = minX1 + xrange_max;
    minX1 += xrange_min;
    width1 = maxX1 - minX1;
    int minX2 = std::max(minX1 - maxD, 0), maxX2 = std::min(maxX1 - minD, width);
    int width2 = maxX2 - minX2;
    const PixType *row1 = img1.ptr<PixType>(y), *row2 = img2.ptr<PixType>(y);
    PixType *prow1 = buffer + width2*2, *prow2 = prow1 + width*cn*2;

    for( c = 0; c < cn*2; c++ )
    {
        prow1[width*c] = prow1[width*c + width-1] =
        prow2[width*c] = prow2[width*c + width-1] = tab[0];
    }

    int n1 = y > 0 ? -(int)img1.step : 0, s1 = y < img1.rows-1 ? (int)img1.step : 0;
    int n2 = y > 0 ? -(int)img2.step : 0, s2 = y < img2.rows-1 ? (int)img2.step : 0;

    int minX_cmn = std::min(minX1,minX2)-1;
    int maxX_cmn = std::max(maxX1,maxX2)+1;
    minX_cmn = std::max(minX_cmn, 1);
    maxX_cmn = std::min(maxX_cmn, width - 1);
    if( cn == 1 )   //prow1 是左影像的响应值, 
    {
        for( x = minX_cmn; x < maxX_cmn; x++ )
        {
            prow1[x] = tab[(row1[x+1] - row1[x-1])*2 + row1[x+n1+1] - row1[x+n1-1] + row1[x+s1+1] - row1[x+s1-1]];
            prow2[width-1-x] = tab[(row2[x+1] - row2[x-1])*2 + row2[x+n2+1] - row2[x+n2-1] + row2[x+s2+1] - row2[x+s2-1]];

            prow1[x+width] = row1[x];
            prow2[width-1-x+width] = row2[x];
        }
    }
    else
    {
        for( x = minX_cmn; x < maxX_cmn; x++ )
        {
            prow1[x] = tab[(row1[x*3+3] - row1[x*3-3])*2 + row1[x*3+n1+3] - row1[x*3+n1-3] + row1[x*3+s1+3] - row1[x*3+s1-3]];
            prow1[x+width] = tab[(row1[x*3+4] - row1[x*3-2])*2 + row1[x*3+n1+4] - row1[x*3+n1-2] + row1[x*3+s1+4] - row1[x*3+s1-2]];
            prow1[x+width*2] = tab[(row1[x*3+5] - row1[x*3-1])*2 + row1[x*3+n1+5] - row1[x*3+n1-1] + row1[x*3+s1+5] - row1[x*3+s1-1]];

            prow2[width-1-x] = tab[(row2[x*3+3] - row2[x*3-3])*2 + row2[x*3+n2+3] - row2[x*3+n2-3] + row2[x*3+s2+3] - row2[x*3+s2-3]];
            prow2[width-1-x+width] = tab[(row2[x*3+4] - row2[x*3-2])*2 + row2[x*3+n2+4] - row2[x*3+n2-2] + row2[x*3+s2+4] - row2[x*3+s2-2]];
            prow2[width-1-x+width*2] = tab[(row2[x*3+5] - row2[x*3-1])*2 + row2[x*3+n2+5] - row2[x*3+n2-1] + row2[x*3+s2+5] - row2[x*3+s2-1]];

            prow1[x+width*3] = row1[x*3];
            prow1[x+width*4] = row1[x*3+1];
            prow1[x+width*5] = row1[x*3+2];

            prow2[width-1-x+width*3] = row2[x*3];
            prow2[width-1-x+width*4] = row2[x*3+1];
            prow2[width-1-x+width*5] = row2[x*3+2];
        }
    }

    memset( cost + xrange_min*D, 0, width1*D*sizeof(cost[0]) );

    buffer -= width-maxX2;
    cost -= (minX1-xrange_min)*D + minD; // simplify the cost indices inside the loop

    for( c = 0; c < cn*2; c++, prow1 += width, prow2 += width )
    {
        int diff_scale = c < cn ? 0 : 2;

        // precompute
        //   v0 = min(row2[x-1/2], row2[x], row2[x+1/2]) and
        //   v1 = max(row2[x-1/2], row2[x], row2[x+1/2]) and
        //   to process values from [minX2, maxX2) we should check memory location (width - 1 - maxX2, width - 1 - minX2]
        //   so iterate through [width - maxX2, width - minX2)
        for( x = width-maxX2; x < width-minX2; x++ )
        {
            int v = prow2[x];
            int vl = x > 0 ? (v + prow2[x-1])/2 : v;
            int vr = x < width-1 ? (v + prow2[x+1])/2 : v;
            int v0 = std::min(vl, vr); v0 = std::min(v0, v);
            int v1 = std::max(vl, vr); v1 = std::max(v1, v);
            buffer[x] = (PixType)v0;
            buffer[x + width2] = (PixType)v1;
        }

        for( x = minX1; x < maxX1; x++ )
        {
            int u = prow1[x];
            int ul = x > 0 ? (u + prow1[x-1])/2 : u;
            int ur = x < width-1 ? (u + prow1[x+1])/2 : u;
            int u0 = std::min(ul, ur); u0 = std::min(u0, u);
            int u1 = std::max(ul, ur); u1 = std::max(u1, u);

            int d = minD;

            for( ; d < maxD; d++ )
            {
                int v = prow2[width-x-1 + d];
                int v0 = buffer[width-x-1 + d];
                int v1 = buffer[width-x-1 + d + width2];
                int c0 = std::max(0, u - v1); c0 = std::max(c0, v0 - u);
                int c1 = std::max(0, v - u1); c1 = std::max(c1, u0 - v);

                cost[x*D + d] = (CostType)(cost[x*D+d] + (std::min(c0, c1) >> diff_scale));
            }
        }
    }
}

这里计算代价值方式是:(之前BM是直接计算代价, sgbm中采用左右临域平均值进行处理可以减少噪声点的影响,同时根据左右一致性选择最优的代价值)

            int u1l =  (prow1[x] + prow1[x-1])/2 ;
            int u1r =  (prow1[x] + prow1[x+1])/2 ;

            u0  = min(prow1[x], u1l , u1r )
            u1 = max(prow1[x], u1l , u1r )

            u  = prow1[x]

            int u2l =  (prow2[x] + prow2[x-1])/2 ;
            int u2r =  (prow2[x] + prow2[x+1])/2 ;

            v0  = min(prow2[x], u2l , u2r )
            v1 = max(prow2[x], u2l , u2r )

           
            v  = prow2[x]
          
           #计算代价为:
             c0  = max(v- u1, u0-v)
             c1  = max(u- v1, v0 -u)
              
             C = min(c0, c1)

代价聚合

第一步是前向SP聚合

        #计算上一行的-1, 0, 1 和当前一行的-1    四个方向的路径代价
          for( x = x1; x != x2; x += dx )
            {
                int delta0 = P2 + *mem.getMinLr(lrID, x - dx);
                int delta1 = P2 + *mem.getMinLr(1 - lrID, x - 1, 1);
                int delta2 = P2 + *mem.getMinLr(1 - lrID, x,     2);
                int delta3 = P2 + *mem.getMinLr(1 - lrID, x + 1, 3);

                CostType* Lr_p0 = mem.getLr(lrID, x - dx);
                CostType* Lr_p1 = mem.getLr(1 - lrID, x - 1, 1);
                CostType* Lr_p2 = mem.getLr(1 - lrID, x,     2);
                CostType* Lr_p3 = mem.getLr(1 - lrID, x + 1, 3);

                Lr_p0[-1] = Lr_p0[D] = MAX_COST;
                Lr_p1[-1] = Lr_p1[D] = MAX_COST;
                Lr_p2[-1] = Lr_p2[D] = MAX_COST;
                Lr_p3[-1] = Lr_p3[D] = MAX_COST;

                CostType* Lr_p = mem.getLr(lrID, x);
                const CostType* Cp = C + x*Da;
                CostType* Sp = S + x*Da;

                CostType* minL = mem.getMinLr(lrID, x);
            }
          
            #代价sp传递到4个方向,并聚合4个路径代价值

                for( ; d < D; d++ )
                {
                    int Cpd = Cp[d], L;
                    int Spd = Sp[d];

                    L = Cpd + std::min((int)Lr_p0[d], std::min(Lr_p0[d - 1] + P1, std::min(Lr_p0[d + 1] + P1, delta0))) - delta0;
                    Lr_p[d] = (CostType)L;
                    minL[0] = std::min(minL[0], (CostType)L);
                    Spd += L;

                    L = Cpd + std::min((int)Lr_p1[d], std::min(Lr_p1[d - 1] + P1, std::min(Lr_p1[d + 1] + P1, delta1))) - delta1;
                    Lr_p[d + Dlra] = (CostType)L;
                    minL[1] = std::min(minL[1], (CostType)L);
                    Spd += L;

                    L = Cpd + std::min((int)Lr_p2[d], std::min(Lr_p2[d - 1] + P1, std::min(Lr_p2[d + 1] + P1, delta2))) - delta2;
                    Lr_p[d + Dlra*2] = (CostType)L;
                    minL[2] = std::min(minL[2], (CostType)L);
                    Spd += L;

                    L = Cpd + std::min((int)Lr_p3[d], std::min(Lr_p3[d - 1] + P1, std::min(Lr_p3[d + 1] + P1, delta3))) - delta3;
                    Lr_p[d + Dlra*3] = (CostType)L;
                    minL[3] = std::min(minL[3], (CostType)L);
                    Spd += L;

                    Sp[d] = saturate_cast<CostType>(Spd);
                }

第二步是反向计算BP

                for( x = width1 - 1; x >= 0; x-- )
                {
                    CostType* Sp = S + x*Da;
                    CostType minS = MAX_COST;
                    short bestDisp = -1;

                    if( npasses == 1 )
                    {
                        CostType* Lr_p0 = mem.getLr(lrID, x + 1);
                        Lr_p0[-1] = Lr_p0[D] = MAX_COST;
                        CostType* Lr_p = mem.getLr(lrID, x);

                        const CostType* Cp = C + x*Da;

                        d = 0;
                        int delta0 = P2 + *mem.getMinLr(lrID, x + 1);
                        int minL0 = MAX_COST;
                        for( ; d < D; d++ )
                        {
                            int L0 = Cp[d] + std::min((int)Lr_p0[d], std::min(Lr_p0[d-1] + P1, std::min(Lr_p0[d+1] + P1, delta0))) - delta0;

                            Lr_p[d] = (CostType)L0;
                            minL0 = std::min(minL0, L0);

                            CostType Sval = Sp[d] = saturate_cast<CostType>(Sp[d] + L0);
                            if( Sval < minS )
                            {
                                minS = Sval;
                                bestDisp = (short)d;
                            }
                        }
                        *mem.getMinLr(lrID, x) = (CostType)minL0;

通过构建前向BP聚合和反向BP聚合,相当于构建了5个方向的路径代价聚合,相当于一个弱化的全局代价函数。

视差计算

                        for( ; d < D; d++ )
                        {
                            int Sval = Sp[d];
                            if( Sval < minS )
                            {
                                minS = (CostType)Sval;
                                bestDisp = (short)d;
                            }
                        }

找最小的代价聚合对应的视差d。

视差后处理

左右一致性原则

\left \| d_{L,R}(x,y) - d_{R,L}(y, x)\right \| < T

 int _x2 = x + minX1 - d - minD;
                    if( mem.disp2cost[_x2] > minS )
                    {
                        mem.disp2cost[_x2] = (CostType)minS;
                        mem.disp2ptr[_x2] = (DispType)(d + minD);
                    }

                    if( 0 < d && d < D-1 )
                    {
                        // do subpixel quadratic interpolation:
                        //   fit parabola into (x1=d-1, y1=Sp[d-1]), (x2=d, y2=Sp[d]), (x3=d+1, y3=Sp[d+1])
                        //   then find minimum of the parabola.
                        int denom2 = std::max(Sp[d-1] + Sp[d+1] - 2*Sp[d], 1);
                        d = d*DISP_SCALE + ((Sp[d-1] - Sp[d+1])*DISP_SCALE + denom2)/(denom2*2);
                    }
                    else
                        d *= DISP_SCALE;
                    disp1ptr[x + minX1] = (DispType)(d + minD*DISP_SCALE);
                }

                for( x = minX1; x < maxX1; x++ )
                {
                    // we round the computed disparity both towards -inf and +inf and check
                    // if either of the corresponding disparities in disp2 is consistent.
                    // This is to give the computed disparity a chance to look valid if it is.
                    int d1 = disp1ptr[x];
                    if( d1 == INVALID_DISP_SCALED )
                        continue;
                    int _d = d1 >> DISP_SHIFT;
                    int d_ = (d1 + DISP_SCALE-1) >> DISP_SHIFT;
                    int _x = x - _d, x_ = x - d_;
                    if( 0 <= _x && _x < width && mem.disp2ptr[_x] >= minD && std::abs(mem.disp2ptr[_x] - _d) > disp12MaxDiff &&
                       0 <= x_ && x_ < width && mem.disp2ptr[x_] >= minD && std::abs(mem.disp2ptr[x_] - d_) > disp12MaxDiff )
                        disp1ptr[x] = (DispType)INVALID_DISP_SCALED;
                }

唯一性检测

\frac{s}{mins} < \frac{100}{100-uniquenessRatio}

如果次最小视差和最小视差的的比值小于某一个阈值就剔除

 

去除离群的小离群点

参考上一章BM所讲的方式通过四连域构建相同的连通域,如果连通域面积小于一定的阈值就剔除。

视差亚像素化

主要采用相邻的视差值采用二次曲线拟合的方式,二次曲线需要三个点进行插值

(d-1, S(d-1)), (d, S(d)) , (d+1, S(d+1))

然后取曲线中的最小值。

                    if( 0 < d && d < D-1 )
                    {
                        // do subpixel quadratic interpolation:
                        //   fit parabola into (x1=d-1, y1=Sp[d-1]), (x2=d, y2=Sp[d]), (x3=d+1, y3=Sp[d+1])
                        //   then find minimum of the parabola.
                        int denom2 = std::max(Sp[d-1] + Sp[d+1] - 2*Sp[d], 1);
                        d = d*DISP_SCALE + ((Sp[d-1] - Sp[d+1])*DISP_SCALE + denom2)/(denom2*2);
                    }
                    else
                        d *= DISP_SCALE;

滤波处理

采用双边滤波或者均值滤波,会更平滑。双边滤波可以参考https://blog.csdn.net/u013066730/article/details/87859184

 

算法优化

  • 只采用前后左右这四个方向的聚合,这个时候可以先水平方向上的并行聚合处理,然后再进行垂直方向的并行聚合
  • 直接在垂直方向上分多块进行分别求解代价值,每一块进行了一定程度上的重叠这样可以实现并行。

 

  • 0
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值