视觉SLAM:搞定坐标系、三角测量、PnP
Author: [行知SLAM]
Link: [https://zhuanlan.zhihu.com/p/80921759]
1.正文
四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。
世界坐标系：机器人或相机运动过程中，肯定需要知道它的位置，因此需要设定世界坐标系，认定固定不动，作为 参考坐标系，描述世界中的任何一点P（Xw,Yw,Zw)。
相机坐标系：相机或机器人运动的一个坐标系，通过世界坐标系的变换（旋转R,平移T）计算得到。因此主要是将 世界坐标系描述的点坐标P（Xw,Yw,Zw)转换成相机坐标系下描述P（Xc,Yc,Zc)，方便计算得到在成像坐标系的坐 标。
图像（成像）坐标系：描述点在图像坐标系的成像点位置。
像素坐标：在相机中得到的是一个像素，因此主要将图像坐标系的点转换成像素坐标系下。
1.1.将世界坐标系转换得到相机坐标系
旋转 平移
旋转矩阵：R，三维旋转矩阵构成特殊正交群SO(3)。知道它的物理意义就是表示旋转即可。
变换矩阵：T,旋转+平移.变换矩阵构成特殊欧式群SE(3)。
旋转向量（轴角）：一个旋转轴+一个旋转角，外积表示旋转，
欧拉角：绕z轴旋转，得到偏航角yaw,绕旋转之后Y轴旋转，得到俯仰角pitch,绕旋转之后X轴旋转，得到滚转角 roll。
四元数，q=a+bi+cj+dk，类比虚数，同样用来表示旋转。
a.欧式变换：有6个自由度，在变换前后，保持不变的性质是长度、夹角、体积。
b.相似变换：有7个自由度，在变换前后，保持不变的性质是体积比。
c.仿射变换：有12个自由度，在变换前后，保持不变的性质是平行性、体积比。
d.射影变换：有16个自由度，在变换前后，保持不变的性质是接触平面的相交和相切。
计算旋转矩阵
旋转矩阵有9个量，3*3.
1.2相机坐标系与图像坐标系
从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。
此时P点已由上面经过世界坐标系转换成相机坐标系下表述P(Xc,Yc,Zc)。
1.3图像坐标系与像素坐标系
像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成 像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：其中dx和dy 表示每一列和每一行分别代表多少mm，即1pixel=dx mm .
那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。
高翔十四讲简洁描述：K表示相机内参，T表示相机外参，即旋转平移。
其中相机的内参和外参可以通过张正友标定获取（戳这里查看张正友标定的资料)。通过最终的转换关系来看，一个三维中的坐标点，的确可以在图像中找到一个对应的像素点，但是反过来，通过图像中的一个点找到它在三维中 对应的点就很成了一个问题，因为我们并不知道等式左边的Zc的值。
2.视觉里程计
视觉里程计的任务是估算相邻图相间的运动，以及局部地图的样子。由上面的像素坐标系间的关系，我们可以通过 对极约束、三角测量、pnp求得相机的运动。
2D-2D对极约束
预备知识：
A为n维方阵，若有A'=-A，则称矩阵A为反对称矩阵。对于反对称矩阵，它的主对角线上的元素全为零，而位 于主对角线两侧对称的元反号。
外积：axb=ab。将记成一个反对称符号，这样就把外积axb写成了矩阵与向量的乘法a^b。
向量的外积结果就是一个和原来两个向量垂直的向量，更为熟知的叫法是法向量，该向量垂直于a和b向量构 成的平面。
点乘的几何意义是可以用来表征或计算两个向量之间的夹角，以及在b向量在a向量方向上的投影。
对极约束求相机运动，由于我们通过特征点匹配确定了像素点p2的位置，可以推断p的空间位置，以及相机的运动。
世界坐标：
两个像素点p1,p2的位置：
本质矩阵E（Essential Matrix)
基础矩阵F(Fndamental)
单应矩阵(Homography)
求解步骤：
1.根据配对点的像素位置求出E或者F.
2.根据E或者F求出R,t。
也就是只要知道配对点的像素位置就可以通过对极约束求出相机位姿。
三角测量
估计了相机运动后，需要用相机的运动估计特征点的空间位置。
单目SLAM中，通过三角测量估计地图的深度。
3D-2D:PnP（Perspective-n-Point）
描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
特征点的3D位置可以由三角化或RGB-D相机的深度图确定。
在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。
单目视觉里程计中，必须先进行初始化，然后才能使用PnP。
3D-2D方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计。 PnP问题有，用3对点估计位姿的P3P、直接线性变换、EPnP、非线性优化(BA)。
直接线性变换
P3P
目的是求相机位姿。输入数据为3对3D-2D匹配点。3D点为A,B,C,2D点为a,b,c。
知道3D点的世界坐标系中的坐标和2D点的图像位置，不知道相机坐标系的坐标。（之前的是根据已知外参和世界 坐标系的坐标可以求得相机坐标系的坐标，现在不知道相机位姿)。
利用三角形对应关系，求出得到A,B,C相机坐标系下的3D坐标。最后把问题转换成一个3D（世界坐标系）到3D(相 机坐标系)下的位姿估计问题。
Bundle Adjustment
重投影误差的问题。
3D-3D:ICP（Iterative,Closet Point,迭代最近点）
3D-3D的位姿估计问题，假设有一组配对好的3D点。
ICP位姿估计没有出现相机模型，和相机没关系。
在RGBD SLAM中，可以用这种方式估计相机位姿。
求解方式主要两种：线性代数求解（主要是SVD),非线性优化方式求解（类似于BA).
SVD方法
以上部分参考高博PPT及相关博客：https://blog.csdn.net/sunshine_zoe/article/details/73457686?utm_source=blog xgwz0
注：未经授权，请勿转载。