📄 zhang2dmain.cpp
字号:
}
mtxAB(RT,model,3,3,nPoints,XY);
mtxAB(A_inside,XY,3,3,nPoints,UV);
//归一化
for(i=0;i<nPoints;i++)
{
XY[i+nPoints*0]=XY[i+nPoints*0]/XY[i+nPoints*2];
XY[i+nPoints*1]=XY[i+nPoints*1]/XY[i+nPoints*2];
XY[i+nPoints*2]=XY[i+nPoints*2]/XY[i+nPoints*2];
UV[i+nPoints*0]=UV[i+nPoints*0]/UV[i+nPoints*2];
UV[i+nPoints*1]=UV[i+nPoints*1]/UV[i+nPoints*2];
UV[i+nPoints*2]=UV[i+nPoints*2]/UV[i+nPoints*2];
}
for(i=0;i<nPoints;i++)
{
D[j*2*2*nPoints+i*2*2+0]=(UV[i+nPoints*0]-A_inside[2])*(XY[i+nPoints*0]*XY[i+nPoints*0]+XY[i+nPoints*1]*XY[i+nPoints*1]);
D[j*2*2*nPoints+i*2*2+1]=(UV[i+nPoints*0]-A_inside[2])*(XY[i+nPoints*0]*XY[i+nPoints*0]+XY[i+nPoints*1]*XY[i+nPoints*1])*(XY[i+nPoints*0]*XY[i+nPoints*0]+XY[i+nPoints*1]*XY[i+nPoints*1]);
D[j*2*2*nPoints+i*2*2+2]=(UV[i+nPoints*1]-A_inside[5])*(XY[i+nPoints*0]*XY[i+nPoints*0]+XY[i+nPoints*1]*XY[i+nPoints*1]);
D[j*2*2*nPoints+i*2*2+3]=(UV[i+nPoints*1]-A_inside[5])*(XY[i+nPoints*0]*XY[i+nPoints*0]+XY[i+nPoints*1]*XY[i+nPoints*1])*(XY[i+nPoints*0]*XY[i+nPoints*0]+XY[i+nPoints*1]*XY[i+nPoints*1]);
}
for(i=0;i<nPoints;i++)
{
d[j*2*nPoints+i*2+0]=imagePoints[j*2*nPoints+i*2+0]-UV[i+nPoints*0];
d[j*2*nPoints+i*2+1]=imagePoints[j*2*nPoints+i*2+1]-UV[i+nPoints*1];
}
}
mtxAt(D,nPoints*numOfpic*2,2,Dt);
mtxAB(Dt,D,2,nPoints*numOfpic*2,2,Dt_mutil_D);
mtxAxInxn(Dt_mutil_D,2,Dt_mutil_D_inv);
mtxAB(Dt_mutil_D_inv,Dt,2,2,nPoints*numOfpic*2,D);
mtxAB(D,d,2,nPoints*numOfpic*2,1,k);
free(RT);
free(XY);
free(UV);
free(D);
free(Dt);
free(d);
free(model);
}
void zhang_2D_unrefined(int numOfpic,int nPoints,double *modelPoints,double *imagePoints)
{
int i,j;
double *image;
double **H;
double *temp_H;
int sign;
double *v;
double *vt;//v的转置
double v_mutil_vt[36];
double B[6];
////////////内部参数////////////////////
double v0,s,alpha_u,alpha_v,skewness,u0;
//////分配内存空间////////
temp_H = (double*)malloc(9*sizeof(double));
H = (double**)malloc(numOfpic*sizeof(double*));
for(i=0;i<numOfpic;i++)
H[i] = (double*)malloc(9*sizeof(double));
if((image = (double*)malloc(2*nPoints*sizeof(double)))==NULL)
exit(-1);
v = (double*)malloc(6*2*numOfpic*sizeof(double));
vt = (double*)malloc(6*2*numOfpic*sizeof(double));
for(i=0;i<numOfpic;i++)
{
for(j=0;j<nPoints;j++)
{
image[2*j+0]=imagePoints[2*j+0];
image[2*j+1]=imagePoints[2*j+1];
}
if((sign=homography(nPoints,modelPoints,image,temp_H)!=0))
{
printf("there is something mistake!!");
exit(-1);
}
for(j=0;j<9;j++)
H[i][j]=temp_H[j];
imagePoints+=nPoints*2;
}
imagePoints=imagePoints-nPoints*2*numOfpic;
v_compute(numOfpic,H,v);
mtxAt(v,numOfpic*2,6,vt);
mtxAB(vt,v,6,numOfpic*2,6,v_mutil_vt);
solveAx0(/*numOfpic*2*/6,6,v_mutil_vt,B);
///////////求内部参数//////////////////////
v0=(B[1]*B[3]-B[0]*B[4])/(B[0]*B[2]-B[1]*B[1]);
s=B[5]-(B[3]*B[3]+v0*(B[1]*B[3]-B[0]*B[4]))/B[0];
alpha_u=sqrt(s/B[0]);
alpha_v=sqrt(s*B[0]/(B[0]*B[2]-B[1]*B[1]));
skewness=-B[1]*alpha_u*alpha_u*alpha_v/s;
u0=-skewness*v0/alpha_u-B[3]*alpha_u*alpha_u/s;
//获得摄像机的内部参数
A_inside[0]=alpha_u; A_inside[1]=skewness; A_inside[2]=u0;
A_inside[3]=0.0; A_inside[4]=alpha_v; A_inside[5]=v0;
A_inside[6]=0.0; A_inside[7]=0.0; A_inside[8]=1.0;
zhang_2D_extrinsic(numOfpic,H);
zhang_2D_k(numOfpic,nPoints,modelPoints,imagePoints);
free(temp_H);
free(H);
free(image);
free(v);
free(vt);
}
void fcn_lm_all(const int *m, const int *n, const double *x, double *fvec, int *iflag)
{
double temp_in[9];
double temp_entri[9];
double Q1,Q2,Q3;
double u0,v0;
double *XY,*UV;
double kk1,kk2;
int i,j;
XY = (double*)malloc(PointsOfone*3*sizeof(double));
UV = (double*)malloc(PointsOfone*3*sizeof(double));
kk1=x[picNumber*6]; kk2=x[picNumber*6+1];
temp_in[0]=x[picNumber*6+2]; temp_in[1]=x[picNumber*6+3]; temp_in[2]=x[picNumber*6+4];
temp_in[3]=0; temp_in[4]=x[picNumber*6+5]; temp_in[5]=x[picNumber*6+6];
temp_in[6]=0; temp_in[7]=0; temp_in[8]=1;
u0=temp_in[2];
v0=temp_in[5];
for(j=0;j<picNumber;j++)
{
Q1=x[0];
Q2=x[1];
Q3=x[2];
temp_entri[0]=cos(Q2)*cos(Q1);
temp_entri[1]=sin(Q2)*cos(Q1);
temp_entri[6]=sin(Q2)*sin(Q3)+cos(Q2)*sin(Q1)*cos(Q3);
temp_entri[3]=-sin(Q2)*cos(Q3)+cos(Q2)*sin(Q1)*sin(Q3);
temp_entri[4]=cos(Q2)*cos(Q3)+sin(Q2)*sin(Q1)*sin(Q3);
temp_entri[7]=-cos(Q2)*sin(Q3)+sin(Q2)*sin(Q1)*cos(Q3);
temp_entri[2]=x[3];
temp_entri[5]=x[4];
temp_entri[8]=x[5];
mtxAB(temp_entri,GBmodelPoints,3,3,PointsOfone,XY);
mtxAB(temp_in,XY,3,3,PointsOfone,UV);
for(i=0;i<PointsOfone;i++)
{
XY[i+PointsOfone*0]=XY[i+PointsOfone*0]/XY[i+PointsOfone*2];
XY[i+PointsOfone*1]=XY[i+PointsOfone*1]/XY[i+PointsOfone*2];
XY[i+PointsOfone*2]=XY[i+PointsOfone*2]/XY[i+PointsOfone*2];
UV[i+PointsOfone*0]=UV[i+PointsOfone*0]/UV[i+PointsOfone*2];
UV[i+PointsOfone*1]=UV[i+PointsOfone*1]/UV[i+PointsOfone*2];
UV[i+PointsOfone*2]=UV[i+PointsOfone*2]/UV[i+PointsOfone*2];
}
for(i=0;i<PointsOfone;i++)
{
fvec[i*2+0]=UV[i+PointsOfone*0]+(UV[i+PointsOfone*0]-u0)*(XY[i+PointsOfone*0]*XY[i+PointsOfone*0]+XY[i+PointsOfone*1]*XY[i+PointsOfone*1])*kk1+(UV[i+PointsOfone*0]-u0)*(XY[i+PointsOfone*0]*XY[i+PointsOfone*0]+XY[i+PointsOfone*1]*XY[i+PointsOfone*1])*(XY[i+PointsOfone*0]*XY[i+PointsOfone*0]+XY[i+PointsOfone*1]*XY[i+PointsOfone*1])*kk2;
fvec[i*2+1]=UV[i+PointsOfone*1]+(UV[i+PointsOfone*1]-v0)*(XY[i+PointsOfone*0]*XY[i+PointsOfone*0]+XY[i+PointsOfone*1]*XY[i+PointsOfone*1])*kk1+(UV[i+PointsOfone*1]-v0)*(XY[i+PointsOfone*0]*XY[i+PointsOfone*0]+XY[i+PointsOfone*1]*XY[i+PointsOfone*1])*(XY[i+PointsOfone*0]*XY[i+PointsOfone*0]+XY[i+PointsOfone*1]*XY[i+PointsOfone*1])*kk2;
}
for(i=0;i<PointsOfone;i++)
{
fvec[i*2+0]=GBimagePoints[i*2+0]-fvec[i*2+0];
fvec[i*2+1]=GBimagePoints[i*2+1]-fvec[i*2+1];
}
x+=6;
GBimagePoints+=PointsOfone*2;
fvec+=PointsOfone*2;
}
x=x-picNumber*6;
GBimagePoints=GBimagePoints-picNumber*PointsOfone*2;
fvec=fvec-picNumber*PointsOfone*2;
free(XY);
free(UV);
}
void lm_last(double* begin,double *lm)
{
int i;
int m, n, info, lwa, *iwa, one=1;
double tol, *x, *fvec, *wa;
m=PointsOfone*2*picNumber;
n=picNumber*6+7;
lwa = m*n + 5*n + m+1;
tol = sqrt(dpmpar_(&one));
iwa = (int*)malloc((picNumber*6+7)*sizeof(int));
if((x=(double*)malloc((picNumber*6+7)*sizeof(double)))==NULL)
{
printf("Cannot allocate workspace for x!!");
exit(-1);
}
if((fvec=(double*)malloc(picNumber*2*PointsOfone*sizeof(double)))==NULL)
{
printf("Cannot allocate workspace for x!!");
exit(-1);
}
if((wa=(double*)malloc(lwa*sizeof(double)))==NULL)
{
printf("Cannot allocate workspace for x!!");
exit(-1);
}
for(i=0;i<37;i++)
x[i]=begin[i];
lmdif1_(&fcn_lm_all, &m, &n, x, fvec, &tol, &info, iwa, wa, &lwa);
for(i=0;i<37;i++)
lm[i]=x[i];
free(x);
free(fvec);
free(wa);
}
void lm_refine_all(double *refine_begin,double* refine,double *modelPoints,double *imagePoints)
{
int i;
double *model;
model = (double*)malloc(PointsOfone*3*sizeof(double));
for(i=0;i<PointsOfone;i++)
{
model[i+PointsOfone*0]=modelPoints[i*2+0];
model[i+PointsOfone*1]=modelPoints[i*2+1];
model[i+PointsOfone*2]=1;
}
GBmodelPoints = model;
GBimagePoints = imagePoints;
lm_last(refine_begin,refine);
free(model);
}
void zhang_2D_refine(int numOfpic,int nPoints,double *modelPoints,double *imagePoints)
{
int i,j;
double *refine;
double *refine2;
double Q1,Q2,Q3;
picNumber=numOfpic;
PointsOfone=nPoints;
refine = (double*)malloc((picNumber*6+7)*sizeof(double));
refine2 = (double*)malloc((picNumber*6+7)*sizeof(double));
entrinsic_refine = (double**)malloc(picNumber*sizeof(double*));
for(i=0;i<picNumber;i++)
entrinsic_refine[i] = (double*)malloc(12*sizeof(double));
for(i=0;i<numOfpic;i++)
{
refine[0]=-asin(entrinsic[0][2]);
refine[1]=asin(entrinsic[0][1]/cos(refine[0]));
refine[2]=asin(entrinsic[0][6]/cos(refine[0]));
refine[3]=entrinsic[0][3];
refine[4]=entrinsic[0][7];
refine[5]=entrinsic[0][11];
refine+=6;
}
refine=refine-numOfpic*6;
refine[numOfpic*6]=k[0]; refine[numOfpic*6+1]=k[1];
refine[numOfpic*6+2]=A_inside[0]; refine[numOfpic*6+3]=A_inside[1]; refine[numOfpic*6+4]=A_inside[2];
refine[numOfpic*6+5]=A_inside[4]; refine[numOfpic*6+6]=A_inside[5];
lm_refine_all(refine,refine2,modelPoints,imagePoints);
/////优化后的A
A_refined[0]=refine2[numOfpic*6+2]; A_refined[1]=refine2[numOfpic*6+3]; A_refined[2]=refine2[numOfpic*6+4];
A_refined[3]=0; A_refined[4]=refine2[numOfpic*6+5]; A_refined[5]=refine2[numOfpic*6+6];
A_refined[6]=0; A_refined[7]=0; A_refined[8]=1;
/////优化后的k
k_refine[0]=refine2[numOfpic*6]; k_refine[1]=refine2[numOfpic*6+1];
/////优化后的外参数
for(i=0;i<picNumber;i++)
{
Q1=refine2[0];
Q2=refine2[1];
Q3=refine2[2];
entrinsic_refine[i][0]=cos(Q2)*cos(Q1);
entrinsic_refine[i][1]=sin(Q2)*cos(Q1);
entrinsic_refine[i][2]=-sin(Q1);
entrinsic_refine[i][6]=cos(Q1)*sin(Q3);
entrinsic_refine[i][10]=cos(Q1)*cos(Q3);
entrinsic_refine[i][8]=sin(Q2)*sin(Q3)+cos(Q2)*sin(Q1)*cos(Q3);
entrinsic_refine[i][4]=-sin(Q2)*cos(Q3)+cos(Q2)*sin(Q1)*sin(Q3);
entrinsic_refine[i][5]=cos(Q2)*cos(Q3)+sin(Q2)*sin(Q1)*sin(Q3);
entrinsic_refine[i][9]=-cos(Q2)*sin(Q3)+sin(Q2)*sin(Q1)*cos(Q3);
entrinsic_refine[i][3]=refine2[3];
entrinsic_refine[i][7]=refine2[4];
entrinsic_refine[i][11]=refine2[5];
refine2+=6;
}
/////////打印内参数
for(i=0;i<3;i++)
printf("%f %f %f\n",A_refined[i*3+0],A_refined[i*3+1],A_refined[i*3+2]);
printf("\n");
printf("\n");
printf("\n");
//////打印k
printf("%f %f\n",k_refine[0],k_refine[1]);
printf("\n");
printf("\n");
printf("\n");
//////打印外参系数
for(i=0;i<picNumber;i++)
{
printf("这是第%d个图片的外参!!\n",i+1);
printf("\n");
for(j=0;j<3;j++)
{
printf("%f %f %f %f\n",entrinsic_refine[i][j*4+0],entrinsic_refine[i][j*4+1],entrinsic_refine[i][j*4+2],entrinsic_refine[i][j*4+3]);
}
printf("\n");
printf("\n");
printf("\n");
}
free(refine);
}
//test
void zhang_2D_calib(int numOfpic,int nPoints,double *model,double *imagePoints,double **A_in,double ***entri)
{
zhang_2D_unrefined(numOfpic,nPoints,model,imagePoints);
zhang_2D_refine(numOfpic,nPoints,model,imagePoints);
*A_in=A_refined;
*entri=entrinsic_refine;
}
/////////////////////////////////test////////////////////////////////
#include "test.h"
void main()
{
int i,n,j;
int picNumber1=5;
int nPoints=256;
double *model;
model = new double [nPoints*2];
model = Model;
double *image;
image = (double*)malloc((picNumber1+1)*nPoints*2*sizeof(double));
for(i=0;i<nPoints;i++)
{
image[i*2+0]=m1[i*2+0];
image[i*2+1]=m1[i*2+1];
}
image+=nPoints*2;
for(i=0;i<nPoints;i++)
{
image[i*2+0]=m2[i*2+0];
image[i*2+1]=m2[i*2+1];
}
image+=nPoints*2;
for(i=0;i<nPoints;i++)
{
image[i*2+0]=m3[i*2+0];
image[i*2+1]=m3[i*2+1];
}
image+=nPoints*2;
for(i=0;i<nPoints;i++)
{
image[i*2+0]=m4[i*2+0];
image[i*2+1]=m4[i*2+1];
}
image+=nPoints*2;
for(i=0;i<nPoints;i++)
{
image[i*2+0]=m5[i*2+0];
image[i*2+1]=m5[i*2+1];
}
image=image-nPoints*2*(picNumber1-1);
double *A;
double **ent;
zhang_2D_calib(picNumber1,nPoints,model,image,&A,&ent);
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -