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

目录

前言

一、针孔相机的原理

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

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

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

---

前言

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

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

整体的转换流程：

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

一、针孔相机的原理

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

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

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

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

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

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

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

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

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

将αf合并成为，βf合并成为，那么上式就可以转换为：

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

对于上式，定义相机内参矩阵K：

那么上式可以简化为：

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

 

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

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

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

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

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

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

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

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