立体视觉算法-SGBM（一）

快来打我* 2022-04-02 07:49 697阅读 0赞

最近一直在学习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;  
\}  
贴出效果：  
  
![1358498191_8748.png][]  
![1358498205_8187.png][]  
![1358498218_6254.png][]  
  
但是仅仅用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;  
\}  
//效果  
![1358498243_2297.jpg][]  
![1358498254_3454.jpg][]  
![1358498262_1443.jpg][]  
  
  
论文中的多方向的DP暂时还没有实现，等实现时候再贴出来。

（未完待续）

[1358498191_8748.png]: https://img-my.csdn.net/uploads/201301/18/1358498191_8748.png
[1358498205_8187.png]: https://img-my.csdn.net/uploads/201301/18/1358498205_8187.png
[1358498218_6254.png]: https://img-my.csdn.net/uploads/201301/18/1358498218_6254.png
[1358498243_2297.jpg]: https://img-my.csdn.net/uploads/201301/18/1358498243_2297.jpg
[1358498254_3454.jpg]: https://img-my.csdn.net/uploads/201301/18/1358498254_3454.jpg
[1358498262_1443.jpg]: https://img-my.csdn.net/uploads/201301/18/1358498262_1443.jpg