相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标
时间： 23:45:35
&&&& 阅读：383
&&&& 评论：
&&&& 收藏：0
标签：&&&&&&&&&&&&&&&&&&&&&&&&&&&关键词：相机位姿估计，单目尺寸测量，环境探知
用途：基于相机的环境测量，SLAM，单目尺寸测量
文章类型：原理说明、Demo展示
@Author：VShawn
早就写好了....不过doc放在笔记本电脑里，平时一直都在用台式机，所以拖到现在才发:(
写了这么多篇关于位姿估计的博客后，终于要写一篇有点用的东西了：本文将展示位姿估计的一种应用，即通过单目相机对环境进行测量。简单来说，本文的工作就是利用下面的两幅图，在已知P1、P2、P3、P4四点世界坐标的情况下，计算出P5的世界坐标。
该研究的应用范围很广，例如对某建筑群，可以通过设置几个已知的标志点（世界坐标已确定），用本文的方法将建筑的各个角的世界坐标求出来，从而测量出建筑的高度，建筑间的距离，乃至将整个建筑群的环境重构出来。又或者在某个露天货场，设置好标志点后只需要无人机飞一圈，就能知道货场中货物的体积有多少，从而安排货运计划。总之该项应用的前景很大，配合目前很火的无人机应用，可以为生产、研究带来不少的便利。最后，本文基于前几篇文章的结果，建议没有看过我博客的读者先读读前面的几篇原理介绍。
在一开始，先设待求点为P。
根据两条直线确定一个点的原理，在二维平面中只要知道两条相交直线的方程，就可以解出它们的交点坐标。现在假设我们是在二维平面中拍照的，如下图：
根据文章《》的内容，我们根据P1、P2、P3、P4四点的空间坐标，可以估计出两次拍照的相机位姿Oc1与Oc2，也就知道了相机的坐标Oc1与Oc2。那么将Oc1与P，Oc2与P联成直线（如上图的橙色线），则可以获得两条直线方程，组成方程组求解得到它们的交点，即为待求点P的坐标。
到三维空间中，原理跟二维是一样的，只是两条直线从二维空间升到了三维空间成为了两条空间。通过解PNP求出了相机两次拍摄的空间位置Oc1、Oc2，在根据P在图像中的坐标，可以知道P点在空间中位于相机的哪个方向（将二维图像中的P点用公式映射到三维空间中，需要使用到内参数与外参数矩阵），也就是可以确定一条从相机指向点P的射线。用两幅图像确定了关于P的两条射线，那么解方程求出他们的交点坐标，就能得到P的空间坐标。
1.求出P点的相机坐标系坐标Pc
关于P点如何从二维映射到三维，参考上图，Oc的坐标通过解PNP已经求出，待求点P在图像中的像素坐标为(u,v)。根据《图像坐标系-相机坐标系-世界坐标系的关系》（由于懒癌，还没写）可以套公式求出P在相机坐标系中的坐标Pc（也就是上图中的Pc点）。具体的转换公式如下，式中F为相机镜头的焦距(mm)，u、v为点的像素坐标，其余为相机内参数。
代码如下：
代码中使用本人封装好的解PNP问题类解决PNP问题，具体使用方法参见《》。
PNPSolver p4psolver1;
//初始化相机参数
p4psolver1.SetCameraMatrix(fx, fy, u0, v0);
//设置畸变参数
p4psolver1.SetDistortionCoefficients(k1, k2, p1, p2, k3);
p4psolver1.Points3D.push_back(cv::Point3f(0, 0, 0));
//P1三维坐标的单位是毫米
p4psolver1.Points3D.push_back(cv::Point3f(0, 200, 0));
p4psolver1.Points3D.push_back(cv::Point3f(150, 0, 0));
p4psolver1.Points3D.push_back(cv::Point3f(150, 200, 0));
//p4psolver1.Points3D.push_back(cv::Point3f(0, 100, 105));
cout && "特征点世界坐标 = " && endl && p4psolver1.Points3D && endl && endl &&
//求出图一中几个特征点与待求点P的坐标
//cv::Mat img1 = cv::imread("1.jpg");
p4psolver1.Points2D.push_back(cv::Point2f(2985, 1688));
p4psolver1.Points2D.push_back(cv::Point2f(5081, 1690));
p4psolver1.Points2D.push_back(cv::Point2f(2997, 2797));
p4psolver1.Points2D.push_back(cv::Point2f(5544, 2757));
//p4psolver1.Points2D.push_back(cv::Point2f());
cout && "图一中特征点坐标 = " && endl && p4psolver1.Points2D &&
cv::Point2f point2find1_IF = cv::Point2f(4149, 671);//图1中待求点P的图像坐标系坐标
if (p4psolver1.Solve(PNPSolver::METHOD::CV_P3P) != 0)
return -1;
cout && "图一中相机位姿" && endl && "Oc坐标=" && p4psolver1.Position_OcInW && "
相机旋转=" && p4psolver1.Theta_W2C &&
//将P投射到相机坐标系，再经过反旋转求出向量OcP，最终获得图1中，直线OcP上的两个点坐标，确定了直线的方程
cv::Point3f point2find1_CF = p4psolver1.ImageFrame2CameraFrame(point2find1_IF, 350);//待求点P在图一状态下的相机坐标系坐标，输入参数350表示将P投影到350mm外的相机成像平面
2.求出P点在世界坐标系中的方向向量
此时我们得到了Pc(xc,yc,zc)，但这个点坐标是在相机坐标系中的，而我们需要知道的其实是P点在世界坐标系中对应的坐标Pw(xw,yw,cw)。为了将Pc转为Pw，需要使用到解PNP求位姿时得到的三个欧拉角。我们知道相机坐标系C按照z轴、y轴、x轴的顺序旋转以上角度后将与世界坐标系W完全平行（详见《》），在这三次旋转中Pc显然是跟着坐标系旋转的，其在世界系W中的位置会随着改变。为了抵消旋转对P点的影响，保证C系旋转后P点依然保持在世界坐标系W原本的位置，需要对Pc进行三次反向旋转，旋转后得到点Pc在相机坐标系C中新的坐标值记为Pc‘，Pc‘的值等于世界坐标系中向量OP的值。那么Pc‘的值+ Oc的世界坐标值=P点的世界坐标Pw。
代码如下（代码中变量接上一段代码）：
double Oc1P_x1 = point2find1_CF.x;//待求点P的相机坐标系x坐标
double Oc1P_y1 = point2find1_CF.y; //待求点P的相机坐标系y坐标
double Oc1P_z1 = point2find1_CF.z; //待求点P的相机坐标系z坐标
//进行三次反向旋转，得到世界坐标系中向量OcP的值，也就是方向向量
PNPSolver::CodeRotateByZ(Oc1P_x1, Oc1P_y1, p4psolver1.Theta_W2C.z, Oc1P_x1, Oc1P_y1);
PNPSolver::CodeRotateByY(Oc1P_x1, Oc1P_z1, p4psolver1.Theta_W2C.y, Oc1P_x1, Oc1P_z1);
PNPSolver::CodeRotateByX(Oc1P_y1, Oc1P_z1, p4psolver1.Theta_W2C.x, Oc1P_y1, Oc1P_z1);
//两点确定一条直线，a1为Oc的世界坐标，a2为P的世界坐标Pw
cv::Point3f a1(p4psolver1.Position_OcInW.x, p4psolver1.Position_OcInW.y, p4psolver1.Position_OcInW.z);
cv::Point3f a2(p4psolver1.Position_OcInW.x + Oc1P_x1, p4psolver1.Position_OcInW.y + Oc1P_y1, p4psolver1.Position_OcInW.z + Oc1P_z1);
&上面的代码中获得了一条射线A的两个端点，其中a1为相机的世界坐标系坐标，a2为求出的P点映射到世界坐标系时的方向向量+相机的世界坐标系坐标
3.最后，根据两幅图得到的两条直线，计算出P点的世界坐标
对另外一幅图也进行1、2的操作，得到点b1，b2。于是获得两条直线A、B，求出两条直线A与B的交点，大功告成。然而在现实中，由于误差的存在，A与B相交的可能性几乎不存在，因此在计算时，应该求他们之间的最近点坐标。
根据文章《》中给出的GetDistanceOf2linesIn3D类，可以求出两条直线的交点或者说两条直线的最近点坐标。
/*************************求出P的坐标**************************/
//现在我们获得了关于点P的两条直线a1a2与b1b2
//于是两直线的交点便是点P的位置
//但由于存在测量误差，两条直线不可能是重合的，于是退而求其次
//求出两条直线最近的点，就是P所在的位置了。
GetDistanceOf2linesIn3D//初始化
g.SetLineA(a1.x, a1.y, a1.z, a2.x, a2.y, a2.z);//输入直线A上的两个点坐标
g.SetLineB(b1.x, b1.y, b1.z, b2.x, b2.y, b2.z);//输入直线B上的两个点坐标
g.GetDistance();//计算距离
double d = g.//获得距离
//点PonA与PonB分别为直线A、B上最接近的点，他们的中点就是P的坐标
double x = (g.PonA_x + g.PonB_x) / 2;
double y = (g.PonA_y + g.PonB_y) / 2;
double z = (g.PonA_z + g.PonB_z) / 2;
cout && endl && "-------------------------------------------------------------" &&
cout && "解得P世界坐标 = (" && x && "," && y && "," && z && ")" &&
最后解得P点坐标为：
P的实际坐标为(5,100,105)，计算结果的误差在1mm左右，考虑到绘图与测量时产生的误差，以及拍摄的时的视距，这样的误差在可接受范围之内。
将上述理论应用到实际当中，我用130w的工业相机在距离800上对目标拍摄了一系列的图。
该测量的误差来源于以下几个方面：
安装、测量误差
这个误差是由于在设备安装，以及尺寸测量中所形成的。最典型的比如说在用马克笔画点时画歪了一点，又或在用尺子测量P5点高度时度数不准等。
像点坐标的选取误差
这一误差是在确定几个点的像素坐标时，取点不准所造成的。不过由于本文用的图像有2000w像素，因此该误差不太明显。若是用100w的图像，该误差的影响就会被大大增强。
两张拍照位置造成的误差。
理论上两次拍照位置相互垂直时，最后计算出来的P点世界坐标的误差最小，如下图。
但实际情况却不一定这么理想，尤其是在处理无人机拍摄的视频时，可能隔几帧就进行一次处理，这样就会带来较大的误差，所以最好的办法是求多组值，取它们的加权平均值。像本文所用的两幅图像的拍摄角度大概是这样的：
用这两幅图像处理产生的误差就会比较大，但最终计算出的误差也就在1mm左右，能够接受。用这两幅图做实验是因为作者比较懒惰，直接用了以前拍的图片，而没有重新采集图像，好同志不要学。标签：&&&&&&&&&&&&&&&&&&&&&&&&&&&
&&国之画&&&& &&&&chrome插件
版权所有 京ICP备号-2
迷上了代码！君，已阅读到文档的结尾了呢~~
基于视频图像的路口交通事故再现视频,图片,车祸,交通事故,车祸视频
扫扫二维码，随身浏览文档
手机或平板扫扫即可继续访问
基于视频图像的路口交通事故再现
举报该文档为侵权文档。
举报该文档含有违规或不良信息。
反馈该文档无法正常浏览。
举报该文档为重复文档。
推荐理由：
将文档分享至：
分享完整地址
文档地址：
粘贴到BBS或博客
flash地址：
支持嵌入FLASH地址的网站使用
html代码：
&embed src='/DocinViewer--144.swf' width='100%' height='600' type=application/x-shockwave-flash ALLOWFULLSCREEN='true' ALLOWSCRIPTACCESS='always'&&/embed&
450px*300px480px*400px650px*490px
支持嵌入HTML代码的网站使用
您的内容已经提交成功
您所提交的内容需要审核后才能发布，请您等待！
3秒自动关闭窗口宿州学院毕业论文（设计）
国家坐标系与地方坐标系的坐标转换 ?1y21y4?2?3t2???9?t)()?(61?90t?45t)()?ffffff2MfNf360Nf???12??y?1y21y4?2224222l?()?1?(1?2tf??f)()?(5?28tf?24tf?6?f?8?ftf)()?cosBfNf?6N120N?ff??
（3-11） B?Bf?y(yNf?tf（B、L）为大地坐标，（x 、y）为已知高斯平面坐标； 根据国家参考椭球中扁率的不变，国家的参考椭球和特定参考椭球球心重合，这样一来，经度不变。纬度会发生变化，其纬度变化值可以用此公式计算； b :纬度变化值公式：
?B?e2?sinBcosB??a
（3-12） ?M?H?1?e?sinB22 c :国家参考椭球子午圈曲率半径公式：
M?a?1?eN?a 2??1?e2?sinB2?3
（3-13） 1?e2?sinB2△B为新椭球上的纬度变化值，(B、L）西安80坐标，经过高斯反算得出； 运用上述方式进行计算，在特定区域经行中央子午线重新投影，计算新高斯坐标，再在已有的地方坐标和高斯平面坐标采用坐标相似变换求其转换参数，这样就能得到所求的地方独立坐标。 d :坐标的相似变换模型
??X独=?X?m?cosa?X新-m?sina?Y新?
Y =?Y+sina?X-m?cosa?Y?新新?独 △X, △Y为坐标平移量，cos a, sin a 为坐标旋转因子，m为坐标缩放因子，他们都是在原坐标系下转换到独立坐标系后的参数。
16 宿州学院毕业论文（设计）
国家坐标系与地方坐标系的坐标转换 总上所示公式，求得四个转换参数后，就可以完成国家坐标系（1980年西安坐标系）向某地地方独立坐标系的转换。 同理，可以将地方独立系转换为国家坐标系，这里就不在详细介绍。
17 宿州学院毕业论文（设计）
4 算例分析 运用上述方法和公式，国家坐标系坐标向地方独立坐标系的坐标转换。结合软件进行计算，这里我们使用GPS数据处理南方测绘软件解决方案。我们使用的是相互转换的两个坐标转换，1980国家坐标系与地方独立坐标系两者之间的解算和转换。在某地区选取10个控制点，表4-1为选取的1980年国家坐标和局部独立坐标[10]。
表 4-1 已知控制点坐标
单位：m 点号 1 2 3 4 5 6 7 8 9 10
1980国家坐标点 地方独立坐标点
487 338.700 573 421.465 278 624.755
501 601.701 542
656.208 307 338.648
534 509.768 573 229.890 334 482.449
579 234.782 574 543.776 376 814.347
489 546.875
536 271.664 272 575.452
560 874.704 535 824.847 297 489.794
556 578.988 542 547.085 335 683.681
583 433.332 532 483.756 368 473.324
489 331.334 496 642.342 263 572.743
536 662.380 467 321.797 340 241.539 a :根据高斯反算公式（3-11）把1980年国家坐标系坐标转换成大地经纬度坐标； b :结合投影面和中央子午线，选取等分的高程异常面看做投影面，中央子午线选取L=117°13′； c :按照纬度变化值公式和国家参考椭球子午圈曲率半径公式重新计算经纬度； d :根据重新计算的经纬度，选取中央子午线L=117°13′，进行高斯正算，计算出新高斯平面坐标； f :在结合坐标的相似变换公式，将新高斯平面坐标转换为地方独立坐标； 表4-2为1980年国家坐标系转换为独立坐标系内精度参照表； 表4-3为1980年国家坐标系转换为独立坐标系外精度参照表；
宿州学院毕业论文（设计）
表 4-2 内精度对照表
单位：m 点号 地方独立坐标点 解算独立坐标点 X坐标差值 Y坐标差值 1 573 421.465 278 624.755 573 421.463 278 624.754 0.002 0.001 2
542 656.208 307 338.648
542 656.207 307 338.648
0.000 3 573 229.890 334 482.449 573 229.890 334 482.449 0.000 0.000 4 574 543.776 376 814.347 574 543.777 376 814.349 -0.001 -0.002 5 536 271.664 272 575.452 536 271.663 272 575.452 0.001 0.000 6 535 824.847 297 489.794 535 824.844 297 489.792 0.003 0.002 7 542 547.085 335 683.681 542 547.085 335 683.679 0.000 0.002 8 532 483.756 368 473.324 532 483.755 368 473.325 0.001 -0.001 9 496 642.342 263 572.743 496 642.343 263 572.742 0.001 0.001 10 467 321.797 340 241.539 467 321.799 340 241.540 0.002 0.001
表 4-3 外精度对照表
单位：m 点号 地方独立坐标点 W1
解算独立坐标点 X坐标差值 Y坐标差值
363 945.956 0.003
355 000.233 0.002
333 569.441 0.000
300 097.753 0.002
390 806.663 0.003
316 915.482 0.001
331 723.197 0.001
315 280.483 0.000
309 798.530 -0.004
255 835.918 0.001
在反过来计算，根据表4-1 已知控制点坐标中地方独立坐标运用上述办法结合软件进行解算，由地方局部独立坐标转换为1980年国家坐标； 表4-4为某地局部独立坐标系转换为1980年国家坐标系的内符合精度对照表；
19 宿州学院毕业论文（设计）
表 4-4 内符合精度对照表
单位：m 点号 1 2 3 4 5 6 7 8 9 10
X坐标差Y坐标差值 值
487 338.700
487 338.703 0.005 0.003
501 601.701
501 601.703 0.000 -0.002
534 509.768
534 509.770 -0.001
579 234.782
579 234.781 0.001
489 546.875
489 546.877
560 874.704
560 874.705 -0.002
556 578.988
556 578.988
583 433.332
583 433.330 -0.003
489 331.334
489 331.335 0.003
536 662.380
536 662.377 0.000 0.003
已知点80国家坐标点 解算点80国家坐标点 根据上述的解算原理，在结合软件的应用，从上面的三线表可以得出这样的结论：外符合精度还是内符合精度都是±5mm； 其精度要求较高，符合实际需求，达到国家坐标系与地方坐标系相互转换的精度要求，继而实现1980国家坐标系统到一个地方独立坐标系和地方独立坐标系到1980国家坐标系。COORD坐标转换下载4.2 绿色免费版_西西软件下载
西西软件园多重安全检测下载网站、值得信赖的软件下载站！
相关软件 /英文/ /中文/ /中文/ /中文/ /中文/ /中文/ /中文/ /中文/ /英文/ /中文/顶好评:50%踩坏评:50%请简要描述您遇到的错误，我们将尽快予以修正。轮坛转帖HTML方式轮坛转帖UBB方式
700KB/中文/2.3
59.6M/中文/5.3
10.5M/中文/5.8
27.3M/多国语言[中文]/6.9
1.21G/英文/7.0
597.0M/中文/6.0
157.2M/中文/4.0
Coord坐标转换工具，用于各个球面坐标和平面坐标之间的转换，支持文件格式，它可以实现不同椭球之间的坐标转换，同一椭球的直角坐标、地理坐标和平面坐标相互转换。如北京54到西安80坐标的转换，西安80到地方坐标的转换，西安80坐标系下的直角坐标到地理坐标转换等等，非常容易使用，而且此工具有帮助实例，可以让新手轻松上手，是款相当不错的软件。COORD坐标转换使用说明：本软件采用坐标系统库管理，用户可以将一种转换在系统库里保存下来，下次使用时从文件菜单中选择打开库文件来调用所有已有的转换参数。实例一：转换要求：用户在一个佛山测区内使用RTK G接收机接受了一些点的WGS-84的坐标,现在希望将其转换为北京54和佛山坐标系下的坐标。用户有佛山测区的一些控制点，这些控制点有WGS-84坐标，也有北京-54坐标也有佛山坐标。分析：WGS-84坐标和北京54坐标是不同两个椭球的坐标转换，所以要求得三参数或七参数，而北京54和佛山坐标都是同一个椭球，所以他们之间的转换是地方坐标转换，需要求得地方转化四参数，因为要求得到的北京54是平面坐标所以需要设置投影参数。：步骤：1．& 新建坐标转换文件，便于下次使用转换是不用重新输入，直接打开即可。2．& 设置投影参数。3．& 用一个已知点（WGS84坐标和北京54坐标），计算不同椭球转换的三参数（或七参数）。4．& 确定转换参数。5．& 打开七参数转换，完成WGS84到北京54的转换。6．& 利用多个已知点（北京54坐标和佛山坐标），计算同一个椭球的地方坐标转换（四参数）。7．& 确定转换参数8．& 同时打开七参数和平面转换。完成WGS-84到佛山的转换。北京54的坐标转换为国家80坐标：转换要求：用户在一个测区内有一些点的北京54的坐标,现在希望将其转换为国家80坐标。用户有测区的一些控制点，这些控制点既有北京-54坐标也有国家80坐标。文件转换用户如果需要转换的是一个文件里的所有的点，可以用文件转换来完成。1．& 确定转换关系：按照上面的步骤完成1~8步的操作，这样就确立了转换关系，也就是说文件里所有的点都按照上面确定的转换关系来完成转换。2．& 确定转换格式：在主界面中选择文件转换，点击格式按钮。新建格式：在名称，扩展名中输入相应的内容，然后自己选择数据列表中的内容并添加来确定格式，如果列表中没有的就用其他来表示，完成后点击完成新建。&&&&&&&&&&&&& 选择格式：在格式列表中选择格式&&&&&&&&&&&&& 文件转换方式：转换后的文件有三种方式，如原来的文件是C:\File.txt文件，用户选择其中的一种，这样转换后就会根据这个方式来完成新建文件。却省是在文件扩展名后+1，即转换后会新生成一个C:\File.txt1文件。&&&&&&&&&&&&& 在上述工作完成后，单击确定按钮即可。3．& 选择转换文件并完成转换单击浏览按钮，打开文件选择对话框，选择文件并确定，在左边会显示文件，用户单击 =〉按钮即可完成转换，右边会显示出转换后的文件名和路径，用户可以通过双击列表中的文件即可查看文件内容。
安卓官方手机版
IOS官方手机版
COORD坐标转换 4.2 绿色免费版
下载帮助西西破解版软件均来自互联网, 如有侵犯您的版权, 请与我们联系。