SLAM公式引出、推导和理解 2-1

---

2 相机成像—观测方程

2.1 单目相机

以针孔相机为例，小孔成像的原理用数学表示可视为相似三角形。设 $O-x-y-z$ 为相机坐标系，$O$ 为相机光心，空间一点 $P$ 在相机坐标系下的坐标为 $[X,Y,Z{]}^T$，点 $P$ 对应的像 $P^{\prime }$ 的坐标为 $[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$，相机的焦距设为 $f$ （光心到像的距离）则有$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$（将像置于物的同一侧，即去掉坐标系下的负号）
整理得$$\begin{array}{r}{X}^{\prime }=f\frac{X}{Z}\\ Y^{\prime }=f\frac{Y}{Z}\end{array}\text{\hspace{1em}(2.1.1)}$$上述公式单位都为米（m）。在相机中，我们得到的照片都是像素（pixel）为单位，还需在成像平面上对像进行采样和量化（量化的最小单位为像素）。因此，设像素坐标系 $o-u-v$（二维）固定在成像的平面上，$o$ 点位于图像的左上角，$u$ 轴向右与 $x$ 轴平行，$v$ 轴向下与 $y$ 轴平行。像素坐标系和成像平面之间差了一个缩放和平移（成像原点一般位于图像的中心），设像素坐标在 $u$ 轴上缩放了 $\alpha$ 倍，在 $v$ 轴上缩放了 $\beta$ 倍，原点平移了 $[c_x,c_y{]}^T$。则 $P^{\prime }$ 点坐标与像素坐标 $[u,v{]}^T$ 的关系为$$\{\begin{array}{l}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}$$把式子(2.1.1)带入该式，令 $\alpha f=f_x$，$\beta f=f_y$可得$$\{\begin{array}{l}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{array}$$根据 $f$ 单位为米，$\alpha$，$\beta$ 单位为像素/米，因此 $f_x$，$f_y$ 和 $c_x$，$c_y$ 的单位为像素。上式通过齐次坐标改写为矩阵形式$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\stackrel{def}{=}\frac{1}{Z}KP\text{\hspace{1em}(2.1.2)}$$或者$$Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\stackrel{def}{=}KP\text{\hspace{1em}(2.1.3)}$$称矩阵 $K$ 为相机内参（Camera Intrinsics）矩阵。通常认为，相机在出厂之后的内参就是固定的，有时厂商会告诉内参，有时需要通过标定来确认内参。有内参，就有相对的外参。在式子(2.1.2)中的 $P$ 是在相机坐标系下的坐标 ($P_c$)，用世界坐标表示，点 $P$ 的在相机坐标下的坐标为 $P(P_c)=T{P}_w=R{P}_w+t$，那么有$$Z{P}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K(R{P}_w+t)=KT{P}_w\text{\hspace{1em}(2.1.4)}$$该式描述了点 $P$ 的世界坐标 $P_w$ 到像素坐标 $P_{uv}$ 的投影关系，其中相机的位姿 $R$，$t$ 又称为相机的外参数（Camera Extrinsics），外参会随着相机运动而改变，也就是SLAM中待估计的目标，表示机器人的运动轨迹。
对式子(2.1.4)逆着考虑可得出：可通过世界坐标系下坐标 $P_w$ 转化成相机坐标 $P$，对 $P$ 进行深度归一化处理可得在相机平面上的归一化坐标 $P^{\prime }$（归一化坐标可以看成相机前方 $z=1$ 的平面上的坐标，$z=1$ 的平面也称归一化平面），然后再对 $P^{\prime }$ 左乘相机内参矩阵，可得像素坐标 $P_{uv}$。 像素坐标可以视为归一化坐标，但是不是归一平面上的坐标，两者相差一个内参矩阵。
$$R{P}_w+t⟶P=[X,Y,Z{]}^T⟶P^{\prime }=[X/Z,Y/Z,1{]}^T⟶P_{uv}=[u,v,1{]}^T\text{\hspace{1em}(2.1.5)}$$注意到，$P$ 坐标中的深度在投影中被丢失了，所以单目相机无法得到像的深度值。

---

2.2 畸变问题

由于相机安装了的透镜，可能是因为透镜对光线传播的影响，而出现的畸变（Distortion，也成为失真）问题，称为径向畸变（Radial Distortion），又或者是因为在组装时，透镜与成像平面没有完全平行，而出现的畸变问题为切向畸变（Tangential Distortion）。对于归一化平面上的任意一点 $p=[x,y{]}^T=[r,\theta {]}^T$，其中 $r$ 为点 $p$ 与坐标系原点之间的距离，$\theta$ 表示与水平轴的夹角（极坐标形式）。径向畸变可以看成坐标点沿着长度方向（距离原点的长度）发生了变化，切向畸变可以看成坐标点沿着切向方向（水平夹角）发生了变化。假设畸变呈多项式关系，即对径向畸变来说$$\begin{gathered} x_{distorted}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \\ {y}_{distorted}=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6) \end{gathered}$$其中，$[x_{distorted},y_{distorted}{]}^T$ 表示发生畸变点的归一化坐标。 对于切向畸变，可以使用 参数$p_1$，$p_2$ 进行纠正：$$\begin{gathered} x_{distorted}=x+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{distorted}=y+p_1(r^2+2y^2)+2p_{2}xy \end{gathered}$$整合上述两个公式$$\{\begin{array}{l}{x}_{distorted}=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p_2(r^2+2x^2)\\ y_{distorted}=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+p_1(r^2+2y^2)+2p_{2}xy\end{array}\text{\hspace{1em}(2.2.1)}$$

切向畸变通过工艺手段可以忽略，只需要考虑径向畸变。$x_{distorted}$ 角标已表明为发生畸变的坐标。通常来讲，$xy$为发生畸变之前的，也就是我们想要的，为未知变量，而$x_{distorted}$为发生了畸变的，为已知变量。按照常规想法对于式子(2.2.1)来说，根据已知求未知是较为麻烦的逆运算。但是我们假设得到$xy$坐标的点，代入公式，得出发生畸变的图像，将$x_{distorted}$的坐标替换掉$xy$坐标。不过到的$x_{distorted}$可能不是整数，需要插值计算。

将畸变之后的点通过内参矩阵投影到像素平面$$\{\begin{array}{l}u=f_x{x}_{distorted}+c_x\\ v=f_y{y}_{distorted}+c_y\end{array}\text{\hspace{1em}(2.2.2)}$$因此在式子(2.1.5)中插入式子(2.2.1)(2.2.2)则有$$P^{\prime }=[X/Z,Y/Z,1{]}^T⟶P_{distorted}^{\prime }=[x_{distorted},y_{distorted},1{]}^T\stackrel{(\mathrm{2.2.2})}{⟶}{P}_{uv}=[u,v,1{]}^T)\text{\hspace{1em}(2.2.3)}$$

---

2.3 双目相机

由于单目相机无法根据一个像素测量物体深度，因此提出双目相机。双目相机测量深度的原理类似与人的双眼，是通过左右眼的间距(视差)来判断物体与我们的距离。
双目相机都是水平放置在同一横轴上，两者光心的距离为基线（Baseline），记作 $b$。考虑空间点 $P$，左右眼相机各成一像，记作 $P_L$ 和 $P_R$。由于基线 $b$ 的存在，两个像的位置不同。理想情况下，左右相机只在 $x$ 轴上有位移，对应图像上的 $u$ 轴，记左侧坐标为 $u_L$，右侧坐标为 $u_R$，如图所示，由相似关系得$$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$$整理得$$z=\frac{fb}{d}d\stackrel{def}{=}{u}_L-u_R\text{\hspace{1em}(2.3.1)}$$其中 $d$ 定义为左右图的横坐标（左右横坐标均为整数）之差，称为视差（disparity）。由上式可知，视差 $d$ 与物体深度 $z$ 成反比，$d$ 越小，$z$ 越大。同时，视差最小为一个像素，因此深度存在一个理论上的最大值，由 $fb$ 确定。基线 $b$ 越长，看到的距离越远。
虽然双目成像的原理很简单，但是对于计算机来说，视差 $d$ 的计算却比较困难。我们需要确切地知道左眼图像的某个像素出现在右眼图像的哪个位置（即对应关系），当我们想计算每个像素的深度时，其计算量与精度都是一个很大的问题，而且只有在图像纹理变化丰富的地方才能计算视差。

---

齐次坐标

参考 https://zhuanlan.zhihu.com/p/258437902
欧式空间中，两条平行直线不可能相交。但在投影空间中，两条平行直线可以相交于无穷远点（如火车铁轨在无穷远处相交—投影）。 一个2D点在笛卡尔坐标系中表示为$(x,y)$，欧式空间无法表示相交于无穷远点。
齐次坐标是用N+1个维度来表示N维坐标。$(x,y)$ 有相应的齐次坐标$(x,y,w)$，其中 $w$ 为额外变量。
两个坐标相互转化
$$\begin{array}{c}(x,y,w)\\ \text{ Homogeneous }\end{array}\iff \begin{array}{c}(\frac{x}{w},\frac{y}{w})\\ \text{ Cartesian }\end{array}$$

如
$$\begin{array}{lcc}\text{Homogeneous}& & \text{Cartesian}\\ (1,2,3)& \Rightarrow & \left(\frac{1}{3},\frac{2}{3}\right)\\ (2,4,6)& \Rightarrow & \left(\frac{2}{6},\frac{4}{6}\right)=\left(\frac{1}{3},\frac{2}{3}\right)\\ ⋮& & ⋮\\ (1a,2a,3a)& \Rightarrow & \left(\frac{1a}{3a},\frac{2a}{3a}\right)=\left(\frac{1}{3},\frac{2}{3}\right)\end{array}$$
可以看出 $(1,2,3)$ 和 $(2,4,6)$ 对应于同一欧式点 $(1/3,2/3)$。因此这些点都是齐次(Homogeneous)的，视为在欧式空间中代表同一点。

数学证明
欧式空间的线性系统
$$\{\begin{array}{l}Ax+By+C=0\\ Ax+By+D=0\end{array}$$
因为 $C\ne D$（两条直线不重叠），方程无解。
改为投影空间方程，将 $x$ 、$y$ 替换为 $\frac{x}{w}$ 、$\frac{y}{w}$。
$$\{\begin{array}{l}A\frac{x}{w}+B\frac{y}{w}+C=0\\ A\frac{x}{w}+B\frac{y}{w}+D=0\end{array}\Rightarrow \{\begin{array}{l}Ax+By+Cw=0\\ Ax+By+Dw=0\end{array}$$
两个方程相减，$(C-D)w=0$ ，则 $w=0$。因此两条直线在 $(x,y,0)$ 处相交。

齐次坐标通过增加一个额外的维度，来对几何体进行缩放、旋转、平移和透视投影等矩阵变换。
任何N维的齐次坐标，只要 $w\ne 0$ ，都可以转换为 $w=1$ 的向量，然后获得其N-1维欧式空间的点值。而当 $w=0$ ，该坐标表示无限长的向量，为N-1维的矢量。

欧式几何是投影几何的子集，如$(a,b)\in (a,b,1)$。