相机投影变换(位姿)

相机投影变换(位姿).md
针对SLAM新手中常见的对各种变换的迷惑，我这里答疑一下。

针孔相机模型

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-cPyc6VkK-1587194378847)(:/8acde09162b7499bb062e947abb2db78)]

其中:

• $M_{ext}$是等距变换，也就是欧式刚体变换
• $M_{proj}$是投影变换（摄影变换/透视变换），表达了空间中的一个正方形线框投影到平面上是什么形状。
• $M_{aff}$是一个仿射变换，但是实际上其本质我认为是一个相似变换+一个翻转形成的

参考
图像变换——等距变换，相似变换，仿射变换，投影变换
仿射变换与投影变换
摄像机模型

什么是相机位姿，投影矩阵，欧式变换矩阵

相机位姿T_{wc}：4*4矩阵，即能够将相机座标系下3D点$P_c$变换为世界座标系下点$P_w$，注意它不是投影矩阵，变换公式为:
$P_w=T_{wc}\times{P_c} \\ p_w=R_{wc}\times{p_c} \\ t^{pw}=t^{pc}+t^{cw}$

位姿的三种等价解释：

• 一种是位姿指的是Twc，可以将相机座标系下的点变换到世界座标系下的点
• 一种是位姿指的是相机座标系的原点在世界座标系下的3D座标
• 一种是把世界座标系变换为相机座标系这个过程对应的那个变换

注：ORBSLAM中的Tcw代表将一个点从世界座标变换到相机座标，这不是相机位姿，而是相机位姿的逆
注意分清楚对一个3D点的变换，以及对座标系的变换，两者叙述刚好相反。

下面是论文中常见的使用表述

两个座标系F1，F2之间的欧式变换：指的是将新的座标系F2中的点变换为原来的旧的座标系F1中点的座标，标记为$T_{12}$

座标系F1到F2的欧式变换： 指的是将座标系F1中的点变换到F2中的点

Sensor extrinsics wrt. the body frame: 指的是该外参将sensor座标系下的3D点变换到Body座标系下的3D点

$T^a_b$: 表示将b座标系中的3D点变换到a座标系中的3D点

投影矩阵

投影矩阵 ： 一般的投影矩阵被广泛认为是相机内参乘以从世界座标系变为相机座标系的3*4矩阵，得到的一个3*4的矩阵，其可以把一个世界座标系下的点直接变为像素座标，（但是我下面论述的时候有可能会把世界座标系变换为相机座标系的3*4矩阵叫做投影矩阵，最终得到相机座标系下的3D点座标（非归一化相机座标系），注意区分，这两种叫法都有，一般以第一种包含内参K的叫法为准）

投影矩阵公式：

$p_{uv}=KT\times{p_w}=P_{proj}\times{p_w} \\$
展开为:

$\begin{bmatrix} p_{pixel-x} \\ p_{pixel-y} \\ 1 \end{bmatrix} =\left[ \begin{array}{ccc} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \end{array} \right] \times{} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_1 \\ r_{21} & r_{22} & r_{23} & t_2 \\ r_{31} & r_{32} & r_{33} & t_3 \\ \end{bmatrix} \times{} \begin{bmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{bmatrix} =P_{proj}\times{} \begin{bmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{bmatrix}$

顺着双目相机看的方向，左眼为左目，右眼为右目。
假设现在双目相机左右目已经校正过了，光心平行，现在以左目座标系为世界座标系，现有一世界座标系下点
$P=[p_{wx},p_{wy},p_{wz}]^T$，
（注意：令将点从左目座标系变换到右目座标系的平移矩阵t={-30,0,0},所以在左目座标系下，右目座标系相对于左目座标系的基线baseline=30m，即右目在左目的X轴正方向30m处。）
则将该点P变换到右目像素座标系的像素座标，即右目座标系的投影矩阵为：

$\begin{bmatrix} p_{pixel-x} \\ p_{pixel-y} \\ 1 \end{bmatrix} =\left[ \begin{array}{ccc} fx & 0 & cx & -baseline*fx\\ 0 & fy & cy & 0\\ 0 & 0 & 1 & 0 \end{array} \right] \times{} \begin{bmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{bmatrix}$

投影矩阵与相机位姿： 投影矩阵是3*4矩阵，指的是将世界座标系中的3D点变为像素座标系中的2D点，相机位子是将相机座标系下的3D点变为世界座标系下的3D点，有一点互逆的关系，但是投影矩阵比相机位姿多了一个内参矩阵K的值

世界座标系下“3D座标” 转 像素座标系下“像素座标”

投影公式为：
$P_{uv}=KTP_{w}$
其中：

• $P_{uv}$是一个3维列向量，前两维为像素座标u和v，是二维齐次座标。
• $K$矩阵为3*3矩阵，就是相机内参数矩阵
• $T$矩阵为34矩阵，为从世界座标投影到相机座标系的投影矩阵，也是该相机此时刻的位姿，左边33维为相对于最初始相机位姿旋转矩阵，右边的3*1维为平移向量
• $P_{w}$为场景点在世界座标系下的3D位置，是4*1向量，前三个元素为X，Y，Z轴座标，最后一个为1，是三维齐次座标。
• KT合起来叫做相机的投影矩阵，可以直接将世界座标系变为像素座标系

注，如果有畸变的话，那上述uv值为去畸变后的uv值，之后计算过程一致

展开为:
$\left[ \begin{array}{c} u \\ v \\ 1 \end{array} \right]= \left[ \begin{array}{ccc} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \end{array} \right] \times{} \left[ \begin{array}{cccc} 1 & 0 & 0 & 7\\ 0 & 1 & 0 & 8\\ 0 & 0 & 1 & 9 \end{array} \right] \times{} \left[ \begin{array}{c} X \\ Y \\ Z \\ 1 \end{array} \right]$

纯粹两个座标系之间的单一变换

这里假设我们要把一个在相机座标系中的3D点变换到世界座标系中的3D点
$P_w=T_{wc}\times{}P_c$
旋转矩阵变换为：
$p_w=R_{wc}\times{}p_c$
平移向量变换为：从相机座标系原点指向世界座标系原点。

其中：

• $P_c$为相机座标系下3D空间点座标
• $T_{wc}$为位姿变换矩阵，指的是从相机座标系到世界座标系之间的变换，也就是相机位姿。注意其3*4部分仍然不是投影矩阵！！！
• $P_w$为世界座标系下3D空间点座标

展开为：

$\begin{pmatrix} P_{wx} \\ P_{wy} \\ P_{wz} \\ 1 \end{pmatrix} =\begin{pmatrix} r_{11} & r_{12} & r_{13} & t_1 \\ r_{21} & r_{22} & r_{23} & t_2 \\ r_{31} & r_{32} & r_{33} & t_3 \\ 0& 0 & 0 & 1 \end{pmatrix} \times{} \begin{pmatrix} P_{cx} \\ P_{cy} \\ P_{cz} \\ 1 \end{pmatrix}$

约定：$T_{wc}代表一个能将3D点从相机座标系转换为世界座标系下的3D点，这样的一个的变换矩阵$

多重位姿变换

现在有:

• 世界座标系下点$P_{w}=(P_{wx},P_{wy},P_{wz},1)$，
• 相机座标系F1对应的相机位姿$T_1$,以及F1下$P_{w}$对应的相机座标系座标$P_{c1}$
• 相机座标系F2对应的相机位姿$T_2$,以及F1下$P_{w}$对应的相机座标系座标$P_{c2}$
• 座标系F1到F2之间的欧式变换$T_{12}$，即将心座标系F2中的点变换到F1中的点，
  满足：
  $P_{w}=T_1*P_{c1} \\ P_{c1}=T_{12}*P_{c2} \\ \quad得到: \\ P_{w}=T_{12}*T_1*P_{c2}$

相机位姿就是当前相机座标系和世界座标系之间的欧式变换矩阵，这个矩阵将点从当前相机座标系变换为世界座标系中的点

初始相机位姿为4*4单位矩阵，代表世界座标系经过单位矩阵变换仍然是同一个变换矩阵，其中世界座标系与初始的相机朝向有一个固定关系，比如一般定义为Z轴正方向为相机看的方向。从而建立相机看的方向与欧式变换矩阵真正的关联

相机座标系下“相机座标” 转 像素座标系下“像素座标”

座标转换公式为：
$Z\times{}P_{uv}=K\times{}P_{c} \\ 即 ZP_{uv}=KP_{c}$
其中：

• $P_{uv}$为一个三维列向量，前两维为像素座标u和v，是二维齐次座标
• Z为相机座标系下3D点的Z座标
• K矩阵为3*3相机内参矩阵
• $P_{c}$为相机座标系下的3D座标

展开为：

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