单向空间后方交会C++代码实现

摄影测量作业
空间后方交会的代码
各位小伙伴要用的话只需修改main函数里边的像点地面点坐标，摄影比例尺分母，焦距等这些已知元素就行。

#include<iostream>
#include <iomanip>
#include<string.h>
#include<math.h>
using namespace std;
void PrintArray(double* a, int b, int c);//打印矩阵
void TranspositionArray(double* a, double* aT, int b, int c);//转置矩阵
void MultiplyArray(double* a, double* b, double* c, int m, int n, int l);//矩阵相乘
void make2array(double**& a, int n);
void deletarray(double**& a, int n);
int ConverseArray(double* ip, double* rp, int n);//矩阵求逆
void SubtratArray(double* a, double* b, double* c, int m, int n);//矩阵相减int main() {//像点坐标和地面点坐标double IroundControl[4][2] = { -86.15,-68.99,-53.40,82.21,-14.78,-76.63,10.46,64.43 };double GroundControl[4][3] = { 36589.41, 25273.32,2195.17,37631.08,31324.51,728.69,39100.97,24934.98,2386.50,40426.54,30319.81,757.31 };//摄影比例尺分母double scale = 40000.0;double f = 0.15324;//转换单位for (int i = 0; i < 4; i++){for (int j = 0; j < 2; j++){IroundControl[i][j] /= 1000.0;}}//定义外方位元素double Xs = 0.0, Ys = 0.0, Zs = 0.0, pitch = 0.0, roll = 0.0, yaw = 0.0;//外方位元素的改正数double delta[6] = { 0.0 };//定义旋转矩阵double Rotate[3][3] = { 0.0 };Xs = (GroundControl[0][0] + GroundControl[1][0] + GroundControl[2][0] + GroundControl[3][0]) / 4.0;Ys = (GroundControl[0][1] + GroundControl[1][1] + GroundControl[2][1] + GroundControl[3][1]) / 4.0;Zs = scale * f;//打印外方位元素的初始值cout << "外方位元素的初始值为：" << endl;cout << Xs << "   " << Ys << "   " << Zs << endl;cout << pitch << "   " << roll << "   " << yaw << endl;double x[4] = { 0 }, y[4] = { 0 }, A[8][6] = { 0 }, AT[6][8] = { 0 }, ATA[6][6] = { 0 }, ATAInv[6][6] = { 0 }, LL[8] = { 0 }, ATAAT[6][8] = { 0 };double v[8] = { 0 }, m0 = 0, m[6] = { 0 }, AX[8] = { 0 }, vv = 0;int n = 0;//累计循环次数while (1){//旋转矩阵Rotate[0][0] = cos(pitch) * cos(yaw) - sin(pitch) * sin(roll) * sin(yaw);Rotate[0][1] = (-1.0) * cos(pitch) * sin(yaw) - sin(pitch) * sin(roll) * cos(yaw);Rotate[0][2] = (-1.0) * sin(pitch) * cos(roll);Rotate[1][0] = cos(roll) * sin(yaw);Rotate[1][1] = cos(roll) * cos(yaw);Rotate[1][2] = (-1.0) * sin(roll);Rotate[2][0] = sin(pitch) * cos(yaw) + cos(pitch) * sin(roll) * sin(yaw);Rotate[2][1] = (-1.0) * sin(pitch) * sin(yaw) + cos(pitch) * sin(roll) * cos(yaw);Rotate[2][2] = cos(pitch) * cos(roll);for (int i = 0; i < 4; i++){x[i] = (-1.0) * f * (Rotate[0][0] * (GroundControl[i][0] - Xs) + Rotate[1][0] * (GroundControl[i][1] - Ys)+ Rotate[2][0] * (GroundControl[i][2] - Zs)) / (Rotate[0][2] * (GroundControl[i][0] - Xs)+ Rotate[1][2] * (GroundControl[i][1] - Ys) + Rotate[2][2] * (GroundControl[i][2] - Zs));y[i] = (-1.0) * f * (Rotate[0][1] * (GroundControl[i][0] - Xs) + Rotate[1][1] * (GroundControl[i][1] - Ys)+ Rotate[2][1] * (GroundControl[i][2] - Zs)) / (Rotate[0][2] * (GroundControl[i][0] - Xs)+ Rotate[1][2] * (GroundControl[i][1] - Ys) + Rotate[2][2] * (GroundControl[i][2] - Zs));LL[i * 2] = IroundControl[i][0] - x[i];LL[i * 2 + 1] = IroundControl[i][1] - y[i];A[i * 2][0] = (-1.0) * f / (Zs - GroundControl[i][2]);A[i * 2][1] = 0.0;A[i * 2][2] = (-1.0) * x[i] / (Zs - GroundControl[i][2]);A[i * 2][3] = (-1.0) * f * (1 + (x[i] * x[i]) / (f * f));A[i * 2][4] = (-1.0) * x[i] * y[i] / f;A[i * 2][5] = y[i];A[i * 2 + 1][0] = 0.0;A[i * 2 + 1][1] = A[i * 2][0];A[i * 2 + 1][2] = (-1.0) * y[i] / (Zs - GroundControl[i][2]);A[i * 2 + 1][3] = A[i * 2][4];A[i * 2 + 1][4] = (-1.0) * f * (1 + (y[i] * y[i]) / (f * f));A[i * 2 + 1][5] = (-1.0) * x[i];}TranspositionArray(*A, *AT, 8, 6);MultiplyArray(*AT, *A, *ATA, 6, 8, 6);ConverseArray(*ATA, *ATAInv, 6);MultiplyArray(*ATAInv, *AT, *ATAAT, 6, 6, 8);MultiplyArray(*ATAAT, LL, delta, 6, 8, 1);Xs += delta[0];Ys += delta[1];Zs += delta[2];pitch += delta[3];roll += delta[4];yaw += delta[5];cout << "第" << ++n << "次循环" << endl;if ((fabs(delta[3]) < 1e-6) && (fabs(delta[4]) < 1e-6) && (fabs(delta[5]) < 1e-6)){break;}}cout << "循环结束!" << endl;Rotate[0][0] = cos(pitch) * cos(yaw) - sin(pitch) * sin(roll) * sin(yaw);Rotate[0][1] = (-1.0) * cos(pitch) * sin(yaw) - sin(pitch) * sin(roll) * cos(yaw);Rotate[0][2] = (-1.0) * sin(pitch) * cos(roll);Rotate[1][0] = cos(roll) * sin(yaw);Rotate[1][1] = cos(roll) * cos(yaw);Rotate[1][2] = (-1.0) * sin(roll);Rotate[2][0] = sin(pitch) * cos(yaw) + cos(pitch) * sin(roll) * sin(yaw);Rotate[2][1] = (-1.0) * sin(pitch) * sin(yaw) + cos(pitch) * sin(roll) * cos(yaw);Rotate[2][2] = cos(pitch) * cos(roll);MultiplyArray(*A, delta, AX, 8, 6, 1);SubtratArray(AX, LL, v, 8, 1);for (int i = 0; i < 8; i++){vv += v[i] * v[i];}m0 = sqrt(vv / 2);for (int i = 0; i < 6; i++){m[i] = m0 * sqrt(fabs(ATAInv[i][i]));}cout << "相片的外方位元素为：" << endl;cout << "Xs=" << Xs << "    " << "m=" << m[0] << endl;cout << "Ys=" << Ys << "    " << "m=" << m[1] << endl;cout << "Zs=" << Xs << "    " << "m=" << m[2] << endl;cout << "pitch=" << pitch << "    " << "m=" << m[3] << endl;cout << "roll=" << roll << "    " << "m=" << m[4] << endl;cout << "yaw=" << yaw << "    " << "m=" << setiosflags(ios::fixed) << m[5] << endl;cout << "旋转矩阵R为：" << endl;PrintArray(*Rotate, 3, 3);return 0;}void PrintArray(double* a, int b, int c) {//打印矩阵  for (int i = 0; i < b; i++) {for (int j = 0; j < c; j++) {cout << a[i * c + j] << " ";}cout << endl;}
}void TranspositionArray(double* a, double* aT, int b, int c) {//转置矩阵for (int i = 0; i < b; i++) {for (int j = 0; j < c; j++) {aT[j * b + i] = a[i * c + j];}}
}void MultiplyArray(double* a, double* b, double* c, int m, int n, int l) {//矩阵相乘for (int i = 0; i < m * l; i++) {//矩阵初始化c[i] = 0;}for (int i = 0; i < m; i++) {for (int j = 0; j < l; j++) {for (int k = 0; k < n; k++) {c[i * l + j] += a[i * n + k] * b[k * l + j];}}}
}void make2array(double**& a, int n)
{int i, j;a = new double* [n];for (i = 0; i < n; i++){a[i] = new double[2 * n];}for (i = 0; i < n; i++){for (j = 0; j < 2 * n; j++){a[i][j] = 0;}}
}
void deletarray(double**& a, int n)
{int i;for (i = 0; i < n; i++){delete[]a[i];}delete[]a;
}
//矩阵求逆
//ip为要求逆的矩阵，rp是返回的逆矩阵，矩阵维数为n
//行列式为0，返回0；否则返回值为1
int ConverseArray(double* ip, double* rp, int n)
{double** mat = NULL; //做行变换的矩阵   int i, j, r;double k, temp;//初始化mat为0,大小为n*2nmake2array(mat, n);for (i = 0; i < n; i++){for (j = 0; j < n; j++){mat[i][j] = ip[i * n + j];}mat[i][n + i] = 1; //初始化右侧的单位矩阵   }//做行变换化为上三角阵   for (i = 0; i < n; i++){if (mat[i][i] == 0) //第i行i列为0   {for (j = i + 1; j < n; j++){if (mat[j][i] != 0)     //找一个非0行   {for (r = i; r < 2 * n; r++){temp = mat[j][r];mat[j][r] = mat[i][r];mat[i][r] = temp;}break; //跳出j循环   }}}if (mat[i][i] == 0)   return   0; //行列式为0则返回   for (j = i + 1; j < n; j++){if (mat[j][i] != 0) //i行i列下方的j行i列元素不为0   {k = -mat[j][i] / mat[i][i]; //做行变换   for (r = i; r < 2 * n; r++)mat[j][r] = mat[j][r] + k * mat[i][r];}}}//化成单位矩阵   for (i = n - 1; i >= 0; i--){k = mat[i][i];for (r = i; r < 2 * n; r++)mat[i][r] = mat[i][r] / k;for (j = i - 1; j >= 0; j--){k = -mat[j][i];for (r = i; r < 2 * n; r++)mat[j][r] = mat[j][r] + k * mat[i][r];}}//将结果输出   for (i = 0; i < n; i++)for (j = 0; j < n; j++)rp[i * n + j] = mat[i][j + n];//mat矩阵释放 deletarray(mat, n);return 1;
}void SubtratArray(double* a, double* b, double* c, int m, int n)
{int i, j;for (i = 0; i < m; i++){for (j = 0; j < n; j++){c[i * n + j] = a[i * n + j] - b[i * n + j];}}
}

单向空间后方交会C++代码实现相关推荐

单片空间后方交会程序设计（代码共享）
单片空间后方交会程序设计 1 目的用程序设计语言(VC或者VB)编写一个完整的单片空间后方交会程序,通过对提供的试验数据进行计算,输出像片的外方位元素并评定精度.本实验的目的在于让学生深入理解单片空 ...
空间后方交会前方交会 MFC实现 CSU 摄影测量学
空间后方交会前方交会 MFC 实现 CSU 摄影测量学空间后方交会前方交会 MFC 实现 CSU 摄影测量学摄影测量学基础一. 实验目的二.实验内容与要求三.设计与实现: 3.1设计思路 3 ...
空间后方交会c++程序和matlab（可直接运行）
本文不详细说明空间后方交会的原理,只着重说明空间后方交会的程序,并附带一个样例. 样例来源:<摄影测量学>(第二版)武汉大学出版社,张剑清,潘励,王树根. 空间后方交会的误差方程式: 可以 ...
史上最简单的openshift免费空间上传代码教程！没有之一！
史上最简单的openshift免费空间上传代码教程!没有之一! 最近因为想弄一个免费的空间,而且最好是java的空间,找了一大片,jsp的空间少不说,免费的更是寥寥无几. 找了一大推垃圾空间,终于让 ...
摄影测量学空间后方交会
本书是根据武汉大学出版社出版的第三版<摄影测量学>进行编写的单像空间后方交会程序 from numpy import * data_list=[[1,-86.15,-68.99,36589 ...
CSDN新版个人空间介绍之三——代码与收藏
各位尊敬的CSDN用户: 你们好! 新版个人空间上线日益临近,我们改版的各项进程也在如期顺利进行,4月初个人空间就将以崭新的面貌和大家见面咯!如各位用户对新版个人空间有任何意见.建议,欢迎发送邮件至 ...
最简单的openshift免费空间上传代码教程！和FTP一样简单!
史上最简单的openshift免费空间上传代码教程!没有之一! 最近因为想弄一个免费的空间,而且最好是java的空间,找了一大片,jsp的空间少不说,免费的更是寥寥无几. 找了一大推垃圾空间,终于让我 ...
『教程分享』卡QQ空间小尾巴，全部QQ空间小尾巴代码分享
全部小尾巴分享:复制代码发到QQ空间即可看到效果! 『教程分享』卡QQ空间小尾巴,全部QQ空间小尾巴代码分享代码: [em]e10001[/em]guguaiwu.cn [em]e10002[/em ...
单向链表双向链表 java代码实现
文章目录单向链表代码实现单元测试控制台打印头插法尾插法双向链表代码实现单元测试控制台打印头插法尾插法单向链表代码实现 package csdn.dreamzuora.lis ...

单向空间后方交会C++代码实现

单向空间后方交会C++代码实现相关推荐

最新文章

热门文章