////////////////////////////////////////////////////////
//二次畸變校正參數
//寫死在程序中!!!!!!!!!
////////////////////////////////////////////////////////
double fc[2] = {192.55704  , 192.16682 };
double cc[2] = {163.52686  , 126.04836};
double alpha_c =0.00000;
double kc[5]= { -0.31735 ,  0.11324  , 0.00406  , -0.00148 , 0.00000};
long Image_id=0;

//二次畸變校正函數
void CorrDistortion(double& normalX,double& normalY,double distortX,double distortY,double fc[2],double cc[2],double kc[5],double alpha_c)
{
	double x_distort=(distortX-cc[0])/fc[0];
	double y_distort=(distortY-cc[1])/fc[1];

	x_distort=x_distort-alpha_c*y_distort;

	if (sqrt(kc[0]*kc[0]+kc[1]*kc[1]+kc[2]*kc[2]+kc[3]*kc[3]+kc[4]*kc[4])!=0)
	{
		double k1 = kc[0];
		double k2 = kc[1];
		double k3 = kc[4];
		double p1 = kc[2];
		double p2 = kc[3];
		double r_2;
		double k_radial;

		double delta_x;
		double delta_y;

		normalX=x_distort;
		normalY=y_distort;

		for (int i=0;i<20;i++)
		{
			r_2=normalX*normalX+normalY*normalY;
			k_radial=1+k1*r_2+k2*r_2*r_2+k3*r_2*r_2*r_2;
			delta_x=2*p1*normalX*normalY+p2*(r_2+2*normalX*normalX);
			delta_y=p1*(r_2+2*normalY*normalY)+2*p2*normalX*normalY;

			normalX=(x_distort-delta_x)/k_radial;
			normalY=(y_distort-delta_y)/k_radial;
		}
	}

	else
	{
		normalX=x_distort;
		normalY=y_distort;
	}
}
//繪制十字點
void  DrawCross(IplImage* img,int X, int Y,int r, int g,int b)
{
	CvPoint pt1,pt2;
	pt1.x=(int)(X-5);
	pt1.y=(int)Y;
	pt2.x=(int)(X+5);
	pt2.y=(int)Y;
	cvLine(img,pt1,pt2,CV_RGB(r,g,b), 1, 4, 0 );

	pt1.x=(int)X;
	pt1.y=(int)(Y-5);
	pt2.x=(int)X;
	pt2.y=(int)(Y+5);
	cvLine(img,pt1,pt2,CV_RGB(r,g,b), 1, 4, 0 );




}



//計算
void computeXY(double x,double y,double w,double h,double krr1,double krr2,double &XX,double &YY)
{
	double r2 = x * x + y * y;
	XX = x * (1 + krr1 * r2) + w / 2;
	YY = y * (1 + krr2 * r2) + h / 2;
}