标签（空格分隔）： SummerCamp SLAM
[toc]
一般来说，现阶段SLAM和SfM的基本理论依据是多视图几何中的知识，本章将主要介绍二维和三维中的几何基础知识。
本课程将会介绍多视图几何中最基础的点线面和线性变换的知识，具有高度的概括性，建议读者结合本课程的内容以及多视图几何书籍进行学习。
本课程要重点理解二维中的射影变换，三维中的相似变换和欧氏变换，针孔模型，本质矩阵，基础矩阵之间的关系。
首先，我们约定$(x,y)$默认为列向量，$(x,y)^T$为行向量。
在欧氏空间二维平面上的一个点是由一个有序实数对（x，y）表示。我们可以给这对增加一个额外的坐标，给出一个三元坐标（x，y，1），我们声明代表相同的点。
这似乎是没什么区别，因为我们可以简单地通过添加或移除最后一个坐标来从一个点到另一个表示。我们现在着重从概念上来解释为什么最后一个坐标需要为1，毕竟其他两个坐标不受约束。三元坐标（x，y，2）呢？。在这里，我们作一个定义，说（x，y，1）和（2x，2y，2）代表同一点，当然对任何非零值K，（Kx，Ky，K）也表示同一点。正式地，点由三元坐标的等价类别表示，其中两个三元组在它们不同的公倍数时是等效的。这称为点的齐次坐标。给定一个三元坐标（Kx，Ky，K），我们在得到原来的坐标后除以K得到（x，y）。
读者会注意到，虽然（x，y，1）与坐标对（x，y）表示相同的点，但没有对应于三元坐标（x，y，0）的点。如果我们试着用最后一个坐标来划分，我们得到了无穷的点（x / 0，y / 0）。无穷远点就是这样产生的。它们是齐次坐标最后那个坐标为零的点。
现在我们知道了如何在二维欧氏空间中做到这一点，通过把点表示为齐次向量，然后可以把它扩展到射影空间，很明显，我们可以在任何维度上做同样的事情。欧氏空间的IRⁿ可通过将点表示成齐次向量而扩展到射影空间的IPⁿ。结果表明，二维射影空间中无穷远点形成一条线，通常称为无穷远处的直线。在三个维度中，它们在无穷远处形成平面。
二维平面中点的齐次表达为$x=(x,y,w)^T$，其物理意义等同于$(x/w,y/w)$,其中(x,y,0)是无穷远点。
对于二维中的直线可用方程$ax+by+c=0$表达,其对应的齐次坐标表达是$l=(a,b,c)^T$,$kax+kbx+kc=0$与前式代表同一条直线,即$kl=l$。
二维中的点和线存在以下关系：
- 点在直线上：点x在直线l上的充要条件是$x^Tl=l^Tx=0$.
- 两直线相交：两直线的交点为其的叉积$x=l \times l'$.
- 两点的连线：连接两点x,x'的直线为x,x'的叉积$l=x \times x'$.
- 点到直线距离: 点x=(x,y,1)到直线$l=(a,b,c)^T的距离为$$distance=\frac{x^Tl}{\sqrt{a^2+b^2}}$.
一般来说，椭圆，抛物线，双曲线统称为二次曲线，其在非齐次坐标中，二次曲线的一般方程为：
若点$x_i=(x,y)$在二次曲线C上，则：
二次曲线的切线：过（非退化）二次曲线C上点x的切线l由$l=Cx$确定.
由于二维平面上点和直线之间的对偶性质，二次曲线也可由5条切线确定：
当det（C）=0，也就是C不满秩时，其表达的是退化二次曲线。
定义:对于非奇异变换矩阵$H_{33}$,可提供二维点集之间的可逆映射：$x'=Hx$，这个映射关系称为射影变换，或者单应性变换（Homography）.
根据直线和二次曲线定义： **直线的射影变换：**直线l变换为$l'=H^{-T}l$ **二次曲线的射影变换:**二次曲线C变换为$C'=H^{-T}CH^{-1}$
由直线的射影变换可知，在射影变换条件下，直线变换后仍为直线，射影变换由于其特性，又被称为保线变换。 二次曲线的射影变换在任意两个非退化二次曲线中找到对应，也就是说，所有非退化二次曲线都是射影等价的。
| 变换类型 | 矩阵 | 不变性质 | 自由度 | 正方形变换后 |
|---|---|---|---|---|
| 射影 | $H=\begin{bmatrix}A & t \p^T & 1\end{bmatrix}$ | 直线，共点，共线，交比 | 8 |  |
| 仿射 | $A=\begin{bmatrix}A & t \0^T & 1\end{bmatrix}$ | 平行，面积比，平行长度比，无穷远线 | 6 |  |
| 相似 | $S=\begin{bmatrix}sR & t \0^T & 1\end{bmatrix}$ | 形状，长度比，夹角，虚圆点 | 4 |  |
| 欧氏 | $T=\begin{bmatrix}R & t \0^T & 1\end{bmatrix}$ | 刚体变换，长度，面积 | 3 |  |
由于R是正交单位阵（酉矩阵）只有一个自由度，因此对于相似变换和欧氏变换，在优化求解时都存在参数化问题，一般采用二维中的李群来对其进行流型上的优化。
在三维空间中，点(X,Y,Z)在平面上可写成：
- **三点确定一个平面:**由三个点$\begin{bmatrix}X_i\1\end{bmatrix}$构成的平面为$\begin{bmatrix}(X_1-X_2) \times (X_2-X3) \ - X_3^T(X1 \times X_2)\end{bmatrix}$
在三维空间中,两个点的连接或者两平面相交定义一条直线，由于三维中的直线拥有4个自由度，因此其表示相当难处理，因为4个自由度的对象在齐次坐标中用5维矢量表示，但问题是5维齐次矢量和平面的4维矢量很难在数学表达式中使用。
由于以上问题，直线的表达一般都由两个点的连接或者两个面的相交表示。这里面用到的核心知识是零空间的概念。
在三维齐次坐标系下，二次曲面由下列方程定义：
- 一个二次曲面有9个自由度，对应4X4对称矩阵的10个元素减去一个尺度
- 一般位置上的9个点确定一个二次曲面
- 如果Q是奇异的，那么二次曲面是退化的，可由较少点来确定
- 类似于二次曲线的极线，$\pi=QX$是二次曲面Q在点X处的极平面，当Q为非奇异并且X在Q外时，极平面由过X且与Q相切的射线组成的锥与Q相接触的点来定义。
- 平面$\pi$与二次曲面Q的交线是二次曲线C。计算该二次曲线有些麻烦，这里就不具体介绍了。
三次绕线可以看成是2D曲线的3维类推，2维射影平面上的一条曲线可由下列方程给出的一条参数曲线描述 $$\begin{bmatrix}x_1\x_2\x_3 \end{bmatrix}=A\begin{bmatrix}1\ \theta \ \theta^2 \end{bmatrix}$$ 其中A是非奇异的3*3矩阵,$\theta$是参数方程自变量。
类似地，一条三次绕线定义为三维空间中的一条曲线，其参数形式如下： $$\begin{bmatrix}X_1\X_2\X_3\X4 \end{bmatrix}=A\begin{bmatrix}1\ \theta \ \theta^2 \ \theta^3 \end{bmatrix}$$ 其中A是4*4的非奇异矩阵，所有的三次绕线都是射影等价的。
| 变换类型 | 矩阵 | 不变性质 | 自由度 | 正方体变换后 |
|---|---|---|---|---|
| 射影 | $H=\begin{bmatrix}A & t \p^T & 1\end{bmatrix}$ | 接触表面的相交和相切，高斯曲率的符号 | 15 |  |
| 仿射 | $A=\begin{bmatrix}A & t \0^T & 1\end{bmatrix}$ | 平面的平行性，体积比，形心，无穷远平面 | 12 |  |
| 相似 | $S=\begin{bmatrix}sR & t \0^T & 1\end{bmatrix}$ | 绝对二次曲线，形状 | 7 |  |
| 欧氏 | $T=\begin{bmatrix}R & t \0^T & 1\end{bmatrix}$ | 形状大小体积，刚体变换 | 6 |  |
相机拍摄的过程本质上是将三维世界投影到二维图像上，我们将这样的投影关系定义为：
理想的针孔模型相机在现实中是不存在的，因此产生了很多各样的投影模型和畸变模型以及其对应的标定方法，这些将在课程slam/camera中详细介绍。
这里我们将E称为本质矩阵（Essential Matrix），F称为基础矩阵（Fundamental Matrix），[R t]为两图像之间的变换，由上面的推导可以发现这三者之间存在着一些对应关系，这个关系在SfM和SLAM初始化中扮演着重要角色。
现有分辨率为640*480的针孔相机，其焦距和主点坐标分别为fx=fy=400,cx=320,cy=240.用其拍摄了两张图片并匹配对应关键点，获得的匹配关系在matches.txt文件中，已知两相机之间的距离为0.05米，且第一个相机的位置为原点，请编写程序，根据已知的匹配关系和相机内参求解第二个相机的位置和姿态。
文件格式说明： 每行4个float型表示一对点匹配，分别代表 x_left,y_left,x_right,y_right