disparity.cpp

一个基于MFC的SIFT图像配准算法代码。好用

// Disparity.cpp: implementation of the CDisparity class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "disparity 200712.h"
#include "Disparity.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CDisparity::CDisparity()
{

}

CDisparity::~CDisparity()
{

}

//初始化视差图
void CDisparity::Initialization(int w_l, int h_l, int w_r, int h_r)
{
	width = w_l;
	height = h_l;

	//给视差图分配空间
	map = new CvPoint *[height];
	for (int y=0; y<height; y++)
	{
		map[y] = new CvPoint[width];
	}

	map_r = new bool *[h_r];
	for (y=0; y<h_r; y++)
	{
		map_r[y] = new bool[w_r];
	}

	//map置-1，表示未匹配
	for (y=0; y<height; y++)
	{
		for (int x=0; x<width; x++)
		{
			MAP(x,y) = cvPoint(-1,-1);
		}
	}
	for (y=0; y<h_r; y++)
	{
		for (int x=0; x<w_r; x++)
		{
			MAPR(x,y) = 0;
		}
	}
}


void CDisparity::SetSeed ( IplImage *l_img, CKeypoint *l_kp, int l_n_kp, IplImage *r_img, CKeypoint *r_kp, int r_n_kp )
{
	float **diff = new float *[l_n_kp];	//构造一个相异度矩阵,其元素初始值为-1，表示相异度无穷大
	for (int i=0; i<l_n_kp; i++)		//用法：diff[左图特征编号][右图特征编号]
	{
		diff[i] = new float[r_n_kp];
	}
	for (i=0; i<l_n_kp; i++)			//置-1
	{
		for(int j=0; j<r_n_kp; j++)
		{
			diff[i][j] = -1;
		}
	}//初始化完毕

	printf("构造相异度矩阵...");
	//在相异度矩阵中填入相异度
	for (i=0; i<l_n_kp; i++)
	{
		for (int j=0; j<r_n_kp; j++)
		{
		//	printf("%f\t",ELEM(l_img,l_kp[i].x,l_kp[i].y)-ELEM(r_img,r_kp[j].x,r_kp[j].y));
		//	if( fabs(ELEM(l_img,l_kp[i].x,l_kp[i].y)-ELEM(r_img,r_kp[j].x,r_kp[j].y))<40 )
			{
				//如果灰度值之差较小，继续计算128维的特征
				float s=0;
				for(int n=0; n<74; n++)//这里只用了74维,前面已经改了
				{
					//用欧式距离速度慢，但分离度高，门限可设为0.4
					//绝对距离速度快，门限设为0.6
					s += (float)fabs(l_kp[i].feature[n]-r_kp[j].feature[n]);
					//s += (float)pow(l_kp[i].feature[n]-r_kp[j].feature[n],2);
				}

				diff[i][j] = s;
				//可用下面以行来替换,加入了区域的约束
				//diff[i][j] = s + (float)sqrt(pow(l_kp[i].x-r_kp[j].x,2)-pow(l_kp[i].y-r_kp[j].y,2));
			}
		}
	}
	printf("ok\n");

	printf("匹配特征点...");
	//根据相异度矩阵计算特征点的匹配
	for (i=0; i<l_n_kp; i++)
	{
		float	diff_min = 4*256*128;	//最小的相异度,初始值设为一个大数
		int		diff_min_i = -1;
		int		diff_min_j = -1;	//最小相异度的编号
		
		//找出最小的相异度,并记录下其编号
		for (int j=0; j<r_n_kp; j++)
		{
			if ( diff_min>diff[i][j] && diff[i][j]>0 )
			{
				diff_min = diff[i][j];
				diff_min_i = i;
				diff_min_j = j;
			}
		}

		/*/反过来检查也必须是相异度最小的,否则抛弃（似乎加上这条限制就一个特征点也检测不出来了）
		for(int ii=0; ii<l_n_kp; ii++)
		{
			if ( ii!=i && diff_min>0.99*diff[ii][diff_min_j] )
			{
				diff_min_i = -1; //置-1，即抛弃该特征点
				diff_min_j = -1;
				break;
			}
		}/*/

		//如果其余的特征点相异度有大于最小相异度的0.6（门限）倍，则抛弃该特征点
		for (j=0; j<r_n_kp; j++)
		{
			if ( j!=diff_min_j && diff_min>0.4*diff[i][j] && diff[i][j]!=-1 )
			{
				diff_min_i = -1; //置-1，即抛弃该特征点
				diff_min_j = -1;
				break;
			}
		}//

		if ( diff_min_i>=0 && diff_min_j>=0 )
		{
			//在视差图上标记出匹配的特征点
			MAP( l_kp[diff_min_i].x, l_kp[diff_min_i].y ) = cvPoint( r_kp[diff_min_j].x, r_kp[diff_min_j].y );
	
			MAPR(r_kp[diff_min_j].x, r_kp[diff_min_j].y) = 1;

			//重新计算一个5x5临域的相异度(diff5)用来作种子
			double diff5=0;
			for (int ii=-2; ii<=2; ii++)
			{
				for (int jj=-2; jj<=2; jj++)
				{
					diff5 += fabs(ELEM(l_img,l_kp[diff_min_i].x,l_kp[diff_min_i].y)-ELEM(r_img,r_kp[diff_min_j].x,r_kp[diff_min_j].y));
				}
			}

			CSeed s;
			s.diff = diff5;
			s.l_x = l_kp[diff_min_i].x;
			s.l_y = l_kp[diff_min_i].y;
			s.r_x = r_kp[diff_min_j].x;
			s.r_y = r_kp[diff_min_j].y;

			heap.Insert(s);


		}
	}
	printf("ok\n");//匹配特征点ok
	
}

void CDisparity::DrawMatchedKp(IplImage *l_img, IplImage *r_img)
{
	for (int x=0; x<width; x++)
	{
		for (int y=0; y<height; y++)
		{
			if ( MAP(x,y).x != -1 && MAP(x,y).y != -1 )
			{
				CvScalar temp_color = color.getcolor();//cvScalar(0,255,0);//

				cvLine( l_img, cvPoint(x-5,y), cvPoint(x+5,y), temp_color );
				cvLine( l_img, cvPoint(x,y-5), cvPoint(x,y+5), temp_color );
				cvLine( l_img, cvPoint(x-5,y-1), cvPoint(x+5,y+1), temp_color );
				cvLine( l_img, cvPoint(x-1,y-5), cvPoint(x+1,y+5), temp_color );
				cvLine( l_img, cvPoint(x-5,y+1), cvPoint(x+5,y-1), temp_color );
				cvLine( l_img, cvPoint(x+1,y-5), cvPoint(x-1,y+5), temp_color );

				cvLine( r_img, cvPoint(MAP(x,y).x-5,MAP(x,y).y), cvPoint(MAP(x,y).x+5,MAP(x,y).y), temp_color );
				cvLine( r_img, cvPoint(MAP(x,y).x,MAP(x,y).y-5), cvPoint(MAP(x,y).x,MAP(x,y).y+5), temp_color );
				cvLine( r_img, cvPoint(MAP(x,y).x-5,MAP(x,y).y-1), cvPoint(MAP(x,y).x+5,MAP(x,y).y+1), temp_color );
				cvLine( r_img, cvPoint(MAP(x,y).x-1,MAP(x,y).y-5), cvPoint(MAP(x,y).x+1,MAP(x,y).y+5), temp_color );
				cvLine( r_img, cvPoint(MAP(x,y).x-5,MAP(x,y).y+1), cvPoint(MAP(x,y).x+5,MAP(x,y).y-1), temp_color );
				cvLine( r_img, cvPoint(MAP(x,y).x+1,MAP(x,y).y-5), cvPoint(MAP(x,y).x-1,MAP(x,y).y+5), temp_color );

				cvLine( r_img, cvPoint(MAP(x,y).x,MAP(x,y).y), cvPoint(x,y), cvScalar(0,255,0) );
				
			}
		}
	}
}

void CDisparity::Propagation(IplImage *l_img, IplImage *r_img)
{
	printf("匹配区域自动生长...");
	while(!heap.IsEmpty())
	{
		CSeed seed;
		seed = heap.PullMin();
		for (int i=-1; i<=1; i++)
		{
			for (int j=-1; j<=1; j++)
			{
				double TH=2;

				if (
					//此点不是边缘点
					seed.l_x>4 && seed.l_x<l_img->width-4 && seed.l_y>4 && seed.l_y<l_img->height-4
					&&
					//此点没有被分配过
					MAP(seed.l_x+i,seed.l_y+j).x == -1	
					&& 
					//此点不在灰度平滑的区域内
					( fabs(ELEM(l_img,seed.l_x+i,seed.l_y+j)-ELEM(l_img,seed.l_x+i+3,seed.l_y+j))>TH 
					||fabs(ELEM(l_img,seed.l_x+i,seed.l_y+j)-ELEM(l_img,seed.l_x+i,seed.l_y+j+3))>TH 
					||fabs(ELEM(l_img,seed.l_x+i,seed.l_y+j)-ELEM(l_img,seed.l_x+i-3,seed.l_y+j))>TH 
					||fabs(ELEM(l_img,seed.l_x+i,seed.l_y+j)-ELEM(l_img,seed.l_x+i,seed.l_y+j-3))>TH ))
				{
					double diff_min=255*255;
					int ii_min,jj_min;

					for(int ii=-1; ii<=1; ii++)
					{
						for (int jj=-1; jj<=1; jj++)
						{
							if (
								MAPR(seed.r_x+ii, seed.r_y+jj) == 0
								&&
								seed.r_x+ii>4 && seed.r_x+ii<r_img->width-4 && seed.r_y+jj>4 && seed.r_y+jj<r_img->height-4
								&&
								 (fabs(ELEM(r_img,seed.r_x+ii,seed.r_y+jj)-ELEM(r_img,seed.r_x+ii+3,seed.r_y+jj))>TH 
								||fabs(ELEM(r_img,seed.r_x+ii,seed.r_y+jj)-ELEM(r_img,seed.r_x+ii,seed.r_y+jj+3))>TH 
								||fabs(ELEM(r_img,seed.r_x+ii,seed.r_y+jj)-ELEM(r_img,seed.r_x+ii-3,seed.r_y+jj))>TH 
								||fabs(ELEM(r_img,seed.r_x+ii,seed.r_y+jj)-ELEM(r_img,seed.r_x+ii,seed.r_y+jj-3))>TH ))
							{	
								double diff=0;
								double l_avg=0,r_avg=0;

								//先计算临域的平均值
								for(int iii=-3; iii<=3; iii++)
								{
									for (int jjj=-3; jjj<=3; jjj++)
									{
										{
											l_avg += ELEM(l_img,seed.l_x+i+iii,seed.l_y+j+jjj)/49.0;
											r_avg += ELEM(r_img,seed.r_x+ii+iii,seed.r_y+jj+jjj)/49.0;
											//diff += fabs(ELEM(l_img,seed.l_x+i+iii,seed.l_y+j+jjj)-ELEM(r_img,seed.r_x+ii+iii,seed.r_y+jj+jjj));
										}
									}
								}
							
								double lr=0, ll=0, rr=0;//这些是自相关和互相关
								for(iii=-3; iii<=3; iii++)
								{
									for (int jjj=-3; jjj<=3; jjj++)
									{
										lr += (ELEM(l_img,seed.l_x+i+iii,seed.l_y+j+jjj)-l_avg)*(ELEM(r_img,seed.r_x+ii+iii,seed.r_y+jj+jjj)-r_avg);
										ll += (ELEM(l_img,seed.l_x+i+iii,seed.l_y+j+jjj)-l_avg)*(ELEM(l_img,seed.l_x+i+iii,seed.l_y+j+jjj)-l_avg);
										rr += (ELEM(r_img,seed.r_x+ii+iii,seed.r_y+jj+jjj)-r_avg)*(ELEM(r_img,seed.r_x+ii+iii,seed.r_y+jj+jjj)-r_avg);
									}
								}

								diff = sqrt(ll*rr)/lr;


							 //	if (diff<diff_min && diff<2 && diff>1 )
								if (diff<diff_min && diff<2 )
								{
									diff_min = diff;
									ii_min = ii;
									jj_min = jj;
								}
							}
						}
					}
					if (diff_min < 255*255)
					{
							
						CSeed	s;
						s.diff = diff_min;
						s.l_x = seed.l_x+i;
						s.l_y = seed.l_y+j;
						s.r_x = seed.r_x+ii_min;
						s.r_y = seed.r_y+jj_min;

						heap.Insert(s);

						//保存在视差图中
						MAP(seed.l_x+i,seed.l_y+j).x = seed.r_x+ii_min;
						MAP(seed.l_x+i,seed.l_y+j).y = seed.r_y+jj_min;

						MAPR(seed.r_x+ii_min, seed.r_y+jj_min)=1;

						/*////////////////////测试用///////////////////////
						//用来显示每次匹配的对应点

						IplImage *l,*r,*lc,*rc;
						l = cvCreateImage(cvSize(l_img->width,l_img->height),IPL_DEPTH_8U,1);
						r = cvCreateImage(cvSize(r_img->width,r_img->height),IPL_DEPTH_8U,1);
						lc = cvCreateImage(cvSize(l_img->width,l_img->height),IPL_DEPTH_8U,3);
						rc = cvCreateImage(cvSize(r_img->width,r_img->height),IPL_DEPTH_8U,3);
						cvConvert( l_img, l);	
						cvConvert( r_img, r);	
						cvCvtColor(l,lc, CV_GRAY2BGR);	
						cvCvtColor(r,rc, CV_GRAY2BGR);	
						

						cvLine( lc, cvPoint(s.l_x-8,s.l_y), cvPoint(s.l_x+8,s.l_y), cvScalar(0,0,255) );
						cvLine( lc, cvPoint(s.l_x,s.l_y-8), cvPoint(s.l_x,s.l_y+8), cvScalar(0,0,255) );
		
						cvLine( rc, cvPoint(s.r_x-8,s.r_y), cvPoint(s.r_x+8,s.r_y), cvScalar(0,0,255) );
						cvLine( rc, cvPoint(s.r_x,s.r_y-8), cvPoint(s.r_x,s.r_y+8), cvScalar(0,0,255) );

						cvNamedWindow("l", 1 );
						cvShowImage( "l", lc);	
						cvNamedWindow("r", 1 );
						cvShowImage( "r", rc);	

						cvWaitKey();
						/////////////////////测试用////////////////////*///
					}


				}//end of if
			}
		}//end of for
	}

	printf("ok\n");
}

void CDisparity::ShowMatch(IplImage *show, int w, int h)
{
	show = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 3);

	for (int x=0; x<w; x++)
	{
		for (int y=0; y<h; y++)
		{
			ELEM8(show,x,y) = 0;
		}
	}

	for (x=0; x<w; x+=5)
	{
		for (int y=0; y<h; y+=5)
		{
			if (MAP(x,y).x>-1)
			{
				//画出特征点的方向矢量
				cvLine( show, cvPoint(x,y), cvPoint( MAP(x,y).x, MAP(x,y).y), cvScalar(0,0,250) );
				//在彩色图像上用白色“+”表示出特征点的位置
				cvLine( show, cvPoint(x,y), cvPoint(x,y), cvScalar(255,255,255) );
			}
		}
	}

	cvNamedWindow("匹配图", 1 );
	cvShowImage( "匹配图", show );	
}

void CDisparity::ShowDisp(IplImage *show, int w, int h)
{
	show = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 3);
	double max=0;

	for (int x=0; x<w; x++)
	{
		for (int y=0; y<h; y++)
		{
			if( sqrt(pow(MAP(x,y).x-x,2)+pow(MAP(x,y).y-y,2))>max )
			{
				max = sqrt(pow(MAP(x,y).x-x,2)+pow(MAP(x,y).y-y,2));
			}
		}
	}
	max=max/8;
	for (x=0; x<w; x++)
	{
		for (int y=0; y<h; y++)
		{
			if (MAP(x,y).x == -1)
			{
				cvLine( show, cvPoint(x,y), cvPoint(x,y), cvScalar(0,0,0));
			}
			else
			{
				int dist = (int)(sqrt(pow(MAP(x,y).x-x,2)+pow(MAP(x,y).y-y,2)));

				CvScalar color;

				if (dist > max)
				{
					color = cvScalar(255,255,255);
					dist=255;
				}
				else
				{
					dist=int(dist/max*255);
					color = cvScalar( dist, dist, dist );
				}
				cvLine( show, cvPoint(x,y), cvPoint(x,y), color);
			}
		}
	}

	cvNamedWindow("视差图", 1 );
	cvShowImage( "视差图", show );
}