立体像对空间前方交会程序设计
运用了eigen矩阵库
程序原理
00
【实验数据】：
左影像：x0=0; y0=0; f=152.91;
Xs=970302.448784;
Ys=-1138644.971216;
Zs=3154.584941;
phi=0.010425;
omega=-0.012437;
kapa=0.003380;
右影像：x0=0; y0=0; f=152.91;
Xs=971265.303768;
Ys=-1138634.245942;
Zs=3154.784258;
phi=0.008870;
omega=-0.005062;
kapa=-0.008703;
同名像点坐标：左：（0.153，91.798），右：（-78.672，89.122）
c++代码
写入头文件
#pragma once#include <iostream>#include "Eigen/Dense"#include "Eigen/Core"using namespace std;using namespace Eigen;class function1{public:double point1[8]={0};// XS YS ZS Q W K X Y 依次输入double point2[8]={0};double x0, y0, f=0;//内方位元素double X[3] = {0 };int FUzhi(double* a1, double* a2, int n)//数组赋值函数{for (int i = 0; i < n; i++){a1[i] = a2[i];}return 0;}MatrixXd XZmatrix(double Q1, double W1, double K1)//RRRR 计算转转矩阵{double a1, a2, a3, b1, b2, b3, c1, c2, c3;a1 = cos(Q1) * cos(K1) - sin(Q1) * sin(W1) * sin(K1);a2 = -cos(Q1) * sin(K1) - sin(Q1) * sin(W1) * cos(K1);a3 = -sin(Q1) * cos(W1);b1 = cos(W1) * sin(K1);b2 = cos(W1) * cos(K1);b3 = -sin(W1);c1 = sin(Q1) * cos(K1) + cos(Q1) * sin(W1) * sin(K1);c2 = -sin(Q1) * sin(K1) + cos(Q1) * sin(W1) * cos(K1);c3 = cos(Q1) * cos(W1);MatrixXd mat(3, 3);mat << a1, a2, a3,b1, b2, b3,c1, c2, c3;return mat;}MatrixXd Matchange(double x, double y, double f, MatrixXd R)//mar change 坐标转换{MatrixXd P(3, 1);P << x,y,-f;MatrixXd XYZ(3, 1);XYZ = R * P;return XYZ;}double pointratio(double a1, double a2)// Bx BY/求BX BY BZ{return a2 - a1;}double* Finalcalc(double* P1, double* P2, double Bx, double By, double Bz, double N, double N2, MatrixXd P11,MatrixXd P22)//计算最后结果{double point[3] = {Bx,By,Bz };double tmp[3] = {0 };double tmp2[3] = {0 };double* fianl = (double*)malloc(3 * sizeof(double));for (int i = 0; i < 3; i++){tmp[i] = P1[i] + N * P11(i, 0);tmp2[i] = P2[i] + N2 * P22(i, 0);*(fianl + i) = (tmp[i] + tmp2[i])/2;}return fianl;}function1(double* BS, double* Mp, double x01, double y01, double f01){FUzhi(point1, BS, 8);FUzhi(point2, Mp, 8);x0 = x01;y0 = y01;f = f01;};void function1calc(){MatrixXd R1(3, 3);R1 = XZmatrix(point1[3], point1[4], point1[5]);MatrixXd R2(3, 3);R2 = XZmatrix(point2[3], point2[4], point2[5]);double Bx = pointratio(point1[0], point2[0]);double By = pointratio(point1[1], point2[1]);double Bz = pointratio(point1[2], point2[2]);MatrixXd P1(3, 1);MatrixXd P2(3, 1);P1 = Matchange(point1[6], point1[7], f, R1);P2 = Matchange(point2[6], point2[7], f, R2);double N = (Bx * P2(2, 0) - Bz * P2(0, 0)) / (P1(0, 0) * P2(2, 0) - P1(2, 0) * P2(0, 0));double N2 = (Bx * P1(2, 0) - Bz * P1(0, 0)) / (P1(0, 0) * P2(2, 0) - P1(2, 0) * P2(0, 0));FUzhi(X, Finalcalc(point1, point2, Bx, By, Bz, N, N2, P1, P2), 3);}double* result(){return X;}/*~function1(){free(&X[0]*//*);*///}};class funtion2 :public function1{public:double po1[8]={0};//xs ys zs q w k x y double po2[8]={0};double x0, y0, H;double X[3] = {0};funtion2(double*po4,double*po3,double x,double y,double h):function1(po1,po2,1,1,1){FUzhi(po1, po4, 8);FUzhi(po2, po3, 8);this->x0 = x;this->y0 = y;this->H = h;cout << "赋值完成"<<endl;cout << "共线法" << endl;};MatrixXd CalcA(MatrixXd R1,double x,double y,double f,double xs,double ys,double zs,double x0,double y0){MatrixXd A(2, 3);for (int j = 1; j <=3; j++){A(0,j-1) = f * R1(j-1, 0) + (x-x0 )* R1(j - 1, 2);}for (int j = 1; j <= 3; j++){A(1, j - 1) = f * R1(j-1, 1) + (y-y0 )* R1(j - 1, 2);}return A;}MatrixXd CalcL(MatrixXd R1, double x, double y, double f, double xs, double ys, double zs, double x0, double y0){double lx = f * R1(0, 0) * xs + f * R1(1, 0) * ys + f * R1(2, 0) * zs + (x - x0) * (R1(0, 2) * xs + R1(1, 2) * ys + zs * R1(2, 2));double ly = f * R1(0, 1) * xs + f * R1(1, 1) * ys + f * R1(2, 1) * zs + (y - y0) * (R1(0, 2) * xs + R1(1, 2) * ys + zs * R1(2, 2));MatrixXd L(2, 1);L(0, 0) = lx;L(1, 0) = ly;return L;}int calc(){MatrixXd R1(3, 3);MatrixXd R2(3, 3);R1 = XZmatrix(po1[3], po1[4], po1[5]);R2 = XZmatrix(po2[3], po2[4], po2[5]);MatrixXd A1(2, 3);MatrixXd A2(2, 3);A1 = CalcA(R1, po1[6], po1[7], this->H, po1[0], po1[1], po1[2], this->x0, this->y0);A2= CalcA(R2, po2[6], po2[7], this->H, po2[0], po2[1], po2[2], this->x0, this->y0);MatrixXd A(4, 3);A << A1, A2;MatrixXd L1(1, 2);MatrixXd L2(1, 2);L1= CalcL(R1, po1[6], po1[7], this->H, po1[0], po1[1], po1[2], this->x0, this->y0);L2= CalcL(R2, po2[6], po2[7], this->H, po2[0], po2[1], po2[2], this->x0, this->y0);MatrixXd L(4, 1);L << L1, L2;MatrixXd XX(1, 3);XX = (A.transpose() * A).inverse() * A.transpose() * L;double* final = NULL;MatrixXd V(1, 4);V = A * XX - L;MatrixXd V1(1, 1);V1 = V.transpose() * V;double q0 = sqrt(V1(0,0) /2);final = (double*)malloc(3 * sizeof(double));if(final==NULL){cout << " 内存分配失败" << endl;return 0;}for(int i=0;i<3;i++){*(final + i) = XX(i, 0);}FUzhi(X, final, 3);/*cout << "精度评定" << endl;cout << q0 << endl;*/return 0;}};
调用格式
#include "Function.h"#include <iomanip>int main(){double a[] = {970302.448784,-1138644.971216,3154.584941,0.010425,-0.012437,0.003380,0.153,91.798};double b[] = {971265.303768,-1138634.245942,3154.784258,0.008870,-0.005062,-0.008703,-78.672,89.122};function1 obj(b,a,0,0,152.91);obj.function1calc();printf("点系数法\n");for(int i=0;i<3;i++)printf("%2f\n",obj.X[i]);funtion2 ojb(a, b, 0, 0, 152.91);ojb.calc();for (int i = 0; i < 3; i++)printf("%2f\n", ojb.X[i]);system("pause");return 0;}
