//使用鐘形曲線作為匹配概率，差值越小則匹配的概率越大，最終的要求是使匹配的概率最大，概率曲線使用matlab生成
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
};

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 = 5;

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 rightPixel =0;
int m,n,l;

int t1 = clock();
for (int i = 0 ; i < imageHeight;i++)
{
for (int j = 0; j= imageWidth)
{imageData + i*leftImage->widthStep + k;
pRightPixel= (unsigned char*)rightImage->imageData+i*rightImage->widthStep + j;
leftPixel = *pLeftPixel;
rightPixel = *pRightPixel;
//之前概率最大的點加上當前的概率

Ddynamic[j + 1][k + 1] = max(Ddynamic[j][k],max(Ddynamic[j][k+1],Ddynamic[j+1][k]))
+ Probability[abs(leftPixel - rightPixel)];
/* if (abs(leftPixel - rightPixel) <= max12Diff)
{
Ddynamic[j + 1][k + 1] = Ddynamic[j][k] +1;
}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<= 1 && n >= 1)
{
pPixel = (unsigned char *)DPImage->imageData + i*DPImage->widthStep + m;
*pPixel = (n-m)*8;
//標記有效匹配點
pPixel = (unsigned char *)effectiveImage->imageData + i*effectiveImage->widthStep + m;
*pPixel = 255;
if (Ddynamic[m-1][n] >= Ddynamic[m][n -1] && Ddynamic[m-1][n] >= Ddynamic[m-1][n -1])
m--;
else if (Ddynamic[m][n-1] >= Ddynamic[m-1][n] && Ddynamic[m][n -1] >= Ddynamic[m-1][n -1])
n--;
else
{
//s[--l]=a[i-1];
// l -= Ddynamic[m][n];
m--;
n--;
}
}

//cvWaitKey(0);
}

//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++;
}
}
}
int t2 = clock();
cout<<"dt: "<