oubleX0[4],Y0[4],Z0[4];//地面坐标蚅?doublem; //比例尺肀?doublea1,a2,a3,b1,b2,b3,c1,c2,c3;//旋转矩阵袈?intn=0; //迭代次数薆?//地面坐标蚆?X0[0]=36589.41;莂?X0[1]=37631.08;芇?X0[2]=39100.97;芆?X0[3]=40426.54;蒃?Y0[0]=25273.32;蒁?Y0[1]=31324.51;羁?Y0[2]=24934.98;肆?Y0[3]=30319.81;薅?Z0[0]=2195.17;袃?Z0[1]=728.69;莀?Z0[2]=2386.50;螇?Z0[3]=757.31;莂?//影像坐标羂?x[0]=-0.08615;衿?x[1]=-0.05340;薇?x[2]=-0.01478;莃?x[3]=0.01046;肀?y[0]=-0.06899;艿?y[1]=0.08221;芈?y[2]=-0.07663;蒅?y[3]=0.06443;蒂蚈?//确定内外方位元素初始值羈?x0=0;y0=0;节?f=153.24/1000;薁?m=15000;膇?Xs=0;Ys=0;螈?q=0;w=0;k=0;芄?for(inti=0;i<4;i++)羃?{袁 Xs+=X0[i];芅 Ys+=Y0[i];莅?}肁?Xs/=4;芀?Ys/=4;羅?Zs=f*m;膂薈?MatrixA(8,6);肈?MatrixL(8,1);肄?MatrixAT(6,8);薂?MatrixATA(6,6);羁?MatrixATA_(6,6);蒇?MatrixXx(6,1);袄蚃?//迭代计算聿?do袇?{薅//旋转矩阵R蒁 a1=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);莁 a2=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
全文预览
