摄影测量连续定向

#include <cmath>
#include <iomanip>
#include <iostream>
using namespace std;

void MatrixMuti(double *pDFMatrix, double *pDSMatrix, double *pDTMatrix, int nX, int nY, int nZ);
void MatrixInversion(double *R,double *E,int n);
void SwapRow(double *A,int i,int rowindex,int n);

int main(int argc, char* argv[])
{

FILE *fp = fopen("D:\\相对定向测试数据.txt", "r");
int nNum = 0;
int nTemp = 0;
double dFocal = 0;
fscanf(fp, "%d%lf", &nNum, &dFocal);
double lx[31];
double ly[31];
double rx[31];
double ry[31];
int i = 0, j = 0; 
for (i=0; i<31; i++)
{
fscanf(fp, "%d%lf%lf%lf%lf", &nTemp, &lx[i], &ly[i], &rx[i], &ry[i]);
}
fclose(fp);
//初始化
double dPhi =0, dOmega = 0, dKappa =0;
double dU = 0, dV = 0;

double dBx = lx[0] - rx[0];

//计算左片各像点的像空间辅助坐标(X,Y,Z)
double dR1[9] = {1, 0, 0,
0, 1, 0,
0, 0, 1};

double dLxyz[3];
double dLXYZ[3];
for (i=0; i<3; i++)
{
dLxyz[i] = 0;
dLXYZ[i] = 0;
}
dLxyz[2] = -dFocal;

double dX[5]={0,0,0,0,0};
double dR2[9];
do 
{
dU += dX[0];
dV += dX[1];
dPhi += dX[2];
dOmega += dX[3];
dKappa += dX[4];

double dBy = dBx * dU;
double dBz = dBx * dV;

//计算右片旋转矩阵