|
|
发表于 2013-6-21 16:07:49
|
显示全部楼层
本帖最后由 cnxh 于 2013-6-21 16:15 编辑
Iar,MEGA16,环境,手动设置ReferencePoint 初始化成校准点位置,手动设置SamplePoint初始化成采样点位置,就是校准点和采样点重合,怎么软件模拟不对,软件模拟时采样的x,y为零,计算结果,x=0,y=-1,不是都为0,怎么回事,有测试了下,随意改变,x,y的值返回的x全部为0,y全部为-1
void Do_Calibration(signed short int *Px, signed short int *Py);
int Get_Calibration_Coefficient();
int main( void )
{
signed short int x = 0,y = 0;
Get_Calibration_Coefficient();
while(1)
{
Do_Calibration(&x,&y);
}
}
#define N 9 //校准的参考点的数目
#define CALIBRATION_WIDTH 800
#define CALIBRATION_HEIGHT 400
signed short int ReferencePoint[N][2]=
{
{ CALIBRATION_WIDTH , CALIBRATION_HEIGHT },
{ 400 , CALIBRATION_HEIGHT },
{ 800-CALIBRATION_WIDTH , CALIBRATION_HEIGHT },
{ CALIBRATION_WIDTH , 240 },
{ 400 , 240 },
{ 800-CALIBRATION_WIDTH , 240 },
{ CALIBRATION_WIDTH , 480-CALIBRATION_HEIGHT },
{ 400 , 480-CALIBRATION_HEIGHT },
{ 800-CALIBRATION_WIDTH , 480-CALIBRATION_HEIGHT },
}; //理想位置的参考点
signed short int SamplePoint[N][2] =
{
{ CALIBRATION_WIDTH , CALIBRATION_HEIGHT },
{ 400 , CALIBRATION_HEIGHT },
{ 800-CALIBRATION_WIDTH , CALIBRATION_HEIGHT },
{ CALIBRATION_WIDTH , 240 },
{ 400 , 240 },
{ 800-CALIBRATION_WIDTH , 240 },
{ CALIBRATION_WIDTH , 480-CALIBRATION_HEIGHT },
{ 400 , 480-CALIBRATION_HEIGHT },
{ 800-CALIBRATION_WIDTH , 480-CALIBRATION_HEIGHT },
}; //采样位置的参考点
double KX1, KX2, KX3, KY1, KY2, KY3; //系数的校准算法
void Do_Calibration(signed short int *Px, signed short int *Py) // 做校准点 (Px，Py) 使用的计算的系数
{
*Px=(signed short int)(KX1*(*Px)+KX2*(*Py)+KX3+0.5);
*Py=(signed short int)(KY1*(*Px)+KY2*(*Py)+KY3+0.5);
}
int Get_Calibration_Coefficient() //计算的系数，校准算法： KX1、 KX2、 KX3、 KY1、 KY2、 KY3
{
int i;
int Points=N;
double a[3],b[3],c[3],d[3],k;
if(Points<3)
{
return 0;
}
else
{
if(Points==3)
{
for(i=0; i<Points; i++)
{
a=(double)(SamplePoint[0]);
b=(double)(SamplePoint[1]);
c=(double)(ReferencePoint[0]);
d=(double)(ReferencePoint[1]);
}
}
else if(Points>3)
{
for(i=0; i<3; i++)
{
a=0;
b=0;
c=0;
d=0;
}
for(i=0; i<Points; i++)
{
a[2]=a[2]+(double)(SamplePoint[0]);
b[2]=b[2]+(double)(SamplePoint[1]);
c[2]=c[2]+(double)(ReferencePoint[0]);
d[2]=d[2]+(double)(ReferencePoint[1]);
a[0]=a[0]+(double)(SamplePoint[0])*(double)(SamplePoint[0]);
a[1]=a[1]+(double)(SamplePoint[0])*(double)(SamplePoint[1]);
b[0]=a[1];
b[1]=b[1]+(double)(SamplePoint[1])*(double)(SamplePoint[1]);
c[0]=c[0]+(double)(SamplePoint[0])*(double)(ReferencePoint[0]);
c[1]=c[1]+(double)(SamplePoint[1])*(double)(ReferencePoint[0]);
d[0]=d[0]+(double)(SamplePoint[0])*(double)(ReferencePoint[1]);
d[1]=d[1]+(double)(SamplePoint[1])*(double)(ReferencePoint[1]);
}
a[0]=a[0]/a[2];
a[1]=a[1]/b[2];
b[0]=b[0]/a[2];
b[1]=b[1]/b[2];
c[0]=c[0]/a[2];
c[1]=c[1]/b[2];
d[0]=d[0]/a[2];
d[1]=d[1]/b[2];
a[2]=a[2]/Points;
b[2]=b[2]/Points;
c[2]=c[2]/Points;
d[2]=d[2]/Points;
}
k=(a[0]-a[2])*(b[1]-b[2])-(a[1]-a[2])*(b[0]-b[2]);
KX1=((c[0]-c[2])*(b[1]-b[2])-(c[1]-c[2])*(b[0]-b[2]))/k;
KX2=((c[1]-c[2])*(a[0]-a[2])-(c[0]-c[2])*(a[1]-a[2]))/k;
KX3=(b[0]*(a[2]*c[1]-a[1]*c[2])+b[1]*(a[0]*c[2]-a[2]*c[0])+b[2]*(a[1]*c[0]-a[0]*c[1]))/k;
KY1=((d[0]-d[2])*(b[1]-b[2])-(d[1]-d[2])*(b[0]-b[2]))/k;
KY2=((d[1]-d[2])*(a[0]-a[2])-(d[0]-d[2])*(a[1]-a[2]))/k;
KY3=(b[0]*(a[2]*d[1]-a[1]*d[2])+b[1]*(a[0]*d[2]-a[2]*d[0])+b[2]*(a[1]*d[0]-a[0]*d[1]))/k;
return Points;
}
}
|
|
楼主
好资料，mark
