Photogrammetry Bundle Adjustment programming

A recent curriculum design need to use the knowledge bundle block adjustment, I realized its entire bundle block adjustment process of the C ++ programming language, the code is based on the orientation of programming within photogrammetry released in front of me, photogrammetry is the programming code in some places do not understand the small partners, you can take a look at the contents of my two articles described above, the following is my code:

//光束法区域网平差
#include<iostream>
#include<fstream>
#include <iomanip>
//#include "core/core.hpp"
#define _CRT_SECURE_NO_WARNINGS

using namespace std;
//using namespace cv;

int main()
{
	//定义内方位元素
	double x0 = -0.00000;//mm
	double y0 = 0.00000;//mm
	double f = 0.0705;//mm
	//从文件中读取相应的数值到数组
	double array[12] = { 0.0 };
	ifstream infile;//定义文件流对象
	infile.open("C:\\Users\\ASUS\\Desktop\\7\\2036\\8.txt");//打开文档C:\Users\ASUS\Desktop\7\1036
	double *ptr = &array[0];
	while (!infile.eof())
	{
		infile >> *ptr;//这是把文档里面的数对应放在ptr位置上的数值上
		ptr++;
	}
	infile.close();
	double n = array[0];
	double x = array[1];
	double y = array[2];
	double X = array[3];
	double Y = array[4];
	double Z = array[5];
	double Xs = array[6];
	double Ys = array[7];
	double Zs = array[8];
	double phi = array[9];
	double omiga = array[10];
	double kappa = array[11];
	/*double flag = array[12];*/
	//计算phi、omiga、kappa的方向余弦
	double a1 = cos(phi)*cos(kappa) - sin(phi)*sin(omiga)*sin(kappa);
	double a2 = -cos(phi)*sin(kappa) - sin(phi)*sin(omiga)*cos(kappa);
	double a3 = -sin(phi)*cos(omiga);
	double b1 = cos(omiga)*sin(kappa);
	double b2 = cos(omiga)*cos(kappa);
	double b3 = -sin(omiga);
	double c1 = sin(phi)*cos(kappa) + cos(phi)*sin(omiga)*sin(kappa);
	double c2 = -sin(phi)*sin(kappa) + cos(phi)*sin(omiga)*cos(kappa);
	double c3 = cos(phi)*cos(omiga);
	//利用共线条件方程就求解Lx、Ly,构建矩阵L
	double x1 = (-f * (a1*(X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs)))/(a3*(X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs));
	double y1=(-f * (a2*(X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs))) / (a3*(X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs));
	double Lx = x - x1;
	double Ly = y - y1;

//构建A、B矩阵
//建立矩阵A、B,使用严密解形式
double a11 = (a1 * f + a3 * x)*(1.0/Z);
double a12 = (b1 * f + b3 * x)*(1.0 / Z);
double a13 = (c1 * f + c3 * x)*(1.0 / Z);
//老师提供的严密解形式
double a14 = y * sin(omiga) - ((x*(x*cos(kappa) - y * sin(kappa)) / f + f * cos(kappa)))*cos(omiga);
double a15 = -f * sin(kappa) - (x*(x*sin(kappa)+y*cos(kappa))) / f;
//课本上严密解形式
/*double a14 = (b1*x*y) / f - b2 * (f + (x*x) / f) - b3 * y;
double a15 = -(sin(kappa)*x*x) / f - (x*y*sin(kappa)) / f - f * sin(kappa);*/
double a16 = y;
double a21 = (a2 * f + a3 * y)*(1.0 / Z);
double a22 = (b2 * f + b3 * y)*(1.0 / Z);
double a23 =(c2 * f + c3 * y)*(1.0 / Z);
double a24 = -x * sin(omiga) - cos(omiga)*((x*(x*cos(kappa) - y * sin(kappa))) / f - f * sin(kappa));
double a25 = -f*cos(kappa)-(y*(x*sin(kappa)+y*cos(kappa)))/f;
/*double a24 = b1 * (f + (y*y) / f) - (b2*x*y) / f + b3 * x;
double a25 = -(x*y*sin(kappa)) / f - (y*y*cos(kappa)) / f - f * cos(kappa);*/
double a26 = -x;
double A[2][6] = {0.0};

A[0][0] = a11;
A[0][1] = a12;
A[0][2] = a13;
A[0][3] = a14;
A[0][4] = a15;
A[0][5] = a16;
A[1][0] = a21;
A[1][1] = a22;
A[1][2] = a23;
A[1][3] = a24;
A[1][4] = a25;
A[1][5] = a26;
ofstream fout("C:\\Users\\ASUS\\Desktop\\7\\2036\\8_result.txt", ios_base::app);
int i, j;
fout <<"像片号："<< n << endl;
fout << "A=" << endl;
for (i = 0; i < 2; i++)
{
	for (j = 0; j < 6; j++)
		
		fout << A[i][j] << " ";
	if (j % 6 == 0)
		fout << endl;
}
fout << "B=" << endl;
for (int m = 0; m < 2; m++)
{
	for (n = 0; n < 3; n++)
	
	fout << -A[m][(int )n] << " ";
	if ((int)n % 3 == 0)
		fout << endl;
}
fout << "L=" << endl;
fout<< Lx*1000 << endl;
fout << Ly * 1000 << endl;
//cout << Lx << "  " << Ly << endl;
/*cout.precision(12);
cout << Lx << endl;
cout << Ly << endl;*/
/*Mat L = Mat::zeros(2, 1, CV_64F);
L.at<double>(0, 0) = Lx;
L.at<double>(1, 0) = Ly;*/
/*Mat A = Mat::zeros(2, 6, CV_64F);
A.at<double>(0, 0) = a11;
A.at<double>(0, 1) = a12;
A.at<double>(0, 2) = a13;
A.at<double>(0, 3) = a14;
A.at<double>(0, 4) = a15;
A.at<double>(0, 5) = a16;
A.at<double>(1, 0) = a21;
A.at<double>(1, 1) = a22;
A.at<double>(1, 2) = a23;
A.at<double>(1, 3) = a24;
A.at<double>(1, 4) = a25;
A.at<double>(1, 5) = a26;*/
/*Mat B = Mat::zeros(2, 3, CV_64F);
B.at<double>(0, 0) = -a11;
B.at<double>(0, 1) = -a12;
B.at<double>(0, 2) = -a13;
B.at<double>(1, 0) = -a21;
B.at<double>(1, 1) = -a22;
B.at<double>(1, 2) = -a23;*/
/*cout.precision(3);*/
/*cout << A << endl;*/
//cout << B << endl;
/*fout << "L=" << endl;
fout << L << endl;*/
//int i, j;
//for ( i = 0; i< 2;i++)
//{
//	for(j=0;j<6;j++)
//	  fout.precision(12);//控制输出的位数
//	fout << A.at<double>(i, j)<< "  ";
//		if (j % 6 == 0)
//			fout << '\n';
//}
/*fout << n << endl;
fout << "A=" << endl;
fout << A << endl;
fout << "B=" << endl;
fout << B << endl;
fout << "L=" << endl;
fout << L << endl;*/
/*fout << a11 << endl;*/
cout << "You have finished the work, congrtulations!!!!" << endl;
system("pause");
return 0;

}