[C++知识库]2021-07-22相机检校

1> 提交C++代码，能够完成像点、控制点的自动输入、以及相机检校结果输出。
2> 编写相机检校报告：包括流程分析、代码分析、精度分析。
3> 能够针对全画幅相机畸变模型进行不同畸变模型的分析（K3、K4、K5、K6不同次项径向畸变模型的拟合情况分析。
4> PT_含粗查.txt文件中包含了10个粗查点，因此在实现算法时，需要考虑如何进行粗查剔除（不做硬性规定）。

Manger.h

#pragma once

#include<fstream>
#include<iostream>
#include<vector>
#include<math.h>
#include<string>
#include<stdio.h>
#include <iomanip>
#include<algorithm>

#include"Eigen/Dense"
#include "ceres/problem.h"
#include"gflags/gflags.h"
#include "glog/logging.h"
#include "ceres/ceres.h"

//定义ceres参数模型
struct res_cen_K2_BA
{
	double m_dx, m_dy;
	double m_dX, m_dY, m_dZ;
	res_cen_K2_BA(double x, double y, double X, double Y, double Z)
	{
		m_dx = x;
		m_dy = y;
		m_dX = X;
		m_dY = Y;
		m_dZ = Z;
	}
	template <typename T>
	bool operator()(const T* const camera_K,
		const T* const image_Rt,
		const T* const camera_dp,
		T* out_residuals) const
	{
		T pos_proj[3], Xs, Ys, Zs, phi, omg, kap;
		T R[9], sOmg, cOmg, sPhi, cPhi, sKap, cKap;
		phi = image_Rt[0];
		omg = image_Rt[1];
		kap = image_Rt[2];
		Xs = image_Rt[3];
		Ys = image_Rt[4];
		Zs = image_Rt[5];

		sOmg = sin(omg);
		cOmg = cos(omg);
		sPhi = sin(phi);
		cPhi = cos(phi);
		sKap = sin(kap);
		cKap = cos(kap);

		//求R矩阵
		R[0] = cPhi * cKap - sPhi * sOmg * sKap;
		R[1] = -cPhi * sKap - sPhi * sOmg * cKap;
		R[2] = -sPhi * cOmg;
		R[3] = cOmg * sKap;
		R[4] = cOmg * cKap;
		R[5] = -sOmg;
		R[6] = sPhi * cKap + cPhi * sOmg * sKap;
		R[7] = -sPhi * sKap + cPhi * sOmg * cKap;
		R[8] = cPhi * cOmg;

		//求X_,Y_,Z_近似值
		pos_proj[0] = R[0] * (m_dX - Xs) + R[3] * (m_dY - Ys) + R[6] * (m_dZ - Zs);
		pos_proj[1] = R[1] * (m_dX - Xs) + R[4] * (m_dY - Ys) + R[7] * (m_dZ - Zs);
		pos_proj[2] = R[2] * (m_dX - Xs) + R[5] * (m_dY - Ys) + R[8] * (m_dZ - Zs);

		//求X_/Z_,Y_/Z_
		const T x_u = pos_proj[0] / pos_proj[2];
		const T y_u = pos_proj[1] / pos_proj[2];

		const T& focal = camera_K[0];
		const T& ppx = camera_K[1];
		const T& ppy = camera_K[2];
		const T& k1 = camera_dp[0];
		const T& k2 = camera_dp[1];
		const T& p1 = camera_dp[2];
		const T& p2 = camera_dp[3];
		const T& a = camera_dp[4];
		const T& b = camera_dp[5];

		const T r2 = (T(m_dx) - ppx) * (T(m_dx) - ppx) + (T(m_dy) - ppy) * (T(m_dy) - ppy);
		const T r4 = r2 * r2;

		const T dtx = (T(m_dx) - ppx) * (k1 * r2 + k2 * r4) + p1 * (r2 + 2.0 * (T(m_dx) - ppx) * (T(m_dx) - ppx)) + p2 * 2.0 * (T(m_dx) - ppx) * (T(m_dy) - ppy) + a * (T(m_dx) - ppx) + b * (T(m_dy) - ppy);
		const T dty = (T(m_dy) - ppy) * (k1 * r2 + k2 * r4) + p2 * (r2 + 2.0 * (T(m_dx) - ppx) * (T(m_dx) - ppx)) + p1 * 2.0 * (T(m_dx) - ppx) * (T(m_dy) - ppy);

		out_residuals[0] = ppx - dtx - focal * x_u-T(m_dx);
		out_residuals[1] = ppy - dty - focal * y_u-T(m_dy);
		return true;
	}
};

//void ceres_BA();

class PT //图像坐标
{
public:
	int number;
	double X;
	double Y;
public:
	PT() { number = 0; X = 0.0; Y = 0.0; };
	~PT() {};
	friend bool operator<(PT a, PT b)//重载PT, 使得可以用sort进行排序
	{
		return a.number < b.number;
	}
};

class GCP//控制点坐标
{
public:
	int number;
	double X;
	double Y;
	double Z;
public:
	GCP() { number = 0; X = 0.0; Y = 0.0; Z = 0.0; };
	~GCP() {};
};

class PT_GCP//像点坐标和控制点坐标对应
{
public:
	PT_GCP() { id = 0; use = 1; number = 0; PT_X = 0.0; PT_Y = 0.0; Vx = Vy = 0.0; GCP_X = 0.0; GCP_Y = 0.0; GCP_Z = 0.0; };
	~PT_GCP() {};
public:
	int id;
	bool use;
	int number;
	double PT_X;
	double PT_Y;
	double Vx;
	double Vy;
	double GCP_X;
	double GCP_Y;
	double GCP_Z;
};

class Manger
{
public:
	Manger() ;
	~Manger() {};
public:
	std::vector<PT>PT_XY;
	std::vector<GCP>GCP_XYZ;
	std::vector<PT_GCP>PT_GCP_XYZ;

public:
	double Lp[11] = { 0.0 };
	double R[3][3] = { 0.0 };
	double A[3][3] = { 0.0 };
	double Vxy[2 * 285] = { 0.0 };

	double mer;//中误差
	double fx2;//计算参数
	double fy2;
	double Xs, Ys, Zs;//15个元素和相关计算参数
	double Phi, Omg, Kap;
	double f, x0, y0;
	double a1, a2, a3, b1, b2, b3, c1, c2, c3;
	double L1, L2, L3, L4, L5, L6, L7, L8, L9, L10, L11;
	double k1, k2, k3, k4, k5, k6, p1, p2, da, db;
	

public:
	
	void OpenFile_PT(std::string filename_);//读取像点

	void OpenFile_GCP(std::string filename_);//读取控制点

	//void OpenImage(std::string filename_);//读取图像

	void Matching();//像点坐标和控制点坐标匹配

	bool BLT(std::vector<PT_GCP>& img_, double* L);//BLT

	void InnitValue(double* L);

	bool BA(std::vector<PT_GCP>&img_);

	bool BA_K3(std::vector<PT_GCP>& img_);//K3次项径向畸变模型

	bool BA_K4(std::vector<PT_GCP>& img_);//K4次项径向畸变模型

	bool BA_K5(std::vector<PT_GCP>& img_);//K5次项径向畸变模型

	bool BA_K6(std::vector<PT_GCP>& img_);//K5次项径向畸变模型

	void calculate();//不含粗差，计算流程

	void  calculate_c();//含粗差，计算流程

	void calculate_K3();//K3次项径向畸变模型,不含粗差，计算流程

	void calculate_K4();//K4次项径向畸变模型,不含粗差，计算流程

	void calculate_K5();//K5次项径向畸变模型,不含粗差，计算流程
	
	void calculate_K6();//K6次项径向畸变模型,不含粗差，计算流程

	bool ceres_BA();//用ceres解求BA

	void OutputFile();//输出文件K2次项径向畸变模型,不含粗差

	void OutputFile_ceres();//输出文件ceres求解K2次项径向畸变模型,不含粗差

	void OutputFile_app();//输出文件K2次项径向畸变模型,含粗差

	void OutputFile_app_K3();//输出文件K3次项径向畸变模型,不含粗差

	void OutputFile_app_K4();//输出文件K4次项径向畸变模型,不含粗差

	void OutputFile_app_K5();//输出文件K5次项径向畸变模型,不含粗差

	void OutputFile_app_K6();//输出文件K6次项径向畸变模型,不含粗差
};



void test(Manger&p);

Manger.cpp

#include"Manger.h"
#include"Matrix.h"

Manger::Manger()
{
  	fx2 = fy2 = 0.0;
	Xs = Ys = Zs = 0.0;
	Phi = Omg = Kap = 0.0;
	f = x0 = y0 = 0.0;
	a1 = a2 = a3 = b1 = b2 = b3 = c1 = c2 = c3=0.0;
	L1 = L2 = L3 = L4 = L5 = L6 = L7 = L8 = L9 = L10 = L11 = 0.0;
	k1 = k2 = k3 = k4 = k5 = k6 = p1 = p2 = da = db = 0.0;
}

void Manger::OpenFile_PT(std::string filename_)
{
	PT_XY.clear();
	int nu;
	double x, y;
	std::ifstream fin(filename_, std::ios::in);
	if (fin.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	std::string sin;
	while (!fin.eof())
	{
		fin >> nu >> x >> y;
		PT store;
		store.number = nu;
		store.X = x;
		store.Y = y;
		if (fin.fail())break;
		PT_XY.push_back(store);
	}
	std::sort(PT_XY.begin(), PT_XY.end());//排好序，为与控制点匹配做准备
	fin.close();
}

void Manger::OpenFile_GCP(std::string filename_)
{
	GCP_XYZ.clear();
	int nu;
	double x, y, z;
	std::ifstream fin(filename_, std::ios::in);
	if (fin.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	std::string sin;
	while (!fin.eof())
	{
		fin >> nu >> x >> y >> z;
		GCP store;
		store.number = nu;
		store.X = x;
		store.Y = y;
		store.Z = z;
		if (fin.fail())break;
		GCP_XYZ.push_back(store);
	}
	fin.close();
}

void Manger::Matching()
{
	PT_GCP_XYZ.clear();
	int ID = 0;
	for (size_t i = 0; i < PT_XY.size(); i++)
	{
		for (size_t j = 0; j < GCP_XYZ.size(); j++)
		{
			if (PT_XY[i].number == GCP_XYZ[j].number)
			{
				ID++;
				PT_GCP store;
				store.number = PT_XY[i].number;
				store.PT_X = PT_XY[i].X;
				store.PT_Y = PT_XY[i].Y;
				store.GCP_X = GCP_XYZ[j].X;
				store.GCP_Y = GCP_XYZ[j].Y;
				store.GCP_Z = GCP_XYZ[j].Z;
				store.id=ID;
				PT_GCP_XYZ.push_back(store);

			}
		}
	}
}

bool Manger::BLT(std::vector<PT_GCP>& img_, double *pL)
{
	//系数矩阵
	double A[2 * 11];
	double AT[11 * 2];
	double ATL[11 * 1];
	double ATL_t[11 * 1];
	double ATA[11 * 11];
	double ATA_t[11 * 11];
	double L[2 * 1];
	double X[11 * 1];

	//清空矩阵
	memset(A, 0, 2 * 11 * sizeof(double));
	memset(AT, 0, 11 * 2 * sizeof(double));
	memset(ATL, 0, 11 * 1 * sizeof(double));
	memset(ATL_t, 0, 11 * 1 * sizeof(double));
	memset(ATA, 0, 11 * 11 * sizeof(double));
	memset(ATA_t, 0, 11 * 11 * sizeof(double));
	memset(L, 0, 2 * 1 * sizeof(double));

	//迭代器
	std::vector<PT_GCP>::iterator it = img_.begin();
	for (; it != img_.end(); it++)
	{
		//逐点化法
		A[0 * 11 + 0] = it->GCP_X;
		A[0 * 11 + 1] = it->GCP_Y;
		A[0 * 11 + 2] = it->GCP_Z;
		A[0 * 11 + 3] = 1.0;
		A[0 * 11 + 4] = 0.0;
		A[0 * 11 + 5] = 0.0;
		A[0 * 11 + 6] = 0.0;
		A[0 * 11 + 7] = 0.0;
		A[0 * 11 + 8] = it->PT_X * it->GCP_X;
		A[0 * 11 + 9] = it->PT_X * it->GCP_Y;
		A[0 * 11 + 10]= it->PT_X * it->GCP_Z;

		A[1 * 11 + 0] = 0.0;
		A[1 * 11 + 1] = 0.0;
		A[1 * 11 + 2] = 0.0;
		A[1 * 11 + 3] = 0.0;
		A[1 * 11 + 4] = it->GCP_X;
		A[1 * 11 + 5] = it->GCP_Y;
		A[1 * 11 + 6] = it->GCP_Z;
		A[1 * 11 + 7] = 1.0;
		A[1 * 11 + 8] = it->PT_Y * it->GCP_X;
		A[1 * 11 + 9] = it->PT_Y * it->GCP_Y;
		A[1 * 11 + 10]= it->PT_Y * it->GCP_Z;

		L[0] = -it->PT_X;
		L[1] = -it->PT_Y;

		//AT
		MatrixTra(2, 11, A, AT);
		
		//ATA+=ATA
		MatrixMul(AT, A, 11, 2 , 11,ATA_t);
		MatrixSum(11, 11, 11, 11, ATA, ATA_t, ATA);

		//ATL+=ATL
		MatrixMul(AT, L, 11, 2, 1, ATL_t);
		MatrixSum(11, 1, 11, 1, ATL, ATL_t, ATL);
	}

	//求解法方程
    MatrixInv(11, ATA);
	if (ATA == 0)return false;//矩阵求逆失败，方程已经无法收敛

	MatrixMul(ATA, ATL, 11, 11, 1, X);

	//为结果赋值
	for (int i = 0; i < 11; i++)
		pL[i] = X[i];

	return true;
}

void Manger::InnitValue(double* L)
{
	L1 = L[0], L2 = L[1], L3 = L[2], L4 = L[3], L5 = L[4], L6 = L[5], L7 = L[6], L8 = L[7], L9 = L[8], L10 = L[9], L11 = L[10];
	
	double  y3, fx, fy, L_[3] = { 0.0 }, R_A_[3][3] = { 0.0 }, Xs_Ys_Zs[3] = { 0.0 };
	
	x0 = -(L1 * L9 + L2 * L10 + L3 * L11) / (pow(L9, 2.0) + pow(L10, 2.0) + pow(L11, 2.0));
	y0 = -(L5 * L9 + L6 * L10 + L7 * L11) / (pow(L9, 2.0) + pow(L10, 2.0) + pow(L11, 2.0));

	fx2 = -pow(x0, 2.0) + (pow(L1, 2.0) + pow(L2, 2.0) + pow(L3, 2.0)) / (pow(L9, 2.0) + pow(L10, 2.0) + pow(L11, 2.0));
	fy2 = -pow(y0, 2.0) + (pow(L5, 2.0) + pow(L6, 2.0) + pow(L7, 2.0)) / (pow(L9, 2.0) + pow(L10, 2.0) + pow(L11, 2.0));

	fx = sqrt(fx2);
	fy = sqrt(fy2);

	y3 = 1 / (sqrt(pow(L9, 2.0) + pow(L10, 2.0) + pow(L11, 2.0)));
	f = (fx + fy) / 2.0;

	a3 = y3 * L9;
	b3 = y3 * L10;
	c3 = y3 * L11;

	a1 = 1 / fx * (y3 * L1 + x0 * a3);
	b1 = 1 / fx * (y3 * L2 + x0 * b3);
	c1 = 1 / fx * (y3 * L3 + x0 * c3);

	a2 = 1 / fy * (y3 * L5 + y0 * a3);
	b2 = 1 / fy * (y3 * L6 + y0 * b3);
	c2 = 1 / fy * (y3 * L7 + y0 * c3);


	Phi = atan(-a3/c3) - 3.141592653589793; //角元素
	Omg = asin(b3);
	Kap = atan(b1/ b2);

	R[0][0] = a1; 
	R[0][1] = b1; 
	R[0][2] = c1;

	R[1][0] = a2;
	R[1][1] = b2; 
	R[1][2] = c2;

	R[2][0] = a3;
	R[2][1] = b3; 
	R[2][2] = c3;

	A[0][0] = fx / y3;   
	A[0][2] = -x0 / y3;    
	A[1][1] = fx / y3;
	A[1][2] = -y0 / y3;       
	A[2][2] = 1 / y3;

	L_[0] = -L4;
	L_[1] = -L8;
	L_[2] = -1;

	MatrixInv(3, *R);
	MatrixInv(3, *A);
	MatrixMul(*R, *A, 3, 3, 3, *R_A_);
	MatrixMul(*R_A_, L_, 3, 3, 1, Xs_Ys_Zs);

	/*Phi = atan2(-c1, c3);
	Omg = asin(-c2);
	Kap = atan2(a2, b2);*/

	Xs = Xs_Ys_Zs[0];
	Ys = Xs_Ys_Zs[1];
	Zs = Xs_Ys_Zs[2];//外方位元素
}

bool Manger::BA(std::vector<PT_GCP>&img_)
{
	//系数矩阵
	double A[2 * 15];
	double AT[15 * 2];
	double ATL[15 * 1];
	double ATL_t[15 * 1];
	double ATA[15 * 15];
	double ATA_t[15 * 15];
	double L[2 * 1];
	double Xv[15 * 1];
	double AXv[2 * 1];
	double Vxy[2 * 1];

	int iRepeat = 0;//迭代次数
	do
	{
		iRepeat++;
		if (iRepeat > 1000)
		{
			break;
			return false;
		}
		//清空矩阵
		memset(A, 0, 2 * 15 * sizeof(double));
		memset(AT, 0, 15 * 2 * sizeof(double));
		memset(ATL, 0, 15 * 1 * sizeof(double));
		memset(ATL_t, 0, 15 * 1 * sizeof(double));
		memset(ATA, 0, 15 * 15 * sizeof(double));
		memset(ATA_t, 0, 15 * 15 * sizeof(double));
		memset(L, 0, 2 * 1 * sizeof(double));
		memset(Xv, 0, 15 * 1 * sizeof(double));
		memset(AXv, 0, 2 * 1 * sizeof(double));
		memset(Vxy, 0, 2 * 1 * sizeof(double));

		//迭代器
		std::vector<PT_GCP>::iterator it = img_.begin();
		for (; it != img_.end(); it++)
		{
			double x = it->PT_X;
			double y = it->PT_Y;
			double X = it->GCP_X;
			double Y = it->GCP_Y;
			double Z = it->GCP_Z;

			double r2 = pow(x - x0, 2.0) + pow(y - y0, 2.0);

			double dltx = (x - x0) * (k1 * r2 + k2 * r2 * r2) + p1 * (r2 + 2 * (x - x0) * (x - x0)) + p2 * 2 * (x - x0) * (y - y0) + da * (x - x0) + db * (y - y0);
			double dlty = (y - y0) * (k1 * r2 + k2 * r2 * r2) + p2 * (r2 + 2 * (x - x0) * (x - x0)) + p1 * 2 * (x - x0) * (y - y0);

			double X_ = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs);
			double Y_ = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs);
			double Z_ = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs);

			double xj = x0 + dltx - f * X_ / Z_;//x的近似值
			double yj = y0 + dlty - f * Y_ / Z_;//y的近似值

			A[0 * 15 + 0] = 1 / Z_ * (a1 * f + a3 * (x - x0));
			A[0 * 15 + 1] = 1 / Z_ * (b1 * f + b3 * (x - x0));
			A[0 * 15 + 2] = 1 / Z_ * (c1 * f + c3 * (x - x0));

			A[0 * 15 + 3] = (y - y0) * sin(Omg) - ((x - x0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) + f * cos(Kap)) * cos(Omg);
			A[0 * 15 + 4] = -f * sin(Kap) - (x - x0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[0 * 15 + 5] = y - y0;
			A[0 * 15 + 6] = (x - x0) / f;
			A[0 * 15 + 7] = 1;
			A[0 * 15 + 8] = 0;

			A[0 * 15 + 9] = (x - x0) * r2;
			A[0 * 15 + 10] = (x - x0) * r2*r2;
			A[0 * 15 + 11] = r2 + 2 * pow(x - x0, 2.0);
			A[0 * 15 + 12] = 2 * (x - x0) * (y - y0);
			A[0 * 15 + 13] = x - x0;
			A[0 * 15 + 14] = y - y0;


			A[1 * 15 + 0] = 1 / Z_ * (a2 * f + a3 * (y - y0));
			A[1 * 15 + 1] = 1 / Z_ * (b2 * f + b3 * (y - y0));
			A[1 * 15 + 2] = 1 / Z_ * (c2 * f + c3 * (y - y0));

			A[1 * 15 + 3] = -(x - x0) * sin(Omg) - ((y - y0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) - f * sin(Kap)) * cos(Omg);
			A[1 * 15 + 4] = -f * cos(Kap) - (y - y0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[1 * 15 + 5] = -(x - x0);
			A[1 * 15 + 6] = (y - y0) / f;
			A[1 * 15 + 7] = 0;
			A[1 * 15 + 8] = 1;

			A[1 * 15 + 9] = (y - y0) * r2;
			A[1 * 15 + 10] = (y - y0) * r2 * r2;
			A[1 * 15 + 11] = 2 * (x - x0) * (y - y0);
			A[1 * 15 + 12] = r2 + 2 * pow(y - y0, 2.0);
			A[1 * 15 + 13] = 0.0;
			A[1 * 15 + 14] = 0.0;

			L[0] = x - xj;
			L[1] = y - yj;

			//AT
			MatrixTra(2, 15, A, AT);

			//ATA+=ATA
			MatrixMul(AT, A, 15, 2, 15, ATA_t);
			MatrixSum(15, 15, 15, 15, ATA, ATA_t, ATA);

			//ATL+=ATL
			MatrixMul(AT, L, 15, 2, 1, ATL_t);
			MatrixSum(15, 1, 15, 1, ATL, ATL_t, ATL);

			//求中误差
			MatrixMul(A, Xv, 2, 15, 1, AXv);
			MatrixDiff(2, 1, 2, 1, AXv, L, Vxy);
			it->Vx = Vxy[0];
			it->Vy = Vxy[1];
		}
		//求解法方程
		MatrixInv(15, ATA);
		if (ATA == 0)
		{
			mer = 10;
			break; 
			return false;//矩阵求逆失败，方程已经无法收敛
		}
		MatrixMul(ATA, ATL, 15, 15, 1, Xv);

		Xs  += Xv[0];//更新数据
		Ys  += Xv[1];
		Zs  += Xv[2];
		Phi += Xv[3];
		Omg += Xv[4];
		Kap += Xv[5];
		f   += Xv[6];
		x0  += Xv[7];
		y0  += Xv[8];
		k1  += Xv[9];
		k2  += Xv[10];
		p1  += Xv[11];
		p1  += Xv[12];
		da  += Xv[13];
		db  += Xv[14];

		a1 = cos(Phi) * cos(Kap) - sin(Phi) * sin(Omg) * sin(Kap);
		a2 = -cos(Phi) * sin(Kap) - sin(Phi) * sin(Omg) * cos(Kap);
		a3 = -sin(Phi) * cos(Omg);
		b1 = cos(Omg) * sin(Kap);
		b2 = cos(Omg) * cos(Kap);
		b3 = -sin(Omg);
		c1 = sin(Phi) * cos(Kap) + cos(Phi) * sin(Omg) * sin(Kap);
		c2 = -sin(Phi) * sin(Kap) + cos(Phi) * sin(Omg) * cos(Kap);
		c3 = cos(Phi) * cos(Omg);

	} while (Xv[3] < 0.00001 || Xv[4] < 0.00001 || Xv[5] < 0.00001);
	
	std::vector<PT_GCP>::iterator it = img_.begin();
	double toutle=0;
	for (; it != img_.end(); it++)
		toutle = it->Vx * it->Vx + it->Vy * it->Vy;
	mer = sqrt(toutle /( img_.size()));	
	if (iRepeat > 1000)
		mer = 10;
	return  true;
}

void Manger::OutputFile()
{
	std::fstream fp("D:\\6_1.txt", std::ios::app);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "K2次项径向畸变模型,不含粗差：\n";
	fp << "Xs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs << "\nPhi:" << Phi << "\nOmg:" <<Omg << "\nKap" << Kap << "\nf:" << f << "\nx0:" <<x0 << "\ny0:" <<y0
		<< "\nk1:" <<k1 << "\nk2:" <<k2 << "\np1:" <<p1 << "\np2:" << p2 << "\nα:" << da << "\nβ:" <<db << "\n";

	fp << "中误差：" <<mer;
	fp.close();
}

void Manger::OutputFile_ceres()
{
	std::fstream fp("D:\\6_1.txt", std::ios::out);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "ceres求解K2次项径向畸变模型,不含粗差：\n";
	fp << "Xs:" <<Xs << "\nYs:" <<Ys << "\nZs:" << Zs << "\nPhi:" << Phi << "\nOmg:" <<Omg << "\nKap" << Kap << "\nf:" << f << "\nx0:" <<x0 << "\ny0:" <<y0
		<< "\nk1:" << k1 << "\nk2:" << k2 << "\np1:" <<p1 << "\np2:" << p2 << "\nα:" <<da << "\nβ:" <<db << "\n";

	fp << "中误差：\n" << mer;
	fp.close();
}

void Manger::OutputFile_app()
{
	std::fstream fp("D:\\6_1.txt", std::ios::app);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "\n\nK2次项径向畸变模型,含粗差：\n";
	fp <<"Xs:"<< Xs << "\nYs:" << Ys << "\nZs:" <<Zs << "\nPhi:" << Phi << "\nOmg:" << Omg << "\nKap" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0:" << y0
		<< "\nk1:" <<k1 << "\nk2:" <<k2 << "\np1:" <<p1 << "\np2:" << p2 << "\nα:" <<da << "\nβ:" << db << "\n";

	fp << "中误差：" << mer;
	fp.close();
}

void Manger::OutputFile_app_K3()
{
	std::fstream fp("D:\\6_1.txt", std::ios::app);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "\n\nK3次项径向畸变模型,不含粗差：\n";
	fp << "Xs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs << "\nPhi:" << Phi << "\nOmg:" << Omg << "\nKap" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0:" <<y0
		<< "\nk1:" << k1 << "\nk2:" << k2 <<"\nK3:"<<k3<< "\np1:" << p1 << "\np2:" << p2 << "\nα:" << da << "\nβ:" << db << "\n";

	fp << "中误差：" <<mer;
	fp.close();
}

void Manger::OutputFile_app_K4()
{
	std::fstream fp("D:\\6_1.txt", std::ios::app);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "\n\nK4次项径向畸变模型,不含粗差：\n";
	fp << "Xs:" << Xs << "\nYs:" << Ys << "\nZs:" <<Zs << "\nPhi:" << Phi << "\nOmg:" << Omg << "\nKap" <<Kap << "\nf:" << f << "\nx0:" <<x0 << "\ny0:" << y0
		<< "\nk1:" << k1 << "\nk2:" <<k2 << "\nK3:" << k3 << "\nK4:" << k4 << "\np1:" << p1 << "\np2:" <<p2 << "\nα:" << da << "\nβ:" <<db << "\n";

	fp << "中误差：" <<mer;
	fp.close();
}

void Manger::OutputFile_app_K5()
{
	std::fstream fp("D:\\6_1.txt", std::ios::app);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "\n\nK5次项径向畸变模型,不含粗差：\n";
	fp << "Xs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs << "\nPhi:" <<Phi << "\nOmg:" << Omg << "\nKap" << Kap << "\nf:" << f << "\nx0:" <<x0 << "\ny0:" <<y0
		<< "\nk1:" << k1 << "\nk2:" << k2 << "\nK3:" << k3 << "\nK4:" << k4 << "\nK5:" <<k5 << "\np1:" << p1 << "\np2:" << p2 << "\nα:" << da << "\nβ:" <<db << "\n";

	fp << "中误差：" <<mer;
	fp.close();
}

void Manger::OutputFile_app_K6()
{
	std::fstream fp("D:\\6_1.txt", std::ios::app);
	if (fp.fail())
	{
		std::cerr << "open file error!!!";
		exit(1);
	}
	fp << "\n\nK6次项径向畸变模型,不含粗差：\n";
	fp << "Xs:" <<Xs << "\nYs:" << Ys << "\nZs:" <<Zs << "\nPhi:" <<Phi << "\nOmg:" <<Omg << "\nKap" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0:" <<y0
		<< "\nk1:" <<k1 << "\nk2:" <<k2 << "\nK3:" <<k3 << "\nK4:" << k4 << "\nK5:" << k5 << "\nK6:" << k6 << "\np1:" << p1 << "\np2:" <<p2 << "\nα:" << da << "\nβ:" << db << "\n";

	fp << "中误差：" <<mer;
	fp.close();
}

void Manger::calculate()
{
	OpenFile_PT("PT_不含粗差.txt");//

	OpenFile_GCP("GCP_XH.txt");//

	Matching();

	BLT(PT_GCP_XYZ, Lp);

	InnitValue(Lp);
	
	BA(PT_GCP_XYZ);

	std::cout << "K2次项径向畸变模型，不含粗差计算\nXs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs <<
		"\nPhi: " << Phi << "\nOmg:" << Omg << "\nKap:" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0" << y0
		<< "\np1:" << p1 << "\np2:" << p2 << "\nk1:" << k1 << "\nk2:"  <<k2<<"\nα:" << da << "\nβ:" << db << "\n";

	OutputFile();
}

void Manger::calculate_c()
{
	OpenFile_PT("PT_含粗差.txt");//

	OpenFile_GCP("GCP_XH.txt");//

	Matching();

	std::vector<PT_GCP>img_info;//剔除误差的匹配点
	for (int j = 0; j < 1000; j++)
	{
		std::vector<PT_GCP> img_t;//随机取10个匹配点
		int Num[10] = { 0 };
		for (int i = 0; i < 10; i++)
		{
			int n = rand() % PT_GCP_XYZ.size();
			img_t.push_back(PT_GCP_XYZ[n]);
			Num[i] = n;
		}
		BLT(img_t, Lp);
		InnitValue(Lp);
		k1 = k2 = p1 = p2 = da = db = 0;
		BA(img_t);

		if (mer > 1)//若中误差大于0.3，就不使用
		{
			for (int i = 0; i < 10; i++)
				PT_GCP_XYZ[Num[i]].use = 0;
		}
		else if (mer <= 1)//若中误差小于0.3，就使用
		{
			for (int i = 0; i < 10; i++)
				PT_GCP_XYZ[Num[i]].use = 1;
		}
	}

	for (size_t i = 0; i < PT_GCP_XYZ.size(); i++)
	{
		if (PT_GCP_XYZ[i].use == 1)
			img_info.push_back(PT_GCP_XYZ[i]);
	}

	BLT(img_info, Lp);

	InnitValue(Lp);
	k1 = k2 = p1 = p2 = da = db = 0;
	BA(img_info);

	OutputFile_app();

}

void Manger::calculate_K3()
{
	OpenFile_PT("PT_不含粗差.txt");//

	OpenFile_GCP("GCP_XH.txt");//

	Matching();

	BLT(PT_GCP_XYZ, Lp);

	InnitValue(Lp);

	BA_K3(PT_GCP_XYZ);

	std::cout << "K3次项径向畸变模型，不含粗差计算\nXs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs <<
		"\nPhi: " << Phi << "\nOmg:" << Omg << "\nKap:" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0" << y0
		<< "\np1:" << p1 << "\np2:" << p2 << "\nk1:" << k1 << "\nk2:" <<k2<< "\nk3:"<<k3 << "\nα:" << da << "\nβ:" << db<<"\n";

	OutputFile_app_K3();
}

void Manger::calculate_K4()
{
	OpenFile_PT("PT_不含粗差.txt");//

	OpenFile_GCP("GCP_XH.txt");//

	Matching();

	BLT(PT_GCP_XYZ, Lp);

	InnitValue(Lp);

	BA_K4(PT_GCP_XYZ);

	std::cout << "K4次项径向畸变模型，不含粗差计算\nXs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs <<
		"\nPhi: " << Phi << "\nOmg:" << Omg << "\nKap:" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0" << y0
		<< "\np1:" << p1 << "\np2:" << p2 << "\nk1:" << k1 << "\nk2:" <<k2<< "\nk3:" <<k3<<"\nk4:"<<k4<< "\nα:" << da << "\nβ:" << db << "\n";

	OutputFile_app_K4();
}

void Manger::calculate_K5()
{
	OpenFile_PT("PT_不含粗差.txt");//

	OpenFile_GCP("GCP_XH.txt");//

	Matching();

	BLT(PT_GCP_XYZ, Lp);

	InnitValue(Lp);

	BA_K5(PT_GCP_XYZ);

	std::cout << "K5次项径向畸变模型，不含粗差计算\nXs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs <<
		"\nPhi: " << Phi << "\nOmg:" << Omg << "\nKap:" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0" << y0
		<< "\np1:" << p1 << "\np2:" << p2 << "\nk1:" << k1 << "\nk2:"<<k2 << "\nk3:" <<k3<< "\nk4:" << k4 << "\nk5:" << k5 
		<< "\nα:" << da << "\nβ:" << db << "\n";

	OutputFile_app_K5();
}

void Manger::calculate_K6()
{
	OpenFile_PT("PT_不含粗差.txt");//

	OpenFile_GCP("GCP_XH.txt");//

	Matching();

	BLT(PT_GCP_XYZ, Lp);

	InnitValue(Lp);

	BA_K6(PT_GCP_XYZ);

	std::cout << "K6次项径向畸变模型，不含粗差计算\nXs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs <<
		"\nPhi: " << Phi << "\nOmg:" << Omg << "\nKap:" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0" << y0
		<< "\np1:" << p1 << "\np2:" << p2 << "\nk1:" << k1 << "\nk2:" <<k2<< "\nk3:"<<k3 << "\nk4:" << k4 << "\nk5:" << k5
		<< "\nα:" << da << "\nβ:" << db << "\n";

	OutputFile_app_K6();
}

bool Manger::BA_K3(std::vector<PT_GCP>& img_)
{
	//系数矩阵
	double A[2 * 16];
	double AT[16 * 2];
	double ATL[16 * 1];
	double ATL_t[16 * 1];
	double ATA[16 * 16];
	double ATA_t[16 * 16];
	double L[2 * 1];
	double Xv[16 * 1];
	double AXv[2 * 1];
	double Vxy[2 * 1];

	int iRepeat = 0;//迭代次数
	do
	{
		iRepeat++;
		if (iRepeat > 1000)
		{
			break;
			return false;
		}
		//清空矩阵
		memset(A, 0, 2 * 16 * sizeof(double));
		memset(AT, 0, 16 * 2 * sizeof(double));
		memset(ATL, 0, 16 * 1 * sizeof(double));
		memset(ATL_t, 0, 16 * 1 * sizeof(double));
		memset(ATA, 0, 16 * 16 * sizeof(double));
		memset(ATA_t, 0, 16 * 16 * sizeof(double));
		memset(L, 0, 2 * 1 * sizeof(double));
		memset(Xv, 0, 16 * 1 * sizeof(double));
		memset(AXv, 0, 2 * 1 * sizeof(double));
		memset(Vxy, 0, 2 * 1 * sizeof(double));

		//迭代器
		std::vector<PT_GCP>::iterator it = img_.begin();
		for (; it != img_.end(); it++)
		{
			double x = it->PT_X;
			double y = it->PT_Y;
			double X = it->GCP_X;
			double Y = it->GCP_Y;
			double Z = it->GCP_Z;

			double r2 = pow(x - x0, 2.0) + pow(y - y0, 2.0);

			double dltx = (x - x0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + p1 * (r2 + 2 * (x - x0) * (x - x0)) + p2 * 2 * (x - x0) * (y - y0) + da * (x - x0) + db * (y - y0);
			double dlty = (y - y0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2) + p2 * (r2 + 2 * (x - x0) * (x - x0)) + p1 * 2 * (x - x0) * (y - y0);

			double X_ = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs);
			double Y_ = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs);
			double Z_ = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs);

			double xj = x0 + dltx - f * X_ / Z_;//x的近似值
			double yj = y0 + dlty - f * Y_ / Z_;//y的近似值

			A[0 * 16 + 0] = 1 / Z_ * (a1 * f + a3 * (x - x0));
			A[0 * 16 + 1] = 1 / Z_ * (b1 * f + b3 * (x - x0));
			A[0 * 16 + 2] = 1 / Z_ * (c1 * f + c3 * (x - x0));

			A[0 * 16 + 3] = (y - y0) * sin(Omg) - ((x - x0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) + f * cos(Kap)) * cos(Omg);
			A[0 * 16 + 4] = -f * sin(Kap) - (x - x0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[0 * 16 + 5] = y - y0;
			A[0 * 16 + 6] = (x - x0) / f;
			A[0 * 16 + 7] = 1;
			A[0 * 16 + 8] = 0;

			A[0 * 16 + 9] = (x - x0) * r2;
			A[0 * 16 + 10] = (x - x0) * r2 * r2;
			A[0 * 16 + 11] = (x - x0) * r2 * r2 * r2;
			A[0 * 16 + 12] = r2 + 2 * pow(x - x0, 2.0);
			A[0 * 16 + 13] = 2 * (x - x0) * (y - y0);
			A[0 * 16 + 14] = x - x0;
			A[0 * 16 + 15] = y - y0;


			A[1 * 16 + 0] = 1 / Z_ * (a2 * f + a3 * (y - y0));
			A[1 * 16 + 1] = 1 / Z_ * (b2 * f + b3 * (y - y0));
			A[1 * 16 + 2] = 1 / Z_ * (c2 * f + c3 * (y - y0));

			A[1 * 16 + 3] = -(x - x0) * sin(Omg) - ((y - y0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) - f * sin(Kap)) * cos(Omg);
			A[1 * 16 + 4] = -f * cos(Kap) - (y - y0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[1 * 16 + 5] = -(x - x0);
			A[1 * 16 + 6] = (y - y0) / f;
			A[1 * 16 + 7] = 0;
			A[1 * 16 + 8] = 1;

			A[1 * 16 + 9] = (y - y0) * r2;
			A[1 * 16 + 10] = (y - y0) * r2 * r2;
			A[1 * 16 + 11] = (y - y0) * r2 * r2 * r2;
			A[1 * 16 + 12] = 2 * (x - x0) * (y - y0);
			A[1 * 16 + 13] = r2 + 2 * pow(y - y0, 2.0);
			A[1 * 16 + 14] = 0.0;
			A[1 * 16 + 15] = 0.0;

			L[0] = x - xj;
			L[1] = y - yj;

			//AT
			MatrixTra(2, 16, A, AT);

			//ATA+=ATA
			MatrixMul(AT, A, 16, 2, 16, ATA_t);
			MatrixSum(16, 16, 16, 16, ATA, ATA_t, ATA);

			//ATL+=ATL
			MatrixMul(AT, L, 16, 2, 1, ATL_t);
			MatrixSum(16, 1, 16, 1, ATL, ATL_t, ATL);

			//求中误差
			MatrixMul(A, Xv, 2, 16, 1, AXv);
			MatrixDiff(2, 1, 2, 1, AXv, L, Vxy);
			it->Vx = Vxy[0];
			it->Vy = Vxy[1];
		}
		//求解法方程
		MatrixInv(16, ATA);
		if (ATA == 0)return false;//矩阵求逆失败，方程已经无法收敛

		MatrixMul(ATA, ATL, 16, 16, 1, Xv);

		Xs  += Xv[0];//更新数据
		Ys  += Xv[1];
		Zs  += Xv[2];
		Phi += Xv[3];
		Omg += Xv[4];
		Kap += Xv[5];
		f   += Xv[6];
		x0  += Xv[7];
		y0  += Xv[8];
		k1  += Xv[9];
		k2  += Xv[10];
		k3  += Xv[11];
		p1  += Xv[12];
		p1  += Xv[13];
		da  += Xv[14];
		db  += Xv[15];

		a1 = cos(Phi) * cos(Kap) - sin(Phi) * sin(Omg) * sin(Kap);
		a2 = -cos(Phi) * sin(Kap) - sin(Phi) * sin(Omg) * cos(Kap);
		a3 = -sin(Phi) * cos(Omg);
		b1 = cos(Omg) * sin(Kap);
		b2 = cos(Omg) * cos(Kap);
		b3 = -sin(Omg);
		c1 = sin(Phi) * cos(Kap) + cos(Phi) * sin(Omg) * sin(Kap);
		c2 = -sin(Phi) * sin(Kap) + cos(Phi) * sin(Omg) * cos(Kap);
		c3 = cos(Phi) * cos(Omg);

	} while (Xv[3] < 0.00001 || Xv[4] < 0.00001 || Xv[5] < 0.00001);

	std::vector<PT_GCP>::iterator it = img_.begin();
	double toutle = 0;
	for (; it != img_.end(); it++)
		toutle = it->Vx * it->Vx + it->Vy * it->Vy;
	mer = sqrt(toutle / ( img_.size()));
	if (iRepeat > 1000)
		mer = 10;
	return  true;
}

bool Manger::BA_K4(std::vector<PT_GCP>& img_)
{
	//系数矩阵
	double A[2 * 17];
	double AT[17 * 2];
	double ATL[17 * 1];
	double ATL_t[17 * 1];
	double ATA[17 * 17];
	double ATA_t[17 * 17];
	double L[2 * 1];
	double Xv[17 * 1];
	double AXv[2 * 1];
	double Vxy[2 * 1];

	int iRepeat = 0;//迭代次数
	do
	{
		iRepeat++;
		if (iRepeat > 1000)
		{
			break;
			return false;
		}
		//清空矩阵
		memset(A, 0, 2 * 17 * sizeof(double));
		memset(AT, 0, 17 * 2 * sizeof(double));
		memset(ATL, 0, 17 * 1 * sizeof(double));
		memset(ATL_t, 0, 17 * 1 * sizeof(double));
		memset(ATA, 0, 17 * 17 * sizeof(double));
		memset(ATA_t, 0, 17 * 17 * sizeof(double));
		memset(L, 0, 2 * 1 * sizeof(double));
		memset(Xv, 0, 17 * 1 * sizeof(double));
		memset(AXv, 0, 2 * 1 * sizeof(double));
		memset(Vxy, 0, 2 * 1 * sizeof(double));

		//迭代器
		std::vector<PT_GCP>::iterator it = img_.begin();
		for (; it != img_.end(); it++)
		{
			double x = it->PT_X;
			double y = it->PT_Y;
			double X = it->GCP_X;
			double Y = it->GCP_Y;
			double Z = it->GCP_Z;

			double r2 = pow(x - x0, 2.0) + pow(y - y0, 2.0);
			double r4 = r2 * r2;

			double dltx = (x - x0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 + k4 * r4 * r4) + p1 * (r2 + 2 * (x - x0) * (x - x0)) + p2 * 2 * (x - x0) * (y - y0) + da * (x - x0) + db * (y - y0);
			double dlty = (y - y0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 + k4 * r4 * r4) + p2 * (r2 + 2 * (x - x0) * (x - x0)) + p1 * 2 * (x - x0) * (y - y0);

			double X_ = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs);
			double Y_ = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs);
			double Z_ = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs);

			double xj = x0 + dltx - f * X_ / Z_;//x的近似值
			double yj = y0 + dlty - f * Y_ / Z_;//y的近似值

			A[0 * 17 + 0] = 1 / Z_ * (a1 * f + a3 * (x - x0));
			A[0 * 17 + 1] = 1 / Z_ * (b1 * f + b3 * (x - x0));
			A[0 * 17 + 2] = 1 / Z_ * (c1 * f + c3 * (x - x0));

			A[0 * 17 + 3] = (y - y0) * sin(Omg) - ((x - x0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) + f * cos(Kap)) * cos(Omg);
			A[0 * 17 + 4] = -f * sin(Kap) - (x - x0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[0 * 17 + 5] = y - y0;
			A[0 * 17 + 6] = (x - x0) / f;
			A[0 * 17 + 7] = 1;
			A[0 * 17 + 8] = 0;

			A[0 * 17 + 9] = (x - x0) * r2;
			A[0 * 17 + 10] = (x - x0) * r2 * r2;
			A[0 * 17 + 11] = (x - x0) * r2 * r2 * r2;
			A[0 * 17 + 12] = (x - x0) * r4 * r4;
			A[0 * 17 + 13] = r2 + 2 * pow(x - x0, 2.0);
			A[0 * 17 + 14] = 2 * (x - x0) * (y - y0);
			A[0 * 17 + 15] = x - x0;
			A[0 * 17 + 16] = y - y0;


			A[1 * 17 + 0] = 1 / Z_ * (a2 * f + a3 * (y - y0));
			A[1 * 17 + 1] = 1 / Z_ * (b2 * f + b3 * (y - y0));
			A[1 * 17 + 2] = 1 / Z_ * (c2 * f + c3 * (y - y0));

			A[1 * 17 + 3] = -(x - x0) * sin(Omg) - ((y - y0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) - f * sin(Kap)) * cos(Omg);
			A[1 * 17 + 4] = -f * cos(Kap) - (y - y0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[1 * 17 + 5] = -(x - x0);
			A[1 * 17 + 6] = (y - y0) / f;
			A[1 * 17 + 7] = 0;
			A[1 * 17 + 8] = 1;

			A[1 * 17 + 9] = (y - y0) * r2;
			A[1 * 17 + 10] = (y - y0) * r2 * r2;
			A[1 * 17 + 11] = (y - y0) * r2 * r2 * r2;
			A[1 * 17 + 12] = (y - y0) * r4 * r4;
			A[1 * 17 + 13] = 2 * (x - x0) * (y - y0);
			A[1 * 17 + 14] = r2 + 2 * pow(y - y0, 2.0);
			A[1 * 17 + 15] = 0.0;
			A[1 * 17 + 16] = 0.0;

			L[0] = x - xj;
			L[1] = y - yj;

			//AT
			MatrixTra(2, 17, A, AT);

			//ATA+=ATA
			MatrixMul(AT, A, 17, 2, 17, ATA_t);
			MatrixSum(17, 17, 17, 17, ATA, ATA_t, ATA);

			//ATL+=ATL
			MatrixMul(AT, L, 17, 2, 1, ATL_t);
			MatrixSum(17, 1, 17, 1, ATL, ATL_t, ATL);

			//求中误差
			MatrixMul(A, Xv, 2, 17, 1, AXv);
			MatrixDiff(2, 1, 2, 1, AXv, L, Vxy);
			it->Vx = Vxy[0];
			it->Vy = Vxy[1];
		}
		//求解法方程
		MatrixInv(17, ATA);
		if (ATA == 0)return false;//矩阵求逆失败，方程已经无法收敛

		MatrixMul(ATA, ATL, 17, 17, 1, Xv);

		Xs += Xv[0];//更新数据
		Ys += Xv[1];
		Zs += Xv[2];
		Phi += Xv[3];
		Omg += Xv[4];
		Kap += Xv[5];
		f += Xv[6];
		x0 += Xv[7];
		y0 += Xv[8];
		k1 += Xv[9];
		k2 += Xv[10];
		k3 += Xv[11];
		k4 += Xv[12];
		p1 += Xv[13];
		p1 += Xv[14];
		da += Xv[15];
		db += Xv[16];

		a1 = cos(Phi) * cos(Kap) - sin(Phi) * sin(Omg) * sin(Kap);
		a2 = -cos(Phi) * sin(Kap) - sin(Phi) * sin(Omg) * cos(Kap);
		a3 = -sin(Phi) * cos(Omg);
		b1 = cos(Omg) * sin(Kap);
		b2 = cos(Omg) * cos(Kap);
		b3 = -sin(Omg);
		c1 = sin(Phi) * cos(Kap) + cos(Phi) * sin(Omg) * sin(Kap);
		c2 = -sin(Phi) * sin(Kap) + cos(Phi) * sin(Omg) * cos(Kap);
		c3 = cos(Phi) * cos(Omg);

	} while (Xv[3] < 0.00001 || Xv[4] < 0.00001 || Xv[5] < 0.00001);

	std::vector<PT_GCP>::iterator it = img_.begin();
	double toutle = 0;
	for (; it != img_.end(); it++)
		toutle = it->Vx * it->Vx + it->Vy * it->Vy;
	mer = sqrt(toutle / (img_.size()));
	return  true;
}

bool Manger::BA_K5(std::vector<PT_GCP>& img_)
{
	//系数矩阵
	double A[2 * 18];
	double AT[18 * 2];
	double ATL[18 * 1];
	double ATL_t[18 * 1];
	double ATA[18 * 18];
	double ATA_t[18 * 18];
	double L[2 * 1];
	double Xv[18 * 1];
	double AXv[2 * 1];
	double Vxy[2 * 1];

	int iRepeat = 0;//迭代次数
	do
	{
		iRepeat++;
		if (iRepeat > 1000)
		{
			break;
			return false;
		}
		//清空矩阵
		memset(A, 0, 2 * 18 * sizeof(double));
		memset(AT, 0, 18 * 2 * sizeof(double));
		memset(ATL, 0, 18 * 1 * sizeof(double));
		memset(ATL_t, 0, 18 * 1 * sizeof(double));
		memset(ATA, 0, 18 * 18 * sizeof(double));
		memset(ATA_t, 0, 18 * 18 * sizeof(double));
		memset(L, 0, 2 * 1 * sizeof(double));
		memset(Xv, 0, 18 * 1 * sizeof(double));
		memset(AXv, 0, 2 * 1 * sizeof(double));
		memset(Vxy, 0, 2 * 1 * sizeof(double));

		//迭代器
		std::vector<PT_GCP>::iterator it = img_.begin();
		for (; it != img_.end(); it++)
		{
			double x = it->PT_X;
			double y = it->PT_Y;
			double X = it->GCP_X;
			double Y = it->GCP_Y;
			double Z = it->GCP_Z;

			double r2 = pow(x - x0, 2.0) + pow(y - y0, 2.0);
			double r4 = r2 * r2;
			double r8 = r4 * r4;
			double dltx = (x - x0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 + k4 * r4 * r4 + k5 * r8 * r2) + p1 * (r2 + 2 * (x - x0) * (x - x0)) + p2 * 2 * (x - x0) * (y - y0) + da * (x - x0) + db * (y - y0);
			double dlty = (y - y0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 + k4 * r4 * r4 + k5 * r8 * r2) + p2 * (r2 + 2 * (x - x0) * (x - x0)) + p1 * 2 * (x - x0) * (y - y0);

			double X_ = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs);
			double Y_ = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs);
			double Z_ = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs);

			double xj = x0 + dltx - f * X_ / Z_;//x的近似值
			double yj = y0 + dlty - f * Y_ / Z_;//y的近似值

			A[0 * 18 + 0] = 1 / Z_ * (a1 * f + a3 * (x - x0));
			A[0 * 18 + 1] = 1 / Z_ * (b1 * f + b3 * (x - x0));
			A[0 * 18 + 2] = 1 / Z_ * (c1 * f + c3 * (x - x0));

			A[0 * 18 + 3] = (y - y0) * sin(Omg) - ((x - x0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) + f * cos(Kap)) * cos(Omg);
			A[0 * 18 + 4] = -f * sin(Kap) - (x - x0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[0 * 18 + 5] = y - y0;
			A[0 * 18 + 6] = (x - x0) / f;
			A[0 * 18 + 7] = 1;
			A[0 * 18 + 8] = 0;

			A[0 * 18 + 9] = (x - x0) * r2;
			A[0 * 18 + 10] = (x - x0) * r2 * r2;
			A[0 * 18 + 11] = (x - x0) * r2 * r2 * r2;
			A[0 * 18 + 12] = (x - x0) * r4 * r4;
			A[0 * 18 + 13] = (x - x0) * r8 * r2;
			A[0 * 18 + 14] = r2 + 2 * pow(x - x0, 2.0);
			A[0 * 18 + 15] = 2 * (x - x0) * (y - y0);
			A[0 * 18 + 16] = x - x0;
			A[0 * 18 + 17] = y - y0;


			A[1 * 18 + 0] = 1 / Z_ * (a2 * f + a3 * (y - y0));
			A[1 * 18 + 1] = 1 / Z_ * (b2 * f + b3 * (y - y0));
			A[1 * 18 + 2] = 1 / Z_ * (c2 * f + c3 * (y - y0));

			A[1 * 18 + 3] = -(x - x0) * sin(Omg) - ((y - y0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) - f * sin(Kap)) * cos(Omg);
			A[1 * 18 + 4] = -f * cos(Kap) - (y - y0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[1 * 18 + 5] = -(x - x0);
			A[1 * 18 + 6] = (y - y0) / f;
			A[1 * 18 + 7] = 0;
			A[1 * 18 + 8] = 1;

			A[1 * 18 + 9] = (y - y0) * r2;
			A[1 * 18 + 10] = (y - y0) * r2 * r2;
			A[1 * 18 + 11] = (y - y0) * r2 * r2 * r2;
			A[1 * 18 + 12] = (y - y0) * r4 * r4;
			A[1 * 18 + 13] = (y - y0) * r8 * r2;
			A[1 * 18 + 14] = 2 * (x - x0) * (y - y0);
			A[1 * 18 + 15] = r2 + 2 * pow(y - y0, 2.0);
			A[1 * 18 + 16] = 0.0;
			A[1 * 18 + 17] = 0.0;

			L[0] = x - xj;
			L[1] = y - yj;

			//AT
			MatrixTra(2, 18, A, AT);

			//ATA+=ATA
			MatrixMul(AT, A, 18, 2, 18, ATA_t);
			MatrixSum(18, 18, 18, 18, ATA, ATA_t, ATA);

			//ATL+=ATL
			MatrixMul(AT, L, 18, 2, 1, ATL_t);
			MatrixSum(18, 1, 18, 1, ATL, ATL_t, ATL);

			//求中误差
			MatrixMul(A, Xv, 2, 18, 1, AXv);
			MatrixDiff(2, 1, 2, 1, AXv, L, Vxy);
			it->Vx = Vxy[0];
			it->Vy = Vxy[1];
		}
		//求解法方程
		MatrixInv(18, ATA);
		if (ATA == 0)return false;//矩阵求逆失败，方程已经无法收敛

		MatrixMul(ATA, ATL, 18, 18, 1, Xv);

		Xs += Xv[0];//更新数据
		Ys += Xv[1];
		Zs += Xv[2];
		Phi += Xv[3];
		Omg += Xv[4];
		Kap += Xv[5];
		f += Xv[6];
		x0 += Xv[7];
		y0 += Xv[8];
		k1 += Xv[9];
		k2 += Xv[10];
		k3 += Xv[11];
		k4 += Xv[12];
		k5 += Xv[13];
		p1 += Xv[14];
		p1 += Xv[15];
		da += Xv[16];
		db += Xv[17];

		a1 = cos(Phi) * cos(Kap) - sin(Phi) * sin(Omg) * sin(Kap);
		a2 = -cos(Phi) * sin(Kap) - sin(Phi) * sin(Omg) * cos(Kap);
		a3 = -sin(Phi) * cos(Omg);
		b1 = cos(Omg) * sin(Kap);
		b2 = cos(Omg) * cos(Kap);
		b3 = -sin(Omg);
		c1 = sin(Phi) * cos(Kap) + cos(Phi) * sin(Omg) * sin(Kap);
		c2 = -sin(Phi) * sin(Kap) + cos(Phi) * sin(Omg) * cos(Kap);
		c3 = cos(Phi) * cos(Omg);

	} while (Xv[3] < 0.00001 || Xv[4] < 0.00001 || Xv[5] < 0.00001);

	std::vector<PT_GCP>::iterator it = img_.begin();
	double toutle = 0;
	for (; it != img_.end(); it++)
		toutle = it->Vx * it->Vx + it->Vy * it->Vy;
	mer = sqrt(toutle / ( img_.size()));
	return  true;
}

bool Manger::BA_K6(std::vector<PT_GCP>& img_)
{
	//系数矩阵
	double A[2 * 19];
	double AT[19 * 2];
	double ATL[19 * 1];
	double ATL_t[19 * 1];
	double ATA[19* 19];
	double ATA_t[19 * 19];
	double L[2 * 1];
	double Xv[19 * 1];
	double AXv[2 * 1];
	double Vxy[2 * 1];

	int iRepeat = 0;//迭代次数
	do
	{
		iRepeat++;
		if (iRepeat > 1000)
		{
			break;
			return false;
		}
		//清空矩阵
		memset(A, 0, 2 * 19 * sizeof(double));
		memset(AT, 0, 19 * 2 * sizeof(double));
		memset(ATL, 0, 19 * 1 * sizeof(double));
		memset(ATL_t, 0, 19 * 1 * sizeof(double));
		memset(ATA, 0, 19 * 19 * sizeof(double));
		memset(ATA_t, 0, 19 * 19 * sizeof(double));
		memset(L, 0, 2 * 1 * sizeof(double));
		memset(Xv, 0, 19 * 1 * sizeof(double));
		memset(AXv, 0, 2 * 1 * sizeof(double));
		memset(Vxy, 0, 2 * 1 * sizeof(double));

		//迭代器
		std::vector<PT_GCP>::iterator it = img_.begin();
		for (; it != img_.end(); it++)
		{
			double x = it->PT_X;
			double y = it->PT_Y;
			double X = it->GCP_X;
			double Y = it->GCP_Y;
			double Z = it->GCP_Z;

			double r2 = pow(x - x0, 2.0) + pow(y - y0, 2.0);
			double r4 = r2 * r2;
			double r8 = r4 * r4;
			double dltx = (x - x0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 + k4 * r4 * r4 + k5 * r8 * r2 + k6 * r8 * r4) + p1 * (r2 + 2 * (x - x0) * (x - x0)) + p2 * 2 * (x - x0) * (y - y0) + da * (x - x0) + db * (y - y0);
			double dlty = (y - y0) * (k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 + k4 * r4 * r4 + k5 * r8 * r2 + k6 * r8 * r4) + p2 * (r2 + 2 * (x - x0) * (x - x0)) + p1 * 2 * (x - x0) * (y - y0);

			double X_ = a1 * (X - Xs) + b1 * (Y - Ys) + c1 * (Z - Zs);
			double Y_ = a2 * (X - Xs) + b2 * (Y - Ys) + c2 * (Z - Zs);
			double Z_ = a3 * (X - Xs) + b3 * (Y - Ys) + c3 * (Z - Zs);

			double xj = x0 + dltx - f * X_ / Z_;//x的近似值
			double yj = y0 + dlty - f * Y_ / Z_;//y的近似值

			A[0 * 19 + 0] = 1 / Z_ * (a1 * f + a3 * (x - x0));
			A[0 * 19 + 1] = 1 / Z_ * (b1 * f + b3 * (x - x0));
			A[0 * 19 + 2] = 1 / Z_ * (c1 * f + c3 * (x - x0));

			A[0 * 19 + 3] = (y - y0) * sin(Omg) - ((x - x0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) + f * cos(Kap)) * cos(Omg);
			A[0 * 19 + 4] = -f * sin(Kap) - (x - x0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[0 * 19 + 5] = y - y0;
			A[0 * 19 + 6] = (x - x0) / f;
			A[0 * 19 + 7] = 1;
			A[0 * 19 + 8] = 0;

			A[0 * 19 + 9] = (x - x0) * r2;
			A[0 * 19 + 10] = (x - x0) * r2 * r2;
			A[0 * 19 + 11] = (x - x0) * r2 * r2 * r2;
			A[0 * 19 + 12] = (x - x0) * r4 * r4;
			A[0 * 19 + 13] = (x - x0) * r8 * r2;
			A[0 * 19 + 14] = (x - x0) * r8 * r4;
			A[0 * 19 + 15] = r2 + 2 * pow(x - x0, 2.0);
			A[0 * 19 + 16] = 2 * (x - x0) * (y - y0);
			A[0 * 19 + 17] = x - x0;
			A[0 * 19 + 18] = y - y0;


			A[1 * 19 + 0] = 1 / Z_ * (a2 * f + a3 * (y - y0));
			A[1 * 19 + 1] = 1 / Z_ * (b2 * f + b3 * (y - y0));
			A[1 * 19 + 2] = 1 / Z_ * (c2 * f + c3 * (y - y0));

			A[1 * 19 + 3] = -(x - x0) * sin(Omg) - ((y - y0) / f * ((x - x0) * cos(Kap) - (y - y0) * sin(Kap)) - f * sin(Kap)) * cos(Omg);
			A[1 * 19 + 4] = -f * cos(Kap) - (y - y0) / f * ((x - x0) * sin(Kap) + (y - y0) * cos(Kap));

			A[1 * 19 + 5] = -(x - x0);
			A[1 * 19 + 6] = (y - y0) / f;
			A[1 * 19 + 7] = 0;
			A[1 * 19 + 8] = 1;

			A[1 * 19 + 9] = (y - y0) * r2;
			A[1 * 19 + 10] = (y - y0) * r2 * r2;
			A[1 * 19 + 11] = (y - y0) * r2 * r2 * r2;
			A[1 * 19 + 12] = (y - y0) * r4 * r4;
			A[1 * 19 + 13] = (y - y0) * r8 * r2;
			A[1 * 19 + 14] = (y - y0) * r8 * r4;
			A[1 * 19 + 15] = 2 * (x - x0) * (y - y0);
			A[1 * 19 + 16] = r2 + 2 * pow(y - y0, 2.0);
			A[1 * 19 + 17] = 0.0;
			A[1 * 19 + 18] = 0.0;

			L[0] = x - xj;
			L[1] = y - yj;

			//AT
			MatrixTra(2, 19, A, AT);

			//ATA+=ATA
			MatrixMul(AT, A, 19, 2, 19, ATA_t);
			MatrixSum(19, 19, 19, 19, ATA, ATA_t, ATA);

			//ATL+=ATL
			MatrixMul(AT, L, 19, 2, 1, ATL_t);
			MatrixSum(19, 1, 19, 1, ATL, ATL_t, ATL);

			//求中误差
			MatrixMul(A, Xv, 2, 19, 1, AXv);
			MatrixDiff(2, 1, 2, 1, AXv, L, Vxy);
			it->Vx = Vxy[0];
			it->Vy = Vxy[1];
		}
		//求解法方程
		MatrixInv(19, ATA);
		if (ATA == 0)return false;//矩阵求逆失败，方程已经无法收敛

		MatrixMul(ATA, ATL, 19, 19, 1, Xv);

		Xs += Xv[0];//更新数据
		Ys += Xv[1];
		Zs += Xv[2];
		Phi += Xv[3];
		Omg += Xv[4];
		Kap += Xv[5];
		f += Xv[6];
		x0 += Xv[7];
		y0 += Xv[8];
		k1 += Xv[9];
		k2 += Xv[10];
		k3 += Xv[11];
		k4 += Xv[12];
		k5 += Xv[13];
		k6 += Xv[14];
		p1 += Xv[15];
		p1 += Xv[16];
		da += Xv[17];
		db += Xv[18];

		a1 = cos(Phi) * cos(Kap) - sin(Phi) * sin(Omg) * sin(Kap);
		a2 = -cos(Phi) * sin(Kap) - sin(Phi) * sin(Omg) * cos(Kap);
		a3 = -sin(Phi) * cos(Omg);
		b1 = cos(Omg) * sin(Kap);
		b2 = cos(Omg) * cos(Kap);
		b3 = -sin(Omg);
		c1 = sin(Phi) * cos(Kap) + cos(Phi) * sin(Omg) * sin(Kap);
		c2 = -sin(Phi) * sin(Kap) + cos(Phi) * sin(Omg) * cos(Kap);
		c3 = cos(Phi) * cos(Omg);

	} while (Xv[3] < 0.00001 || Xv[4] < 0.00001 || Xv[5] < 0.00001);

	std::vector<PT_GCP>::iterator it = img_.begin();
	double toutle = 0;
	for (; it != img_.end(); it++)
		toutle = it->Vx * it->Vx + it->Vy * it->Vy;
	mer = sqrt(toutle / ( img_.size()));
	return true;
}

void test(Manger&p)
{
	std::vector<PT_GCP>test;
	std::fstream fp("D:\\6_3.txt", std::ios::out);
	if (!fp)std::cerr << "error!!!";
	fp << "控制点：X\t\t\t\tY\t\t\t\tZ\t\t像点：x\t\t\t\ty\t\t反求的:x\t\t\ty\n";
	for (size_t i = 0; i < p.PT_GCP_XYZ.size(); i++)
	{
		double x, y;
		double X, Y, Z;
		X = p.PT_GCP_XYZ[i].GCP_X;
		Y = p.PT_GCP_XYZ[i].GCP_Y;
		Z = p.PT_GCP_XYZ[i].GCP_Z;
		x = -(p.L1 * X + p.L2 * Y + p.L3 * Z + p.L4) / (p.L9 * X + p.L10 * Y + p.L11 * Z + 1);
		y = -(p.L5 * X + p.L6 * Y + p.L7 * Z + p.L8) / (p.L9 * X + p.L10 * Y + p.L11 * Z + 1);
		fp << std::setw(10) << X << "\t" << Y << "\t" << Z << "\t\t" << p.PT_GCP_XYZ[i].PT_X << "\t" << p.PT_GCP_XYZ[i].PT_Y << "\t\t" << x << "\t" << y << "\n";
	}
	fp.close();
}

bool Manger::ceres_BA()//用ceres解求BA
{
	OpenFile_PT("PT_不含粗差.txt");
	//OpenFile_PT("PT_含粗差.txt");
	OpenFile_GCP("GCP_XH.txt");

	Matching();

	BLT(PT_GCP_XYZ, Lp);

	InnitValue(Lp);
	std::cout << "初值\nXs:" << Xs << "\nYs:" << Ys << "\nZs:" << Zs << "\nPhi: " << Phi << "\nOmg:" << Omg << "\nKap:" << Kap << "\nf:" << f << "\nx0:" << x0 << "\ny0:" << y0<<"\n";

	double camera_K[3], image_Rt[6], camera_dp[6], out_residuals[2] = {};
	camera_K[0] = f;
	camera_K[1] = x0;
	camera_K[2] = y0;

	image_Rt[0] = Phi;
	image_Rt[1] = Omg;
	image_Rt[2] = Kap;
	image_Rt[3] = Xs;
	image_Rt[4] = Ys;
	image_Rt[5] = Zs;

	camera_dp[0] = k1;
	camera_dp[1] = k2;
	camera_dp[2] = p1;
	camera_dp[3] = p2;
	camera_dp[4] = da;
	camera_dp[5] = db;

	ceres::Problem problem;
	for (int i = 0; i < PT_GCP_XYZ.size(); ++i) {
		ceres::CostFunction* cost_function =
			new ceres::AutoDiffCostFunction<res_cen_K2_BA, 2, 3, 6, 6>(
				new res_cen_K2_BA(PT_GCP_XYZ[i].PT_X, PT_GCP_XYZ[i].PT_Y, PT_GCP_XYZ[i].GCP_X, PT_GCP_XYZ[i].GCP_Y, PT_GCP_XYZ[i].GCP_Z));
		problem.AddResidualBlock(cost_function, nullptr, camera_K, image_Rt,camera_dp);
		/*problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(0.5), camera_K, image_Rt, camera_dp);*/
	}

	ceres::Solver::Options options;
	options.max_num_iterations = 100;
	options.linear_solver_type = ceres::DENSE_QR;
	options.minimizer_progress_to_stdout = true;
	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	f = camera_K[0];
	x0 = camera_K[1];
	y0 = camera_K[2];

	Phi = image_Rt[0];
	Omg = image_Rt[1];
	Kap = image_Rt[2];
	Xs = image_Rt[3];
	Ys = image_Rt[4];
	Zs = image_Rt[5];

	k1 = camera_dp[0];
	k2 = camera_dp[1];
	p1 = camera_dp[2];
	p2 = camera_dp[3];
	da = camera_dp[4];
	db = camera_dp[5];

	if (summary.termination_type == ceres::TerminationType::CONVERGENCE)
	{
		return  true;
	}
	return false;
}