#include <cstdio>
#include <cstring>
#include <iostream>
#include<cv.h>
#include<highgui.h>
#include <cmath>
using namespace std;
const int Width = 512;
const int Height = 512;
int Ddynamic[Width][Width];
int main()
{
//打开文件
FILE* pFile;
if (pFile = fopen("data.txt","a"))
{
//cout <<"File Open Success"<<endl;
}else{
//cout <<"File Open Failed"<<end;
return 0;
}
// IplImage * leftImage = cvLoadImage("l1.png",0);
// IplImage * rightImage = cvLoadImage("r1.png",0);
//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 * leftDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * rightDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * leftValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * rightValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * leftFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * rightFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * depth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
IplImage * valid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
unsigned char * pPixel = NULL;
unsigned char pixel;
unsigned char * pPixel2 = NULL;
unsigned char pixel2;
cvZero(leftDepth);
cvZero(rightDepth);
cvZero(leftValid);
cvZero(rightValid);
cvZero(leftFilter);
cvZero(rightFilter);
cvZero(depth);
cvZero(valid);
CvSize SADWindowSize = cvSize(5,5); //(widh,height)
int SW2 = SADWindowSize.width/2;
int SH2 = SADWindowSize.height/2;
cout<<"SH2: "<<SH2<<endl;
cout<<"SW2: "<<SW2<<endl;
int minD = 0;
int maxD = 15;
//假设图像是经过矫正的，那么每次都只是需要搜搜同一行的内容
int max12Diff = 8*SADWindowSize.width*SADWindowSize.height;
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 difPixel = 0;
int t1 = clock();
for (int i = SH2 ; i < imageHeight -SH2;i++)
{
for (int j = SW2; j<imageWidth - SW2;j++)
{
for (int k = j + minD; k <= j + maxD;k++)
{
difPixel = 0;
for (m = -SH2;m <= SH2;m++)
{
for (n = -SW2; n <= SW2;n++)
{
pRightPixel= (unsigned char*)rightImage->imageData+
(i+m)*rightImage->widthStep + j+n;
rightPixel = *pRightPixel;
if (k < SW2 || k >= imageWidth -SW2 )
{
difPixel += rightPixel;
}else {
pLeftPixel = (unsigned char*)leftImage->imageData +
(i+m)*leftImage->widthStep + k+n;
leftPixel = *pLeftPixel;
difPixel += abs(leftPixel - rightPixel);
}
}
}
if (difPixel <= 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<<Ddynamic[j +1][k+1]<<" ";
}
//cout<<"\n";
}
//逆向搜索，找出最佳路径
m = imageWidth - SW2;
n = imageWidth - SW2;
int m2 = m, n2 = n;
while( m >= 1 && n >= 1)
{
if ((m2 == m + 1 && n2 >= n +1) || ( m2 > m +1 && n2 == n + 1))
{
pPixel = (unsigned char *)rightDepth->imageData + i*rightDepth->widthStep + m;
*pPixel = (n-m)*10;
//标记有效匹配点
pPixel = (unsigned char *)rightValid->imageData + i*rightValid->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;
//统计未能匹配点的个数
int count = 0;
for (int i = 0 ;i< imageHeight;i++)
{
for (int j= 0; j< imageWidth;j++)
{
pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + j;
pixel = *pPixel;
if (pixel == 0)
{
count++;
}
}
}
cout<<"rightCount: "<<count<<" "<<(double)count/(imageWidth*imageHeight)<<endl;
rightFilter = cvCloneImage(rightDepth);
//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 *)rightValid->imageData + i*rightValid->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 *)rightDepth->imageData + m*rightDepth->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 *)rightFilter->imageData + i*rightFilter->widthStep + j;
*pPixel = medianVal;
}
}
}
}
//两次进行DP计算，分别是左对右和右对左,从右想左扫描
for (int j = 0; j<= imageWidth;j++)
{
for (int k = 0;k <= imageWidth;k++)
{
Ddynamic[j][k] = 0;
}
}
minD = -maxD;
maxD = 0;
for (int i = SH2 ; i < imageHeight -SH2;i++)
{
for (int j = imageWidth-SW2-1; j >= SW2;j--)
{
for (int k = j + maxD; k >= j + minD;k--)
{
difPixel = 0;
for (m = -SH2;m <= SH2;m++)
{
for (n = -SW2; n <= SW2;n++)
{
pLeftPixel = (unsigned char*)leftImage->imageData +
(i+m)*leftImage->widthStep + j+n;
leftPixel = *pLeftPixel;
if (k < SW2 || k >= imageWidth -SW2)
{
difPixel += leftPixel;
}else {
pRightPixel= (unsigned char*)rightImage->imageData+
(i+m)*rightImage->widthStep + k+n;
rightPixel = *pRightPixel;
difPixel += abs(leftPixel - rightPixel);
//cout<<Ddynamic[j +1][k+1]<<" ";
}
}
}
if (difPixel <= max12Diff)
{
Ddynamic[j][k] = Ddynamic[j+1][k+1] +1;
}else if (Ddynamic[j+1][k] > Ddynamic[j][k+1])
{
Ddynamic[j][k] = Ddynamic[j+1][k];
}else{
Ddynamic[j][k] = Ddynamic[j][k+1];
}
}
//cout<<"\n";
}
//逆向搜索，找出最佳路径
m = 0;
n = 0;
int m2 = m, n2 = n;
while( m < imageWidth && n < imageWidth)
{
if ((m == m2 + 1 && n >= n2 +1) || ( m > m2 +1 && n == n2 + 1))
{
pPixel = (unsigned char *)leftDepth->imageData + i*leftDepth->widthStep + m;
*pPixel = (m-n)*10;
//标记有效匹配点
pPixel = (unsigned char *)leftValid->imageData + i*leftValid->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 t3 = clock();
//refine the depth image 7*7中值滤波
//统计未能匹配点的个数
count = 0;
for (int i = 0 ;i< imageHeight;i++)
{
for (int j= 0; j< imageWidth;j++)
{
pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j;
pixel = *pPixel;
if (pixel == 0)
{
count++;
}
}
}
cout<<"Left Count: "<<count<<" "<<(double)count/(imageWidth*imageHeight)<<endl;
//将关键信息计入文件
fprintf(pFile,"\nDP:\nClock: %dms",t3-t1);
fprintf(pFile,"\nInvalid Point: %f",(double)count/(imageWidth*imageHeight));
// cvWaitKey(0);
leftFilter = cvCloneImage(leftDepth);
//7*7中值滤波
count = 0;
for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)
{
for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)
{
pPixel = (unsigned char *)leftValid->imageData + i*leftValid->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 *)leftDepth->imageData + m*leftDepth->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 *)leftFilter->imageData + i*leftFilter->widthStep + j;
*pPixel = medianVal;
}
}
}
}
//假如已知左右图片的深度，进行refinement
unsigned char dLeft = 0, dRight = 0;
count = 0;
for (int i = 0; i< imageHeight;i++)
{
for (int j = 0; j< imageWidth;j++)
{
pRightPixel= (unsigned char*)rightDepth->imageData + i*rightDepth->widthStep + j;
dRight = (*pRightPixel)/10;
if (j + dRight < imageWidth)
{
pPixel= (unsigned char*)depth->imageData + i*depth->widthStep + j;
pLeftPixel= (unsigned char*)leftDepth->imageData + i*leftDepth->widthStep + j + dRight;
dLeft = (*pLeftPixel)/10;
if (abs(int(dRight) -(int)dLeft) <= 1)
{
//深度一致
*pPixel = dRight*10;
}else{
//深度不一致，认为是误匹配、错误匹配
*pPixel = 0;
count++;
}
}
}
}
cout<<"depth Count: "<<count<<" "<<(double)count/(imageWidth*imageHeight)<<endl;
cvNamedWindow("leftImage",1);
cvNamedWindow("rightImage",1);
cvNamedWindow("leftDepth",1);
cvNamedWindow("leftValid",1);
cvNamedWindow("rightDepth",1);
cvNamedWindow("rightValid",1);
cvNamedWindow("leftFilter",1);
cvNamedWindow("rightFilter",1);
cvNamedWindow("depth",1);
cvShowImage("leftImage",leftImage);
cvShowImage("rightImage",rightImage);
cvShowImage("leftDepth",leftDepth);
cvShowImage("rightDepth",rightDepth);
cvShowImage("leftValid",leftValid);
cvShowImage("rightValid",rightValid);
cvShowImage("leftFilter",leftFilter);
cvShowImage("rightFilter",rightFilter);
cvShowImage("depth",depth);
cvSaveImage("leftDepth.jpg",leftDepth);
cvSaveImage("rightDepth.jpg",rightDepth);
cvSaveImage("leftValid.jpg",leftValid);
cvSaveImage("rightValid.jpg",rightValid);
cvSaveImage("leftFilter.jpg",leftFilter);
cvSaveImage("rightFilter.jpg",rightFilter);
cvSaveImage("depth.jpg",depth);
fclose(pFile);
cvWaitKey(0);
return 0;
}
