立體視覺算法--SAD+DP

[cpp]   #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;   }