OpenCV实现SfM（三）：多目三维重建

注意：本文中的代码必须使用OpenCV3.0或以上版本进行编译，因为很多函数是3.0以后才加入的。
目录：

问题简化

终于有时间来填坑了，这次一口气将双目重建扩展为多目重建吧。首先，为了简化问题，我们要做一个重要假设：用于多目重建的图像是有序的，即相邻图像的拍摄位置也是相邻的。多目重建本身比较复杂，我会尽量说得清晰，如有表述不清的地方，还请见谅并欢迎提问。

---

求第三个相机的变换矩阵

由前面的文章我们知道，两个相机之间的变换矩阵可以通过findEssentialMat以及recoverPose函数来实现，设第一个相机的坐标系为世界坐标系，现在加入第三幅图像（相机），如何确定第三个相机（后面称为相机三）到到世界坐标系的变换矩阵呢？

最简单的想法，就是沿用双目重建的方法，即在第三幅图像和第一幅图像之间提取特征点，然后调用findEssentialMat和recoverPose。那么加入第四幅、第五幅，乃至更多呢？随着图像数量的增加，新加入的图像与第一幅图像的差异可能越来越大，特征点的提取变得异常困难，这时就不能再沿用双目重建的方法了。

那么能不能用新加入的图像和相邻图像进行特征匹配呢？比如第三幅与第二幅匹配，第四幅与第三幅匹配，以此类推。当然可以，但是这时就不能继续使用findEssentialMat和recoverPose来求取相机的变换矩阵了，因为这两个函数求取的是相对变换，比如相机三到相机二的变换，而我们需要的是相机三到相机一的变换。有人说，既然知道相机二到相机一的变换，又知道相机到三到相机二的变换，不就能求出相机三到相机一的变换吗？实际上，通过这种方式，你只能求出相机三到相机一的旋转变换（旋转矩阵R），而他们之间的位移向量T，是无法求出的。这是因为上面两个函数求出的位移向量，都是单位向量，丢失了相机之间位移的比例关系。

说了这么多，我们要怎么解决这些问题？现在请出本文的主角——solvePnP和solvePnPRansac。根据opencv的官方解释，该函数根据空间中的点与图像中的点的对应关系，求解相机在空间中的位置。也就是说，我知道一些空间当中点的坐标，还知道这些点在图像中的像素坐标，那么solvePnP就可以告诉我相机在空间当中的坐标。solvePnP和solvePnPRansac所实现的功能相同，只不过后者使用了随机一致性采样，使其对噪声更鲁棒，本文使用后者。

好了，有这么好的函数，怎么用于我们的三维重建呢？首先，使用双目重建的方法，对头两幅图像进行重建，这样就得到了一些空间中的点，加入第三幅图像后，使其与第二幅图像进行特征匹配，这些匹配点中，肯定有一部分也是图像二与图像一之间的匹配点，也就是说，这些匹配点中有一部分的空间坐标是已知的，同时又知道这些点在第三幅图像中的像素坐标，嗯，solvePnP所需的信息都有了，自然第三个相机的空间位置就求出来了。由于空间点的坐标都是世界坐标系下的（即第一个相机的坐标系），所以由solvePnP求出的相机位置也是世界坐标系下的，即相机三到相机一的变换矩阵。

---

加入更多图像

通过上面的方法得到相机三的变换矩阵后，就可以使用上一篇文章提到的triangulatePoints方法将图像三和图像二之间的匹配点三角化，得到其空间坐标。为了使之后的图像仍能使用以上方法求解变换矩阵，我们还需要将新得到的空间点和之前的三维点云融合。已经存在的空间点，就没必要再添加了，只添加在图像二和三之间匹配，但在图像一和图像三中没有匹配的点。如此反复。

为了方便点云的融合以及今后的扩展，我们需要存储图像中每个特征点在空间中的对应点。在代码中我使用了一个二维列表，名字为correspond_struct_idx，correspond_struct_idx[i][j]代表第i幅图像第j个特征点所对应的空间点在点云中的索引，若索引小于零，说明该特征点在空间当中没有对应点。通过此结构，由特征匹配中的queryIdx和trainIdx就可以查询某个特征点在空间中的位置。

---

代码实现

前一篇文章的很多代码不用修改，还可以继续使用，但是程序的流程有了较大变化。首先是初始化点云，也就是通过双目重建方法对图像序列的头两幅图像进行重建࿰