立体视觉算法-SGBM（一）

最近一直在学习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;

}

贴出效果：







但是仅仅用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]
)

{

m--;

n--;

}else if (Ddynamic[m-1]
>= Ddynamic[m][n -1] && Ddynamic[m-1]
>= 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;

}

//效果







论文中的多方向的DP暂时还没有实现，等实现时候再贴出来。

（未完待续）