ransac.cpp

ransac是常用的稳健计算机视觉的方法

// OutlierDetection.cpp: implementation of the COutlierDetection class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "test.h"
#include "math.h"
#include "OutlierDetection.h"

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

#define expvalue 2.2204e-016
#define BrightValue 250
#define DarkValue 5
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
struct Point_Pair
{
	CPoint LP;
	CPoint RP;
	double march;
	double dtheta;
};
COutlierDetection::COutlierDetection()
{

}

COutlierDetection::~COutlierDetection()
{

}
/*
    输入参数：matchPair：初始匹配的特征点对
	          nMatchNum：初始匹配的特征点数目
			  index：最佳拟和时的拟和点在matchpair中的索引值
			  nAffineNum：拟和出来的点数目

*/
BOOL COutlierDetection::AFFINERANSAC(Point_Pair* matchPair,int nMatchNum,
								 int*& index,double*& dbAffine,int& nAffineNum)
{
	int s=3;//平面仿射变换
    index = NULL;
    if (ransac(matchPair,nMatchNum,s,dbAffine,index,nAffineNum)) 
        return TRUE;
	else
		return FALSE;
}
BOOL COutlierDetection::ransac(Point_Pair* matchpair,int nMatchNum,int s,
			double*& dbaffinepara,int*& index,int& nAffineNum)
{
  	double p = 0.99;    
	int maxTrials = 1000;    
	int maxDataTrials = 100; 
   	
	double* M = new double[6];
	memset(M,0.0,sizeof(double)*6);
	int	trialcount = 0;
    int bestscore =  0;  
	int score;
    int N = 1;
	int* ind = new int[s];
	int* perMIndex = NULL;
	int* bestMIndex = NULL;
	int i,k;
	int temp;
	while (N > trialcount)
	{
		/*产生随机序列*/
		srand((unsigned)time(NULL)); 
	    for(i=0;i<s;i++)
		{
		   temp = rand();
		   ind[i] = ((double)(temp%1000))/1000*nMatchNum;
		}
 		affinetransform(matchpair,ind,s,M);
		EvaluateDist(matchpair,nMatchNum,M,perMIndex,score);
		if(score > bestscore)   
		{
			bestscore = score;
			
			for(k=0;k<6;k++)
			{
  				if (M[k]<0.0000001) {
					M[k] = 0;
				}
			}
			dbaffinepara = M;
			/*删除前一拟和的序列*/
			if (bestMIndex!=NULL) {
				delete[] bestMIndex;
				bestMIndex = NULL;
			}
			bestMIndex = perMIndex;
			double fracinliers =  (double)score/nMatchNum;
			double pNoOutliers = 1 - pow(fracinliers,s);
			pNoOutliers = max(expvalue,pNoOutliers);
			pNoOutliers = min(1-expvalue, pNoOutliers);
			N = log(1-p)/log(pNoOutliers);
			trialcount = 0;
			continue;
		}
		else
		{
			if (perMIndex) {
				delete[] perMIndex;
				perMIndex = NULL;
			}
			trialcount = trialcount+1;
		}
		if (trialcount>maxTrials) {
			break;
		}
	}
	if (dbaffinepara==NULL) {
		return FALSE;
	}
	else{
		index = bestMIndex;
		nAffineNum = bestscore;
		return TRUE;
	}
}
void COutlierDetection::affinetransform(Point_Pair* matchpair,int* ind,int s,double* M)
{
	int i,j;
	CPoint pPointBase[3];
	CPoint pPointSampl[3];
	for(i=0;i<s;i++)
	{
       j = ind[i];
	   pPointBase[i] = matchpair[j].LP;
	   pPointSampl[i] = matchpair[j].RP;
	}
	//*第一个系数 ＝ R*第二个系数矩阵
	GetAffinePara(pPointSampl,pPointBase,M);
	return;
}

void COutlierDetection::EvaluateDist(Point_Pair* matchpair,int nMatchNum,double* M,int*& x,int& ninliers)
{
	CPoint LP,RP;
	ninliers = 0;
	x = new int[nMatchNum];
    double recx,double recy;
	for(int i=0;i<nMatchNum;i++)
	{
		LP = matchpair[i].LP;
		RP = matchpair[i].RP;
		recx = M[0*3+0]*LP.x+M[0*3+1]*LP.y+M[0*3+2];
		recy = M[1*3+0]*LP.x+M[1*3+1]*LP.y+M[1*3+2];
		if (abs(recx-RP.x)<1||abs(recy-RP.y)<1) {
             x[ninliers] = i;
			 ninliers++;	}
	}
	return;
}