程序运行后返回结果为#QNAN；求解

Question

具体代码如下
#include"iostream"
using namespace std;
#include"fstream"
#include
const int n=6;
void inverse(double c[n][n]);
templatevoid transpose (Tl*mat1,T2*mat2,int a,int b);
templatevoid multi(TI*mat1,T2*mat2,T2*result, int a, int b, int c);
int main()
{
double
x[4][2]={-0.08615,-0.06899,-0.05340,0.08221,-0.01478,-0.07663,0.01046,0.06443};
double
X[4][3]={36589.41,25273.32,2195.17,37631.08,31324.51,728.69,39100.97,24934.98, 2386.50,40426.54,30319.81,757.31};
int i,j,num=0;//num为迭代次数
double X0[6]={0};//设定未知数(XS,YS,ZS,三个角度)初始值
double f=0.15324;//摄影机主距f=153.24mm
double a=1/40000.0;//像片比例尺为1:40000
double R[3][3]={0};//初始化旋转知阵R
double approx_x[8]={0};//用于存放像点估计值
double A[8][6]={0};//定义了一个系数阵
double AT[6][8]={0};//定义了A的转置知阵
double L[8]={0};//定义常数项
const double pi=3.1415926535897932;
double Asum[6][6]={0};
double result2[6]={0};
double resultl[6][8]={0};
double sumXYZ[3]={0};
cout.precision(5);
cout<<"已知像点坐标为:\n";
for(i=0;i for(j=0;j {
cout if (j==0)
{
cout }
else
{
cout }
}
cout for(i=0;i for(j=0;j {
if (j==0)
{
cout }
else
if(j==1)
{
cout }
else
{
cout }
}
cout for(j=0;j for(i=0;i sumXYZ[j]+=X[i][j];
for(i=0;i X0[i]=sumXYZ[i]/4;//X0,Y0初始化
X0[i]=1/a*f+sumXYZ[2]/4.0;//对Z0进行初始化
do{
R[0][0]=cos(X0[3])*cos(X0[5])-sin(X0[3])*sin(X0[4])*sin(X0[5]);
R[0][i]=-cos(X0[3])*sin(X0[5])-sin(X0[3])*sin(X0[4])*cos(X0[5]);
R[0][2]=-sin(X0[3])*cos(X0[4]);
R[1][0]=cos(X0[4])*sin(X0[5]);
R[i][1]=cos(X0[4])*cos(X0[5]);
R[1][2]=-sin(X0[4]);
R[2][0]=sin(X0[3])*cos(X0[5])+cos(X0[3])*sin(X0[4])*sin(X0[5]);
R[2][i]=-sin(X0[3])*sin(X0[5])+cos(X0[3])*sin(X0[4])*cos(X0[5]);
R[2][2]=cos(X0[3])*cos(X0[4]);
//第一个像点的估计值，第一个点的坐标存放于X [0] [0],X [0] [1],X [0] [2]
approx_x[0]=-f*(R[0][0]*(X[0][0]-X0[0])+R[1][0]*(X[0][1]-X0[1])+R[2][0]*(X[0][2]-X0[2]))/(R[0][2]*(X[0][0]-X0[0])+R[1][2]*(X[0][1]-X0[1])+R[2][2]*(X[0][2]-X0[2]));
approx_x[1]=-f*(R[0][1]*(X[0][0]-X0[0])+R[1][1]*(X[0][1]-X0[1])+R[2][1]*(X[0][2]-X0[2]))/(R[0][2]*(X[0][0]-X0[0])+R[1][2]*(X[0][1]-X0[1])+R[2][2]*(X[0][2]-X0[2]));
//第二个像点的估计值,第2个点的坐标存放于X[1][0],X[1][1],X[1][2]
approx_x[2]=-f*(R[0][0]*(X[1][0]-X0[0])+R[1][0]*(X[1][1]-X0[1])+R[2][0]*(X[1][2]-X0[2]))/(R[0][2]*(X[1][0]-X0[0])+R[1][2]*(X[1][1]-X0[1])+R[2][2]*(X[1][2]-X0[2]));
approx_x[3]=-f*(R[0][1]*(X[1][0]-X0[0])+R[1][1]*(X[1][1]-X0[1])+R[2][1]*(X[1][2]-X0[2]))/(R[0][2]*(X[1][0]-X0[0])+R[1][2]*(X[1][1]-X0[1])+R[2][2]*(X[1][2]-X0[2]));
//第三个像点的估计值,第3个点的坐标存放于X[2][0],X[2][1],X[2][2]
approx_x[4]=-f*(R[0][0]*(X[2][0]-X0[0])+R[1][0]*(X[2][1]-X0[1])+R[2][0]*(X[2][2]-X0[2]))/(R[0][2]*(X[2][0]-X0[0])+R[1][2]*(X[2][1]-X0[1])+R[2][2]*(X[2][2]-X0[2]));
approx_x[5]=-f*(R[0][1]*(X[2][0]-X0[0])+R[1][1]*(X[2][1]-X0[1])+R[2][1]*(X[2][2]-X0[2]))/(R[0][2]*(X[2][0]-X0[0])+R[1][2]*(X[2][1]-X0[1])+R[2][2]*(X[2][2]-X0[2]));
//第四个像点的估计值,第4个点的坐标存放于X[3][0],X[3][1],X[3][2]
approx_x[6]=-f*(R[0][0]*(X[3][0]-X0[0])+R[1][0]*(X[3][1]-X0[1])+R[2][0]*(X[3][2]-X0[2]))/(R[0][2]*(X[3][0]-X0[0])+R[1][2]*(X[3][1]-X0[1])+R[2][2]*(X[3][2]-X0[2]));
approx_x[7]=-f*(R[0][1]*(X[3][0]-X0[0])+R[1][1]*(X[3][1]-X0[1])+R[2][1]*(X[3][2]-X0[2]))/(R[0][2]*(X[3][0]-X0[0])+R[1][2]*(X[3][1]-X0[1])+R[2][2]*(X[3][2]-X0[2]));
for(i=0;i {
//第i个像点估计值放在approx_x[2*(i-1)],approx_x[2*i-1]
/*a11*/A[2*i][0]=(R[0][0]*f+R[0][2]*approx_x[2*i])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));
/*a12*/A[2*i][1]=(R[1][0]*f+R[1][2]*approx_x[2*i])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));
/*a13*/A[2*i][2]=(R[2][0]*f+R[2][2]*approx_x[2*i])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));
/*a21*/A[2*i+1][0]=(R[0][1]*f+R[0][2]*approx_x[2*i+1])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));
/*a22*/A[2*i+1][1]=(R[1][1]*f+R[1][2]*approx_x[2*i+1])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));
/*a23*/A[2*i+1][2]=(R[2][1]*f+R[2][2]*approx_x[2*i+1])/(R[0][2]*(X[i][0]-X0[0])+R[1][2]*(X[i][1]-X0[1])+R[2][2]*(X[i][2]-X0[2]));
/*a14*/A[2*i][3]=approx_x[2*i+1]*sin(X0[4])-(approx_x[2*i]/f*(approx_x[2*i]*cos(X0[5])-approx_x[2*i+1]*sin(X0[5]))+f*cos(X0[5]))*cos(X0[4]);
/*a15*/A[2*i][4]=-f*sin(X0[5])-approx_x[2*i]/f*(approx_x[2*i]*sin(X0[5])+approx_x[2*i+1]*cos(X0[5]));
/*a16*/A[2*i][5]=approx_x[2*i+1];
/*a24*/A[2*i+1][3]=-1*approx_x[2*i]*sin(X0[4])-(approx_x[2*i+1]/f*(approx_x[2*i]*cos(X0[5])-approx_x[2*i+1]*sin(X0[5]))-f*sin(X0[5]))*cos(X0[4]);
/*a25*/A[2*i+1][4]=-1*f*cos(X0[5])-approx_x[2*i+1]/f*(approx_x[2*i]*sin(X0[5])+approx_x[2*i+1]*cos(X0[5]));
/*a26*/A[2*i+1][5]=-approx_x[2*i];
}
//进行常数项的初始化
for(i=0;i {
L[2*i]=x[i][0]-approx_x[2*i];
L[2*i+1]=x[i][1]-approx_x[2*i+1];
}
//A的转置矩阵
for(i=0;i for(j=0;j {
AT[j][i]=A[i][j];
}
//实现A与AT相乘
int k=0;
for(i=0;i for(j=0;j Asum[i][j]=0;
for(i=0;i for(k=0;k for(j=0;j Asum[i][k]+=AT[i][j]*A[j][k];
//得到AT*A的逆矩阵存放在inverseAsum[6][6]中
inverse(Asum);
//实现矩阵Asum[6][6]与AT[6][8]的相乘,结果存放在result1[6][8]中
for(i=0;i for(j=0;j resultl[i][j]=0;
for(i=0;i for(k=0;k for(j=0;j resultl[i][k]+=Asum[i][j]*AT[j][k];
//实现result1[6][8]与l[8]的相乘，得到结果放在result2[6]中；
for(i=0;i result2[i]=0;
for(i=0;i for(j=0;j result2[i]+=resultl[i][j]*L[j];
num++;
for(i=0;i {
X0[i]=X0[i]+result2[i];
}
ofstream f7("d:\\A.txt");
f7 cout for(i=0;i {
cout }
cout f7.close();
getchar();
}
while(abs(result2[3]*206265.0)>6||abs(result2[4]*206265.0)>6||abs(result2[5]*206265.0)>6);

cout<<"\n满足限差条件结束循环，最终结果为：\n";

cout< ofstream f7("d:\\A.txt");
f7 cout.precision(4);
for(i=0;i {
cout }
f7.close();
//今
double XG[6][1];
for(i=0;i XG[i][0]=result2[i];
double AXG[8][1],V[8][1],VT[1][8],VTV[1][1],m0,D[6][6];multi(A,XG,AXG,8,6,1);
for( i=0;i //计算改正数
V[i][0]=AXG[i][0]-L[i];
transpose (V,VT,1,8);
multi(VT,V,VTV,1,8,1);
m0=VTV[0][0]/2;
cout ofstream f6("d:\\what.txt");
// f6 for(i=0;i {
for(int j=0;j {
D[i][j]=m0*Asum[i][j];
cout f6 }
cout f6 }
for(i=0;i cout getchar();
return 0;
}
void inverse(double c[n][n])
{ int i,j,h,k; double p;
double q[n][12];
for(i=0;i for(j=0;j q[i][j]=c[i][j];
for(i=0;i for(j=n;j {
if(i+6==j)
q[i][j]=1;
else
q[i][j]=0;
}
for(h=k=0;k for(i=k+1;i {
if(q[i][h]==0)
continue;
p=q[k][h]/q[i][h];
for(j=0;j {
q[i][j]*=p;
q[i][j]-=q[k][j];
}
}
for(h=k=n-1;k>0;k--,h--) // 消去对角线以上的数据

for(i=k-1;i>=0;i--)

{

if(q[i][h]==0)

continue;

p=q[k][h]/q[i][h];

for(j=0;j {
q[i][j]*=p;
q[i][j]-=q[k][j];
}
}
for(i=0;i {
p=1.0/q[i][i];
for(j=0;j q[i][j]*=p;
}
for(i=0;i for(j=0;j c[i][j]=q[i][j+6];
}
templatevoid transpose(T1*mat1,T2*mat2,int a,int b)
{ int i,j;

for(i=0;i for(j=0;j mat2[j][i]=mat1[i][j];
return;
}
templatevoid multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c)

{

int i,j,k;

for(i=0;i<a;i++)

{
for(j=0;j<c;j++)

{
result[i][j]=0;

for(k=0;k<b;k++)

result[i][j]+=mat1[i][k]*mat2[k][j];

}

}

return;
}

Answer 1

#QNAN错误通常是因为发生浮点数非法运算，如：

除零、给负数取对数等。

你的代码太长，如果人工走读排查有困难，
可以把代码先减少，看有没有错误，然后慢慢往上增加代码，直到找出错误行。