adjustment.cpp

用于相机标定的程序。工业测量的一个重要手段是近景摄影测量。其中要用到相机标定。

                if (t>d) d=t;
                sm=s[mm-1]/d; sm1=s[mm-2]/d;
                em1=e[mm-2]/d;
                sk=s[kk-1]/d; ek=e[kk-1]/d;
                b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0;
                c=sm*em1; c=c*c; shh=0.0;
                if ((b!=0.0)||(c!=0.0))
                  { shh=sqrt(b*b+c);
                    if (b<0.0) shh=-shh;
                    shh=c/(b+shh);
                  }
                fg[0]=(sk+sm)*(sk-sm)-shh;
                fg[1]=sk*ek;
                for (i=kk; i<=mm-1; i++)
                  { Adjustment::sss(fg,cs);
                    if (i!=kk) e[i-2]=fg[0];
                    fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
                    e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
                    fg[1]=cs[1]*s[i];
                    s[i]=cs[0]*s[i];
                    if ((cs[0]!=1.0)||(cs[1]!=0.0))
                      for (j=1; j<=n; j++)
                        { ix=(j-1)*n+i-1;
                          iy=(j-1)*n+i;
                          d=cs[0]*v[ix]+cs[1]*v[iy];
                          v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
                          v[ix]=d;
                        }
                    Adjustment::sss(fg,cs);
                    s[i-1]=fg[0];
                    fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
                    s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
                    fg[1]=cs[1]*e[i];
                    e[i]=cs[0]*e[i];
                    if (i<m)
                      if ((cs[0]!=1.0)||(cs[1]!=0.0))
                        for (j=1; j<=m; j++)
                          { ix=(j-1)*m+i-1;
                            iy=(j-1)*m+i;
                            d=cs[0]*u[ix]+cs[1]*u[iy];
                            u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
                            u[ix]=d;
                          }
                  }
                e[mm-2]=fg[0];
                it=it-1;
              }
            else
              { if (ks==mm)
                  { kk=kk+1;
                    fg[1]=e[mm-2]; e[mm-2]=0.0;
                    for (ll=kk; ll<=mm-1; ll++)
                      { i=mm+kk-ll-1;
                        fg[0]=s[i-1];
                        Adjustment::sss(fg,cs);
                        s[i-1]=fg[0];
                        if (i!=kk)
                          { fg[1]=-cs[1]*e[i-2];
                            e[i-2]=cs[0]*e[i-2];
                          }
                        if ((cs[0]!=1.0)||(cs[1]!=0.0))
                          for (j=1; j<=n; j++)
                            { ix=(j-1)*n+i-1;
                              iy=(j-1)*n+mm-1;
                              d=cs[0]*v[ix]+cs[1]*v[iy];
                              v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
                              v[ix]=d;
                            }
                      }
                  }
                else
                  { kk=ks+1;
                    fg[1]=e[kk-2];
                    e[kk-2]=0.0;
                    for (i=kk; i<=mm; i++)
                      { fg[0]=s[i-1];
                        Adjustment::sss(fg,cs);
                        s[i-1]=fg[0];
                        fg[1]=-cs[1]*e[i-1];
                        e[i-1]=cs[0]*e[i-1];
                        if ((cs[0]!=1.0)||(cs[1]!=0.0))
                          for (j=1; j<=m; j++)
                            { ix=(j-1)*m+i-1;
                              iy=(j-1)*m+kk-2;
                              d=cs[0]*u[ix]+cs[1]*u[iy];
                              u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
                              u[ix]=d;
                            }
                      }
                  }
              }
          }
      }
    return(1);
  }

/******广意矩阵求逆*********
 本函数的功能为：将矩阵a的逆矩阵放入aa中
 a为m*n矩阵的地址；
 aa为用于存放a逆矩阵的地址；
 u为m*m矩阵的地址；
 v为n*n矩阵的地址；
 esp为0.00001。
 *********************************/
int Adjustment::bginv(double *a,int m,int n,double *aa,double eps,double *u,double *v,int ka)
  { int i,j,k,l,t,p,q,f;
    extern int bmuav();
    i=Adjustment::bmuav(a,m,n,u,v,eps,ka);
    if (i<0) return(-1);
    j=n;
    if (m<n) j=m;
    j=j-1;
    k=0;
    while ((k<=j)&&(a[k*n+k]!=0.0)) k=k+1;
    k=k-1;
    for (i=0; i<=n-1; i++)
    for (j=0; j<=m-1; j++)
      { t=i*m+j; aa[t]=0.0;
        for (l=0; l<=k; l++)
          { f=l*n+i; p=j*m+l; q=l*n+l;
            aa[t]=aa[t]+v[f]*u[p]/a[q];
          }
      }
    return(1);
  }



/*********矩阵相乘**********
本函数的功能为:将矩阵a与b相乘的结果放入c中
 a为m*n矩阵的地址; 
 b为n*k矩阵的地址；
 c为m*k矩阵的地址。
 *********************************/
void Adjustment::brmul(double *a,double *b,int m,int n,int k,double *c)
  { int i,j,l,u;
    for (i=0; i<=m-1; i++)
    for (j=0; j<=k-1; j++)
      { u=i*k+j; c[u]=0.0;
        for (l=0; l<=n-1; l++)
          c[u]=c[u]+a[i*n+l]*b[l*k+j];
      }
    return;
  }

/*********参数平差**********
本函数的功能为:将观测方程形式为V=AX-L的平差结果放入pra矩阵中
 A为m*n的系数矩阵首地址; 
 P为m*m的权矩阵首地址；
 L为m*1的常数矩阵首地址；
 pra为n*1的矩阵首地址,用于存放平差结果。
 *********************************/
void Adjustment::ParameterAdjustment(double *A,double *P,double *L,int m,int n,double *Pra)
{
	int i,j;
	int ka=n+1;
	double *P1,*P2,*P3,*P4,*P5,*P6,*P7,eps;
	eps=0.00001;

    P1=new double[m*n];
	P2=new double[n*m];
	P3=new double[n*n];
	P4=new double[n];
	P5=new double[n*n];
	P6=new double[n*n];
	P7=new double[n*n];
	
	//矩阵转秩，P1存放A的转秩阵
	for(i=0;i<m;i++)
		for(j=0;j<n;j++)
		{
			P1[j*m+i]=A[i*n+j];
		}

    //计算法方程矩阵N即A'pA
	brmul(P1,P, n, m,m,P2);
	brmul(P2,A, n, m,n,P3);
    //计算法方程矩阵L即A'pl
	brmul(P2,L, n, m,1,P4);
    
	//求矩阵N的逆
	bginv(P3, n, n,P5,eps,P6,P7,ka);
    
	//求平差结果
	brmul(P5,P4, n, n,1,Pra);

	for(i=0;i<n;i++)
	{
		Pra[i]=-Pra[i];
	}
	delete[] P1;
	delete[] P2;
	delete[] P3;
	delete[] P4;
	delete[] P5;
	delete[] P6;
	delete[] P7;

}
/*****************具有条件的参数平差**************************************
 本函数的功能为:将观测方程形式为V=AX-L,条件方程为：BX+w=0的平差结果放入pra矩阵中
   *A参数系数,  *P权阵, *L参数式中的常数项, 
   *B条件式系数, *w条件式常数项,
   m--A的行数,n-- A、B的列数 , r--B的行数, 
   *Pra--存储参数的或然值, *DitXYZ--各参数的中误差

********************************************************************/
void Adjustment::ParameterAdjustmentWithCondi(double *A, double *P, double *L, double *B, double *w, int m, int n, int r, double *Pra, double *DitPra)
{
	
	int i,j;
	int ka=n+1;
	double *P1,*P2,*P3,*P4,*P5,*P6,*P7,eps;
	double *P8,*P9,*P10,*P11,*P12,*P13,*P14,*P15,*P16,*P17;
	eps=0.00001;

    P1=new double[m*n];
	P2=new double[n*m];
	P3=new double[n*n];
	P4=new double[n];
	P5=new double[n*n];
	P6=new double[n*n];
	P7=new double[n*n];
	P8=new double[r*n];
	P9=new double[r*n];
	P10=new double[r*r];
	P11=new double[n*1];
	P12=new double[r*r];
	P13=new double[r*r];
	P14=new double[r*r];
	P15=new double[r];
	P16=new double[r];
	P17=new double[n];
	//A的转秩
	for(i=0;i<n;i++)
		for(j=0;j<m;j++)
		{
			P1[i*m+j]=A[j*n+i];
		}

   //B的转秩=P8
	for(i=0;i<n;i++)
		for(j=0;j<r;j++)
		{
			P8[i*r+j]=B[j*n+i];
		}
    //N=P3
    brmul(P1,P,n,m,m,P2);//矩阵相乘
	brmul(P2,A, n, m,n,P3);
	//L=P4,即A'Pl
    brmul(P2,L, n, m,1,P4);
    //N的逆阵=P5
	bginv(P3, n, n,P5,eps,P6,P7,ka);//矩阵求逆

    //K
    brmul(B,P5, r, n,n,P9);//矩阵相乘B*N-1
    brmul(P9,P8, r, n,r,P10);//B*N-1*Bt=P10
    bginv(P10, r, r,P12,eps,P13,P14,ka);//矩阵求逆
    brmul(P9,P4, r, n,1,P15);//B*N-1*Bt=P10
	
	for(i=0;i<r;i++)
		P15[i]=w[i]-P15[i];//Bt*B*(N-1)*U+U=P4
    //参数值
	brmul(P12,P15, r, r,1,P16);//K
    brmul(P8,P16, n, r,1,P17);//BT*k
	//BT*k+U
    for(i=0;i<n;i++)
	{
		P17[i]+=P4[i];
	}
	brmul(P5,P17, n, n,1,Pra);//BT*k
    
	for(i=0;i<n;i++)
	{
		Pra[i]=-Pra[i];
	}

	delete[] P1;
	delete[] P2;
	delete[] P3;
	delete[] P4;
	delete[] P5;
	delete[] P6;
	delete[] P7;
	delete[] P8;
	delete[] P9;
	delete[] P10;
	delete[] P11;
	delete[] P12;
	delete[] P13;
	delete[] P14;
	delete[] P15;
	delete[] P16;
	delete[] P17;

}