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);//BLTvoid 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解求BAvoid 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;//ATMatrixTra(2, 11, A, AT);//ATA+=ATAMatrixMul(AT, A, 11, 2 , 11,ATA_t);MatrixSum(11, 11, 11, 11, ATA, ATA_t, ATA);//ATL+=ATLMatrixMul(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;//ATMatrixTra(2, 15, A, AT);//ATA+=ATAMatrixMul(AT, A, 15, 2, 15, ATA_t);MatrixSum(15, 15, 15, 15, ATA, ATA_t, ATA);//ATL+=ATLMatrixMul(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;//ATMatrixTra(2, 16, A, AT);//ATA+=ATAMatrixMul(AT, A, 16, 2, 16, ATA_t);MatrixSum(16, 16, 16, 16, ATA, ATA_t, ATA);//ATL+=ATLMatrixMul(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;//ATMatrixTra(2, 17, A, AT);//ATA+=ATAMatrixMul(AT, A, 17, 2, 17, ATA_t);MatrixSum(17, 17, 17, 17, ATA, ATA_t, ATA);//ATL+=ATLMatrixMul(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;//ATMatrixTra(2, 18, A, AT);//ATA+=ATAMatrixMul(AT, A, 18, 2, 18, ATA_t);MatrixSum(18, 18, 18, 18, ATA, ATA_t, ATA);//ATL+=ATLMatrixMul(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;//ATMatrixTra(2, 19, A, AT);//ATA+=ATAMatrixMul(AT, A, 19, 2, 19, ATA_t);MatrixSum(19, 19, 19, 19, ATA, ATA_t, ATA);//ATL+=ATLMatrixMul(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;
}

Matrix.h

#pragma oncevoid MatrixTra(int m, int n, double* A, double* AT);    //矩阵转置
void MatrixMul(double* a, double* b, int m, int n, int k, double* c);  //a(mXn) * b(nXk) = c(mXk)
int  MatrixInv(int n, double* a);    //求 nXn 的矩阵 a  的逆矩阵
void MatrixSum(int Am, int An, int Bm, int Bn, double* A, double* B, double* R); //R = A + B
void MatrixDiff(int Am, int An, int Bm, int Bn, double* A, double* B, double* R);//R = A - B

Matrix.cpp


#include "Matrix.h"
#include <math.h>
#include<stdlib.h>
#include<stdio.h>//矩阵求逆
int MatrixInv(int n, double* a)
{int* is, * js, i, j, k, l, u, v;double d, p;is = (int*)malloc(n * sizeof(int));js = (int*)malloc(n * sizeof(int));for (k = 0; k <= n - 1; k++){d = 0.0;for (i = k; i <= n - 1; i++)for (j = k; j <= n - 1; j++){l = i * n + j; p = fabs(a[l]);if (p > d) { d = p; is[k] = i; js[k] = j; }}if (d + 1.0 == 1.0){free(is); free(js); printf("err**not inv\n");return(0);}if (is[k] != k)for (j = 0; j <= n - 1; j++){u = k * n + j; v = is[k] * n + j;p = a[u]; a[u] = a[v]; a[v] = p;}if (js[k] != k)for (i = 0; i <= n - 1; i++){u = i * n + k; v = i * n + js[k];p = a[u]; a[u] = a[v]; a[v] = p;}l = k * n + k;a[l] = 1.0 / a[l];for (j = 0; j <= n - 1; j++)if (j != k){u = k * n + j; a[u] = a[u] * a[l];}for (i = 0; i <= n - 1; i++)if (i != k)for (j = 0; j <= n - 1; j++)if (j != k){u = i * n + j;a[u] = a[u] - a[i * n + k] * a[k * n + j];}for (i = 0; i <= n - 1; i++)if (i != k){u = i * n + k; a[u] = -a[u] * a[l];}}for (k = n - 1; k >= 0; k--){if (js[k] != k)for (j = 0; j <= n - 1; j++){u = k * n + j; v = js[k] * n + j;p = a[u]; a[u] = a[v]; a[v] = p;}if (is[k] != k)for (i = 0; i <= n - 1; i++){u = i * n + k; v = i * n + is[k];p = a[u]; a[u] = a[v]; a[v] = p;}}free(is); free(js);return(1);
}//矩阵求积
void MatrixMul(double* a, double* b, int m, int n, int k, double* c)
{int i, j, l, u;for (i = 0; i <= m - 1; i++)for (j = 0; j <= k - 1; j++){u = i * k + j; c[u] = 0.0;for (l = 0; l <= n - 1; l++)c[u] = c[u] + a[i * n + l] * b[l * k + j];}return;
}//矩阵转置
void MatrixTra(int m, int n, double* A, double* AT)
{int i, j;for (i = 0; i < m; i++)for (j = 0; j < n; j++)AT[j * m + i] = A[i * n + j];
}//矩阵求和
void MatrixSum(int Am, int An, int Bm, int Bn, double* A, double* B, double* R)
{int r = Am;int c = An;int n = r * c, i;if (Am != Bm || An != Bn) {printf("[matrix_sum] Error: mismatched dimensions\n");return;}for (i = 0; i < n; i++) {R[i] = A[i] + B[i];}
}//矩阵求差 R = A - B
void MatrixDiff(int Am, int An, int Bm, int Bn, double* A, double* B, double* R)
{int r = Am;int c = An;int n = r * c, i;if (Am != Bm || An != Bn) {printf("[matrix_sum] Error: mismatched dimensions\n");return;}for (i = 0; i < n; i++) {R[i] = A[i] - B[i];}
}

Main.cpp

#include"Manger.h"int main()
{Manger p;p.ceres_BA();p.calculate();//不含粗差计算p.calculate_K3();//K3次项径向畸变模型，不含粗差计算p.calculate_K4();//K4次项径向畸变模型，不含粗差计算p.calculate_K5();//K5次项径向畸变模型，不含粗差计算p.calculate_K6();//K6次项径向畸变模型，不含粗差计算p.calculate_c();//含粗差计算test(p);//测试，反向验证精度return  0;
}

2021-07-22相机检校相关推荐

2021.07.22禾赛提前批一面面经
2021.07.22禾赛提前批一面面经 1.LUTRAM的意思 2.LUTRAM和block RAM的区别 3.时序约束和时序优化 4.跨时钟域 5.为什么不能多bit采用同步寄存器打两拍(就这个问题 ...
OpenCV学习笔记#002 OpenCV相机检校例程运行
OpenCV学习笔记#002 OpenCV相机检校例程运行 OpenCV学习笔记#002 OpenCV相机检校例程运行所需文件修改文件修改in_VID5.xml 修改VID5.xml VS 设置 ...
2021.07.22
并行和并发并行:多个cpu同时执行多个任务,比如:多人同时做不同的时并发:一个cpu(采用时间片)同时执行多个任务.比如秒杀,多个人做同一件事. 优点背景:以单核CPU为例,只使用单个线程完成多 ...
哈工大自动出校申请（selenium+chromedriver）（2021.3.22更新）
哈工大自动出校申请注意事项安装selenium 安装chromedriver 代码修改 2021.3.22更新 selenium 安装与 chromedriver安装可以参考以下: https:/ ...
训练实录 | 2021牛客暑期多校训练营2
2021牛客暑期多校训练营2 传送门 C - Draw Grids solved by oye. 00:22:07(+) 题意: 给n,m两个数,表示有n行点和m列点,已知每次只能连两个相邻的点(斜的 ...
2021牛客暑期多校训练营
2021牛客暑期多校训练营1 A- Alice and Bob /* 博弈问题两堆石头,每人每次从其中一堆拿k(k>0)个, 同时从另一堆拿s*k个(s>=0),不能进行操作的即失败 * ...
2021牛客暑期多校训练营2，签到题CDFKI
2021牛客暑期多校训练营2 题号标题已通过代码通过率团队的状态 A Arithmetic Progression 点击查看 6/72 未通过 B Cannon 点击查看 34/104 未通过 ...
2021牛客暑期多校训练营1, 签到题DFBG
2021牛客暑期多校训练营1 题号标题已通过代码通过率团队的状态 A Alice and Bob 点击查看 1365/5586 通过(博弈论-打表) B Ball Dropping 点击查看 ...
哔哩哔哩“2021.07.13 我们是这样崩的”报告的学习-1
哔哩哔哩"2021.07.13 我们是这样崩的"报告的学习-1 这份报告是我学计算机两年来第一次真实看到大厂的员工到底在干什么.出现了很多专有名词,以及当前最先进的互联网企业的应用 ...

2021-07-22相机检校

2021-07-22相机检校相关推荐

最新文章

热门文章