Bundel Ajustment（BA）基础

---

本周任务及进展

\qquad 这周的任务是把PoseOptimation函数的优化方程推导出来；通过学习发现PoseOptimation函数中使用了BA的相关知识，向前递推前置知识点：BA——>DLA——>相机模型——>齐次方程。这周把知识点整个串了一便，并列出BA的优化方程及初始的计算思路。

---

一、齐次方程

1.两条直线能不能相交？

\qquad “平面空间内的平行两条直线永不相交”，欧式（几何）空间内的两条平行直线是不会相交的。但是我们在日常生活中会遇到如下图的一种现象：道路两旁的车道线是平行的，越向远方两条线距离越近，最终在远方相交了。我们不去讨论这种现象设计到的透视空间原理，只关注结果：如果用几何坐标表示远处的那个点将是 $(\infty ,\infty )$，对于机器人来说没有任何意义。为了表示平行线在空间中无穷远位置相交为一个点，便有了齐次方程。

2.齐次坐标的具体表示

\qquad 齐次方程一句话来说就是用N+1维坐标来表示N维坐标。对于二维坐标$(X,Y)$，在齐次坐标下变为$(x,y,w)$，x与X，y与Y的关系如下：
$$X=\frac{x}{w}$$ $$Y=\frac{y}{w}$$
例如，二维坐标$(2,3)$，对应的齐次坐标可以为$(2,3,1)$。
\qquad 为什么说可以为而不是一定为，是由于齐次坐标有一个很显著的特性：用一个常数去乘齐次坐标，其对应的几何坐标不变。
\qquad 例如：将刚才的齐次坐标同乘$5$变为$(10,15,5)$，转换为几何坐标为$(2,3)$，所以可能见到的许多齐次坐标都对应同一个几何坐标。
\qquad 总结来说：把齐次坐标转化为笛卡尔坐标的方法是前面n-1个坐标分量分别除以最后一个分量即可。齐次坐标就是一个比例关系，多出来的一维让前两维有了不变成$\infty$的意义。回到最开始的$(\infty ,\infty )$坐标，可以用$(1,2,0)$来表示成齐次坐标，这样前两维就不用表示成无穷形式了。

二、相机模型

1.四个坐标系

1.1世界坐标系

如上图蓝色坐标系所示，世界坐标系就是现实场景中的坐标系，也是我们构建的点云排布的坐标系。记作：
P w = { x , y , z } P_w=\lbrace x,y,z \rbrace Pw​={x,y,z}
单位往往是米(m)

1.2相机坐标系

如上图红色坐标系所示，相机坐标系位于相机平面，可以简单的理解为相机的表面，其坐标原点为相机中心（根据相机不同位置可能不同）。记作：
P c = { x , y , z } P_c=\lbrace x,y,z \rbrace Pc​={x,y,z}
单位是米(m)
我们把相机坐标系和世界坐标系拿出来看看他们的关系：

不难看出这两个三维坐标系，只需要经过旋转和平移便可以重合，用数学模型表示为：
$$P_c=T_{cw}{P}_w$$
$T_{cw}$为旋转平移矩阵，具体转化公式如下：
$$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$

1.3图像坐标系

图像坐标系是相机图像传感器（CMOS）所在平面的坐标系，相机通过小孔成像原理将三维世界映射为二维图像，这也就是相机坐标系转为图像坐标系的关键。

$f$为相机焦距，$y_c$和$z_c$为相机坐标系，$y$为图像坐标系，图像坐标系的原点也为图像传感器的中心，数学模型表示为：
P : { x , y } P:\lbrace x,y \rbrace P:{x,y}
单位往往是毫米(mm)
根据图像我们可以把两个坐标系的关系表示出来：
$$\frac{z_c}{f}=\frac{x_c}{x}=\frac{y_c}{y}$$
通过$z$和$f$的比例关系来推出图像坐标系的$x$和$y$。

1.4像素坐标系

像素坐标系和图像坐标系是在同一平面上的，区别在于两个：1.坐标原点不同。2.坐标系单位不同。 对于第一点，如下图所示图像坐标系的原点为图像平面中心，像素坐标原点往往是图像平面的左上角。第二点，像素坐标系单位为像素(pixel)，而图像坐标单位为毫米(mm)。
p : { u , v } p:\lbrace u,v \rbrace p:{u,v}

图像坐标转变为像素坐标就简单多了：
$$u=\alpha x+c_{x}v=\beta y+c_y$$
这里我有一个思考，$\alpha$和$\beta$应该是一样的，毕竟像素也是一个小方块，经过查到的一些资料，$\alpha$和$\beta$大多数时间并不相等，要受到相机本身的约束，有效像素这个概念说的就是会有一些像素点在成像的时候并不使用或是用来记录其他信息。

2.相机坐标系向像素坐标系的转换

把上面的式子带入得到我们最终的结果：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]={\frac{1}{z}}_c\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_x& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]={\frac{1}{z}}_{c}K{P}_c=p$$
这里的$K$指的是相机的内参，也就是我们在图像坐标系转像素坐标系里面的平移和换算。${\frac{1}{z}}_c$这里可以省略变成：
$$K{P}_c=p$$
我们在齐次坐标系里面得出的结论：用一个常数去乘齐次坐标，其对应的几何坐标不变。
这里在最后思考一个问题：世界坐标系该怎么去选，我们怎么去知道相机坐标系和它之间的旋转和平移，有一个思路是相机开始工作的时候，来选取两个坐标系，让两个坐标系重合，换言之：相机初始位置为世界坐标系原点，坐标轴和相机坐标系重合。由于相机的运动是连续线性的，我们只需要关注每一时刻相机的移动就得出了$T_{cw}$矩阵了。

三、PnP模型（DLA法）

1.已知条件

我们拿相机去拍摄照片能得到什么信息：1.图像特征点的像素坐标。2.相机的内参。3.两张图中特征点的对应关系。4.第一张图的深度信息$Z_c$
我们要求的是旋转平移矩阵$T_{cw}$，求出该矩阵三维点的坐标自然也就求得了。

2.公式表示

我们回顾一下相机坐标系映射像素坐标的公式：
$$K{P}_c=p$$
接下来我们向世界坐标系做推广，也就是加上旋转平移矩阵$T_{cw}$：
$$p=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]={\frac{1}{z}}_c\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_x& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]={\frac{1}{z}}_{c}K{T}_{cw}{P}_c$$
我们来想想一种情况，相机在刚开始就捕获了一些特征点，此时$P_c$和$P_w$两个坐标系重合，也就是说我们在一开始就获得了特征点的三维坐标，在下一时刻我们捕获了相同的特征点，根据这个特征点我们可以算出此时的$T_{cw}$矩阵，而此时又发现了新的特征点，同样可以取得他们的三维点坐标，由此循环往复，所有的位姿信息都可以得到。

下一时刻的公式表示如下：
$$p_2=\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]={\frac{1}{z}}_c\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_x& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right]\left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\\ 1\end{array}\right]={\frac{1}{z}}_{c}K{T}_{cw2}{P}_{c1}$$
接下来就是求解$T_{cw}$，用多个匹配的特征点带入求得。

四、Bundel Ajustment(BA优化)

1.引入BA

我们可以根据DLA求出位姿信息，那么我们就可以得到第二帧的二维点坐标（根据位姿求得，不是真实点坐标）:
$$p_2={\frac{1}{z}}_{c2}K{T}_{cw}{P}_{w2}$$
但是，算出来的p2一定精确吗，换句话说，我们求得的位姿一定精确吗，求解$t_i$时我们需要多个点来求解，难免有一些点匹配的不准确、质量低等等，也就是存在误差，出现下图中的情况，计算位置偏离了真实位置：

Bunbel Ajustment就是来解决这个问题，尽可能的去减少误差，优化位姿矩阵，甚至去优化三维点坐标。
往下继续做铺垫，我们回看最开始的等式，发现存在未知数$P_{w2}$第二时刻的变换矩阵，接下来把思路从世界坐标系转变成相机坐标系：相机运动是根据时间线性变化的。于是：
$$P_{c2}=T{P}_{c1}$$

2.优化方程

$$e=p_2-{\frac{1}{z}}_{c2}K{P}_{c2}=p_2-{\frac{1}{z}}_{c2}Kexp({\xi }^{!})P_{c1}$$

• $e$是定义的误差；

• $p_2$是第二时刻的真实像素坐标；

• $K$是相机内参；

• $P_{c2}$是第二时刻的相机坐标；

• $\xi$是旋转向量（李代数）；

• ${\xi }^{!}$是旋转向量的矩阵表达；

• $exp({\xi }^{!})$表达从向量（李代数）到矩阵（李 群）的对数映射。

这里回顾一下李群和李代数上的一些思想：李群（矩阵）对加法不封闭（无法求导），要通过对数映射转成李代数（可以求导）。

3.优化方程求解

我们要优化的就是旋转向量$exp({\xi }^{!})$，下面我们把它提出来：
$$E(\xi \oplus +\Delta \xi )=e^T(\xi \oplus +\Delta \xi ){\Sigma }^{-1}e(\xi \oplus +\Delta \xi )$$
对$e(\xi \oplus +\Delta \xi )$进行线性近似就等于$e(\xi )+J\Delta \xi$
接下来就是求雅各比矩阵$J$了，也就是$e$对$\Delta \xi$求导了。
$$\frac{\partial e}{\partial \Delta \xi }=\frac{\partial e}{\partial P_{c2}}\frac{\partial P_{c2}}{\partial \Delta \xi }$$