cpp实现四参数坐标转化的时候,过程没有报错,但是结果从参数就开始数据丢失,不知道哪里开始计算有问题,请懂得大佬帮忙指正,万分感谢
错误截图
详细代码如下
#include<iostream>
#include<iomanip>
#include<cmath>
#include<Eigen/Dense>
#include<string>
#include<fstream>
using namespace std;
using Eigen::MatrixXd;
using namespace Eigen;
int main()
{
double X1[8], X2[8], Y1[8], Y2[8], X3[8], Y3[8], X4[8], Y4[8];
ifstream file1, file2;
string line_1, str_temp1, line_2, str_temp2;
int i;
int start1, postion_1, start2, postion_2, point_id1[50], point_id2[50];
MatrixXd X_1(8, 1);//起始数据
MatrixXd X_T1(8, 1);//目标已知值
MatrixXd X_2(8, 1);
MatrixXd X_T2(8, 1);//核验点
MatrixXd B1;
B1 = MatrixXd::Zero(8, 4);//公共点B矩阵
MatrixXd B2;
B2 = MatrixXd::Zero(8, 4);//核验B矩阵
MatrixXd x(4, 1);//四参数矩阵
MatrixXd V1(8, 1);//转化公共点改正数
MatrixXd V2(8, 1);//核验点改正数
file1.open("XYZ_origin_1.xyz", ios::in);
file2.open("XYZ_target_1.xyz", ios::in);//读取文件
if (!file1)cout << "error" << endl;
else
{
for (i = 0; i < 4; i++)
{
getline(file1, line_1);
getline(file2, line_2);
postion_1 = 0;
start1 = 0;
postion_1 = line_1.find(",", start1);
str_temp1 = line_1.substr(start1, postion_1 - start1);
point_id1[i] = atoi(str_temp1.c_str());//origin
postion_2 = 0;
start2 = 0;
postion_2 = line_2.find(",", start2);
str_temp2 = line_2.substr(start2, postion_2 - start2);
point_id2[i] = atoi(str_temp2.c_str());//target
//cout << point_id2[i] << endl;
start1 = postion_1 + 1;
postion_1 = line_1.find(",", start1);
str_temp1 = line_1.substr(start1, postion_1 - start1);
X1[i] = atof(str_temp1.c_str());
X_1(2 * i, 0) = X1[i];//origin
start2 = postion_2 + 1;
postion_2 = line_2.find(",", start2);
str_temp2 = line_2.substr(start2, postion_2 - start2);
X_T1(2 * i, 0) = X2[i] = atof(str_temp2.c_str());//target
start1 = postion_1 + 1;
postion_1 = line_1.find(",", start1);
str_temp1 = line_1.substr(start1, postion_1 - start1);
Y1[i] = atof(str_temp1.c_str());
X_1((2 * i + 1), 0) = Y1[i];//origin
start2 = postion_2 + 1;
postion_2 = line_2.find(",", start2);
str_temp2 = line_2.substr(start2, postion_2 - start2);
X_T1(2 * i + 1, 0) = Y2[i] = atof(str_temp2.c_str());//target
//start1 = postion_1 + 1;
//postion_1 = line_1.find('/n', start1);
//str_temp1 = line_1.substr(start1, postion_1 - start1);
//Z1[i] = atof(str_temp1.c_str());
//X_1((2 * i + 2), 0) = Z1[i];//origin
//start2 = postion_2 + 1;
//postion_2 = line_2.find('/n', start2);
//str_temp2 = line_2.substr(start2, postion_2 - start2);
//X_T1(2 * i + 2, 0) = Z2[i] = atof(str_temp2.c_str());//target
//cout << setiosflags(ios::fixed) << setprecision(4)<<X1[i] << ' ' << Y1[i] <<' ' <<Z1[i] <<endl;
//cout << setiosflags(ios::fixed) << setprecision(4) << X2[i] << ' ' << Y2[i] << ' ' << Z2[i] << endl;
B1(2 * i, 0) = 1;
B1(2 * i + 1, 1) = 1;
B1(2 * i, 3) = X1[i];
B1(2 * i, 2) = Y1[i];
B1(2 * i + 1, 3) = Y1[i];
B1(2 * i + 1, 2) = -X1[i];
}
//转换公共点读取
for (i = 0; i < 4; i++)
{
getline(file1, line_1);
getline(file2, line_2);
postion_1 = 0;
start1 = 0;
postion_1 = line_1.find(",", start1);
str_temp1 = line_1.substr(start1, postion_1 - start1);
point_id1[i] = atoi(str_temp1.c_str());//origin
postion_2 = 0;
start2 = 0;
postion_2 = line_2.find(",", start2);
str_temp2 = line_2.substr(start2, postion_2 - start2);
point_id2[i] = atoi(str_temp2.c_str());//target
//cout << point_id2[i] << endl;
start1 = postion_1 + 1;
postion_1 = line_1.find(",", start1);
str_temp1 = line_1.substr(start1, postion_1 - start1);
X3[i] = atof(str_temp1.c_str());
X_2(2 * i, 0) = X3[i];//origin
start2 = postion_2 + 1;
postion_2 = line_2.find(",", start2);
str_temp2 = line_2.substr(start2, postion_2 - start2);
X_T2(2 * i, 0) = X4[i] = atof(str_temp2.c_str());//target
start1 = postion_1 + 1;
postion_1 = line_1.find(",", start1);
str_temp1 = line_1.substr(start1, postion_1 - start1);
Y3[i] = atof(str_temp1.c_str());
X_2((2 * i + 1), 0) = Y3[i];//origin
start2 = postion_2 + 1;
postion_2 = line_2.find(",", start2);
str_temp2 = line_2.substr(start2, postion_2 - start2);
X_T2(2 * i + 1, 0) = Y4[i] = atof(str_temp2.c_str());//target
//start1 = postion_1 + 1;
//postion_1 = line_1.find('/n', start1);
//str_temp1 = line_1.substr(start1, postion_1 - start1);
//Z3[i] = atof(str_temp1.c_str());
//X_2((3 * i + 2), 0) = Z3[i];//origin
//start2 = postion_2 + 1;
//postion_2 = line_2.find('/n', start2);
//str_temp2 = line_2.substr(start2, postion_2 - start2);
//X_T2(3 * i + 2, 0) = Z4[i] = atof(str_temp2.c_str());//target
//cout << setiosflags(ios::fixed) << setprecision(4)<<X3[i] << ' ' << Y3[i] <<' ' <<Z3[i] <<endl;
//cout << setiosflags(ios::fixed) << setprecision(4) << X4[i] << ' ' << Y4[i] << ' ' << Z4[i] << endl;
B2(2 * i, 0) = 1;
B2(2 * i + 1, 1) = 1;
B2(2 * i, 3) = X3[i];
B2(2 * i, 2) = Y3[i];
B2(2 * i + 1, 3) = Y3[i];
B2(2 * i + 1, 2) = -X3[i];
}
//核验点读取
}
file1.close();
file2.close();
//cout << setiosflags(ios::fixed) << setprecision(4) << X_O;
//cout << setiosflags(ios::fixed) << setprecision(4) << X_T;
B1 = -B1;//系数阵
B2 = -B2;
//cout << setiosflags(ios::fixed) << setprecision(4) << B1<<endl;
MatrixXd mat;
mat = MatrixXd::Identity(8, 8);
MatrixXd A, C, D;
A = B1.transpose();
C = A * mat * B1;
D = C.inverse();
x = -D * A * mat * (X_T1 - X_1);//四参数阵
//cout << setiosflags(ios::fixed) << setprecision(10) <<x;
V1 = B1 * x + X_T1 - X_1;
V1 = -V1;//得出残差
//cout << setiosflags(ios::fixed) << setprecision(10) << V1 << endl;
V2 = B2 * x + X_T2 - X_2;
V2 = -V2;//精度检验残差
//cout << setiosflags(ios::fixed) << setprecision(10) << V2;
X_1 = X_1 + V1;
X_2 = X_2 + V2;
//cout << setiosflags(ios::fixed) << setprecision(10) << X_1;
MatrixXd sgm02;
double sgm0;
MatrixXd VT;
VT = V1.transpose();
sgm02 = (VT * mat * V1) / 4;//单位权方差
sgm0 = sqrt(sgm02(0, 0));//单位权中误差
//cout <<setiosflags(ios::scientific)<< sgm02;
double a, b;
a = -2100256.2187;
b = 5496256.0187;
MatrixXd yizhi(2, 1);
MatrixXd daiqiu(2, 1);
MatrixXd Ba;
Ba = MatrixXd::Zero(2, 4);
Ba(0, 0) = Ba(1, 1) = 1;
Ba(0, 3) = a; Ba(1, 3) = b;
Ba(0, 2) = b;
Ba(1, 2) = -a;
//cout << setiosflags(ios::fixed) << setprecision(4) << Ba;
yizhi(0, 0) = a;
yizhi(1, 0) = b;
daiqiu = yizhi + Ba * x;//求出待求点
//cout << setiosflags(ios::fixed) << setprecision(4) << daiqiu;//求出待求点
cout << "----------------4 parameters---------------" << endl;
cout << setiosflags(ios::fixed) << setprecision(4) << "DX=" << x(0, 0) << endl << "DY=" << x(1, 0) << endl;
cout << setiosflags(ios::fixed) << setprecision(10) << "DR=" << x(2, 0) << endl << "DK=" << x(3, 0) << endl;
cout << "----------------条件数与冗余度---------------" << endl;
cout << "n=" << 8 << endl << "r=" << 4 << endl;
cout << "----------------单位权中误差---------------" << endl;
cout << "单位权中误差为" << setiosflags(ios::fixed) << setprecision(8) << sgm0;
cout << "----------------公共点坐标残差---------------" << endl;
cout << "Vx1=" << V1(0, 0) << " " << "Vy1=" << V1(1, 0) << " " << endl;
cout << "Vx2=" << V1(2, 0) << " " << "Vy2=" << V1(3, 0) << " " << endl;
cout << "Vx3=" << V1(4, 0) << " " << "Vy3=" << V1(5, 0) << " " << endl;
cout << "Vx4=" << V1(6, 0) << " " << "Vy4=" << V1(7, 0) << " " << endl;
i = 0;
do
{
i++;
} while (labs(V2(i, 0)) < 0.1 && i < 7);
if (i = 7 && labs(V2(7, 0)) < 0.1)
cout << "精度检验结果为: 核验点符合精度" << endl;//精度检验
cout << "----------------待求点的值---------------" << endl;
cout << "x=" << daiqiu(0, 0) << " " << "y=" << daiqiu(1, 0) << " " << endl;
return 0;
}
想知道是哪里出了错,具体应该怎么修改比较好,万分感谢