立体视觉算法-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 =

补充：综合编程 , 其他综合 ,