相机投影变换(位姿)

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

针孔相机模型

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

其中:

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

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

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

相机位姿T_{wc}：4*4矩阵，即能够将相机坐标系下3D点PcP_cPc变换为世界坐标系下点PwP_wPw，注意它不是投影矩阵，变换公式为:
Pw=Twc×Pcpw=Rwc×pctpw=tpc+tcwP_w=T_{wc}\times{P_c} \\ p_w=R_{wc}\times{p_c} \\ t^{pw}=t^{pc}+t^{cw} Pw=Twc×Pcpw=Rwc×pctpw=tpc+tcw

位姿的三种等价解释：

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

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

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

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

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

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

TbaT^a_bTba: 表示将b坐标系中的3D点变换到a坐标系中的3D点

投影矩阵

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

投影矩阵公式：

puv=KT×pw=Pproj×pwp_{uv}=KT\times{p_w}=P_{proj}\times{p_w} \\ puv=KT×pw=Pproj×pw
展开为:

[ppixel−xppixel−y1]=[fx0cx0fycy001]×[r11r12r13t1r21r22r23t2r31r32r33t3]×[PwxPwyPwz1]=Pproj×[PwxPwyPwz1]\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} ⎣⎡ppixel−xppixel−y1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤×⎣⎡r11r21r31r12r22r32r13r23r33t1t2t3⎦⎤×⎣⎢⎢⎡PwxPwyPwz1⎦⎥⎥⎤=Pproj×⎣⎢⎢⎡PwxPwyPwz1⎦⎥⎥⎤

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

[ppixel−xppixel−y1]=[fx0cx−baseline∗fx0fycy00010]×[PwxPwyPwz1]\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} ⎣⎡ppixel−xppixel−y1⎦⎤=⎣⎡fx000fy0cxcy1−baseline∗fx00⎦⎤×⎣⎢⎢⎡PwxPwyPwz1⎦⎥⎥⎤

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

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

投影公式为：
Puv=KTPwP_{uv}=KTP_{w} Puv=KTPw
其中：

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

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

展开为:
[uv1]=[fx0cx0fycy001]×[100701080019]×[XYZ1]% \begin{matrix} \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] % \end{matrix} ⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤×⎣⎡100010001789⎦⎤×⎣⎢⎢⎡XYZ1⎦⎥⎥⎤

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

这里假设我们要把一个在相机坐标系中的3D点变换到世界坐标系中的3D点
Pw=Twc×PcP_w=T_{wc}\times{}P_c Pw=Twc×Pc
旋转矩阵变换为：
pw=Rwc×pcp_w=R_{wc}\times{}p_c pw=Rwc×pc
平移向量变换为：从相机坐标系原点指向世界坐标系原点。

其中：

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

展开为：

(PwxPwyPwz1)=(r11r12r13t1r21r22r23t2r31r32r33t30001)×(PcxPcyPcz1)\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} ⎝⎜⎜⎛PwxPwyPwz1⎠⎟⎟⎞=⎝⎜⎜⎛r11r21r310r12r22r320r13r23r330t1t2t31⎠⎟⎟⎞×⎝⎜⎜⎛PcxPcyPcz1⎠⎟⎟⎞

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

多重位姿变换

现在有:

世界坐标系下点Pw=(Pwx,Pwy,Pwz,1)P_{w}=(P_{wx},P_{wy},P_{wz},1)Pw=(Pwx,Pwy,Pwz,1)，
相机坐标系F1对应的相机位姿T1T_1T1,以及F1下PwP_{w}Pw对应的相机坐标系坐标Pc1P_{c1}Pc1
相机坐标系F2对应的相机位姿T2T_2T2,以及F1下PwP_{w}Pw对应的相机坐标系坐标Pc2P_{c2}Pc2
坐标系F1到F2之间的欧式变换T12T_{12}T12，即将心坐标系F2中的点变换到F1中的点，
满足：
Pw=T1∗Pc1Pc1=T12∗Pc2得到:Pw=T12∗T1∗Pc2P_{w}=T_1*P_{c1} \\ P_{c1}=T_{12}*P_{c2} \\ \quad得到: \\ P_{w}=T_{12}*T_1*P_{c2} Pw=T1∗Pc1Pc1=T12∗Pc2得到:Pw=T12∗T1∗Pc2

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

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

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

坐标转换公式为：
Z×Puv=K×Pc即ZPuv=KPcZ\times{}P_{uv}=K\times{}P_{c} \\ 即 ZP_{uv}=KP_{c} Z×Puv=K×Pc即ZPuv=KPc
其中：

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

展开为：

Z(uv1)=(fx0cx0fycy001)×(XYZ)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} Z⎝⎛uv1⎠⎞=⎝⎛fx000fy0cxcy1⎠⎞×⎝⎛XYZ⎠⎞