/////////单像后方交会/////////
int num = PointNum1.size();//点的个数
double a1, a2, a3, b1, b2, b3, c1, c2, c3;
double Q = 0.0;
Mat A(2, 13, CV_64FC1, Scalar(0.0));
Mat At(13, 2, CV_64FC1, Scalar(0.0));
Mat AtA(13, 13, CV_64FC1, Scalar(0.0));
Mat AtL(13, 1, CV_64FC1, Scalar(0.0));
Mat NAtA(13, 13, CV_64FC1, Scalar(0.0));
Mat L(2, 1, CV_64FC1, Scalar(0.0));
Mat XX(13, 1, CV_64FC1);//误差方程式和法方程式
double* LL = new double[2 * num];
double Qii[13] = { 0.0 };
double mXs, mYs, mZs, mphi, momega, mkappa, sum = 0.0;
double mf, mx, my;
double mk1, mk2, mp1, mp2;
int times = 0;
double phi = (30.0 * Pi) / 180.0, omega = 0.0, kappa = 0.0; //外方位角元素
double Xs = 1000.0, Ys = -30.0, Zs = -2000.0; //外方位线元素
double x0 = 0.0, y0 = 0.0, f = 23; //内方位元素
double k1 = 0.0, k2 = 0.0, p1 = 0.0, p2 = 0.0;//畸变改正数
double datax, datay;
do
{
a1 = cos(phi) * cos(kappa) - sin(phi) * sin(omega) * sin(kappa);
a2 = -cos(phi) * sin(kappa) - sin(phi) * sin(omega) * cos(kappa);
a3 = -sin(phi) * cos(omega);
b1 = cos(omega) * sin(kappa);
b2 = cos(omega) * cos(kappa);
b3 = -sin(omega);
c1 = sin(phi) * cos(kappa) + cos(phi) * sin(omega) * sin(kappa);
c2 = -sin(phi) * sin(kappa) + cos(phi) * sin(omega) * cos(kappa);
c3 = cos(phi) * cos(omega);
Mat AtA1(13, 13, CV_64FC1, Scalar(0.0));
Mat AtA1_inv(13, 11, CV_64FC1, Scalar(0.0));
Mat AtL1(13, 1, CV_64FC1, Scalar(0.0));
for (int i = 0; i < num; i++)
{
double rr = (x.at(i) - x0) * (x.at(i) - x0) + (y.at(i) - y0) * (y.at(i) - y0);//定义常数项
double Xp = a1 * (X.at(i) - Xs) + b1 * (Y.at(i) - Ys) + c1 * (Z.at(i) - Zs);
double Yp = a2 * (X.at(i) - Xs) + b2 * (Y.at(i) - Ys) + c2 * (Z.at(i) - Zs);
double Zp = a3 * (X.at(i) - Xs) + b3 * (Y.at(i) - Ys) + c3 * (Z.at(i) - Zs);
A.at<double>(0, 0) = 1.0 * (a1 * f + a3 * (x.at(i) - x0)) / Zp;
A.at<double>(0, 1) = 1.0 * (b1 * f + b3 * (x.at(i) - x0)) / Zp;
A.at<double>(0, 2) = 1.0 * (c1 * f + c3 * (x.at(i) - x0)) / Zp;
A.at<double>(0, 3) = (y.at(i) - y0) * sin(omega) - ((x.at(i) - x0) * ((x.at(i) - x0) * cos(kappa) - (y.at(i) - y0) * sin(kappa)) / f + f * cos(kappa)) * cos(omega);
A.at<double>(0, 4) = -f * sin(kappa) - (x.at(i) - x0) * ((x.at(i) - x0) * sin(kappa) + (y.at(i) - y0) * cos(kappa)) / f;
A.at<double>(0, 5) = (y.at(i) - y0);
A.at<double>(0, 6) = (x.at(i) - x0) / f;
A.at<double>(0, 7) = 1.0;
A.at<double>(0, 8) = 0.0;
A.at<double>(0, 9) = -(x.at(i) - x0) * rr;
A.at<double>(0, 10) = -(x.at(i) - x0) * rr * rr;
A.at<double>(0, 11) = -rr - 2 * (x.at(i) - x0) * (x.at(i) - x0);
A.at<double>(0, 12) = -2 * (x.at(i) - x0) * (y.at(i) - y0);
A.at<double>(1, 0) = 1.0 * (a2 * f + a3 * (y.at(i) - y0)) / Zp;
A.at<double>(1, 1) = 1.0 * (b2 * f + b3 * (y.at(i) - y0)) / Zp;
A.at<double>(1, 2) = 1.0 * (c2 * f + c3 * (y.at(i) - y0)) / Zp;
A.at<double>(1, 3) = -(x.at(i) - x0) * sin(omega) - ((y.at(i) - y0) * ((x.at(i) - x0) * cos(kappa) - (y.at(i) - y0) * sin(kappa)) / f - f * sin(kappa)) * cos(omega);
A.at<double>(1, 4) = -f * cos(kappa) - (y.at(i) - y0) * ((x.at(i) - x0) * sin(kappa) + (y.at(i) - y0) * cos(kappa)) / f;
A.at<double>(1, 5) = -(x.at(i) - x0);
A.at<double>(1, 6) = (y.at(i) - y0) / f;
A.at<double>(1, 7) = 0.0;
A.at<double>(1, 8) = 1.0;
A.at<double>(1, 9) = -(y.at(i) - y0) * rr;
A.at<double>(1, 10) = -(y.at(i) - y0) * rr * rr;
A.at<double>(1, 11) = -2 * (x.at(i) - x0) * (y.at(i) - y0);
A.at<double>(1, 12) = -rr - 2 * (y.at(i) - y0) * (y.at(i) - y0);
datax = (x.at(i) - x0) * (k1 * rr + k2 * rr * rr) + p1 * (rr + 2 * (x.at(i) - x0) * (x.at(i) - x0)) + 2 * p2 * (x.at(i) - x0) * (y.at(i) - y0);
datay = (y.at(i) - y0) * (k1 * rr + k2 * rr * rr) + p2 * (rr + 2 * (y.at(i) - y0) * (y.at(i) - y0)) + 2 * p1 * (x.at(i) - x0) * (y.at(i) - y0);
L.at<double>(0, 0) = x.at(i) + f * Xp / Zp - x0 + datax;
L.at<double>(1, 0) = y.at(i) + f * Yp / Zp - y0 + datay;
At = A.t();
AtA = At * A;
AtL = At * L;
AtA1 += AtA;
AtL1 += AtL;
LL[2 * i] = L.at<double>(0, 0);
LL[2 * i + 1] = L.at<double>(1, 0);
}
AtA1_inv = AtA1.inv();
XX = AtA1_inv * AtL1;
for (int i = 0; i < 13; i++)
{
Qii[i] = AtA1_inv.at<double>(i, i);
}
Xs += XX.at<double>(0, 0);
Ys += XX.at<double>(1, 0);
Zs += XX.at<double>(2, 0);
phi += XX.at<double>(3, 0);
omega += XX.at<double>(4, 0);
kappa += XX.at<double>(5, 0);
f += XX.at<double>(6, 0);
x0 += XX.at<double>(7, 0);
y0 += XX.at<double>(8, 0);
k1 += XX.at<double>(9, 0);
k2 += XX.at<double>(10, 0);
p1 += XX.at<double>(11, 0);
p2 += XX.at<double>(12, 0);
times += 1;
} while ((fabs(XX.at<double>(0, 0)) > 0.000001 || fabs(XX.at<double>(1, 0)) > 0.000001 || fabs(XX.at<double>(2, 0)) > 0.000001
|| fabs(XX.at<double>(3, 0)) > 0.0000001 || fabs(XX.at<double>(4, 0)) > 0.0000001 || fabs(XX.at<double>(5, 0)) > 0.0000001
|| fabs(XX.at<double>(6, 0)) > 0.000001 || fabs(XX.at<double>(7, 0)) > 0.000001 || fabs(XX.at<double>(8, 0)) > 0.000001
|| fabs(XX.at<double>(9, 0)) > 0.000001 || fabs(XX.at<double>(10, 0)) > 0.000001 || fabs(XX.at<double>(11, 0)) > 0.000001
|| fabs(XX.at<double>(12, 0)) > 0.000001));
for (int i = 0; i < 2 * num; i++)
{
Q += LL[i] * LL[i];
}
Q = sqrt(Q / (2 * num - 6));
mXs = sqrt(Qii[0]) * Q;
mYs = sqrt(Qii[1]) * Q;
mZs = sqrt(Qii[2]) * Q;
mphi = sqrt(Qii[3]) * Q;
momega = sqrt(Qii[4]) * Q;
mkappa = sqrt(Qii[5]) * Q;
mf = sqrt(Qii[6]) * Q;
mx = sqrt(Qii[7]) * Q / 0.0051966;
my = sqrt(Qii[8]) * Q / 0.0051966;
mk1 = sqrt(Qii[9]) * Q;
mk2 = sqrt(Qii[10]) * Q;
mp1 = sqrt(Qii[11]) * Q;
mp2 = sqrt(Qii[12]) * Q;
FILE* fResult;
fopen_s(&fResult,"Result.txt", "w");
fprintf(fResult, "单片后方空间交会结果如下:\n\n");
fprintf(fResult, "*****************************************************************************\n\n");
fprintf(fResult, "迭代次数为：%d\n", ×);
fprintf(fResult, "计算结果如下：\n");
fprintf(fResult, "单位权中误差为：±%lf\n", &Q);
fprintf(fResult, "外方位元素：\n");
fprintf(fResult, "Xs=%lf, ±%lf mm\n", &Xs, &mXs);
fprintf(fResult, "Ys=%lf, ±%lf mm\n", &Ys, &mYs);
fprintf(fResult, "Zs=%lf, ±%lf mm\n", &Zs, &mZs);
fprintf(fResult, "phi=%lf, ±%lf rad\n", &phi, &mphi);
fprintf(fResult, "omega=%lf, ±%lf rad\n", &omega, &momega);
fprintf(fResult, "kappa=%lf, ±%lf rad\n", &kappa, &mkappa);
fprintf(fResult, "内方位元素：\n");
fprintf(fResult, "f=%lf, ±%lf mm\n", &f, &mf);
fprintf(fResult, "x0=%lf, ±%lf pixel\n", &x0, &mx);
fprintf(fResult, "y0=%lf, ±%lf pixel\n", &y0, &my);
fprintf(fResult, "畸变改正系数：\n");
fprintf(fResult, "k1=%lf, ±%lf \n", &k1, &mk1);
fprintf(fResult, "k2=%lf, ±%lf \n", &k2, &mk2);
fprintf(fResult, "p1=%lf, ±%lf \n", &p1, &mp1);
fprintf(fResult, "p2=%lf, ±%lf \n", &p2, &mp2);
cout << "结果已输出至文档中!" << endl;
https://blog.csdn.net/liuyujie3229166/article/details/106888245/