c++间接平差法对闭合导线进行平差
环境：vs2017+qt+eigen-3.3.8
对源代码文件读取和输出进行适当修改可以不搭配qt工具
算法
（1）基本公式
（2）对角度和边长观测值进行线性化
角度值：
边长值：
程序介绍
ui界面
示例数据：
90, 30, 49.73
268, 10, 9.48
94, 6, 33.22
87, 54, 54.23
180, 17, 42.47
90, 57, 23.22
179, 20, 51.72
89, 14, 10.73
132, 10, 25.47
227, 16, 59.73
200.78625
51.02175
110.6125
198.17775
276.3395
166.78125
200.0075
128.38675
65.24225
258.52325
只适合如图所示的闭合导线类型（即不带支导线）数据仅供参考
代码
头文件：
#pragma once
#include <QtWidgets/QWidget>
#include "ui_horizontal_control_network.h"
class horizontal_control_network : public QWidget
{
Q_OBJECT
public:
horizontal_control_network(QWidget *parent = nullptr);
int compute();
~horizontal_control_network();
private:
Ui::horizontal_control_networkClass ui;
};
mian函数
#include "horizontal_control_network.h"
#include <QtWidgets/QApplication>
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
horizontal_control_network w;
w.show();
return a.exec();
}
主要计算函数
int horizontal_control_network::compute()函数为窗口响应函数
#include "horizontal_control_network.h"
#include<qpushbutton.h>
#include<qmessagebox.h>
#include <QTextEdit>
#include <QStringList>
#include<qdebug.h>
#include<cmath>
#include<Eigen/Dense>
#include<stdio.h>
#include<iostream>
#include <QFileDialog> //文件对话框
#include <fstream>
#define PI acos(-1)
//根据反正切值、delta_y\delta_x返回边长方位角
double pd(double fw, double deltay, double deltax) {
if (fw > 0) {
if (deltay > 0 && deltax > 0)return fw;
else if (deltay < 0 && deltax <0) {
return PI + fw;
}
else {
return fw;
}
}
else if(fw<0){
if (deltay < 0 && deltax>0)return 2 * PI + fw;
else if(deltay >0 && deltax<0)return PI +fw;
else return fw + 2 * PI;
}
else if((int)(fw*1e10)==0){
if(deltax<0 ) return PI;
else return 0;
}
}
//double类型的数据做等于判断
bool dy(double a,double b) {
if (int(a*1e10) == int(b*1e10)) return true;
else return 0;
}
//根据两个坐标方位角，求出之间的夹角
double pda(double a, double b) {
double jiao;
if (a - b < 0)return a - b + 2 * PI;
else return a +b;
}
using namespace std;
//转换角度 ,单位秒
double tsa(double a, double b, double c) { return a * 3600 + b * 60 + c; }
//转换角度，单位度
double tsan(double a, double b, double c) { return a + b / 60 + c / 3600; }
double yushu(double x, int a) {//double类型的做余数
double f = x - (int)x;
int b = (int)x;
int z = (b %= a);
return z + f;
}
//让方位角的角度变为正数
void pn(double fw[20][3], int i) {
int a, b, c, y, j;
double sum;
for (j = 0; j < i; j++) {
sum = fw[j][0] * 3600 + fw[j][1] * 60 + fw[j][2];
if (fw[j][0] < 0) {
y = (-sum) / 360 / 3600;
sum = sum + (y + 1) * 360 * 3600;
}
fw[j][0] = (int)(sum / 3600), sum = yushu(sum, 3600);
fw[j][1] = (int)(sum / 60), sum = yushu(sum, 60);
fw[j][2] = sum;
}
}
//求两点之间的距离
double enorm(double x1, double y1, double x2, double y2) {
return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
}
horizontal_control_network::horizontal_control_network(QWidget *parent)
: QWidget(parent)
{
ui.setupUi(this);
//读取文件并输出按钮的槽函数
connect(ui.pushButton_2, &QPushButton::clicked, this, [=] {
compute();
});
//清空
connect(ui.pushButton_3, &QPushButton::clicked, this, [=]() {
ui.lineEdit->clear();
ui.lineEdit_2->clear();
ui.lineEdit_3->clear();
ui.lineEdit_4->clear();
});
//退出功能
connect(ui.pushButton_4, &QPushButton::clicked, this, &QWidget::close);
}
int horizontal_control_network::compute()//计算函数
{
int n, i = 0, j = 0, chose = 1;
double rou = 180 / PI * 3600;
n = ui.lineEdit->text().toInt();
//初始化二维数组
double s[20],fwj[20],xy[20][2],angle[20][3] ,fw[20][3];
if (ui.lineEdit->text().isEmpty()) {
QMessageBox::critical(this, "warning", "Please input the number of edges");
return 0;
}
//判断已知数据是否为空，并赋值
if (ui.lineEdit_2->text().isEmpty()) { xy[0][0] = 0; }
else { xy[0][0] = ui.lineEdit_2->text().toDouble(); }
if (ui.lineEdit_3->text().isEmpty()) { xy[0][1] = 0; }
else { xy[0][1] = ui.lineEdit_3->text().toDouble(); }
if (ui.lineEdit_4->text().isEmpty()) { fw[0][0] = 0; fw[0][1] = 0; fw[0][2] = 0; }
else {
QString fw0 = ui.lineEdit_4->text();
QStringList flist = fw0.split(','); i = 0;
for (QString str : flist) { fw[0][i] = str.toDouble(); i++; }
if (i != 3) {
QMessageBox::critical(this, "warning", "The beginning azimuth's format error");
return 0;
}
}
if (ui.radioButton_2->isChecked()) chose = 2;
//文件对话框
QString in_path = QFileDialog::getOpenFileName(this, "OPEN", "../", "TXT(*.txt)");//设置文件路径 文件格式
if (in_path.isEmpty() == false) {//路径正确
//创建文件对象 并且关联起来
QFile file(in_path);
bool isok = file.open(QIODevice::ReadOnly);//利用只读模式打开文件
if (isok) {//打开成功
QString text = file.readAll();//换行符号也是读取的
QStringList list = text.split('\n');// 使用换行分隔字符串，将其转换为QStringList
i = 0;
for (QString str : list) {
if (i < n) {//读取角度观测值
QStringList list1 = str.split(',');
j = 0;
for (QString str1 : list1) {
angle[i][j] = str1.toDouble();
j++;
}
if (j != 3) {
QMessageBox::critical(this, "warning", "Angle format error"); file.close();return 0;
}
}
else {//读取边长观测值
s[i - n] = str.toDouble();
}
i++;
}
if (i != 2 * n) {
QMessageBox::critical(this, "warning", "data error"); file.close();return 0;
}
}
else {
QMessageBox::critical(this, "warning", "Failed to open observation data file, exiting function"); file.close(); return 0;
}
}
else {
QMessageBox::critical(this, "warning", "The observation data file path is empty"); return 0;
}
//fwj[0] = fw[0][0] + fw[0][1] / 60 + fw[0][2] / 3600;
//储存初始方位角
if (chose == 1)
for (j = 1; j < n; j++) {
fw[j][0] = (int)fw[j - 1][0] + 180 - angle[j][0], fw[j][1] = (int)fw[j - 1][1] - angle[j][1], fw[j][2] = fw[j - 1][2] - angle[j][2];
}
else
for (j = 1; j < n; j++) {
fw[j][0] = (int)fw[j - 1][0] - 180 + angle[j][0], fw[j][1] = angle[j][1] + (int)fw[j - 1][1], fw[j][2] = angle[j][2] + fw[j - 1][2];
}
//保证角度格式
for (j = 0; j < n; j++) {
if (fw[j][2] >= 60) fw[j][1] += (int)(fw[j][2] / 60), fw[j][2] = yushu(fw[j][2], 60);
if (fw[j][1] >= 60) fw[j][0] += (int)(fw[j][1] / 60), fw[j][1] = (int)(yushu(fw[j][1], 60));
if (fw[j][2] <= -60) fw[j][1] += (int)(fw[j][2] / 60), fw[j][2] = -yushu(abs(fw[j][2]), 60);
if (fw[j][1] <= -60) fw[j][0] += (int)(fw[j][1] / 60), fw[j][1] = (int)(-yushu(abs(fw[j][1]), 60));
}
pn(fw, n);//该函数是将负的分、秒值改正
for (j = 0; j < n; j++) fwj[j] = fw[j][0] + fw[j][1] / 60 + fw[j][2] / 3600;
//起始方位角不能输入90度或270度
if (dy(fwj[0], 90)||dy(fwj[0],270)) {
QMessageBox::critical(this, "warning", "Please do not enter an azimuth angle of 90 degrees or 270 degrees"); return 0;
}
//计算近似坐标
for (j = 1; j < n; j++) {
xy[j][0] = xy[j - 1][0] + s[j - 1] * cos(fwj[j - 1]/180*PI);
xy[j][1] = xy[j - 1][1] + s[j - 1] * sin(fwj[j - 1]/180*PI);
}
//误差方程系数的计算
//创建矩阵,并初始化为0矩阵
Eigen::MatrixXd l = Eigen::MatrixXd::Zero(2 * n , 1);
Eigen::MatrixXd L = Eigen::MatrixXd::Zero(2 * n , 1);
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(2 * n , 2 * n - 3);
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2 * n , 2 * n );
Eigen::MatrixXd X = Eigen::MatrixXd::Zero(2 * n - 3, 1);//参数近似值矩阵
//近似参数赋值
//把第2个点的x坐标当做参数
X(0, 0) = xy[1][0];
for (i = 2; i < n; i++) {
X(2 * (i - 2)+1, 0) = xy[i][0];
X(2 * (i - 2) + 2, 0) = xy[i][1];
}
//边长近似值和常数向量赋值
//边长近似值
for (i = n; i < 2 * n ; i++) {
if (i == 2 * n - 1) {
L(i) = enorm(xy[i - n ][0], xy[i - n ][1], xy[0][0], xy[0][1]);
}
else
L(i) = enorm(xy[i - n ][0], xy[i - n ][1], xy[i - n +1][0], xy[i - n +1][1]);
l(i) = (s[i - n ] - L(i))*1000;//单位毫米
}
//角度观测值系数的计算
//L向量和l向量赋值
for (i = 0; i < n; i++) {
double a, b,day,dax,dby,dbx;
if (i == 0) {
a = atan((xy[n - 1][1] - xy[0][1]) / (xy[n - 1][0] - xy[0][0])); day = xy[n - 1][1] - xy[0][1]; dax = xy[n - 1][0] - xy[0][0];
b = atan((xy[1][1] - xy[0][1]) / (xy[1][0] - xy[0][0])); dby = xy[1][1] - xy[0][1]; dbx = xy[1][0] - xy[0][0];
}
else if (i == n - 1) {
a = atan((xy[n - 2][1] - xy[n - 1][1]) / (xy[n - 2][0] - xy[n - 1][0])); day = xy[n - 2][1] - xy[n - 1][1]; dax = xy[n - 2][0] - xy[n - 1][0];
b = atan((xy[0][1] - xy[n - 1][1]) / (xy[0][0] - xy[n - 1][0])); dby = xy[0][1] - xy[n - 1][1]; dbx = xy[0][0] - xy[n - 1][0];
}
else {
a = atan((xy[i - 1][1] - xy[i][1]) / (xy[i - 1][0] - xy[i][0])); day = xy[i - 1][1] - xy[i][1]; dax = xy[i - 1][0] - xy[i][0];
b = atan((xy[i + 1][1] - xy[i][1]) / (xy[i + 1][0] - xy[i][0])); dby = xy[i + 1][1] - xy[i][1]; dbx = xy[i + 1][0] - xy[i][0];
}
a = pd(a, day, dax);//确定后视边长方位角
b = pd(b, dby, dbx);
if(chose==1) L(i) = pda(a ,b);//单位弧度
else L(i) = pda(b, a);
P(i, i) = 1;//权阵赋值
}
//改正角度正负号
/*for (i = 0; i < n; i++) {
double ch = L(i)*180/PI;
if ((int)(L(i) * 180 / PI) != angle[i][0]) {
QMessageBox::critical(this, "warning", "Angle sign correction error"); return 0;
};
}*/
//观测值-近似值
for (i = 0; i < n; i++) {
l(i) = (tsan(angle[i][0], angle[i][1], angle[i][2])*PI / 180 - L(i))*rou;//弧度秒
}
//B矩阵赋值
/*在闭合圈中i为循环遍历所在点（角）；
又因为第一点xy[0][0],xy[0][1]和第二点的xy[1][1]为已知点，其微分值为0，当其有计算程序时，需要分类讨论
并且B矩阵的第一列参数应从第二点的x坐标开始，
j为当前点的x坐标参数在B矩阵的列的索引，k为后视点x坐标索引，h为前视点x坐标索引
*/
for (i = 0; i < n; i++) {
int j, k, h;
j = 2 * (i - 2)+1, k = 2 * (i - 2)-1, h = 2 * (i - 2) + 3;//i>=3
if (i == 0) {
B(i, 2 * n - 5) = -rou / pow(L(2*n - 1), 2)*(xy[n - 1][1] - xy[0][1])/1000;
B(i, 2 * n - 4) = rou / pow(L(2 * n - 1), 2)*(xy[n - 1][0] - xy[0][0]) / 1000;
B(i, 0) = rou / pow(s[0], 2)*(xy[1][1] - xy[0][1]) / 1000;
}
else if (i == n - 1) {
B(i, 2 * n - 5) = rou * ((xy[n - 2][1] - xy[n - 1][1]) / pow(L(2 * n - 2), 2) - (xy[0][1] - xy[n - 1][1]) / pow(L(2 * n - 1), 2)) / 1000;
B(i, 2 * n - 4) = -rou * ((xy[n - 2][0] - xy[n - 1][0]) / pow(L(2 * n - 2), 2) - (xy[0][0] - xy[n - 1][0]) / pow(L(2 * n - 1), 2)) / 1000;
B(i, 2 * n - 7) = -rou / pow(L(2 * n - 2), 2)*(xy[n - 2][1] - xy[n - 1][1]) / 1000;
B(i, 2 * n - 6) = rou / pow(L(2 * n - 2), 2)*(xy[n - 2][0] - xy[n - 1][0]) / 1000;
}
else if (i == 1) {
B(i, 0) = rou * ((xy[0][1] - xy[1][1]) / pow(s[0], 2) - (xy[2][1] - xy[1][1]) / pow(L(n+1), 2)) / 1000;
B(i, 1) = rou / pow(L(n+1), 2)*(xy[2][1] - xy[1][1]) / 1000;
B(i, 2) = -rou / pow(L(n+1), 2)*(xy[2][0] - xy[1][0]) / 1000;
}
else if (i == 2) {
B(i, j) = rou * ((xy[i - 1][1] - xy[i][1]) / pow(L(n + i - 1), 2) - (xy[i + 1][1] - xy[i][1]) / pow(L(n + i ), 2)) / 1000;
B(i, j + 1) = -rou * ((xy[i - 1][0] - xy[i - 1][0]) / pow(L(n + i -1), 2) - (xy[i + 1][0] - xy[i][0]) / pow(L(n + i ), 2)) / 1000;
B(i, 0) = -rou / pow(L(n + i -1), 2)*(xy[i - 1][1] - xy[i][1]) / 1000;
B(i, h) = rou / pow(L(n + i ), 2)*(xy[i + 1][1] - xy[i][1]) / 1000;
B(i, h + 1) = -rou / pow(L(n + i ), 2)*(xy[i + 1][0] - xy[i][0]) / 1000;
}
else {
B(i, j) = rou * ((xy[i - 1][1] - xy[i][1]) / pow(L(n + i - 1), 2) - (xy[i + 1][1] - xy[i][1]) / pow(L(n+i), 2)) / 1000;
B(i, j + 1) = -rou * ((xy[i - 1][0] - xy[i - 1][0]) / pow(L(n + i - 1), 2) - (xy[i + 1][0] - xy[i][0]) / pow(L(n + i ), 2)) / 1000;
B(i, k) = -rou / pow(L(n + i - 1), 2)*(xy[i - 1][1] - xy[i][1]) / 1000;
B(i, k + 1) = rou / pow(L(n + i - 1), 2)*(xy[i - 1][0] - xy[i][0]) / 1000;
B(i, h) = rou / pow(L(n + i ), 2)*(xy[i + 1][1] - xy[i][1]) / 1000;
B(i, h + 1) = -rou / pow(L(n + i ), 2)*(xy[i + 1][0] - xy[i][0]) / 1000;
}
}
//距离观测值系数计算
for (i = n; i < 2 * n ; i++) {
//B矩阵系数赋值
int j = 2 * (i - n-1)-1, h = 2 * (i - n -1)+1;//i>n+1;j为后视点，h为前视点
if (i == 2 * n - 1) {
B(i, j) = -(xy[0][0] - xy[i - n ][0]) / L(i);
B(i, j + 1) = -(xy[0][1] - xy[i - n ][1]) / L(i);
}
else if (i==n) {
B(i, 0) = (xy[1][0]-xy[0][0] ) / L(i);
}
else if (i == n + 1) {
B(i, 0) = -(xy[i - n + 1][0] - xy[i - n][0]) / L(i);
B(i, h) = (xy[i - n + 1][0] - xy[i - n][0]) / L(i);
B(i, h + 1) = (xy[i - n + 1][1] - xy[i - n][1]) / L(i);
}
else {
B(i, j) = -(xy[i - n +1][0] - xy[i - n ][0]) / L(i);
B(i, j + 1) = -(xy[i - n +1][1] - xy[i - n][1]) / L(i);
B(i, h) = (xy[i - n +1][0] - xy[i - n ][0]) / L(i);
B(i, h + 1) = (xy[i - n+1 ][1] - xy[i - n ][1]) / L(i);
}
//权阵
P(i, i) = 1/ pow((2 + 3 * L(i)*1e-3), 2);
}
//矩阵解算
Eigen::MatrixXd NBB(2 * n - 3, 2 * n - 3);
Eigen::MatrixXd x(2 * n - 3, 1);
Eigen::MatrixXd v(2 * n , 1);
Eigen::MatrixXd L_(2 * n , 1);
Eigen::MatrixXd QXX(2 * n - 3, 2*n-3);
Eigen::MatrixXd DXX(2 * n - 3, 2 * n - 3);
NBB = B.transpose()*P*B;
QXX = NBB.inverse();
x = NBB.inverse()*B.transpose()*P*l;//毫米
//当起始方位角为90度或270度时，将第2个坐标的y值变为参数，x置零
v = B * x - l;//角度为弧度秒,距离是毫米
Eigen::MatrixXd zh(1,1);//从一行一列矩阵提取单位权中误差所需数字
zh= v.transpose()*P*v;
double jm = zh(0, 0);
double o = sqrt(jm/3);//单位权中误差
//创建文件并输出
QString filePath = QFileDialog::getSaveFileName(this, "Save As", "E:/output", tr("TXT(*.txt)"));
QByteArray mstr = filePath.toLocal8Bit();
std::string path = std::string(mstr);//将qstring(包含中文)转换为string
ofstream out_file(path);
if (out_file.is_open()) {
out_file << "单位权中误差为（mm）：" << std::endl;
out_file << to_string(o) << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "观测值近似值矩阵为（角度观测值单位为弧度，边长单位为米）：" << std::endl;
out_file << L << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "参数近似值矩阵为(单位米)：" << std::endl;
out_file << X << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "B矩阵为：" << std::endl;
out_file << B << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "l矩阵为：" << std::endl;
out_file << l << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "权阵P矩阵为：" << std::endl;
out_file << P << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "x参数改正数矩阵矩阵为单位为mm：" << std::endl;
out_file << x << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "v观测值改正数矩阵（角度观测值单位为秒，边长单位为毫米）为：" << std::endl;
out_file << v << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
for (i = 0; i < n; i++) {
L_(i, 0) = angle[i][0] + angle[i][1] / 60 + (angle[i][2] + v(i, 0)) / 3600;
}
for (i = n; i < 2*n; i++) {
L_(i, 0) = s[i-n]+v(i,0)/1000;
}
out_file << "改正后的观测阵矩阵为（角度观测值单位为度，边长单位为米）：" << std::endl;
out_file << L_ << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
X = X + x/1000;
out_file << "改正后的参数矩阵为：(单位m)" << std::endl;
out_file << X<< std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "协因数阵QXX为：(单位mm)" << std::endl;
out_file << QXX << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
out_file << "方差协方差阵DXX为：(单位mm)" << std::endl;
DXX = QXX * o;
out_file << DXX << std::endl;
out_file << "--------------------------------------------------------------------" << std::endl;
}
else {
QMessageBox::critical(this, "warning", "The outfile is not opened!"); return 0;
};
out_file.close();
return 0;
}
horizontal_control_network::~horizontal_control_network()
{
}附：
上述示例数据测量误差非常大，仅供运行。
下面的数据是我自己编的近似正五边形，可以代替示例
108,0,4
108,0,3
108,0,1
107,59,58
108,0,1
100.009
100
100.005
100.004
100
输出
扫一扫
3087
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
