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

最新推荐文章于 2024-04-24 07:59:38 发布

「已注销」

最新推荐文章于 2024-04-24 07:59:38 发布

阅读量6.7k

点赞数

分类专栏：计算机视觉

计算机视觉专栏收录该内容

12 篇文章 0 订阅

订阅专栏

注意：本文中的代码必须使用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就可以查询某个特征点在空间中的位置。

代码实现

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

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> init_structure(
    Mat K,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><KeyPoint></span>></span>& key_points_for_all, 
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Vec3b></span>></span>& colors_for_all,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><DMatch></span>></span>& matches_for_all,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span>& structure,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>></span>></span>& correspond_struct_idx,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Vec3b></span>& colors,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Mat></span>& rotations,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Mat></span>& motions
    )
{
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//计算头两幅图像之间的变换矩阵</span>
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point2f></span> p1, p2;
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Vec3b></span> c2;
    Mat R, T;   <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//旋转矩阵和平移向量</span>
    Mat mask;   <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//mask中大于零的点代表匹配点，等于零代表失配点</span>
    get_matched_points(key_points_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], key_points_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], matches_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], p1, p2);
    get_matched_colors(colors_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], colors_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], matches_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>], colors, c2);
    find_transform(K, p1, p2, R, T, mask);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//对头两幅图像进行三维重建</span>
    maskout_points(p1, mask);
    maskout_points(p2, mask);
    maskout_colors(colors, mask);

    Mat R0 = Mat::eye(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, CV_64FC1);
    Mat T0 = Mat::zeros(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>, CV_64FC1);
    reconstruct(K, R0, T0, R, T, p1, p2, structure);
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//保存变换矩阵</span>
    rotations = { R0, R };
    motions = { T0, T };

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//将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" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < key_points_for_all.size(); ++i)
    {
        correspond_struct_idx[i].resize(key_points_for_all[i].size(), -<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);
    }

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//填写头两幅图像的结构索引</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> idx = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><DMatch></span>& matches = matches_for_all[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>];
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < matches.size(); ++i)
    {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (mask.at<uchar>(i) == <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>)
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;

        correspond_struct_idx[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>][matches[i].queryIdx] = idx;
        correspond_struct_idx[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>][matches[i].trainIdx] = idx;
        ++idx;
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li></ul>

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

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> get_objpoints_and_imgpoints(
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><DMatch></span>& matches,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>></span>& struct_indices, 
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span>& structure, 
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><KeyPoint></span>& key_points,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span>& object_points,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point2f></span>& image_points)
{
    object_points.clear();
    image_points.clear();

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < matches.size(); ++i)
    {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> query_idx = matches[i].queryIdx;
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> train_idx = matches[i].trainIdx;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> struct_idx = struct_indices[query_idx];
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (struct_idx < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;

        object_points.push_back(structure[struct_idx]);
        image_points.push_back(key_points[train_idx].pt);
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li></ul>

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

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> reconstruct(Mat& K, Mat& R1, Mat& T1, Mat& R2, Mat& T2, <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point2f></span>& p1, <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point2f></span>& p2, <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span>& structure)
{
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//两个相机的投影矩阵[R T]，triangulatePoints只支持float型</span>
    Mat proj1(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">4</span>, CV_32FC1);
    Mat proj2(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">4</span>, CV_32FC1);

    R1.convertTo(proj1(Range(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>), Range(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>)), CV_32FC1);
    T1.convertTo(proj1.col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>), CV_32FC1);

    R2.convertTo(proj2(Range(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>), Range(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>)), CV_32FC1);
    T2.convertTo(proj2.col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>), CV_32FC1);

    Mat fK;
    K.convertTo(fK, CV_32FC1);
    proj1 = fK*proj1;
    proj2 = fK*proj2;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//三角重建</span>
    Mat s;
    triangulatePoints(proj1, proj2, p1, p2, s);

    structure.clear();
    structure.reserve(s.cols);
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < s.cols; ++i)
    {
        Mat_<<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span>> col = s.col(i);
        col /= col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>);  <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//齐次坐标，需要除以最后一个元素才是真正的坐标值</span>
        structure.push_back(Point3f(col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>), col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>), col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)));
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li></ul>

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

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> fusion_structure(
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><DMatch></span>& matches, 
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>></span>& struct_indices, 
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>></span>& next_struct_indices,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span>& structure, 
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span>& next_structure,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Vec3b></span>& colors,
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Vec3b></span>& next_colors
    )
{
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; i < matches.size(); ++i)
    {
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> query_idx = matches[i].queryIdx;
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> train_idx = matches[i].trainIdx;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> struct_idx = struct_indices[query_idx];
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (struct_idx >= <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//若该点在空间中已经存在，则这对匹配点对应的空间点应该是同一个，索引要相同</span>
        {
            next_struct_indices[train_idx] = struct_idx;
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;
        }

        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//若该点在空间中已经存在，将该点加入到结构中，且这对匹配点的空间点索引都为新加入的点的索引</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" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;
    }
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li></ul>

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

<code class="language-cpp hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//初始化结构（三维点云）</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" style="color: rgb(136, 0, 0); box-sizing: border-box;">//增量方式重建剩余的图像</span>
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span> i = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>; i < matches_for_all.size(); ++i)
{
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span> object_points;
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point2f></span> image_points;
    Mat r, R, T;
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//Mat mask;</span>

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//获取第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" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], 
        object_points,
        image_points
        );

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//求解变换矩阵</span>
    solvePnPRansac(object_points, image_points, K, noArray(), r, T);
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//将旋转向量转换为旋转矩阵</span>
    Rodrigues(r, R);
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//保存变换矩阵</span>
    rotations.push_back(R);
    motions.push_back(T);

    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point2f></span> p1, p2;
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Vec3b></span> c1, c2;
    get_matched_points(key_points_for_all[i], key_points_for_all[i + <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], matches_for_all[i], p1, p2);
    get_matched_colors(colors_for_all[i], colors_for_all[i + <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>], matches_for_all[i], c1, c2);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//根据之前求得的R，T进行三维重建</span>
    <span class="hljs-stl_container" style="box-sizing: border-box;"><span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">vector</span><Point3f></span> next_structure;
    reconstruct(K, rotations[i], motions[i], R, T, p1, p2, next_structure);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//将新的重建结果与之前的融合</span>
    fusion_structure(
        matches_for_all[i], 
        correspond_struct_idx[i], 
        correspond_struct_idx[i + <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>],
        structure, 
        next_structure,
        colors,
        c1
        );
}</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li></ul><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li><li style="box-sizing: border-box; padding: 0px 5px;">42</li><li style="box-sizing: border-box; padding: 0px 5px;">43</li><li style="box-sizing: border-box; padding: 0px 5px;">44</li><li style="box-sizing: border-box; padding: 0px 5px;">45</li><li style="box-sizing: border-box; padding: 0px 5px;">46</li><li style="box-sizing: border-box; padding: 0px 5px;">47</li><li style="box-sizing: border-box; padding: 0px 5px;">48</li><li style="box-sizing: border-box; padding: 0px 5px;">49</li><li style="box-sizing: border-box; padding: 0px 5px;">50</li><li style="box-sizing: border-box; padding: 0px 5px;">51</li><li style="box-sizing: border-box; padding: 0px 5px;">52</li><li style="box-sizing: border-box; padding: 0px 5px;">53</li><li style="box-sizing: border-box; padding: 0px 5px;">54</li><li style="box-sizing: border-box; padding: 0px 5px;">55</li><li style="box-sizing: border-box; padding: 0px 5px;">56</li><li style="box-sizing: border-box; padding: 0px 5px;">57</li><li style="box-sizing: border-box; padding: 0px 5px;">58</li><li style="box-sizing: border-box; padding: 0px 5px;">59</li></ul>

测试

我用了八幅图像进行测试，正如问题简化中所要求的那样，图像是有序的。
图片序列
程序的大部分时间花在特征提取和匹配上，真正的重建过程耗时很少。最终结果如下。
重建结果
图中每个彩色坐标系都代表一个相机。

思考

这个多目三维重建程序，要求图像必须是有序的，如果图像无序，比如只是对某个目标在不同角度的随意拍摄，程序应该如何修改？
增量式三维重建方法，有一个很大的缺点——随着图像的不断增加，误差会不断累积，最后误差过大以至于完全偏离重建的目标，怎么解决？

有兴趣的读者可以思考一下上面两个问题，第二个问题比较难，我会在下一篇文章中详细介绍。

下载

程序使用VS2015开发，OpenCV版本为3.1且包含扩展部分，如果不使用SIFT特征，可以修改源代码，然后使用官方未包含扩展部分的库。软件运行后会将三维重建的结果写入Viewer目录下的structure.yml文件中，在Viewer目录下有一个SfMViewer程序，直接运行即可读取yml文件并显示三维结构。

代码下载地址

「已注销」

关注

0
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
OpenCV实现SfM（三）：多目三维重建

目录：极线约束与本征矩阵特征点提取与匹配三维重建测试极线约束与本征矩阵在三维重建前，我们先研究一下同一点在两个相机中的像的关系。假设在世界坐标系中有一点p，坐标为X，它在1相机中的像为x1，在2相机中的像为x2（注意x1和x2为齐次坐标，最后一个元素是1），如下图。设X到两个相机像面的垂直距离分别为s1和s2，且这两个相机具有相同的内参矩
复制链接

扫一扫