立体像对空间前方交会算法

https://blog.csdn.net/qq_37554556/article/details/88363572基于这位优秀的博主有感而发，写完后和老师的答案还是不太一样，正在找原因中。

//前方交会
#include<stdio.h>
#include<iostream>
#include<opencv2/opencv.hpp>
#include<math.h>using namespace std;
using namespace cv;int main()
{//定义同名像点的坐标double image[2][2] = { 0.0 };double x1 = 29.214000;double y1 = -47.640000;double x2 = 3.936000;double y2 = -47.910000;x1 = image[0][0];y1 = image[0][1];x1 = image[1][0];y1 = image[1][1];//定义内方位元素double x0 = 0.0;double y0 = 0.0;double f = 70.50;//定义外方位元素double W_ele[2][6] = { {432438.565, 3894841.049, 1274.012 , -0.005785603 , 0.0003784083 , -0.01880947 },{ 432847.537 , 3894836.718 , 1272.414 , -0.006604103 , 0.0005394704 , 0.0007619388} };double XS1 = W_ele[0][0];double YS1 = W_ele[0][1];double ZS1 = W_ele[0][2];double phi1 = W_ele[0][3];double omig1 = W_ele[0][4];double kappa1 = W_ele[0][5];double XS2 = W_ele[1][0];double YS2 = W_ele[1][1];double ZS2 = W_ele[1][2];double phi2 = W_ele[1][3];double omig2 = W_ele[1][4];double kappa2 = W_ele[1][5];//计算基线分量double Bx = XS2 - XS1;double By = YS2 - YS1;double Bz = ZS2 - ZS1;//cout << kappa1 << endl;//确定外方位角元素double a1 = cos(phi1)*cos(kappa1) - sin(phi1)*sin(omig1)*sin(kappa1);double a2= -1.0*cos(phi1)*sin(kappa1) - sin(phi1)*sin(omig1)*cos(kappa1);double a3= -1.0*sin(phi1)*cos(omig1);double b1= cos(omig1)*sin(kappa1);double b2= cos(omig1)*cos(kappa1);double b3 = -1.0*sin(omig1);double c1 = sin(phi1)*cos(kappa1) + cos(phi1)*sin(omig1)*sin(kappa1);double c2 = -1.0*sin(phi1)*sin(kappa1) + cos(phi1)*sin(omig1)*cos(kappa1);double c3 = cos(phi1)*cos(omig1);//定义旋转矩阵R1Mat R1(3, 3, CV_64F,0.0);R1.at<double>(0, 0) = a1;R1.at<double>(0, 1) = a2;R1.at<double>(0, 2) = a3;R1.at<double>(1, 0) = b1;R1.at<double>(1, 1) = b2;R1.at<double>(1, 2) = b3;R1.at<double>(2, 0) = c1;R1.at<double>(2, 1) = c2;R1.at<double>(2, 2) = c3;double aa1 = cos(phi2)*cos(kappa1) - sin(phi2)*sin(omig2)*sin(kappa2);double aa2 = -1.0*cos(phi2)*sin(kappa1) - sin(phi2)*sin(omig2)*cos(kappa2);double aa3 = -1.0*sin(phi2)*cos(omig1);double bb1 = cos(omig2)*sin(kappa1);double bb2 = cos(omig2)*cos(kappa1);double bb3 = -1.0*sin(omig2);double cc1 = sin(phi2)*cos(kappa2) + cos(phi2)*sin(omig2)*sin(kappa2);double cc2 = -1.0*sin(phi2)*sin(kappa2) + cos(phi2)*sin(omig2)*cos(kappa2);double cc3 = cos(phi2)*cos(omig2);//定义旋转矩阵R2Mat R2(3, 3, CV_64F, 0.0);R2.at<double>(0, 0) = aa1;R2.at<double>(0, 1) = aa2;R2.at<double>(0, 2) = aa3;R2.at<double>(1, 0) = bb1;R2.at<double>(1, 1) = bb2;R2.at<double>(1, 2) = bb3;R2.at<double>(2, 0) = cc1;R2.at<double>(2, 1) = cc2;R2.at<double>(2, 2) = cc3;//printf("a1=%lf", aa1);//定义同名像点的像空间辅助坐标系RR1(X1，Y1，Z1)和RR2(X2，Y2，Z)Mat RR1(3, 1, CV_64F, 0.0);Mat RR2(3, 1, CV_64F, 0.0);Mat RR11(3, 1, CV_64F, 0.0);RR11.at<double>(0, 0) = x1;RR11.at<double>(1, 0) = y1;RR11.at<double>(2, 0) = -f;Mat RR22(3, 1, CV_64F, 0.0);RR22.at<double>(0, 0) = x2;RR22.at<double>(1, 0) = y2;RR22.at<double>(2, 0) = -f;RR1 = R1*RR11;double X1 = RR1.at<double>(0, 0);double Y1 = RR1.at<double>(1, 0);double Z1 = RR1.at<double>(2, 0);RR2 = R2*RR22;double X2 = RR2.at<double>(0, 0);double Y2 = RR2.at<double>(1, 0);double Z2 = RR2.at<double>(2, 0);cout << "左像空间辅助坐标系：" << endl;cout << X1 << endl;cout << Y1 << endl;cout << Z1 << endl;cout << "右像空间辅助坐标系：" << endl;cout << X2 << endl;cout << Y2 << endl;cout << Z2 << endl;//计算点投影系数double N1 = (Bx*Z2 - Bz*X2) / (X1*Z2 - Z1*X2);double N2 = (Bx*Z1 - Bz*X1) / (X1*Z2 - Z1*X2);//计算模型点坐标double deteX = N1*X1;double deteY = 0.5*(N1*Y1 + N2*Y2 + By);double deteZ = N1*Z1;cout << "模型点坐标：" << endl;cout << deteX << endl;cout << deteY << endl;cout << deteZ << endl;//计算地面点的地面坐标（X，Y，Z）double XP = XS1 + deteX;double YP = YS1 + deteY;double ZP = ZS1 + deteZ;cout << "地面点坐标是：" << endl;cout <<"XP="<< XP << endl;cout <<"YP="<< YP<< endl;cout << "ZP="<<ZP<< endl;//std::cout << RR1 << std::endl;getchar();return 0;
}//运行结果
左像空间辅助坐标系：
-0.407883
0.0266778
-70.4988
右像空间辅助坐标系：
2.56837
-47.9375
-70.5505
模型点坐标：
56.0753
3289.33
9692.11
地面点坐标是：
XP=432495
YP=3.89813e+06
ZP=10966.1

正确结果是：

立体像对空间前方交会算法相关推荐

立体像对空间前方交会（利用外方位元素交会出地面点三维坐标）
要通过外方位元素进行前方交会首先需要的已知量有: 然后计算左右片在地辅坐标系中旋转矩阵的方向余弦再计算基线分量计算像点的像空间辅助坐标计算投影系数计算地面点的左像辅系坐标计算地面点的地面坐标 ...
空间前方交会（利用相机外方位元素和像点坐标进行解算）
目录一.前言二.空间前方交会 1. 前方交会的概念 2. 基本公式三.代码展示四.小结一.前言在摄影测量过程中,得到相机的外方位元素以及地面控制点对应的像点坐标之后,如何解算地面控制点 ...
双象空间前方交会代码_基于联觉效应的VR交互式图形与图像混合建模
唐偲祺摘要:VR交互式体验的出现,使用户在虚拟场景中感觉到自己是场景中的主体,用户通过三维等交互设备直接控制虚拟世界中的对象,以一种新的观看世界的方式将图像演进到一个新的阶段--图像场景阶段.为传 ...
双象空间前方交会代码_空间前方交会公式推导.PPT
空间前方交会公式推导 [二]空间前方交会公式推导 * ① ③ 式联立为何①③式联立? 4.基本关系式点的投影系数 [二]空间前方交会公式推导 * 对于左片对于右片 5.前方交会基本公式地面辅助 ...
matlab paticalcoff,关于DOA估计中加权前后向空间平滑算法的仿真问题
参考论文:<相干信源波达方向估计的加权空间平滑算法> 王布宏王永良陈辉武汉空军雷达学院重点实验室武汉430010 摘要提出了一种用于空间相干源 DOA 估计的加权空间平滑算法 ...
C语言矩阵M*N节省空间的算法(附完整源码)
C语言矩阵M*N节省空间的算法 C语言矩阵M*N节省空间的算法完整源码(定义,实现,main函数测试) C语言矩阵M*N节省空间的算法完整源码(定义,实现,main函数测试) #include < ...
路径规划之空间采样算法研究现状简述
关注同名微信公众号"混沌无形",有趣好文! 原文链接:机器人空间采样算法研究现状简述(包含原文PDF百度云下载链接) 摘要:运动规划是移动机器人自主导航系统中的重要模块之一,相关算 ...
基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）
写在前面学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 在前面的博文中: 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab) 基于 ...
圆填充( CIRCLE PACKING)算法圆堆图圆形空间填充算法可视化
原文链接:http://tecdat.cn/?p=24658 圆填充Circle packing算法已经开发了大量确定性和随机性的圆填充算法. RepelLayout 通过成对排斥迭代移动圆圈来搜索 ...

立体像对空间前方交会算法

立体像对空间前方交会算法相关推荐

最新文章

热门文章