相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标

关键词：相机位姿估计，单目尺寸测量，环境探知

用途：基于相机的环境测量，SLAM，单目尺寸测量

文章类型：原理说明、Demo展示

@Author：VShawn

@Date:2016-11-28

@Lab: CvLab202@CSU

目录

• • 《相机位姿估计0：基本原理之如何解PNP问题》
  • 《相机位姿估计1：根据四个特征点估计相机姿态》
  • 《相机位姿估计1_1：OpenCV:solvePnP二次封装与性能测试》
  • 《相机位姿估计2：[应用]实时位姿估计与三维重建相机姿态》
  • 《相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标》

前言

早就写好了....不过doc放在笔记本电脑里，平时一直都在用台式机，所以拖到现在才发:(

 

写了这么多篇关于位姿估计的博客后，终于要写一篇有点用的东西了：本文将展示位姿估计的一种应用，即通过单目相机对环境进行测量。简单来说，本文的工作就是利用下面的两幅图，在已知P1、P2、P3、P4四点世界坐标的情况下，计算出P5的世界坐标。

该研究的应用范围很广，例如对某建筑群，可以通过设置几个已知的标志点（世界坐标已确定），用本文的方法将建筑的各个角的世界坐标求出来，从而测量出建筑的高度，建筑间的距离，乃至将整个建筑群的环境重构出来。又或者在某个露天货场，设置好标志点后只需要无人机飞一圈，就能知道货场中货物的体积有多少，从而安排货运计划。总之该项应用的前景很大，配合目前很火的无人机应用，可以为生产、研究带来不少的便利。最后，本文基于前几篇文章的结果，建议没有看过我博客的读者先读读前面的几篇原理介绍。

原理

在一开始，先设待求点为P。

根据两条直线确定一个点的原理，在二维平面中只要知道两条相交直线的方程，就可以解出它们的交点坐标。现在假设我们是在二维平面中拍照的，如下图：

根据文章《相机位姿估计1：根据四个特征点估计相机姿态》的内容，我们根据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