立体视觉算法--SAD+DP

 

立体视觉算法--SAD+DP

标签: 立体视觉
  1867人阅读  评论(1)  收藏  举报
[cpp]  view plain  copy
  1. #include <cstdio>  
  2. #include <cstring>  
  3. #include <iostream>  
  4. #include<cv.h>  
  5. #include<highgui.h>  
  6. #include <cmath>  
  7.   
  8. using namespace std;  
  9. const int Width =  512;  
  10. const int Height = 512;  
  11. int Ddynamic[Width][Width];  
  12.   
  13. int  main()  
  14. {    
  15.     //打开文件  
  16.     FILE* pFile;  
  17.     if (pFile = fopen("data.txt","a"))  
  18.     {  
  19.         //cout <<"File Open Success"<<endl;   
  20.     }else{  
  21.         //cout <<"File Open Failed"<<end;  
  22.         return 0;  
  23.     }  
  24.     // IplImage * leftImage = cvLoadImage("l1.png",0);  
  25.     // IplImage * rightImage = cvLoadImage("r1.png",0);  
  26.   
  27.     //IplImage * leftImage = cvLoadImage("l2.jpg",0);  
  28.     //IplImage * rightImage = cvLoadImage("r2.jpg",0);  
  29.   
  30.   
  31.     IplImage * leftImage = cvLoadImage("left.bmp",0);                             
  32.     IplImage * rightImage = cvLoadImage("right.bmp",0);  
  33.   
  34.     int imageWidth = leftImage->width;  
  35.     int imageHeight =leftImage->height;  
  36.   
  37.     IplImage * leftDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  38.     IplImage * rightDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  39.   
  40.     IplImage * leftValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  41.     IplImage * rightValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  42.   
  43.     IplImage * leftFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  44.     IplImage * rightFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  45.   
  46.     IplImage * depth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  47.     IplImage * valid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);  
  48.   
  49.     unsigned char * pPixel = NULL;  
  50.     unsigned char  pixel;  
  51.     unsigned char * pPixel2 = NULL;  
  52.     unsigned char  pixel2;  
  53.     cvZero(leftDepth);  
  54.     cvZero(rightDepth);  
  55.     cvZero(leftValid);  
  56.     cvZero(rightValid);  
  57.     cvZero(leftFilter);  
  58.     cvZero(rightFilter);  
  59.     cvZero(depth);  
  60.     cvZero(valid);  
  61.   
  62.   
  63.   
  64.     CvSize SADWindowSize = cvSize(5,5);  //(widh,height)  
  65.     int SW2 = SADWindowSize.width/2;  
  66.     int SH2 = SADWindowSize.height/2;  
  67.     cout<<"SH2:  "<<SH2<<endl;  
  68.     cout<<"SW2:  "<<SW2<<endl;  
  69.   
  70.     int minD = 0;  
  71.     int maxD = 15;  
  72.     //假设图像是经过矫正的,那么每次都只是需要搜搜同一行的内容  
  73.     int max12Diff = 8*SADWindowSize.width*SADWindowSize.height;  
  74.   
  75.     for (int i = 0;i < imageWidth;i++)  
  76.     {  
  77.         Ddynamic[0][i] = 0;  
  78.         Ddynamic[i][0] = 0;  
  79.     }  
  80.     unsigned char * pLeftPixel  = NULL;  
  81.     unsigned char * pRightPixel = NULL;  
  82.     unsigned char leftPixel = 0;  
  83.     unsigned char rightPixel =0;  
  84.     int m,n,l;  
  85.     int difPixel = 0;  
  86.     int t1 = clock();  
  87.     for (int i = SH2 ; i < imageHeight -SH2;i++)  
  88.     {  
  89.         for (int j = SW2; j<imageWidth - SW2;j++)  
  90.         {  
  91.             for (int k = j + minD; k <= j + maxD;k++)  
  92.             {  
  93.                 difPixel = 0;  
  94.                 for (m = -SH2;m <= SH2;m++)  
  95.                 {  
  96.                     for (n = -SW2; n <= SW2;n++)  
  97.                     {  
  98.                         pRightPixel= (unsigned char*)rightImage->imageData+  
  99.                             (i+m)*rightImage->widthStep + j+n;  
  100.                         rightPixel = *pRightPixel;  
  101.                         if (k < SW2  || k  >= imageWidth -SW2 )  
  102.                         {  
  103.                             difPixel += rightPixel;  
  104.                         }else {  
  105.                             pLeftPixel = (unsigned char*)leftImage->imageData +   
  106.                                 (i+m)*leftImage->widthStep + k+n;    
  107.                             leftPixel  = *pLeftPixel;   
  108.                             difPixel += abs(leftPixel - rightPixel);  
  109.                         }  
  110.   
  111.                     }  
  112.                 }  
  113.                 if (difPixel <= max12Diff)  
  114.                 {  
  115.                     Ddynamic[j + 1][k + 1] = Ddynamic[j][k] +1;   
  116.                 }else if (Ddynamic[j][k+1] > Ddynamic[j+1][k])  
  117.                 {  
  118.                     Ddynamic[j + 1][k + 1] = Ddynamic[j][k+1];  
  119.                 }else{  
  120.                     Ddynamic[j+1][k+1] = Ddynamic[j+1][k];  
  121.                 }  
  122.   
  123.                 //cout<<Ddynamic[j +1][k+1]<<"  ";  
  124.             }  
  125.             //cout<<"\n";  
  126.         }  
  127.         //逆向搜索,找出最佳路径  
  128.         m = imageWidth - SW2;  
  129.         n = imageWidth - SW2;  
  130.         int m2 = m, n2 = n;  
  131.   
  132.         while( m >= 1 && n >= 1)  
  133.         {  
  134.             if ((m2 == m + 1 && n2 >= n +1) || ( m2 > m +1 && n2 == n + 1))  
  135.             {  
  136.                 pPixel = (unsigned char *)rightDepth->imageData + i*rightDepth->widthStep + m;  
  137.                 *pPixel = (n-m)*10;  
  138.                 //标记有效匹配点  
  139.                 pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + m;  
  140.                 *pPixel = 255;  
  141.   
  142.                 m2 = m;  
  143.                 n2 = n;  
  144.             }  
  145.             if (Ddynamic[m-1][n-1] >= Ddynamic[m][n -1] && Ddynamic[m-1][n -1] >= Ddynamic[m-1][n])  
  146.             {  
  147.   
  148.                 m--;   
  149.                 n--;  
  150.             }else if (Ddynamic[m-1][n] >= Ddynamic[m][n -1] && Ddynamic[m-1][n] >= Ddynamic[m-1][n -1])  
  151.             {   
  152.                 m--;  
  153.             }  
  154.             else  
  155.             {   
  156.                 n--;  
  157.             }  
  158.   
  159.         }  
  160.   
  161.         //cvWaitKey(0);  
  162.     }  
  163.     int t2 = clock();  
  164.     cout<<"dt: "<<t2-t1<<endl;  
  165.     //统计未能匹配点的个数  
  166.     int count = 0;  
  167.     for (int i = 0 ;i< imageHeight;i++)  
  168.     {  
  169.         for (int j= 0; j< imageWidth;j++)  
  170.         {  
  171.             pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + j;  
  172.             pixel = *pPixel;  
  173.             if (pixel == 0)  
  174.             {  
  175.                 count++;  
  176.             }  
  177.         }  
  178.     }  
  179.     cout<<"rightCount:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;  
  180.     rightFilter = cvCloneImage(rightDepth);  
  181.     //7*7中值滤波  
  182.     int halfMedianWindowSize = 3;  
  183.     int medianWindowSize = 2*halfMedianWindowSize + 1;  
  184.     int medianArray[100] = {0};  
  185.     count = 0;  
  186.     int temp = 0;  
  187.     int medianVal = 0;  
  188.   
  189.     for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)  
  190.     {  
  191.         for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)  
  192.         {  
  193.             pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + j;  
  194.             pixel = *pPixel;  
  195.             if (pixel == 0)  
  196.             {  
  197.                 count = 0;  
  198.                 for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++)  
  199.                 {  
  200.                     for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++)  
  201.                     {  
  202.                         pPixel2 = (unsigned char *)rightDepth->imageData + m*rightDepth->widthStep + n;  
  203.                         pixel2 = *pPixel2;  
  204.                         if (pixel2 != 0)  
  205.                         {  
  206.                             medianArray[count] = pixel2;  
  207.                             count++;  
  208.                         }  
  209.   
  210.                     }  
  211.                     //排序  
  212.                     for (int k = 0; k< count;k++)  
  213.                     {  
  214.                         for (int l = k + 1; l< count;l++)  
  215.                         {  
  216.                             if (medianArray[l] < medianArray[l-1] )  
  217.                             {  
  218.                                 temp = medianArray[l];  
  219.                                 medianArray[l] = medianArray[l-1];  
  220.                                 medianArray[l-1] = temp;  
  221.                             }  
  222.                         }  
  223.                     }  
  224.                     medianVal = medianArray[count/2];  
  225.                     pPixel = (unsigned char *)rightFilter->imageData + i*rightFilter->widthStep + j;  
  226.                     *pPixel = medianVal;  
  227.                 }  
  228.   
  229.             }  
  230.         }  
  231.     }  
  232.   
  233.     //两次进行DP计算,分别是左对右和右对左,从右想左扫描  
  234.     for (int j = 0; j<= imageWidth;j++)  
  235.     {  
  236.         for (int k = 0;k <= imageWidth;k++)  
  237.         {  
  238.             Ddynamic[j][k] = 0;  
  239.         }  
  240.     }  
  241.     minD = -maxD;  
  242.     maxD = 0;  
  243.     
  244.     for (int i = SH2 ; i < imageHeight -SH2;i++)  
  245.     {  
  246.         for (int j = imageWidth-SW2-1; j >= SW2;j--)  
  247.         {  
  248.             for (int k = j + maxD; k >= j + minD;k--)  
  249.             {  
  250.                 difPixel = 0;  
  251.                 for (m = -SH2;m <= SH2;m++)  
  252.                 {  
  253.                     for (n = -SW2; n <= SW2;n++)  
  254.                     {  
  255.                         pLeftPixel = (unsigned char*)leftImage->imageData +   
  256.                                      (i+m)*leftImage->widthStep + j+n;  
  257.                         leftPixel  = *pLeftPixel;  
  258.                         if (k < SW2 || k  >= imageWidth -SW2)  
  259.                         {  
  260.                              difPixel += leftPixel;  
  261.                         }else {  
  262.                               
  263.                             pRightPixel= (unsigned char*)rightImage->imageData+  
  264.                                          (i+m)*rightImage->widthStep + k+n;   
  265.                             rightPixel = *pRightPixel;  
  266.                             difPixel += abs(leftPixel - rightPixel);  
  267.                             //cout<<Ddynamic[j +1][k+1]<<"  ";  
  268.                         }  
  269.                     }  
  270.                 }  
  271.   
  272.                 if (difPixel <= max12Diff)  
  273.                 {  
  274.                     Ddynamic[j][k] = Ddynamic[j+1][k+1] +1;   
  275.                 }else if (Ddynamic[j+1][k] > Ddynamic[j][k+1])  
  276.                 {  
  277.                     Ddynamic[j][k] = Ddynamic[j+1][k];  
  278.                 }else{  
  279.                     Ddynamic[j][k] = Ddynamic[j][k+1];  
  280.                 }  
  281.   
  282.             }  
  283.             //cout<<"\n";  
  284.         }  
  285.         //逆向搜索,找出最佳路径  
  286.   
  287.         m = 0;  
  288.         n = 0;  
  289.         int m2 = m, n2 = n;  
  290.         while( m < imageWidth && n < imageWidth)  
  291.         {  
  292.             if ((m == m2 + 1 && n >= n2 +1) || ( m > m2 +1 && n == n2 + 1))  
  293.             {  
  294.                 pPixel = (unsigned char *)leftDepth->imageData + i*leftDepth->widthStep + m;  
  295.                 *pPixel = (m-n)*10;  
  296.                 //标记有效匹配点  
  297.                 pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + m;  
  298.                 *pPixel = 255;  
  299.   
  300.                 m2 = m;  
  301.                 n2 = n;  
  302.             }  
  303.             if (Ddynamic[m+1][n+1] >= Ddynamic[m][n+1] && Ddynamic[m+1][n+1] >= Ddynamic[m+1][n])  
  304.             {  
  305.   
  306.                 m++;   
  307.                 n++;  
  308.             }else if (Ddynamic[m+1][n] >= Ddynamic[m][n +1] && Ddynamic[m+1][n] >= Ddynamic[m+1][n+1])  
  309.             {   
  310.                 m++;  
  311.             }  
  312.             else  
  313.             {   
  314.                 n++;  
  315.             }  
  316.   
  317.         }  
  318.         //cvWaitKey(0);  
  319.     }  
  320.     int t3 = clock();  
  321.     //refine the depth image  7*7中值滤波  
  322.     //统计未能匹配点的个数  
  323.     count = 0;  
  324.     for (int i = 0 ;i< imageHeight;i++)  
  325.     {  
  326.         for (int j= 0; j< imageWidth;j++)  
  327.         {  
  328.             pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j;  
  329.             pixel = *pPixel;  
  330.             if (pixel == 0)  
  331.             {  
  332.                 count++;  
  333.             }  
  334.         }  
  335.     }  
  336.   
  337.     cout<<"Left Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;  
  338.   
  339.     //将关键信息计入文件  
  340.     fprintf(pFile,"\nDP:\nClock:   %dms",t3-t1);  
  341.     fprintf(pFile,"\nInvalid Point:   %f",(double)count/(imageWidth*imageHeight));  
  342.     // cvWaitKey(0);  
  343.     leftFilter = cvCloneImage(leftDepth);  
  344.     //7*7中值滤波  
  345.     count = 0;  
  346.     for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)  
  347.     {  
  348.         for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)  
  349.         {  
  350.             pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j;  
  351.             pixel = *pPixel;  
  352.             if (pixel == 0)  
  353.             {  
  354.                 count = 0;  
  355.                 for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++)  
  356.                 {  
  357.                     for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++)  
  358.                     {  
  359.                         pPixel2 = (unsigned char *)leftDepth->imageData + m*leftDepth->widthStep + n;  
  360.                         pixel2 = *pPixel2;  
  361.                         if (pixel2 != 0)  
  362.                         {  
  363.                             medianArray[count] = pixel2;  
  364.                             count++;  
  365.                         }  
  366.   
  367.                     }  
  368.                     //排序  
  369.                     for (int k = 0; k< count;k++)  
  370.                     {  
  371.                         for (int l = k + 1; l< count;l++)  
  372.                         {  
  373.                             if (medianArray[l] < medianArray[l-1] )  
  374.                             {  
  375.                                 temp = medianArray[l];  
  376.                                 medianArray[l] = medianArray[l-1];  
  377.                                 medianArray[l-1] = temp;  
  378.                             }  
  379.                         }  
  380.                     }  
  381.                     medianVal = medianArray[count/2];  
  382.                     pPixel = (unsigned char *)leftFilter->imageData + i*leftFilter->widthStep + j;  
  383.                     *pPixel = medianVal;  
  384.                 }  
  385.   
  386.             }  
  387.         }  
  388.     }  
  389.     //假如已知左右图片的深度,进行refinement  
  390.     unsigned char dLeft = 0, dRight = 0;  
  391.     count = 0;  
  392.     for (int i = 0; i< imageHeight;i++)  
  393.     {  
  394.         for (int j = 0; j< imageWidth;j++)  
  395.         {  
  396.             pRightPixel= (unsigned char*)rightDepth->imageData + i*rightDepth->widthStep + j;  
  397.             dRight = (*pRightPixel)/10;  
  398.             if (j + dRight < imageWidth)  
  399.             {      
  400.                 pPixel= (unsigned char*)depth->imageData + i*depth->widthStep + j;  
  401.                 pLeftPixel= (unsigned char*)leftDepth->imageData + i*leftDepth->widthStep + j + dRight;  
  402.                 dLeft = (*pLeftPixel)/10;  
  403.                 if (abs(int(dRight) -(int)dLeft) <= 1)  
  404.                 {  
  405.                     //深度一致    
  406.                     *pPixel = dRight*10;  
  407.                       
  408.                 }else{  
  409.                     //深度不一致,认为是误匹配、错误匹配  
  410.                     *pPixel = 0;  
  411.                     count++;  
  412.                 }  
  413.             }  
  414.         }  
  415.     }  
  416.     cout<<"depth Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;  
  417.   
  418.   
  419.     cvNamedWindow("leftImage",1);  
  420.     cvNamedWindow("rightImage",1);  
  421.     cvNamedWindow("leftDepth",1);  
  422.     cvNamedWindow("leftValid",1);  
  423.     cvNamedWindow("rightDepth",1);  
  424.     cvNamedWindow("rightValid",1);  
  425.     cvNamedWindow("leftFilter",1);  
  426.     cvNamedWindow("rightFilter",1);  
  427.     cvNamedWindow("depth",1);  
  428.   
  429.       
  430.     cvShowImage("leftImage",leftImage);  
  431.     cvShowImage("rightImage",rightImage);  
  432.   
  433.     cvShowImage("leftDepth",leftDepth);  
  434.     cvShowImage("rightDepth",rightDepth);  
  435.   
  436.     cvShowImage("leftValid",leftValid);   
  437.     cvShowImage("rightValid",rightValid);  
  438.   
  439.     cvShowImage("leftFilter",leftFilter);  
  440.     cvShowImage("rightFilter",rightFilter);  
  441.     cvShowImage("depth",depth);  
  442.   
  443.     cvSaveImage("leftDepth.jpg",leftDepth);  
  444.     cvSaveImage("rightDepth.jpg",rightDepth);  
  445.     cvSaveImage("leftValid.jpg",leftValid);  
  446.     cvSaveImage("rightValid.jpg",rightValid);  
  447.     cvSaveImage("leftFilter.jpg",leftFilter);  
  448.     cvSaveImage("rightFilter.jpg",rightFilter);  
  449.     cvSaveImage("depth.jpg",depth);  
  450.   
  451.   
  452.   
  453.     fclose(pFile);  
  454.     cvWaitKey(0);  
  455.     return 0;  
  456. }  

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值