立体视觉SGBM算法的实现，多方向DP

最近一直在学习SGBM算法，作为一种全局匹配算法，立体匹配的效果明显好于局部匹配算法，但是同时复杂度上也要远远大于局部匹配算法。算法主要是参考Stereo Processing by Semiglobal Matching and Mutual Information，里面有讲完整的算法实现。
OpenCV中实际上是提供了SGBM类进行SGBM算法的实现。
#include <highgui.h>
#include <cv.h>
#include <cxcore.h>
#include <iostream>
using namespace std;
using namespace cv;
int main()
{

IplImage * img1 = cvLoadImage("left.png",0);
IplImage * img2 = cvLoadImage("right.png",0);
cv::StereoSGBM sgbm;
int SADWindowSize = 9;
sgbm.preFilterCap = 63;
sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
int cn = img1->nChannels;
int numberOfDisparities=64;
sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
sgbm.minDisparity = 0;
sgbm.numberOfDisparities = numberOfDisparities;
sgbm.uniquenessRatio = 10;
sgbm.speckleWindowSize = 100;
sgbm.speckleRange = 32;
sgbm.disp12MaxDiff = 1;
Mat disp, disp8;
int64 t = getTickCount();
sgbm((Mat)img1, (Mat)img2, disp);
t = getTickCount() - t;
cout<<"Time elapsed:"<<t*1000/getTickFrequency()<<endl;
disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));

namedWindow("left", 1);
cvShowImage("left", img1);
namedWindow("right", 1);
cvShowImage("right", img2);
namedWindow("disparity", 1);
imshow("disparity", disp8);
waitKey();
imwrite("sgbm_disparity.png", disp8); 
cvDestroyAllWindows();
return 0;
}
贴出效果：





但是仅仅用OpenCV自带的SGBM类来实现并不能满足，我还是希望能够自己实现该算法，然后最关键是移植到FPGA上去。

于是我尝试自已去写SGBM的代码，论文中提高Dynamic programming算法,实际上SGBM中也用到了多方向的Dynamic programming，但是我目前只是实现了单方向的DP。

//引入概率公式

//引入CBT的插值方法
//加上相邻匹配点位置之间的限制
#include <cstdio>
#include <cstring>
#include <iostream>
#include<cv.h>
#include<highgui.h>
#include <cmath>

using namespace std;
const int Width = 1024;
const int Height = 1024;
int Ddynamic[Width][Width];

//使用钟形曲线作为匹配概率，差值越小则匹配的概率越大，最终的要求是使匹配的概率最大，概率曲线使用matlab生成

//均方差30
//int Probability[256] = {
// 255, 255, 254, 252, 250, 247, 244, 240, 235, 230, 225, 219, 213, 206, 200, 192, 185, 178, 170, 162, 
// 155, 147, 139, 132, 124, 117, 110, 103, 96, 89, 83, 77, 71, 65, 60, 55, 50, 46, 42, 38, 35, 31, 28, 
// 25, 23, 20, 18, 16, 14, 13, 11, 10, 9, 8, 7, 6, 5, 4, 4, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 0, 0, 
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
// 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
//};

//均方差 5
int Probability[256] = {
255, 250, 235, 213, 185, 155, 124, 96, 71, 50, 35, 23, 14, 9, 5, 3, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};

int main()
{ 

IplImage * leftImage = cvLoadImage("l2.jpg",0);
IplImage * rightImage = cvLoadImage("r2.jpg",0);

//IplImage * leftImage = cvLoadImage("left.bmp",0); 
//IplImage * rightImage = cvLoadImage("right.bmp",0);

int imageWidth = leftImage->width;
int imageHeight =leftImage->height;

IplImage * DPImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * effectiveImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * FilterImage = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

unsigned char * pPixel = NULL;
unsigned char pixel;
unsigned char * pPixel2 = NULL;
unsigned char pixel2;
for (int i = 0; i< imageHeight;i++)
{
for (int j =0; j < imageWidth;j++ )
{
pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + j;
*pPixel = 0;
pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;
*pPixel = 0;
}
}



cvNamedWindow("Left",1);
cvNamedWindow("Right",1);
cvNamedWindow("Depth",1);
cvNamedWindow("effectiveImage",1);

cvShowImage("Left",leftImage);
cvShowImage("Right",rightImage);

int minD = 0;
int maxD = 31;
//假设图像是经过矫正的，那么每次都只是需要搜搜同一行的内容
int max12Diff = 10;

for (int i = 0;i < imageWidth;i++)
{
Ddynamic[0][i] = 0;
Ddynamic[i][0] = 0;
}

unsigned char * pLeftPixel = NULL;
unsigned char * pRightPixel = NULL;
unsigned char leftPixel = 0;
unsigned char leftMax = 0;
unsigned char leftMin = 0;
unsigned char tempLeft1 = 0;
unsigned char tempLeft2 = 0;
unsigned char rightPixel =0;
unsigned char difPixel = 0;
int m,n,l;

int t1 = clock();
for (int i = 0 ; i < imageHeight;i++)
{
for (int j = 0; j<imageWidth;j++)
{
for (int k = j + minD; k <= j + maxD;k++)
{
if (k <= 0 || k >= imageWidth -1)
{

}else {
pLeftPixel = (unsigned char*)leftImage->imageData + i*leftImage->widthStep + k;
pRightPixel= (unsigned char*)rightImage->imageData+i*rightImage->widthStep + j;
leftPixel = *pLeftPixel;
rightPixel = *pRightPixel;
leftPixel = *pLeftPixel;
tempLeft1 = (*pLeftPixel +(*(pLeftPixel -1)))/2;
tempLeft2 = (*pLeftPixel +(*(pLeftPixel +1)))/2;

leftMax = max(max(tempLeft1,tempLeft2),leftPixel);
leftMin = min(min(tempLeft1,tempLeft2),leftPixel);
difPixel = max(max(rightPixel - leftMax,leftMin - rightPixel),0);


if (difPixel <= max12Diff)
{
Ddynamic[j + 1][k + 1] = Ddynamic[j][k] + Probability[difPixel]; 
}else if (Ddynamic[j][k+1] > Ddynamic[j+1][k])
{
Ddynamic[j + 1][k + 1] = Ddynamic[j][k+1];
}else{
Ddynamic[j+1][k+1] = Ddynamic[j+1][k];
}

//cout<<Ddynamic[j +1][k+1]<<" ";
}

}
//cout<<"\n";
}
//逆向搜索，找出最佳路径
m = imageWidth;
n = imageWidth;
int m2 = imageWidth, n2 = imageWidth;
l = Ddynamic[imageWidth][imageWidth];
while( m >= 1 && n >= 1)
{
if ((m2 == m + 1 && n2 >= n +1) || ( m2 > m +1 && n2 == n + 1))
{
pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + m;
*pPixel = (n-m)*10;
//标记有效匹配点
pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + m;
*pPixel = 255;

m2 = m;
n2 = n;

}
if (Ddynamic[m-1][n-1] >= Ddynamic[m][n -1] && Ddynamic[m-1][n -1] >= Ddynamic[m-1][n])
{

m--; 
n--;
}else if (Ddynamic[m-1][n] >= Ddynamic[m][n -1] && Ddynamic[m-1][n] >= Ddynamic[m-1][n -1])
{ 
m--;
}
else
{ 
n--;
}

}

//cvWaitKey(0);
}
int t2 = clock();
cout<<"dt: "<<t2-t1<<endl;

//显示Ddynamic最后一行
/* for (int i = 0 ;i<= imageWidth;i++)
{
for (int j= 0; j<= imageWidth;j++)
{
cout<<Ddynamic[i][j]<<" ";
}
cout<<"\n\n";
}*/


//refine the depth image 7*7中值滤波
//统计未能匹配点的个数
int count = 0;
for (int i = 0 ;i< imageHeight;i++)
{
for (int j= 0; j< imageWidth;j++)
{
pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;
pixel = *pPixel;
if (pixel == 0)
{
count++;
}
}
}

cout<<"Count: "<<count<<" "<<(double)count/(imageWidth*imageHeight)<<endl;
cvShowImage("Depth",DPImage);
cvShowImage("effectiveImage",effectiveImage);
// cvWaitKey(0);


FilterImage = cvCloneImage(DPImage);

//7*7中值滤波
int halfMedianWindowSize = 3;
int medianWindowSize = 2*halfMedianWindowSize + 1;
int medianArray[100] = {0};
count = 0;
int temp = 0;
int medianVal = 0;

for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)
{
for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)
{
pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + j;
pixel = *pPixel;
if (pixel == 0)
{
count = 0;
for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++)
{
for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++)
{
pPixel2 = (unsigned char *)DPImage->imageData + m*DPImage->widthStep + n;
pixel2 = *pPixel2;
if (pixel2 != 0)
{
medianArray[count] = pixel2;
count++;
}

}
//排序
for (int k = 0; k< count;k++)
{
for (int l = k + 1; l< count;l++)
{
if (medianArray[l] < medianArray[l-1] )
{
temp = medianArray[l];
medianArray[l] = medianArray[l-1];
medianArray[l-1] = temp;
}
}
}
medianVal = medianArray[count/2];
pPixel = (unsigned char *)FilterImage->imageData + i*DPImage->widthStep + j;
*pPixel = medianVal;
}

}
}
}
cvShowImage("Depth",DPImage);
cvShowImage("effectiveImage",effectiveImage);
cvShowImage("Filter",FilterImage);

cvSaveImage("depth.jpg",DPImage);
cvSaveImage("Filter.jpg",FilterImage);
cvSaveImage("effective.jpg",effectiveImage);

cvWaitKey(0);
return 0;
}
//效果






但是论文中多方向的DP却看了好几天都没有头绪该怎么写？因此这里贴出进展，希望各位大侠能够帮帮忙，共同学习。