推荐开源项目：简单的SLAM与机器人教程与编程实践-github

破解对极几何中的概念障碍

首先介绍下三维重建到底要做什么事。我们知道一个物体在现实世界的位置是三维的，但是照片是二维的。这意味照片拍摄到的场景丢失了一维。而这一维就是物体离相机的远近。对极几何三维重建就是希望通过多张角度的照片还原各个像素点相对相机的三维坐标。
看本文前你需要了解针孔相机成像原理（即小孔成像原理，参见教程《针孔相机模型成像原理与图像变形矫正教程
》）
下面这幅图就描述了对极几何。对极几何主要是介绍同一个物体在两个不同的视角下的几何约束关系。而这些几何约束关系对应着一系列等式。通过求解这些等式我们就可以求解得到P相对于第一个视角和第二个视角的坐标。接下来我们看看到底存在哪些几何约束？

注意：
P是现实世界中某一点。$p_0,p_1$分别是第0个和第1个视角下物体P被拍摄到的点。注意：$p_0,p_1$并不是像素点，它们只是损失掉深度值后的点，他们和像素点坐标$x_0$之间的联系是$x_0=Kp_0$其中K是相机的内部参数矩阵主要是相机的焦距和原点位置。$C_0,C_1$分别是两个不同视角对应的相机焦点。不同视角就是指相机位置姿态发生了变化。
相机成像原理第0个视角下，P点与$p_0$和第一个视角下相机焦点一定在同一条直线上。而且这条直线上的其他现实世界中的点会重合投影到$p_0$。也就是说虽然第0个视角只看到了$p_0$这一个点，但是事实上这个点是由多个点重合投影得到的。那么我怎么知道是否存在重合投影呢？这就是需要第1个视角进行补充约束。大牛们把这约束叫做对极约束。

对极几何进行三维重建的思路

我们从上图中可以看到，$Pp_0$这条直线上的点虽然投影到第0个视角是重合到$p_0$但是投影到第1个视角却并不是重合。因此我们根据这些点可以解出红色这条直线（学术上叫做极线Epiolar Line），然后解出极点（Epiplole），然后根据极点和焦点在同一条直线上解出**$C_0C_1$这条直线（学术上叫做基线）**。现在我们知道了极线和基线。如果相机位姿没发生变化那么$C_0C_1$这条直线（基线）应该要与极线（Epipolar Line）平行。现在他们不平行了我们只用解出这之间的位姿变化就可以解出两个视角发生的位姿变化。

那么怎么求解红色那条直线（极线Epiolar Line）呢？

求解极线（本质矩阵essential matrix）

我们把两条极线，基线独立出来看看他们之间的数学联系。然后根据这些数学联系来计算出极线的数学表达式。

我们知道$C_0p_0,C_1p_1,C_0C_1$这三个向量是共面的。$C_0C_1\times C_1P_1$ 注意中间那个乘叫做叉乘（不是内积）。叉乘的结果是一个向量，这个向量与$C_0C_1, C_1P_1$均垂直。在高中的立体几何里面我们学过“某直线若垂直一个平面中两条相交直线，那么该直线垂直该平面”。也就是说**$C_0C_1\times C_1P_1$这个向量与$C_0p_0,C_1p_1,C_0C_1$所确定的平面垂直，也就是说$C_0C_1\times C_1P_1$这个向量与$C_0p_0$也垂直**。我们知道若两个向量垂直那么这两个向量的内积等于0.

于是我们得到了：$(C_0C_1\times C_1P_1)C_0p_0=0$这个数学表达式。
为了计算方便我们把他们都统一用第0个视角的坐标系表示。当然这意味着我们需要把$C_1P_1$变换到视角0的坐标系下，这个很简单${}^0_1Rp_1$，其中${}^0_1R$表示视角1相对视角0发生的旋转，同时$p_1$表示$C_1P_1$在视角1坐标系下的坐标（注意$C_1$就是原点所以可以直接用一个点坐标表示一个向量）。用$t$表示向量$C_0C_1$
因此有：$(t \times {}^0_1Rp_1)p_0=0$。注意：$p_0,p_1$都是已知的。为什么？因为他们两个是图片上的特征点，当然很容易知道他们所在的像素位置，进而知道他们在某个视角下的三维坐标。但是$t,{}^0_1R$