📄 alibratecamera.cpp
字号:
//相机外参数中的平移参数赋初值
Xs=DLTResult[0];
Ys=DLTResult[1];
Zs=DLTResult[2];
//对fu,fv赋初值
f=(DLTResult[8]+DLTResult[9])/2;
//对u0,v0赋初值
u0=DLTResult[6];
v0=DLTResult[7];
//赋初值
k1=k2=k3=p1=p2=p3=b1=b2=0;
r11=cos(K)*cos(Fi);
r12=-cos(K)*sin(Fi);
r13=sin(K);
r21=sin(W)*sin(K)*cos(Fi)+cos(W)*sin(Fi);
r22=-sin(W)*sin(K)*sin(Fi)+cos(W)*cos(Fi);
r23=-sin(W)*cos(K);
r31=-cos(W)*sin(K)*cos(Fi)+sin(W)*sin(Fi);
r32=cos(W)*sin(K)*sin(Fi)+sin(W)*cos(Fi);
r33=cos(W)*cos(K);
//求定系数阵A
for(j=0;max>0.000000001;j++)
{
for(i=0;i<m;i++)
{
r2=pow((ImagePoint[i*2]-u0),2)+pow((ImagePoint[i*2+1]-v0),2);
xg=r11*(ControlPoint[i*3]-Xs)+r21*(ControlPoint[i*3+1]-Ys)+r31*(ControlPoint[i*3+2]-Zs);
yg=r12*(ControlPoint[i*3]-Xs)+r22*(ControlPoint[i*3+1]-Ys)+r32*(ControlPoint[i*3+2]-Zs);
zg=r13*(ControlPoint[i*3]-Xs)+r23*(ControlPoint[i*3+1]-Ys)+r33*(ControlPoint[i*3+2]-Zs);
// A[2*i*17]=(-fu)/pow(zg,2)*((-r11)*zg+r13*xg); //a11系数
// A[2*i*17+1]=(-fu)/pow(zg,2)*((-r21)*zg+r23*xg); //a12系数
// A[2*i*17+2]=(-fu)/pow(zg,2)*((-r31)*zg+r33*xg); // a13系数
// A[2*i*17+3]=(-fu)/pow(zg,2)*(zg*((-r31)*(ControlPoint[i*3+1]-Ys)+r21*(ControlPoint[i*3+2]-Zs))-xg*((-r33)*(ControlPoint[i*3+1]-Ys)+r23*(ControlPoint[i*3+2]-Zs))); //a14系数
// A[2*i*17+4]=(-fu)/pow(zg,2)*( zg*((ControlPoint[i*3]-Xs)*(-sin(K)*cos(Fi)+(ControlPoint[i*3+1]-Ys)*(sin(W)*cos(K)*cos(Fi))+(ControlPoint[i*3+2]-Zs)*(-cos(W)*cos(K)*cos(Fi))))-xg*((ControlPoint[i*3]-Xs)*cos(K)+(ControlPoint[i*3+1]-Ys)*(sin(W)*sin(K))+(ControlPoint[i*3+2]-Zs)*(-cos(W)*sin(K) ) ) );
// A[2*i*17+5]=(-fu)*yg/zg;
A[2*i*17+0]=1;
A[2*i*17+1]=0;
A[2*i*17+2]=-xg/zg;
A[2*i*17+3]=-(ImagePoint[i*2]-u0)*r2;
A[2*i*17+4]=-(ImagePoint[i*2]-u0)*pow(r2,2);
A[2*i*17+5]=-(ImagePoint[i*2]-u0)*pow(r2,2)*r2;
A[2*i*17+6]=-(r2+2*pow((ImagePoint[i*2]-u0),2));
A[2*i*17+7]=-2*(ImagePoint[i*2]-u0)*(ImagePoint[i*2+1]-v0);
A[2*i*17+8]=-(ImagePoint[i*2]-u0);
A[2*i*17+9]=-(ImagePoint[i*2+1]-v0);
//A矩阵偶数行进行赋值
// A[(2*i+1)*17]=((-fv)/pow(zg,2))*(-r12*zg+r13*yg);
// A[(2*i+1)*17+1]=((-fv)/pow(zg,2))*(-r22*zg+r23*yg);
// A[(2*i+1)*17+2]=((-fv)/pow(zg,2))*(-r32*zg+r33*yg);
// A[(2*i+1)*17+3]=((-fv)/pow(zg,2))*(zg*(-r32*(ControlPoint[i*3+1]-Ys)+r22*(ControlPoint[i*3+2]-Zs) )-yg*((-r33)*(ControlPoint[i*3+1]-Ys)+r23*(ControlPoint[i*3+2]-Zs) ) );
// A[(2*i+1)*17+4]=((-fv)/pow(zg,2))*(zg*((ControlPoint[i*3]-Xs)*sin(K)*sin(Fi)+(ControlPoint[i*3+1]-Ys)*(-sin(W))*cos(K)*sin(Fi)+(ControlPoint[i*3+2]-Zs)*cos(W)*cos(K)*sin(K) )-yg*((ControlPoint[i*3]-Xs)*cos(K)+(ControlPoint[i*3+1]-Ys)*sin(W)*sin(K)+(ControlPoint[i*3+2]-Zs)*(-cos(W))*sin(K)));
// A[(2*i+1)*17+5]=fv*xg/zg;
A[(2*i+1)*17+0]=0;
A[(2*i+1)*17+1]=1;
A[(2*i+1)*17+2]=-yg/zg;
A[(2*i+1)*17+3]=-(ImagePoint[i*2+1]-v0)*r2;
A[(2*i+1)*17+4]=-(ImagePoint[i*2+1]-v0)*pow(r2,2);
A[(2*i+1)*17+5]=-(ImagePoint[i*2+1]-v0)*pow(r2,2)*r2;
A[(2*i+1)*17+6]=-2*(ImagePoint[i*2]-u0)*(ImagePoint[i*2+1]-v0);
A[(2*i+1)*17+7]=-(r2+2*pow((ImagePoint[i*2+1]-v0),2));
A[(2*i+1)*17+8]=0;
A[(2*i+1)*17+9]=0;
//L矩阵行进行赋值
L[2*i]=ImagePoint[i*2]-( (-f)*xg/zg+u0-( (ImagePoint[i*2]-u0)*(k1*r2+k2*pow(r2,2)+k3*pow(r2,2)*r2)+p1*(r2+2*pow((ImagePoint[i*2]-u0),2))+2*p2*(ImagePoint[i*2]-u0)*(ImagePoint[i*2+1]-v0)+b1*(ImagePoint[i*2]-u0)+b2*(ImagePoint[i*2+1]-v0) )) ;
L[2*i+1]=ImagePoint[i*2+1]-( (-f)*yg/zg+v0-( (ImagePoint[i*2+1]-v0)*(k1*r2+k2*pow(r2,2)+k3*pow(r2,2)*r2)+p2*(r2+2*pow((ImagePoint[i*2+1]-v0) ,2) )+2*p1* (ImagePoint[i*2]-u0)*(ImagePoint[i*2+1]-v0) ) );
}
//参数平差，将平差结果放入s1中
adjust.ParameterAdjustment(A,P,L,2*m,10,s1);
for(int k=0;k<11;k++)
s1[k]=-s1[k];
// Xs+=s1[0];
// Ys+=s1[1];
// Zs+=s1[2];
// W+=s1[3];
// K+=s1[4];
// Fi+=s1[5];
u0+=s1[0];
v0+=s1[1];
f+=s1[2];
k1+=s1[3];
k2+=s1[4];
k3+=s1[5];
p1+=s1[6];
p2+=s1[7];
b1+=s1[8];
b2+=s1[9];
//计算邻近两次结果之差的绝对值，当小于0.000001时停止迭代
if(j==0)
{
for(k=0;k<11;k++)
s2[k]=s1[k];
}
else
{
max=0;
for(k=0;k<11;k++)
max+=pow((s1[k]-s2[k]),2);
}
for(k=0;k<11;k++)
s2[k]=s1[k];
}
//OutPra[0]=Xs;
//OutPra[1]=Ys;
//OutPra[2]=Zs;
//OutPra[3]=W;
//OutPra[4]=K;
//OutPra[5]=Fi;
//OutPra[6]=u0;
//OutPra[7]=v0;
//OutPra[8]=fu;
//OutPra[9]=fv;
//OutPra[10]=k1;
//OutPra[11]=k2;
//OutPra[12]=k3;
//OutPra[13]=p1;
//OutPra[14]=p2;
//OutPra[15]=b1;
//OutPra[16]=b2;
delete A;
delete L;
delete P ;
delete s1;
delete s2;
}
/****************通过旋转矩阵计算对应旋转角的弧度***************/
void CalibrateCamera::GetRotAngFromVector(double Vyi, double Vyj, double Vyk, double Vzi, double Vzj, double Vzk, double WKFi[])
{double ang=0;
const double PI=3.1415926;
//求rx
if (fabs(Vzj) < pow(10,-8)) //z轴在当前坐标系的XOZ平面内
{
if (Vzk > pow(10,-8)) //z轴与Z轴同向
ang = 0.0;
else if (Vzk < -pow(10,-8)) //z轴与Z轴反向
ang = PI;
else //z轴在X轴上
ang = 0.0;
}
else
{
if (fabs(Vzk) < pow(10,-8)) //z轴在当前坐标系的XOY平面内
{
if (Vzj > 0.0) //z轴在Y轴正向上
ang = 3.0 * PI / 2.0;
else if (Vzj < 0.0) //z轴在Y轴负向上
ang = PI / 2.0;
}
else //z轴在一般方向上
{
ang = atan(fabs(Vzj / Vzk));
if ((Vzj<0.0) && (Vzk>0.0)) //z轴在YOZ平面第四象限
ang = ang;
else if ((Vzj<0.0) && (Vzk<0.0)) //z轴在YOZ平面第三象限
ang = PI - ang;
else if ((Vzj>0.0) && (Vzk<0.0)) //z轴在YOZ平面第二象限
ang = PI + ang;
else if ((Vzj>0.0) && (Vzk>0.0)) //z轴在YOZ平面第一象限
ang = 2.0 * PI - ang;
}
}
double m_dRx = ang;
///求ry
double sxy= sqrt(pow(Vzj,2.0) + pow(Vzk,2.0));
if (fabs(Vzi) < pow(10,-8))
ang = 0.0;
else
{
if (sxy < pow(10,-8)) //z轴在X轴上
{
if (Vzi > 0.0)
ang = PI / 2.0; //z轴在X轴正向上
else
ang = 3.0 * PI / 2.0; //z轴在X轴负向上
}
else //z轴在一般方向上
{
ang = atan(fabs(Vzi / sxy));
if (Vzi > 0.0)
ang = ang;
else
ang = 2.0 * PI - ang;
}
}
double m_dRy = ang;
//求rz
double x2 = Vyi*cos(m_dRy)+Vyj*sin(m_dRx)*sin(m_dRy)
-Vyk*cos(m_dRx)*sin(m_dRy);
double y2 = Vyj*cos(m_dRx)+Vyk*sin(m_dRx);
double z2 = Vyi*sin(m_dRy)-Vyj*sin(m_dRx)*cos(m_dRy)
+Vyk*cos(m_dRx)*cos(m_dRy);
if (fabs(x2) < pow(10.0,-10.0)) x2 = 0.0;
if (fabs(y2) < pow(10.0,-10.0)) y2 = 0.0;
if (fabs(z2) < pow(10.0,-10.0)) z2 = 0.0;
if (fabs(y2) < pow(10.0,-10.0))
{
if (x2 > 0.0)
ang = 3.0 * PI / 2.0;
else
ang = PI / 2.0;
}
else
{
if (fabs(x2) < pow(10.0,-10.0))
{
if (y2 > 0.0)
ang = 0.0;
else
ang = PI;
}
else
{
ang = atan(fabs(x2 / y2));
if ((x2 > 0.0) && (y2 > 0.0))
ang = 2.0 * PI - ang;
else if ((x2 > 0.0) && (y2 < 0.0))
ang = PI + ang;
else if ((x2 < 0.0) && (y2 < 0.0))
ang = PI - ang;
else if ((x2 < 0.0) && (y2 > 0.0))
ang = ang;
}
}
double m_dRz = ang;
while (m_dRx>=2*PI) m_dRx -= 2*PI;
while (m_dRy>=2*PI) m_dRy -= 2*PI;
while (m_dRz>=2*PI) m_dRz -= 2*PI;
WKFi[0]=m_dRx;
WKFi[1]=m_dRy;
WKFi[2]=m_dRz;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -