Skip to content

Commit 68602df

Browse files
palemoonspalemoons
authored
added lab
1 parent 124f132 commit 68602df

File tree

6 files changed

+824
-0
lines changed

6 files changed

+824
-0
lines changed

‎现代测量学/实验/data1.txt

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
# 读入文件时要求忽略以#字符开头的行
2+
# 内方位元素：像主点相对于影像中心的位置x0，y0（单位毫米）以及镜头中心到影像面的垂距f（单位毫米）
3+
0 0 153.24
4+
# 摄影比例尺分母m
5+
40000
6+
# 控制点对数
7+
4
8+
# 控制点对：控制点影像坐标x，y（单位毫米），控制点地面坐标X，Y，Z（单位米）
9+
-86.15 -68.99 36589.41 25273.32 2195.17
10+
-53.40 82.21 37631.08 31324.51 728.69
11+
-14.78 -76.63 39100.97 24934.98 2386.50
12+
10.46 64.43 40426.54 30319.81 757.31

‎现代测量学/实验/data2.txt

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
# 读入文件时要求忽略以#字符开头的行
2+
# 镜头中心到影像面的垂距f（单位毫米）
3+
24
4+
# 同名像点对数
5+
6
6+
# 同名像点对：左像片像点坐标x_left，y_left（单位毫米），右像片像点坐标x_right，y_right（单位毫米）
7+
1.983 -6.091 -3.202 -5.564
8+
0.924 7.098 -2.830 7.694
9+
1.068 4.538 -2.878 5.098
10+
1.208 6.858 -2.578 7.429
11+
-0.514 -10.050 -5.642 -9.152
12+
1.293 -8.089 -3.981 -7.441

‎现代测量学/实验/data3.txt

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# 读入文件时要求忽略以#字符开头的行
2+
# 控制点对数
3+
4
4+
# 控制点对：像空间辅助坐标X，Y，Z（单位米），地面摄测坐标X_tp，Y_tp，Z_tp（单位米）
5+
159.567 796.866 5.183 5083.205 5852.099 527.925
6+
857.958 786.015 46.818 5780.020 5906.365 571.549
7+
134.978 -801.767 -15.247 5210.879 4258.446 461.810
8+
835.233 -812.758 -23.408 5909.264 4314.283 455.484

‎现代测量学/实验/lab1.cpp

+289
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,289 @@
1+
#include <iostream>
2+
#include <iomanip>
3+
#include <fstream>
4+
#include <math.h>
5+
#include <string>
6+
using namespace std;
7+
8+
class Matrix {
9+
private:
10+
int cols, rows;
11+
double** p;
12+
public:
13+
Matrix(int, int);//声明一个rows行cols列 初始值为0的矩阵
14+
15+
//矩阵操作
16+
Matrix tsp();//转置
17+
Matrix adjt();//伴随
18+
Matrix minor(int r, int c);//余子式
19+
double value();//矩阵的值
20+
Matrix inv();//逆
21+
22+
//矩阵运算
23+
template <size_t r, size_t c>
24+
Matrix operator= (double(&pp)[r][c]);//二维数组赋值
25+
Matrix operator* (const Matrix& m)const;//矩阵乘法
26+
double*& operator[] (int index);
27+
Matrix operator/ (double d);//矩阵除法
28+
friend ostream& operator<< (ostream&, Matrix&);//输出矩阵
29+
friend istream& operator>> (istream&, Matrix&);//按行输入
30+
};
31+
32+
Matrix::Matrix(int rows_num, int cols_num) {
33+
cols = cols_num;
34+
rows = rows_num;
35+
p = new double* [rows];//申请rows个指针，存放cols个元素
36+
for (int i = 0; i < rows; i++) {
37+
p[i] = new double[cols];
38+
for (int j = 0; j < cols; j++) p[i][j] = 0;
39+
}
40+
}
41+
42+
Matrix Matrix::tsp() {
43+
Matrix T(cols, rows);
44+
for (int i = 0; i < rows; i++) {
45+
for (int j = 0; j < cols; j++) {
46+
T.p[j][i] = p[i][j];
47+
}
48+
}
49+
return T;
50+
}
51+
52+
Matrix Matrix::minor(int r, int c) {
53+
Matrix tmp(rows - 1, cols - 1);
54+
for (int i = 0, tmpi = 0; i < rows; i++) {
55+
if (i == r) continue;
56+
for (int j = 0, tmpj = 0; j < cols; j++) {
57+
if (j == c) continue;
58+
else {
59+
tmp.p[tmpi][tmpj] = p[i][j];
60+
tmpj++;
61+
}
62+
}
63+
tmpi++;
64+
}
65+
return tmp;
66+
}
67+
68+
double Matrix::value(){
69+
double value = 0;
70+
if (rows == 1) return p[0][0];
71+
if (rows == 2) return p[0][0] * p[1][1] - p[0][1] * p[1][0];
72+
for (int i = 0; i < rows; i++) {
73+
if (i % 2 == 0) value += minor(i, 0).value() * p[i][0];
74+
else value += minor(i, 0).value() * p[i][0] * (-1);
75+
}
76+
return value;
77+
}
78+
79+
Matrix Matrix::adjt() {
80+
Matrix tmp(rows, cols);
81+
for (int i = 0; i < cols; i++) {
82+
for (int j = 0; j < rows; j++) {
83+
if ((i + j) % 2 == 0) tmp.p[i][j] = minor(j, i).value();
84+
else tmp.p[i][j] = minor(j, i).value() * (-1);
85+
}
86+
}
87+
return tmp;
88+
}
89+
90+
Matrix Matrix::inv() {
91+
Matrix tmp(rows, cols);
92+
tmp = adjt() / value();
93+
return tmp;
94+
}
95+
96+
ostream& operator<< (ostream& out, Matrix& m) {
97+
for (int i = 0; i < m.rows; i++) {
98+
for (int j = 0; j < m.cols; j++) {
99+
out << m.p[i][j] << ' ';
100+
}
101+
out << endl;
102+
}
103+
return out;
104+
}
105+
106+
Matrix Matrix::operator* (const Matrix& m)const {
107+
Matrix result(rows, m.cols);
108+
for (int i = 0; i < rows; i++) {
109+
for (int j = 0; j < m.cols; j++) {
110+
for (int k = 0; k < cols; k++) {
111+
result.p[i][j] += p[i][k] * m.p[k][j];
112+
}
113+
}
114+
}
115+
return result;
116+
}
117+
118+
Matrix Matrix::operator/ (double d) {
119+
Matrix tmp(rows, cols);
120+
for (int i = 0; i < rows; i++) {
121+
for (int j = 0; j < cols; j++) {
122+
tmp.p[i][j] = p[i][j] / d;
123+
}
124+
}
125+
return tmp;
126+
}
127+
128+
template <size_t r, size_t c>
129+
Matrix Matrix::operator= (double(&pp)[r][c]) {
130+
for (int i = 0; i < rows; i++) {
131+
for (int j = 0; j < cols; j++) {
132+
this->p[i][j] = pp[i][j];
133+
}
134+
}
135+
return *this;
136+
}
137+
138+
double*& Matrix::operator[] (int index) {
139+
return p[index];
140+
}
141+
142+
istream& operator>>(istream& in, Matrix& m) {
143+
for (int i = 0; i < m.rows; i++) {
144+
for (int j = 0; j < m.cols; j++) {
145+
in >> m.p[i][j];
146+
}
147+
}
148+
return in;
149+
}
150+
151+
void readFile(double& f, double& x0, double& y0, int& m, int& num, double**& pp, double**& gp) {
152+
ifstream data("./data1.txt");
153+
string tmp = "#";
154+
while (tmp[0] == '#') {
155+
getline(data, tmp);
156+
}
157+
sscanf(tmp.data(), "%lf%lf%lf", &x0, &y0, &f);
158+
f /= 1000.0;
159+
tmp = "#";
160+
while (tmp[0] == '#') {
161+
getline(data, tmp);
162+
}
163+
sscanf(tmp.data(), "%d", &m);
164+
tmp = "#";
165+
while (tmp[0] == '#') {
166+
getline(data, tmp);
167+
}
168+
sscanf(tmp.data(), "%d", &num);
169+
tmp = "#";
170+
pp = new double* [num];
171+
for (int i = 0; i < num; i++) pp[i] = new double[2];
172+
gp = new double* [num];
173+
for (int i = 0; i < num; i++) gp[i] = new double[3];
174+
while (tmp[0] == '#') {
175+
getline(data, tmp);
176+
}
177+
sscanf(tmp.data(), "%lf%lf%lf%lf%lf", &pp[0][0], &pp[0][1], &gp[0][0], &gp[0][1], &gp[0][2]);
178+
for (int i = 1; i < num; i++) {
179+
getline(data, tmp);
180+
sscanf(tmp.data(), "%lf%lf%lf%lf%lf", &pp[i][0], &pp[i][1], &gp[i][0], &gp[i][1], &gp[i][2]);
181+
}
182+
for (int i = 0; i < num; i++) {
183+
pp[i][0] /= 1000.0;
184+
pp[i][1] /= 1000.0;
185+
}
186+
}
187+
188+
void init(double(&eo_coords)[3], double& f, int& m, int& num, double**& gp, double**&ap) {
189+
ap = new double* [num];
190+
for (int i = 0; i < num; i++) {
191+
ap[i] = new double[2];
192+
eo_coords[0] += gp[i][0];
193+
eo_coords[1] += gp[i][1];
194+
eo_coords[2] += gp[i][2];
195+
}
196+
eo_coords[0] /= num;
197+
eo_coords[1] /= num;
198+
eo_coords[2] = f * m;
199+
}
200+
201+
void cal_R(Matrix& R, double(&eo_angles)[3]) {
202+
R[0][0] = cos(eo_angles[0]) * cos(eo_angles[2]) - sin(eo_angles[0]) * sin(eo_angles[1]) * sin(eo_angles[2]);
203+
R[0][1] = -cos(eo_angles[0]) * sin(eo_angles[2]) - sin(eo_angles[0]) * sin(eo_angles[1]) * cos(eo_angles[2]);
204+
R[0][2] = -sin(eo_angles[0]) * cos(eo_angles[1]);
205+
R[1][0] = cos(eo_angles[1]) * sin(eo_angles[2]);
206+
R[1][1] = cos(eo_angles[1]) * cos(eo_angles[2]);
207+
R[1][2] = -sin(eo_angles[1]);
208+
R[2][0] = sin(eo_angles[0]) * cos(eo_angles[2]) + cos(eo_angles[0]) * sin(eo_angles[1]) * sin(eo_angles[2]);
209+
R[2][1] = -sin(eo_angles[0]) * sin(eo_angles[2]) + cos(eo_angles[0]) * sin(eo_angles[1]) * cos(eo_angles[2]);
210+
R[2][2] = cos(eo_angles[0]) * cos(eo_angles[1]);
211+
}
212+
213+
void apxm(double(&eo_coords)[3], int& num, double &f, double**& gp, double**& ap, Matrix& R) {
214+
for (int i = 0; i < num; i++) {
215+
ap[i][0] = -f * (R[0][0] * (gp[i][0] - eo_coords[0]) + R[1][0] * (gp[i][1] - eo_coords[1]) + R[2][0] * (gp[i][2] - eo_coords[2])) / (R[0][2] * (gp[i][0] - eo_coords[0]) + R[1][2] * (gp[i][1] - eo_coords[1]) + R[2][2] * (gp[i][2] - eo_coords[2]));
216+
ap[i][1] = -f * (R[0][1] * (gp[i][0] - eo_coords[0]) + R[1][1] * (gp[i][1] - eo_coords[1]) + R[2][1] * (gp[i][2] - eo_coords[2])) / (R[0][2] * (gp[i][0] - eo_coords[0]) + R[1][2] * (gp[i][1] - eo_coords[1]) + R[2][2] * (gp[i][2] - eo_coords[2]));
217+
}
218+
}
219+
220+
void cal_A(Matrix& A, Matrix& R, double**& pp, double**& gp, double& f, double(&eo_angles)[3], double(&eo_coords)[3], int& num) {
221+
for (int i = 0; i < num; i++) {
222+
double Z = R[0][2] * (gp[i][0] - eo_coords[0]) + R[1][2] * (gp[i][1] - eo_coords[1]) + R[2][2] * (gp[i][2] - eo_coords[2]);
223+
A[i * 2][0] = (R[0][0] * f + R[0][2] * pp[i][0]) / Z;//a11
224+
A[i * 2][1] = (R[1][0] * f + R[1][2] * pp[i][0]) / Z;//a12
225+
A[i * 2][2] = (R[2][0] * f + R[2][2] * pp[i][0]) / Z;//a13
226+
A[i * 2][3] = pp[i][1] * sin(eo_angles[1]) - (pp[i][0] * (pp[i][0] * cos(eo_angles[2])
227+
- pp[i][1] * sin(eo_angles[2])) / f + f * cos(eo_angles[2])) * cos(eo_angles[1]);//a14
228+
A[i * 2][4] = -f * sin(eo_angles[2]) - pp[i][0] * (pp[i][0] * sin(eo_angles[2])
229+
+ pp[i][1] * cos(eo_angles[2])) / f;//a15
230+
A[i * 2][5] = pp[i][1];//a16
231+
A[i * 2 + 1][0] = (R[0][1] * f + R[0][2] * pp[i][1]) / Z;//a21
232+
A[i * 2 + 1][1] = (R[1][1] * f + R[1][2] * pp[i][1]) / Z;//a22
233+
A[i * 2 + 1][2] = (R[2][1] * f + R[2][2] * pp[i][1]) / Z;//a23
234+
A[i * 2 + 1][3] = -pp[i][0] * sin(eo_angles[1]) - (pp[i][1] * (pp[i][0] * cos(eo_angles[2])
235+
- pp[i][1] * sin(eo_angles[2])) / f - f * sin(eo_angles[2])) * cos(eo_angles[1]);//a24
236+
A[i * 2 + 1][4] = -f * cos(eo_angles[2]) - (pp[i][1] * (pp[i][0] * sin(eo_angles[2])
237+
+ pp[i][1] * cos(eo_angles[2]))) / f;//a25
238+
A[i * 2 + 1][5] = -pp[i][0];//a26
239+
}
240+
}
241+
242+
void cal_L(Matrix& L, double**& pp, double**& ap, int& num) {
243+
for (int i = 0; i < num; i++) {
244+
L[i * 2][0] = pp[i][0] - ap[i][0];
245+
L[i * 2 + 1][0] = pp[i][1] - ap[i][1];
246+
}
247+
}
248+
249+
void modify(double(&eo_coords)[3], double(&eo_angles)[3], Matrix& X) {
250+
eo_coords[0] += X[0][0];
251+
eo_coords[1] += X[1][0];
252+
eo_coords[2] += X[2][0];
253+
eo_angles[0] += X[3][0];
254+
eo_angles[1] += X[4][0];
255+
eo_angles[2] += X[5][0];
256+
}
257+
258+
int main()
259+
{
260+
//参数定义
261+
int m, num = 0;//num:控制点对数
262+
double x0, y0, f;
263+
double eo_coords[3] = { 0 }, eo_angles[3] = { 0 };
264+
double** pp = NULL, ** gp = NULL, **ap= NULL;//ap:近似值
265+
readFile(f, x0, y0, m, num, pp, gp);
266+
Matrix A(2 * num, 6);//误差方程系数
267+
Matrix R(3, 3);
268+
Matrix L(2 * num, 1);
269+
Matrix X(6, 1);//修改值
270+
//估算摄站初始位置和姿态
271+
init(eo_coords, f, m, num, gp, ap);
272+
do {
273+
//计算R
274+
cal_R(R, eo_angles);
275+
//计算控制点估计值
276+
apxm(eo_coords, num, f, gp, ap, R);
277+
//误差方程式系数
278+
cal_A(A, R, pp, gp, f, eo_angles, eo_coords, num);
279+
//l计算
280+
cal_L(L, pp, ap, num);
281+
//计算修改值X
282+
X = (A.tsp() * A).inv() * A.tsp() * L;
283+
//修正外方位元素
284+
modify(eo_coords, eo_angles, X);
285+
if (fabs(X[3][0]) < 0.01 && fabs(X[4][0]) < 0.01 && fabs(X[5][0]) < 0.01) break;
286+
} while (true);
287+
fprintf(stdout, "%.2lf %.2lf %.2lf %.6lf %.6lf %.6lf\n", eo_coords[0], eo_coords[1], eo_coords[2], eo_angles[0], eo_angles[1], eo_angles[2]);
288+
return 0;
289+
}

0 commit comments

Comments
 (0)