SFM：从运动中恢复结构

SfM介绍

小孔相机模型

在计算机视觉中，最常用的相机模型就是小孔模型（小孔成像模型），它将相机的透镜组简化为一个小孔，光线透过小孔在小孔后方的像面上成像，如下图所示。

由上图可知，小孔模型成的是倒像，为了表述与研究的方便，我们常常将像面至于小孔之前，且到小孔的距离仍然是焦距f，这样的模型与原来的小孔模型是等价的，只不过成的是正像，符合人的直观感受。在这种情况下，往往将小孔称作光心（Optical Center）。

小孔模型是一种理想相机模型，没有考虑实际相机中存在的场曲、畸变等问题。在实际使用时，这些问题可以通过在标定的过程中引入畸变参数解决，所以小孔模型仍然是目前最广泛使用的相机模型。

坐标系

为了用数学研究SfM，我们需要坐标系。在SfM中主要有两类坐标系，一类为相机坐标系，一类为世界坐标系。在本系列中，所以坐标系均为右手坐标系。
相机坐标系以相机的光心（小孔）作为原点，X轴为水平方向，Y轴为竖直方向，Z轴指向相机所观察的方向。
世界坐标系的原点可以任意选择，与相机的具体位置无关。

内参矩阵

设空间中有一点P，若世界坐标系与相机坐标系重合，则该点在空间中的坐标为(X, Y, Z)，其中Z为该点到相机光心的垂直距离。设该点在像面上的像为点p，像素坐标为(x, y)，那么(X, Y, Z)和(x, y)有什么关系呢？

由上图可知，这是一个简单的相似三角形关系，从而得到

但是，图像的像素坐标系原点在左上角，而上面公式假定原点在图像中心，为了处理这一偏移，设光心在图像上对应的像素坐标为

(cx,cy) ，则

将以上关系表示为矩阵形式，有

其中，将矩阵

称为内参矩阵，因为它只和相机自身的内部参数有关（焦距，光心位置）。

外参矩阵

一般情况下，世界坐标系和相机坐标系不重合，这时，世界坐标系中的某一点P要投影到像面上时，先要将该点的坐标转换到相机坐标系下。设P在世界坐标系中的坐标为X，P到光心的垂直距离为s（即上文中的Z），在像面上的坐标为x，世界坐标系与相机坐标系之间的相对旋转为矩阵R（R是一个三行三列的旋转矩阵），相对位移为向量T（三行一列），则

其中

RX+T 即为P在相机坐标系下的坐标，使用齐次坐标改写上式，有

其中

[RT] 是一个三行四列的矩阵，称为外参矩阵，它和相机的参数无关，只与相机在世界坐标系中的位置有关。

相机的标定

极线约束与本征矩阵

在三维重建前，我们先研究一下同一点在两个相机中的像的关系。假设在世界坐标系中有一点 p ，坐标为 X ，它在1相机中的像为 x1 ，在2相机中的像为 x2 （注意 x1 和 x2 为齐次坐标，最后一个元素是1），如下图。

设 X 到两个相机像面的垂直距离分别为 s1 和 s2 ，且这两个相机具有相同的内参矩阵 K ，与世界坐标系之间的变换关系分别为 [R1  T1] 和 [R2  T2] ，那么我们可以得到下面两个等式

由于K是可逆矩阵，两式坐乘K的逆，有

设

K−1x1=x′1 ，

K−1x2=x′2 ，则有

我们一般称

x′1 和

x′2 为归一化后的像坐标，它们和图像的大小没有关系，且原点位于图像中心。

由于世界坐标系可以任意选择，我们将世界坐标系选为第一个相机的相机坐标系，这时

R1=I, T1=0 。上式则变为

将第一式带入第二式，有

x′2 和

T2 都是三维向量，它们做外积（叉积）之后得到另外一个三维向量

T2ˆx′2 （其中

T2ˆ 为外积的矩阵形式，

T2ˆx′2 代表

T2×x′2 ），且该向量垂直于

x′2 和

T2 ，再用该向量对等式两边做内积，有

即

令

E=T2ˆR2 有

可以看出，上式是同一点在两个相机中的像所满足的关系，它和点的空间坐标、点到相机的距离均没有关系，我们称之为极线约束，而矩阵

E 则称为关于这两个相机的本征矩阵。如果我们知道两幅图像中的多个对应点（至少5对），则可以通过上式解出矩阵

E ，又由于

E 是由

T2 和

R2 构成的，可以从E中分解出

T2 和

R2 。

如何从

E 中分解出两个相机的相对变换关系（即

T2 和

R2 ），背后的数学原理比较复杂，好在OpenCV为我们提供了这样的方法，在此就不谈原理了。

特征点提取与匹配

从上面的分析可知，要求取两个相机的相对关系，需要两幅图像中的对应点，这就变成的特征点的提取和匹配问题。对于图像差别较大的情况，推荐使用SIFT特征，因为SIFT对旋转、尺度、透视都有较好的鲁棒性。如果差别不大，可以考虑其他更快速的特征，比如SURF、ORB等。
本文中使用SIFT特征，由于OpenCV3.0将SIFT包含在了扩展部分中，所以官网上下载的版本是没有SIFT的，为此需要到这里下载扩展包，并按照里面的说明重新编译OpenCV（哎~真麻烦，-_-!）。如果你使用其他特征，就不必为此辛劳了。
下面的代码负责提取图像特征，并进行匹配。

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">void</span> extract_features( <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-built_in">string</span>></span>& image_names, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><KeyPoint></span>></span>& key_points_for_all, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Mat></span>& descriptor_for_all, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span>></span>& colors_for_all ) { key_points_for_all.clear(); descriptor_for_all.clear(); Mat image; <span class="hljs-comment">//读取图像，获取图像特征点，并保存</span> Ptr<Feature2D> sift = xfeatures2d::SIFT::create(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>, <span class="hljs-number">0.04</span>, <span class="hljs-number">10</span>); <span class="hljs-keyword">for</span> (<span class="hljs-keyword">auto</span> it = image_names.begin(); it != image_names.end(); ++it) { image = imread(*it); <span class="hljs-keyword">if</span> (image.empty()) <span class="hljs-keyword">continue</span>; <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><KeyPoint></span> key_points; Mat descriptor; <span class="hljs-comment">//偶尔出现内存分配失败的错误</span> sift->detectAndCompute(image, noArray(), key_points, descriptor); <span class="hljs-comment">//特征点过少，则排除该图像</span> <span class="hljs-keyword">if</span> (key_points.size() <= <span class="hljs-number">10</span>) <span class="hljs-keyword">continue</span>; key_points_for_all.push_back(key_points); descriptor_for_all.push_back(descriptor); <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span> colors(key_points.size()); <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < key_points.size(); ++i) { Point2f& p = key_points[i].pt; colors[i] = image.at<Vec3b>(p.y, p.x); } colors_for_all.push_back(colors); } } <span class="hljs-keyword">void</span> match_features(Mat& query, Mat& train, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><DMatch></span>& matches) { <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><DMatch></span>></span> knn_matches; BFMatcher matcher(NORM_L2); matcher.knnMatch(query, train, knn_matches, <span class="hljs-number">2</span>); <span class="hljs-comment">//获取满足Ratio Test的最小匹配的距离</span> <span class="hljs-keyword">float</span> min_dist = FLT_MAX; <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> r = <span class="hljs-number">0</span>; r < knn_matches.size(); ++r) { <span class="hljs-comment">//Ratio Test</span> <span class="hljs-keyword">if</span> (knn_matches[r][<span class="hljs-number">0</span>].distance > <span class="hljs-number">0.6</span>*knn_matches[r][<span class="hljs-number">1</span>].distance) <span class="hljs-keyword">continue</span>; <span class="hljs-keyword">float</span> dist = knn_matches[r][<span class="hljs-number">0</span>].distance; <span class="hljs-keyword">if</span> (dist < min_dist) min_dist = dist; } matches.clear(); <span class="hljs-keyword">for</span> (size_t r = <span class="hljs-number">0</span>; r < knn_matches.size(); ++r) { <span class="hljs-comment">//排除不满足Ratio Test的点和匹配距离过大的点</span> <span class="hljs-keyword">if</span> ( knn_matches[r][<span class="hljs-number">0</span>].distance > <span class="hljs-number">0.6</span>*knn_matches[r][<span class="hljs-number">1</span>].distance || knn_matches[r][<span class="hljs-number">0</span>].distance > <span class="hljs-number">5</span> * max(min_dist, <span class="hljs-number">10.0f</span>) ) <span class="hljs-keyword">continue</span>; <span class="hljs-comment">//保存匹配点</span> matches.push_back(knn_matches[r][<span class="hljs-number">0</span>]); } }</code>

需要重点说明的是，匹配结果往往有很多误匹配，为了排除这些错误，这里使用了Ratio Test方法，即使用KNN算法寻找与该特征最匹配的2个特征，若第一个特征的匹配距离与第二个特征的匹配距离之比小于某一阈值，就接受该匹配，否则视为误匹配。当然，也可以使用Cross Test（交叉验证）方法来排除错误。

得到匹配点后，就可以使用OpenCV3.0中新加入的函数findEssentialMat()来求取本征矩阵了。得到本征矩阵后，再使用另一个函数对本征矩阵进行分解，并返回两相机之间的相对变换R和T。注意这里的T是在第二个相机的坐标系下表示的，也就是说，其方向从第二个相机指向第一个相机（即世界坐标系所在的相机），且它的长度等于1。

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">bool</span> find_transform(Mat& K, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& p1, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& p2, Mat& R, Mat& T, Mat& mask) { <span class="hljs-comment">//根据内参矩阵获取相机的焦距和光心坐标（主点坐标）</span> <span class="hljs-keyword">double</span> focal_length = <span class="hljs-number">0.5</span>*(K.at<<span class="hljs-keyword">double</span>>(<span class="hljs-number">0</span>) + K.at<<span class="hljs-keyword">double</span>>(<span class="hljs-number">4</span>)); Point2d principle_point(K.at<<span class="hljs-keyword">double</span>>(<span class="hljs-number">2</span>), K.at<<span class="hljs-keyword">double</span>>(<span class="hljs-number">5</span>)); <span class="hljs-comment">//根据匹配点求取本征矩阵，使用RANSAC，进一步排除失配点</span> Mat E = findEssentialMat(p1, p2, focal_length, principle_point, RANSAC, <span class="hljs-number">0.999</span>, <span class="hljs-number">1.0</span>, mask); <span class="hljs-keyword">if</span> (E.empty()) <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>; <span class="hljs-keyword">double</span> feasible_count = countNonZero(mask); <span class="hljs-built_in">cout</span> << (<span class="hljs-keyword">int</span>)feasible_count << <span class="hljs-string">" -in- "</span> << p1.size() << endl; <span class="hljs-comment">//对于RANSAC而言，outlier数量大于50%时，结果是不可靠的</span> <span class="hljs-keyword">if</span> (feasible_count <= <span class="hljs-number">15</span> || (feasible_count / p1.size()) < <span class="hljs-number">0.6</span>) <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>; <span class="hljs-comment">//分解本征矩阵，获取相对变换</span> <span class="hljs-keyword">int</span> pass_count = recoverPose(E, p1, p2, R, T, focal_length, principle_point, mask); <span class="hljs-comment">//同时位于两个相机前方的点的数量要足够大</span> <span class="hljs-keyword">if</span> (((<span class="hljs-keyword">double</span>)pass_count) / feasible_count < <span class="hljs-number">0.7</span>) <span class="hljs-keyword">return</span> <span class="hljs-keyword">false</span>; <span class="hljs-keyword">return</span> <span class="hljs-keyword">true</span>; }</code>

三维重建

现在已经知道了两个相机之间的变换矩阵，还有每一对匹配点的坐标。三维重建就是通过这些已知信息还原匹配点在空间当中的坐标。在前面的推导中，我们有

这个等式中有两个未知量，分别是

s2 和

X 。用

x2 对等式两边做外积，可以消去

s2 ，得

整理一下可以得到一个关于空间坐标X的线性方程

上面的方程不能直接取逆求解，因此化为其次方程

用SVD求X左边矩阵的零空间，再将最后一个元素归一化到1，即可求得X。其几何意义相当于分别从两个相机的光心作过

x1 和

x2 的延长线，延长线的焦点即为方程的解，如文章最上方的图所示。由于这种方法和三角测距类似，因此这种重建方式也被称为三角化（triangulate）。OpenCV提供了该方法，可以直接使用。

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">void</span> reconstruct(Mat& K, Mat& R, Mat& T, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& p1, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& p2, Mat& structure) { <span class="hljs-comment">//两个相机的投影矩阵[R T]，triangulatePoints只支持float型</span> Mat proj1(<span class="hljs-number">3</span>, <span class="hljs-number">4</span>, CV_32FC1); Mat proj2(<span class="hljs-number">3</span>, <span class="hljs-number">4</span>, CV_32FC1); proj1(Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>), Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>)) = Mat::eye(<span class="hljs-number">3</span>, <span class="hljs-number">3</span>, CV_32FC1); proj1.col(<span class="hljs-number">3</span>) = Mat::zeros(<span class="hljs-number">3</span>, <span class="hljs-number">1</span>, CV_32FC1); R.convertTo(proj2(Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>), Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>)), CV_32FC1); T.convertTo(proj2.col(<span class="hljs-number">3</span>), CV_32FC1); Mat fK; K.convertTo(fK, CV_32FC1); proj1 = fK*proj1; proj2 = fK*proj2; <span class="hljs-comment">//三角化重建</span> triangulatePoints(proj1, proj2, p1, p2, structure); }</code>

测试

用了下面两幅图像进行测试

得到了着色后的稀疏点云，是否能看出一点轮廓呢？！

问题简化

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

求第三个相机的变换矩阵

由前面的文章我们知道，两个相机之间的变换矩阵可以通过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就可以查询某个特征点在空间中的位置。

代码实现

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

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">void</span> init_structure( Mat K, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><KeyPoint></span>></span>& key_points_for_all, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span>></span>& colors_for_all, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><DMatch></span>></span>& matches_for_all, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span>& structure, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-keyword">int</span>></span>></span>& correspond_struct_idx, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span>& colors, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Mat></span>& rotations, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Mat></span>& motions ) { <span class="hljs-comment">//计算头两幅图像之间的变换矩阵</span> <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span> p1, p2; <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span> c2; Mat R, T; <span class="hljs-comment">//旋转矩阵和平移向量</span> Mat mask; <span class="hljs-comment">//mask中大于零的点代表匹配点，等于零代表失配点</span> get_matched_points(key_points_for_all[<span class="hljs-number">0</span>], key_points_for_all[<span class="hljs-number">1</span>], matches_for_all[<span class="hljs-number">0</span>], p1, p2); get_matched_colors(colors_for_all[<span class="hljs-number">0</span>], colors_for_all[<span class="hljs-number">1</span>], matches_for_all[<span class="hljs-number">0</span>], colors, c2); find_transform(K, p1, p2, R, T, mask); <span class="hljs-comment">//对头两幅图像进行三维重建</span> maskout_points(p1, mask); maskout_points(p2, mask); maskout_colors(colors, mask); Mat R0 = Mat::eye(<span class="hljs-number">3</span>, <span class="hljs-number">3</span>, CV_64FC1); Mat T0 = Mat::zeros(<span class="hljs-number">3</span>, <span class="hljs-number">1</span>, CV_64FC1); reconstruct(K, R0, T0, R, T, p1, p2, structure); <span class="hljs-comment">//保存变换矩阵</span> rotations = { R0, R }; motions = { T0, T }; <span class="hljs-comment">//将correspond_struct_idx的大小初始化为与key_points_for_all完全一致</span> correspond_struct_idx.clear(); correspond_struct_idx.resize(key_points_for_all.size()); <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < key_points_for_all.size(); ++i) { correspond_struct_idx[i].resize(key_points_for_all[i].size(), -<span class="hljs-number">1</span>); } <span class="hljs-comment">//填写头两幅图像的结构索引</span> <span class="hljs-keyword">int</span> idx = <span class="hljs-number">0</span>; <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><DMatch></span>& matches = matches_for_all[<span class="hljs-number">0</span>]; <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < matches.size(); ++i) { <span class="hljs-keyword">if</span> (mask.at<uchar>(i) == <span class="hljs-number">0</span>) <span class="hljs-keyword">continue</span>; correspond_struct_idx[<span class="hljs-number">0</span>][matches[i].queryIdx] = idx; correspond_struct_idx[<span class="hljs-number">1</span>][matches[i].trainIdx] = idx; ++idx; } }</code><ul style="" class="pre-numbering"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li><li>31</li><li>32</li><li>33</li><li>34</li><li>35</li><li>36</li><li>37</li><li>38</li><li>39</li><li>40</li><li>41</li><li>42</li><li>43</li><li>44</li><li>45</li><li>46</li><li>47</li><li>48</li><li>49</li><li>50</li><li>51</li><li>52</li><li>53</li><li>54</li></ul>

初始点云得到后，就可以使用增量方式重建剩余图像，注意，在代码中为了方便实现，所有图像之间的特征匹配已经事先完成了，并保存在matches_for_all这个列表中。增量重建的关键是调用solvePnPRansac，而这个函数需要空间点坐标和对应的像素坐标作为参数，有了correspond_struct_idx，实现这个对应关系的查找还是很方便的，如下。

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">void</span> get_objpoints_and_imgpoints( <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><DMatch></span>& matches, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-keyword">int</span>></span>& struct_indices, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span>& structure, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><KeyPoint></span>& key_points, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span>& object_points, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& image_points) { object_points.clear(); image_points.clear(); <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < matches.size(); ++i) { <span class="hljs-keyword">int</span> query_idx = matches[i].queryIdx; <span class="hljs-keyword">int</span> train_idx = matches[i].trainIdx; <span class="hljs-keyword">int</span> struct_idx = struct_indices[query_idx]; <span class="hljs-keyword">if</span> (struct_idx < <span class="hljs-number">0</span>) <span class="hljs-keyword">continue</span>; object_points.push_back(structure[struct_idx]); image_points.push_back(key_points[train_idx].pt); } }</code><ul style="" class="pre-numbering"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li></ul>

之后调用solvePnPRansac得到相机的旋转向量和位移，由于我们使用的都是旋转矩阵，所以这里要调用opencv的Rodrigues函数将旋转向量变换为旋转矩阵。之后，使用上一篇文章中用到的reconstruct函数对匹配点进行重建（三角化），不过为了适用于多目重建，做了一些简单修改。

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">void</span> reconstruct(Mat& K, Mat& R1, Mat& T1, Mat& R2, Mat& T2, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& p1, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span>& p2, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span>& structure) { <span class="hljs-comment">//两个相机的投影矩阵[R T]，triangulatePoints只支持float型</span> Mat proj1(<span class="hljs-number">3</span>, <span class="hljs-number">4</span>, CV_32FC1); Mat proj2(<span class="hljs-number">3</span>, <span class="hljs-number">4</span>, CV_32FC1); R1.convertTo(proj1(Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>), Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>)), CV_32FC1); T1.convertTo(proj1.col(<span class="hljs-number">3</span>), CV_32FC1); R2.convertTo(proj2(Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>), Range(<span class="hljs-number">0</span>, <span class="hljs-number">3</span>)), CV_32FC1); T2.convertTo(proj2.col(<span class="hljs-number">3</span>), CV_32FC1); Mat fK; K.convertTo(fK, CV_32FC1); proj1 = fK*proj1; proj2 = fK*proj2; <span class="hljs-comment">//三角重建</span> Mat s; triangulatePoints(proj1, proj2, p1, p2, s); structure.clear(); structure.reserve(s.cols); <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < s.cols; ++i) { Mat_<<span class="hljs-keyword">float</span>> col = s.col(i); col /= col(<span class="hljs-number">3</span>); <span class="hljs-comment">//齐次坐标，需要除以最后一个元素才是真正的坐标值</span> structure.push_back(Point3f(col(<span class="hljs-number">0</span>), col(<span class="hljs-number">1</span>), col(<span class="hljs-number">2</span>))); } }</code><ul style="" class="pre-numbering"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li></ul>

最后，将重建结构与之前的点云进行融合。

<code class="language-cpp hljs has-numbering"><span class="hljs-keyword">void</span> fusion_structure( <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><DMatch></span>& matches, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-keyword">int</span>></span>& struct_indices, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><<span class="hljs-keyword">int</span>></span>& next_struct_indices, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span>& structure, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span>& next_structure, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span>& colors, <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span>& next_colors ) { <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">0</span>; i < matches.size(); ++i) { <span class="hljs-keyword">int</span> query_idx = matches[i].queryIdx; <span class="hljs-keyword">int</span> train_idx = matches[i].trainIdx; <span class="hljs-keyword">int</span> struct_idx = struct_indices[query_idx]; <span class="hljs-keyword">if</span> (struct_idx >= <span class="hljs-number">0</span>) <span class="hljs-comment">//若该点在空间中已经存在，则这对匹配点对应的空间点应该是同一个，索引要相同</span> { next_struct_indices[train_idx] = struct_idx; <span class="hljs-keyword">continue</span>; } <span class="hljs-comment">//若该点在空间中已经存在，将该点加入到结构中，且这对匹配点的空间点索引都为新加入的点的索引</span> structure.push_back(next_structure[i]); colors.push_back(next_colors[i]); struct_indices[query_idx] = next_struct_indices[train_idx] = structure.size() - <span class="hljs-number">1</span>; } }</code><ul style="" class="pre-numbering"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li></ul>

整个增量方式重建图像的代码大致如下。

<code class="language-cpp hljs has-numbering"><span class="hljs-comment">//初始化结构（三维点云）</span> init_structure( K, key_points_for_all, colors_for_all, matches_for_all, structure, correspond_struct_idx, colors, rotations, motions ); <span class="hljs-comment">//增量方式重建剩余的图像</span> <span class="hljs-keyword">for</span> (<span class="hljs-keyword">int</span> i = <span class="hljs-number">1</span>; i < matches_for_all.size(); ++i) { <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span> object_points; <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span> image_points; Mat r, R, T; <span class="hljs-comment">//Mat mask;</span> <span class="hljs-comment">//获取第i幅图像中匹配点对应的三维点，以及在第i+1幅图像中对应的像素点</span> get_objpoints_and_imgpoints( matches_for_all[i], correspond_struct_idx[i], structure, key_points_for_all[i+<span class="hljs-number">1</span>], object_points, image_points ); <span class="hljs-comment">//求解变换矩阵</span> solvePnPRansac(object_points, image_points, K, noArray(), r, T); <span class="hljs-comment">//将旋转向量转换为旋转矩阵</span> Rodrigues(r, R); <span class="hljs-comment">//保存变换矩阵</span> rotations.push_back(R); motions.push_back(T); <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point2f></span> p1, p2; <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Vec3b></span> c1, c2; get_matched_points(key_points_for_all[i], key_points_for_all[i + <span class="hljs-number">1</span>], matches_for_all[i], p1, p2); get_matched_colors(colors_for_all[i], colors_for_all[i + <span class="hljs-number">1</span>], matches_for_all[i], c1, c2); <span class="hljs-comment">//根据之前求得的R，T进行三维重建</span> <span class="hljs-stl_container"><span class="hljs-built_in">vector</span><Point3f></span> next_structure; reconstruct(K, rotations[i], motions[i], R, T, p1, p2, next_structure); <span class="hljs-comment">//将新的重建结果与之前的融合</span> fusion_structure( matches_for_all[i], correspond_struct_idx[i], correspond_struct_idx[i + <span class="hljs-number">1</span>], structure, next_structure, colors, c1 ); }</code><ul style="" class="pre-numbering"><li>1</li><li>2</li><li>3</li><li>4</li><li>5</li><li>6</li><li>7</li><li>8</li><li>9</li><li>10</li><li>11</li><li>12</li><li>13</li><li>14</li><li>15</li><li>16</li><li>17</li><li>18</li><li>19</li><li>20</li><li>21</li><li>22</li><li>23</li><li>24</li><li>25</li><li>26</li><li>27</li><li>28</li><li>29</li><li>30</li><li>31</li><li>32</li><li>33</li><li>34</li><li>35</li><li>36</li><li>37</li><li>38</li><li>39</li><li>40</li><li>41</li><li>42</li><li>43</li><li>44</li><li>45</li><li>46</li><li>47</li><li>48</li><li>49</li><li>50</li><li>51</li><li>52</li><li>53</li><li>54</li><li>55</li><li>56</li><li>57</li><li>58</li><li>59</li></ul>

测试

我用了八幅图像进行测试，正如问题简化中所要求的那样，图像是有序的。

程序的大部分时间花在特征提取和匹配上，真正的重建过程耗时很少。最终结果如下。

图中每个彩色坐标系都代表一个相机。