空间后方交会

///< 像素的行列号内定向转化为相空间坐标系坐标
	m_ImgPts = new st_point[iPntCount];
	double dImgX, dImgY;
	for (int i=0; i<iPntCount; i++)
	{
		mCameraParam->PixelToSpace(pImgPts[i].x, pImgPts[i].y,
			dImgX, dImgY);
		m_ImgPts[i].x = dImgX;
		m_ImgPts[i].y = dImgY;
	}

	///< 初始化外方位元素
	double dSumXs = 0.0, dSumYs = 0.0;
	for (int i=0; i<iPntCount; i++)
	{
		dSumXs += m_GroundPts[i].X;
		dSumYs += m_GroundPts[i].Y;
	}
	mExterior.Xs = dSumXs/iPntCount;
	mExterior.Ys = dSumYs/iPntCount;
	mExterior.Zs = dFlyHeight;
	mExterior.phi	= 0.0;
	mExterior.omega	= 0.0;
	mExterior.kappa = 0.0;

 for (int i=0; i<mControlPntCount; i++)
	{
		dParamMatrix[i*2*6+0] = -(a1*mf + a3*m_ImgPts[i].x);   
				///< dXs = -(a1*f+a3*(x-x0));
		dParamMatrix[i*2*6+1] = -(b1*mf + b3*m_ImgPts[i].x);	
				///< dYs = -(b1*f+b3*(x-x0));
		dParamMatrix[i*2*6+2] = -(c1*mf + c3*m_ImgPts[i].x);	
				///< dZs = -(c1*f+c3*(x-x0));
		dParamMatrix[i*2*6+3] = dZImgSpace * ((mf+m_ImgPts[i].x*
			m_ImgPts[i].x/mf)*b2 - (m_ImgPts[i].x*m_ImgPts[i].y/mf)*b1 +
			m_ImgPts[i].y*b3);
			///< dPhi = Z*((f+(x-x0)*(x-x0)/f)*b2 - ((x-x0)*(y-y0)/f)*b1 +
			///< (y-y0)*b3
		dParamMatrix[i*2*6+4] = dZImgSpace * (mf*sin(mExterior.kappa) + 
			(m_ImgPts[i].x*m_ImgPts[i].x/mf)*sin(mExterior.kappa) +
			(m_ImgPts[i].x*m_ImgPts[i].y/mf)*cos(mExterior.kappa));
		dParamMatrix[i*2*6+5] = -dZImgSpace*m_ImgPts[i].y;

		dParamMatrix[(i*2+1)*6+0] = -(a2*mf + a3*m_ImgPts[i].y);
		dParamMatrix[(i*2+1)*6+1] = -(b2*mf + b3*m_ImgPts[i].y);
		dParamMatrix[(i*2+1)*6+2] = -(c2*mf + c3*m_ImgPts[i].y);
		dParamMatrix[(i*2+1)*6+3] = dZImgSpace * ((m_ImgPts[i].x*
			m_ImgPts[i].y/mf)*b2 - (mf+m_ImgPts[i].y*m_ImgPts[i].y/mf)*b1 +
			m_ImgPts[i].x*b3);
		dParamMatrix[(i*2+1)*6+4] = dZImgSpace * (mf*cos(mExterior.kappa) +
			(m_ImgPts[i].x*m_ImgPts[i].y/mf)*sin(mExterior.kappa) + 
			(m_ImgPts[i].y*m_ImgPts[i].y/mf)*cos(mExterior.kappa));
		dParamMatrix[(i*2+1)*6+5] = dZImgSpace*m_ImgPts[i].x;

		dConstMatrix[i*2+0] = m_ImgPts[i].x - dCalImgX[i];
		dConstMatrix[i*2+1] = m_ImgPts[i].y - dCalImgY[i];
	}

STEP3：求解方程组app

do 
	{
		///< 构造旋转矩阵，共线条件方程计算地面点对应的像空间坐标系的坐标
		if (!RotateMatrixCreation(dRotateMatrix, mExterior.phi, 
			mExterior.omega, mExterior.kappa))
		{
			return false;
		}
		
		for (int i=0; i<mControlPntCount; i++)
		{
			CollinearEquationToPixel(m_GroundPts[i], mExterior, mf, 
				dRotateMatrix, dCalImgX[i], dCalImgY[i]);
		}
		///< 计算权矩阵
		if (!MakeP())
		{
			return false;
		}

		///< 计算偏差方程组的系数矩阵和常数矩阵
		if (!BuildErrorEquation(dParamMatrix, dConstMatrix, dRotateMatrix,
			dCalImgX, dCalImgY))
		{
			return false;
		}

		///< 解方程组，获得改正数
		if (!Solve_Equation(dParamMatrix, mControlPntCount*2, 6, 
			dConstMatrix, mPArray, dResultMatrix))
		{
			return false;
		}

		dMaxRms = 0.0;
		for (int i=0; i<6; i++)
		{
			if (abs(dResultMatrix[i]) > dMaxRms)
			{
				dMaxRms = abs(dResultMatrix[i]);
			}
		}

		mExterior.Xs += dResultMatrix[0];
		mExterior.Ys += dResultMatrix[1];
		mExterior.Zs += dResultMatrix[2];
		mExterior.phi   += dResultMatrix[3];
		mExterior.omega += dResultMatrix[4];
		mExterior.kappa += dResultMatrix[5];

		iIterTime++;
	} while (iIterTime<=6 || dMaxRms < 1e-6);