无人机——像素坐标系转世界坐标系（NED）

前言

一、针孔相机的原理

二、像素坐标系转相机坐标系

三、相机坐标系转机体坐标系

四、机体坐标系转世界坐标系（NED）

前言

正文开始前先推荐我做这个坐标系转换受益最深的一本书《视觉SLAM14讲》，为什么要推荐这本书，首先这本书详细介绍了视觉slam的结构，从前端VO到后端Optimization到回环检测最后建图，十分详细并且通俗易懂。

我还没有读完这本书，因为最近正在做一个比赛，其中一项工作是做了一个圆圈检测，要将检测结果从像素坐标系转换到NED坐标系，起初找了别人的源码，但是还是想从原理上理解这个坐标系变换，所以我细读了《视觉SLAM14讲》这本书的第五讲“相机与图像”，从针孔相机的原理开始了解最后转换到世界坐标系。

整体的转换流程：

本人初次撰写博客，若全文有不当或者不合理之处，请予批评指正

一、针孔相机的原理

话不多说，先细说针孔相机的原理，为什么要介绍这个原理，因为这个原理让我认识清楚了相机内参（fx，fy，cx，cy）的含义，以及我们为什么要在使用相机前做矫正。

首先借用《视觉SLAM14讲》P97中的一张图：

左边的图大家应该很熟悉，和初二物理课本上的图很像，右边的图相当于左边图的俯视图（从上往下看），所以没有考虑y轴分量。这张图已经包含了整个转换过程的前两个坐标系，像素坐标系和相机坐标系。

二、像素坐标系转相机坐标系

现实中的点为 $P[X,Y,Z]^{T}$ ,可以认为P在相机坐标系下，经过针孔投影后的点为 $P'[X',Y',Z']^{T}$ ，P’可以认为在像素坐标系下，但是要注意这个像素坐标系不是我们实际使用的像素坐标系，两者之间还相差缩放以及平移关系，这也就是相机内参的含义，后文将会详细介绍。还有一个比较重要的物理量是焦距(f),即相机光心O到成像平面的距离。

根据上图中的相似三角形的关系很容易可以得到：

$\frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'}$

对上式进行一个变换，将成像平面的坐标点放在左边：

$X' = f \frac{X}{Z}$

$Y' = f\frac{Y}{Z}$

到这一步，像素坐标系到相机坐标系的转换已经完成了一半，还差一个成像平面到像素坐标系的缩放平移。一般像素坐标系定义为o-u-v,其中o是像素坐标系的原点，位于图像的左上角，u轴与成像平面的x轴平行，v轴与成像平面的y轴平行，这个对于熟悉opencv的同学应该很熟悉。然后我们需要定义成像平面到像素坐标系的缩放平移关系，这里一定要弄明白，相机内参就是来源于此，缩放关系：我们假设像素坐标在u轴上缩放了α倍，在v轴上缩放了β倍，这里α和β的单位都是像素/m，因为成像平面和相机坐标系的点都是以m为单位的，平移关系：像素坐标系相对于成像平面平移了 $[cx,cy]^{T}$ ，那么成像平面P'的坐标和像素坐标系 $[u,v]^{T}$ 的关系如下：

$\begin{cases}u =\alpha X' + c_{x}\\ v =\beta Y'+ c_{y}\\ \end{cases}$

将上式代入其中即可得到：

$\begin{cases}u = \alpha f\frac{X}{Z} + c_{x}\\ v = \beta f\frac{Y}{Z}+ c_{y}\\ \end{cases}$

将αf合并成为 $f_{x}$ ，βf合并成为 $f_{y}$ ，那么上式就可以转换为：

$\begin{cases}u = f_{x}\frac{X}{Z} + c_{x}\\ v = f_{y} \frac{Y}{Z}+ c_{y}\\ \end{cases}$

再将其写成矩阵相乘的形式。

$\begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = \frac{1}{Z}\begin{bmatrix} f_{x} & 0 &c_{x} \\ 0&f_{y} & c_{y}\\ 0&0 &1 \end{bmatrix}\begin{bmatrix} X\\ Y\\ Z \end{bmatrix}$

对于上式 $P[X,Y,Z]^{T}$ ，定义相机内参矩阵K：

$K = \begin{bmatrix} f_{x} & 0 &c_{x} \\ 0&f_{y} & c_{y}\\ 0&0 &1 \end{bmatrix}$

那么上式可以简化为：

$\begin{bmatrix} u\\ v\\ 1 \end{bmatrix} = \frac{1}{Z}KP$

将上式做一个简单的变形：

$P = ZK^{-1}\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}$

上式就是像素坐标系转相机坐标系的表达式，可以看出一个点要想从像素坐标系转换到相机坐标系需要三个信息，首先是该点在像素坐标系下的坐标 $(u,v)^{T}$ ,然后需要相机的内参矩阵K，最后一个需要的就是深度信息Z，即该点在相机坐标系下Z轴的分量，如果你使用的是单目相机，那么有单目测距的手段，如果你使用的是双目相机，有双目测距的手段，如果你使用的是RGBD相机，那么直接就可以获取深度信息，如此一下，像素坐标系转相机坐标系就完成了。

三、相机坐标系转机体坐标系

首先相机坐标系转机体坐标系并不存在平移的关系，只是定义的不同，相机坐标系满足右手定则，机体坐标系满足左手定则：

所以两者之间只需要一个简单的转换矩阵，定义机体坐标系下的点为 $P_{b}$ ，定义相机坐标系下的点为 $P_{c}$ ，定义相机坐标系到机体坐标系的转换矩阵为 $R_{c2b}$ :

$R_{c2b} = \begin{bmatrix} 0 &1 &0 \\ 0& 0 & 1\\ 1& 0 &0 \end{bmatrix}$

那么从相机坐标系到机体坐标系的转换如下：

$P_{b} = R_{c2b}P_{c}$

四、机体坐标系转世界坐标系（NED）

首先要理清，机体坐标系是与机体固连的，随着飞机的飞行，机体坐标系的原点以及对应的x,y,z三轴的方向都会发生相应的变化，而世界坐标系（此处我们以NED坐标系，即北东地坐标系为例）的原点以及三轴方向不会发生变化，两者之间相差的就是飞机的位姿关系，位姿即飞机的位置信息和姿态信息（在NED坐标系下），这个信息可以依靠视觉里程计得到，而位姿信息我们一般用旋转矩阵R和平移向量t来描述，旋转矩阵描述的就是姿态信息，通过四元数或者欧拉角都可以得到旋转矩阵，而位置信息就是飞机在NED坐标系下的位置，那么机体坐标系下的某个点 $P_{b}$ 在NED坐标系下即可表示为：

$P_{NED} = RP_{b} + t$

至此，坐标系转换全部完成！