动态规划算法本身存在拖尾效应，视差突变处易产生错误的匹配，利用态规划进行一维能量累积累，会将错误的视差信息传播给后面的路径上。半全局算法利用多个方向上的信息，试图消除错误信息的干扰，能明显减弱动态规划算法产生的拖尾效应。
半全局算法试图通过影像上多个方向上一维路径 的约束，来建立一个全局的马尔科夫能量方程，每个像素最终的匹配代价是所有路径信息的叠加，每个像素的视差选择都只是简单通过 WTA（Winner Takes All）决定的。多方向能量聚集如下图所示：
在每个方向上按照动态规划的思想进行能量累积，然后将各个方向上的匹配代价相加得到总的匹配代价，如下式所示：
式中L为当前路径累积的代价函数，P1、P2为像素点与相邻点视差存在较小和较大差异情况下的平滑惩罚，P1<P2，第三项无意义，仅仅是为了消除各个方向路径长度不同造成的影响。将所有r方向的匹配代价相加得到总的匹配代价，如下：
理论概述完毕，详细的可下载论文研究，此处不再详述。下面请看代码：
////FullPath判断是否全路径匹配
void SGM::Matcher(Mat lImg, Mat rImg, int minDeta, int maxDeta,int wSize, bool FullPath)
{
if (lImg.empty() || rImg.empty() || (lImg.type() != CV_8UC3&&lImg.type() != CV_8UC1) || (rImg.type() != CV_8UC3&&rImg.type() != CV_8UC1)) return;
Mat lGrayImg = lImg.type == CV_8UC3 ? cvtColor(lImg, lGrayImg, COLOR_BGR2GRAY) : lImg;
Mat rGrayImg = rImg.type == CV_8UC3 ? cvtColor(rImg, rGrayImg, COLOR_BGR2GRAY) : rImg;
P1 = 8 * (2 * wSize + 1)*(2 * wSize + 1);
P2 = 32 * (2 * wSize + 1)*(2 * wSize + 1);
printf("半全局SGM匹配: \n");
////*********************************构建视差空间***************************************/////
float ***dif;//////像素的视差空间
float ***Emat;////能量空间
float ***TmpE;////临时能量空间
dif=new float**[lImg.rows];///////初始化视差空间和能量空间
Emat = new float**[lImg.rows];
TmpE = new float**[lImg.rows];
int Deta=maxDeta-minDeta+1;////视差大小
for (int i = 0; i<lImg.rows; i++)
{
printf("构建视差空间和能量空间...%d%%\r", (int)(i*100.0 / (lImg.rows)));
dif[i]=new float*[lImg.cols ];
Emat[i] = new float*[lImg.cols];
TmpE[i] = new float*[lImg.cols];
for (int j = 0; j<lImg.cols; j++)
{
dif[i][j]=new float[Deta];
Emat[i][j]=new float[Deta];
TmpE[i][j]=new float[Deta];
for(int d=0;d<Deta;d++)
{
Emat[i][j][d]=0;
TmpE[i][j][d]=0;
int rc=j-d-minDeta;///当前视差下左像素对应对的右像素坐标
if(rc>=0)
{
dif[i][j][d]=SAD(lGrayImg, rGrayImg,j,i,rc,i,wSize);//////计算匹配代价
}
else
{
dif[i][j][d]=d>0?dif[i][j][d-1]:0;
}
}
}
}
printf("构建视差空间和能量空间...ok \n");
///////////*********************************多方向动态规划***************************************/////
/////////////////////////////******************左向动态规划**************************////////////////
for(int r=0;r<lImg.rows;r++)
{
float minEmin = FLT_MAX;///截断值防止累积的能量值过大
for(int d=0;d<Deta;d++)
{
TmpE[r][0][d] = dif[r][0][d];
minEmin = minEmin < dif[r][0][d] ? minEmin : dif[r][0][d];
}
for(int c=1;c<lImg.cols;c++)
{
float tmpminEmin = FLT_MAX;
for(int match=0;match<Deta;match++)
{
float Emin=FLT_MAX;//////当前状态下路径代价
for(int pred=0;pred<Deta;pred++)
{
float Esmooth=abs(match-pred)>1?P2:(abs(match-pred)==0?0:P1);//////平滑项
float E = dif[r][c][match] + Esmooth + TmpE[r][c - 1][pred];
if(E<Emin)
{
Emin=E;
}
}
TmpE[r][c][match] = Emin - minEmin;
Emat[r][c][match] = Emat[r][c][match] + TmpE[r][c][match];
tmpminEmin = tmpminEmin < Emat[r][c][match] ? tmpminEmin : Emat[r][c][match];
}
minEmin = tmpminEmin;
}
}
///////////////////////******************右向动态规划**************************////////////////
for(int r=0;r<lImg.rows;r++)
{
float minEmin = FLT_MAX;///截断值防止累积的能量值过大
for(int d=0;d<Deta;d++)
{
TmpE[r][lImg.cols-1][d]=dif[r][lImg.cols-1][d];
minEmin = minEmin < dif[r][lImg.cols - 1][d] ? minEmin : dif[r][lImg.cols - 1][d];
}
for(int c=lImg.cols-2;c>=0;--c)
{
float tmpminEmin = FLT_MAX;
for(int match=0;match<Deta;match++)
{
float Emin=FLT_MAX;//////当前状态下路径代价
for (int pred = 0; pred<Deta; pred++)
{
float Esmooth=abs(match-pred)>1?P2:(abs(match-pred)==0?0:P1);//////平滑项
float E=dif[r][c][match]+Esmooth+TmpE[r][c+1][pred];
if(E<Emin)
{
Emin=E;
}
}
TmpE[r][c][match] = Emin - minEmin;
Emat[r][c][match] = Emat[r][c][match] + TmpE[r][c][match];
tmpminEmin = tmpminEmin < TmpE[r][c][match] ? tmpminEmin : TmpE[r][c][match];
}
minEmin = tmpminEmin;
}
}
///////////////////////******************下向动态规划**************************////////////////
for(int c=0;c<lImg.cols;c++)
{
float minEmin = FLT_MAX;///截断值防止累积的能量值过大
for(int d=0;d<Deta;d++)
{
TmpE[0][c][d]=dif[0][c][d];
minEmin = minEmin < dif[0][c][d] ? minEmin : dif[0][c][d];
}
for(int r=1;r<lImg.rows;r++)
{
float tmpminEmin = FLT_MAX;
for(int match=0;match<Deta;match++)
{
float Emin=FLT_MAX;//////当前状态下路径代价
for (int pred = 0; pred<Deta; pred++)
{
float Esmooth=abs(match-pred)>1?P2:(abs(match-pred)==0?0:P1);//////平滑项
float E=dif[r][c][match]+Esmooth+TmpE[r-1][c][pred];
if(E<Emin)
{
Emin=E;
}
}
TmpE[r][c][match] = Emin - minEmin;
Emat[r][c][match] = Emat[r][c][match] + TmpE[r][c][match];
tmpminEmin = tmpminEmin < TmpE[r][c][match] ? tmpminEmin : TmpE[r][c][match];
}
minEmin = tmpminEmin;
}
}
/////////////////////////******************上向动态规划**************************////////////////
for(int c=0;c<lImg.cols;c++)
{
float minEmin = FLT_MAX;///截断值防止累积的能量值过大
for(int d=0;d<Deta;d++)
{
TmpE[lImg.rows-1][c][d]=dif[lImg.rows-1][c][d];
minEmin = minEmin < dif[lImg.rows - 1][c][d] ? minEmin : dif[lImg.rows - 1][c][d];
}
for(int r=lImg.rows-2;r>=0;--r)
{
float tmpminEmin = FLT_MAX;
for(int match=0;match<Deta;match++)
{
float Emin=FLT_MAX;//////当前状态下路径代价
for (int pred = 0; pred<Deta; pred++)
{
float Esmooth=abs(match-pred)>1?P2:(abs(match-pred)==0?0:P1);//////平滑项
float E=dif[r][c][match]+Esmooth+TmpE[r+1][c][pred];
if(E<Emin)
{
Emin=E;
}
}
TmpE[r][c][match] = Emin - minEmin;
Emat[r][c][match] = Emat[r][c][match] + TmpE[r][c][match];
tmpminEmin = tmpminEmin < TmpE[r][c][match] ? tmpminEmin : TmpE[r][c][match];
}
minEmin = tmpminEmin;
}
}
if (FullPath)
{
/////剩余路径我省略了，原理与上面相同
}
Mat dispImg(lImg.size(),CV_8UC1,Scalar::all(0));//////视差图
for(int i=0;i<lImg.rows;i++)
{
for(int j=0;j<lImg.cols;j++)
{
float Emind=FLT_MAX;
int pmind=NULL;
for(int d=0;d<Deta;d++)
{
if(Emat[i][j][d]<Emind)
{
Emind=Emat[i][j][d];
pmind=d;
}
}
if (j - pmind -minDeta >= 0)
{
dispImg.ptr<uchar>(i)[j] = (pmind + minDeta);/////3是为了增加对比度
}
}
}
printf("SGM匹配...ok \n");
}