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_{}$