circlefit.txt

最小二乘法圆拟合

//for circle fit

float slopeTable[361]; //record the slope from 0 to 360 degree
float cosTable[361];
float sinTable[361];



void poDefineAllCircles()
{
	int i, r;
	double theta;

	for(i=0; i<361; i++){
		theta=i*PI/180;
		if(i==90 || i==270)
			slopeTable[i]= float(tan((i-0.1)*PI/180));
		else
			slopeTable[i]=float(tan(theta));
		cosTable[i]=float(cos(theta));
		sinTable[i]=float(sin(theta));
	}

}


int RecursiveGetIndex(float slope, int iStart, int iEnd)
{
	register iMiddle;	

	do {
		iMiddle=(iStart+iEnd)/2;
		if( slope<slopeTable[iMiddle]) iEnd=iMiddle;
		else iStart=iMiddle;
	}while (iEnd-iStart> 1);

	return iMiddle; 
}

int GetIndexBySlope(float x, float y, int iStart)
{
	register i;
	float slope;

	if(fabs(x)<0.01){
		if(y>0) return 90;
		else return 270;
	}
	else{
		slope=y/x;
		if(x>0 && y>=0){ //first quater
			if(x>y) return RecursiveGetIndex(slope, 0, 45);
			else    return RecursiveGetIndex(slope, 45, 90);
		}
		else if(x<0 && y>=0){ //2nd quater
			if(-x<y) return RecursiveGetIndex(slope, 91, 135);
			else     return RecursiveGetIndex(slope, 135, 180);
		}
		else if(x<0 && y<0){ //3rd quater
			if(x<y)	return RecursiveGetIndex(slope, 180, 225);
			else    return RecursiveGetIndex(slope, 225, 270);
		}
		else { //4th quater
			if(x<-y) return RecursiveGetIndex(slope, 271, 315);
			else     return RecursiveGetIndex(slope, 315, 360);
		}
	}
}


__declspec(dllexport) int _stdcall 
LSE_Circle_Fit(TFPoint *arrPts,
				 int numPts,
				 int *bPtsOn,
				 TFPoint *center,  //pass estimated position down and pass accurate position back
				 float *radius)
{
	register i,j, count;
	float a[4][4], b[4], x, y, t; //index used from 1 to 3
	float M_xx, M_yy, M_x, M_y, M_z, M_xy, M_xz, M_yz, xx, yy;
	int id[4];
	float** ppa;
	float* pa[4];

	M_xx=M_yy=M_x=M_y=M_z=M_xy= M_xz= M_yz=0;

	for(i=count=0; i<numPts; i++){
		if(!bPtsOn[i]) continue;
		else count++;
		x=arrPts[i].x-center->x;  //make value smaller to avoid overflow
		y=arrPts[i].y-center->y;
		xx= x*x;
		yy= y*y;
		M_xx+=xx;  // M_xx
		M_yy+=yy;
		M_xy+=x*y;
		M_x +=x;
		M_y +=y;
		M_xz+=x*(xx+yy);
		M_yz+=y*(xx+yy);
	}
	M_z= M_xx+M_yy;
	a[1][1]=M_xx;
	a[1][2]=M_xy;
	a[1][3]=M_x;
	a[2][1]=M_xy;
	a[2][2]=M_yy;
	a[2][3]=M_y;
	a[3][1]=M_x;
	a[3][2]=M_y;
	a[3][3]=float(count);

	b[1]=-M_xz;
	b[2]=-M_yz;
	b[3]=-M_z;

	pa[1]=&(a[1][0]);
	pa[2]=&(a[2][0]);
	pa[3]=&(a[3][0]);
	ppa=pa;
	ludcmp((double **)ppa, 3, id, &t);
	lubksb((double **)ppa, 3, id, (double*)b);
	center->x += -b[1]/float(2);
	center->y += -b[2]/float(2);
	*radius = sqrt(b[1]*b[1]+b[2]*b[2]-b[3]);

	return 0;
}

__declspec(dllexport) int _stdcall 
Circle_Fit(TFPoint *arrPts,
			 int numPts,
			 float fMaxWobble,
			 TFPoint *center,  //pass estimated position down and pass accurate position back
			 float *radius,
			 int *bPtsOn,	
			 int iDebugCode)
{
	register i,j;
	int index, numOutPts,  count=1;
	float fErrSum, fErrThreshold, arrErr[MAXPFITNUM], arrRadius[MAXPFITNUM]; //some problem here 
	TFPoint arrTempPts[MAXPFITNUM];
	int DebugNum=0; float tempRadius; 
	int OnCount;
   	

	poDefineAllCircles();
	for(i=0; i<numPts; i++) bPtsOn[i]=1;

	do{ 
		OnCount=0; //to avoid access vilation
		for(j=0;j<numPts;j++) {if(bPtsOn[j])OnCount+=1;}
        if(OnCount>max(4,numPts/2)) LSE_Circle_Fit(arrPts, numPts, bPtsOn, center, radius);
		else return numPts-OnCount;
		//DrawPixel(center->x,center->y, RED);
		for(i=numOutPts=0, fErrSum=0; i<numPts; i++){
			if(iDebugCode==1 && bPtsOn[i])
				DrawPixel(arrPts[i].x, arrPts[i].y, RED);

			if(!bPtsOn[i]) continue;
			index= GetIndexBySlope(arrPts[i].x-center->x,arrPts[i].y-center->y,0);
			arrRadius[i]=(arrPts[i].x-center->x)*cosTable[index]+(arrPts[i].y-center->y)*sinTable[index];
			if((arrErr[i]=fabs(arrRadius[i]-*radius))>fMaxWobble ){
				numOutPts+=1;
				fErrSum+=arrErr[i];
				if(iDebugCode==1) DrawPixel(arrPts[i].x, arrPts[i].y, BLUE);
			}
			else if(iDebugCode==1) DrawPixel(arrPts[i].x, arrPts[i].y, LIME);
		}
		if(numOutPts){
			fErrThreshold= fErrSum/numOutPts;
			for(i=0; i<numPts; i++){
				if(!bPtsOn[i])continue;
				if(arrErr[i]>=fErrThreshold){
					bPtsOn[i]=0;
					if(iDebugCode==1)
						DrawPixel(arrPts[i].x, arrPts[i].y, YELLOW);
				}
			}
		}
	}while (numOutPts && count++<10 );

        //refine circle //8304
		OnCount=0; 
		for(j=0;j<numPts;j++) {if(bPtsOn[j])OnCount+=1;}
        if(OnCount>max(4,numPts/2)) LSE_Circle_Fit(arrPts, numPts, bPtsOn, center, radius);


	//DrawPixel(center->x,center->y, LIME);
	return numOutPts;
}



int LSFitCircle(TFPoint *pfPoints, int *pNumPnts, float Radius, float CenterRange, float CenterStep, float Cx, float Cy, float *pFitRadius, float *pFitCx, float *pFitCy, int iDebugCode, float DebugRadius, int PartialCircleFlag)
{
	if(*pNumPnts>=MAXPFITNUM||*pNumPnts<=0) return 0;
	*pFitRadius=0; *pFitCx=0; *pFitCy=0;
	TFPoint arrPts[MAXPFITNUM],center, RtnCenter, arrTempPts[MAXPFITNUM], arrTempPts1[MAXPFITNUM];
	float tmp, sumRadius, meanRadius;
	int arrPtsOn[MAXPFITNUM];
	int i,j,NumPnt,Smpl=*pNumPnts/10;
	int DebugNum=0, DebugNum1=0; float tempRadius;
	if(Smpl<=0) Smpl=1;
	if(Smpl==1&&*pNumPnts>15) Smpl=2;
	
	NumPnt=0;
	for(i=0;i<*pNumPnts;i++){
		if(i%Smpl==0){
			arrPts[NumPnt].x=pfPoints[i].x;
			arrPts[NumPnt].y=pfPoints[i].y;
			NumPnt++;
		}		
	}
	center.x=Cx;
	center.y=Cy;
	RtnCenter=center;
	
	
	//pre-processing to reduce the outliers 
	DebugNum=0; sumRadius=0;
    for(i=0;i<NumPnt;i++){				
		tempRadius=sqrt((arrPts[i].x-center.x)*(arrPts[i].x-center.x)+(arrPts[i].y-center.y)*(arrPts[i].y-center.y));
		if(tempRadius<DebugRadius*2){	 //to deal with the current ball using boundary of the previous ball
		//if(tempRadius>DebugRadius*0.1&&tempRadius<DebugRadius*2){	
		//tempRadius=sqrt((arrPts[i].x-center.x)*(arrPts[i].x-center.x)+(arrPts[i].y-center.y)*(arrPts[i].y-center.y));
		//if(abs(arrPts[i].x-center.x)<DebugRadius&&abs(arrPts[i].y-center.y)<DebugRadius&&tempRadius>DebugRadius/1.414){
			arrTempPts[DebugNum].x=arrPts[i].x; arrTempPts[DebugNum].y=arrPts[i].y;
			DebugNum++;
			sumRadius=sumRadius+tempRadius;
		}
		//else{DrawPixel(arrPts[i].x, arrPts[i].y, RED);}
	}

	//if(Radius>DebugRadius/1.2&&Radius<DebugRadius*1.2){	
	//	Circle_Fit(arrPts, NumPnt, Radius/20, &RtnCenter, pFitRadius,arrPtsOn,Debug,DebugRadius);
	//}
	//else{
	//Circle_Fit(arrTempPts, DebugNum, Radius/20, &RtnCenter, pFitRadius,arrPtsOn,Debug,DebugRadius);
	//}

    meanRadius = sumRadius/DebugNum;
	if(iDebugCode==4096) iDebugCode=1;
	else iDebugCode=0; 

    int div=2; 

	if(DebugNum>NumPnt/2){
		Circle_Fit(arrTempPts, DebugNum, Radius/10, &RtnCenter, pFitRadius,arrPtsOn,iDebugCode);
       //re fit circle 
	    if(PartialCircleFlag<8192&&PartialCircleFlag>0){ 
			DebugNum1=0; 
			for(i=0;i<DebugNum;i++){				
				if(arrPtsOn[i]>0){
					if ((PartialCircleFlag==1 && (arrTempPts[i].x>RtnCenter.x+*pFitRadius/div || arrTempPts[i].y > RtnCenter.y+*pFitRadius/div))
					|| (PartialCircleFlag==2 && (arrTempPts[i].x<RtnCenter.x-*pFitRadius/div || arrTempPts[i].y > RtnCenter.y+*pFitRadius/div))
					|| (PartialCircleFlag==3 && (arrTempPts[i].x<RtnCenter.x-*pFitRadius/div || arrTempPts[i].y < RtnCenter.y-*pFitRadius/div))
					|| (PartialCircleFlag==4 && (arrTempPts[i].x>RtnCenter.x+*pFitRadius/div || arrTempPts[i].y < RtnCenter.y-*pFitRadius/div)))					
					{
					arrTempPts1[DebugNum1].x=arrTempPts[i].x; arrTempPts1[DebugNum1].y=arrTempPts[i].y;
					DebugNum1++;}
					else DrawPixel(arrTempPts[i].x,arrTempPts[i].y,BLUE);
				}
			}

			if(DebugNum1>DebugNum/4){
				Circle_Fit(arrTempPts1, DebugNum1, Radius/20, &RtnCenter, pFitRadius,arrPtsOn,iDebugCode);}
		}
	}
	else{
        *pFitRadius=Radius;
		//DrawLine(Cx-2,Cx+2,Cy-2,Cy+2,BLUE);
		//DrawPixel(Cx,Cy,LIME);
		//Circle_Fit(arrPts, NumPnt, Radius/20, &RtnCenter, pFitRadius,arrPtsOn,Debug,DebugRadius);
	     }
    
		
	*pFitCx=RtnCenter.x;
	*pFitCy=RtnCenter.y;

	j=0;
	for(i=0;i<NumPnt;i++){		
		if(arrPtsOn[i]>0){ 
			pfPoints[j].x=arrPts[i].x;
			pfPoints[j].y=arrPts[i].y;
			j++;
		}
	}
	*pNumPnts=j;
	return j;
}

*/