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

目录：

极线约束与本征矩阵

在三维重建前，我们先研究一下同一点在两个相机中的像的关系。假设在世界坐标系中有一点 p ，坐标为 X ，它在1相机中的像为 x1 ，在2相机中的像为 x2 （注意 x1 和 x2 为齐次坐标，最后一个元素是1），如下图。 
 
设 X 到两个相机像面的垂直距离分别为 s1 和 s2 ，且这两个相机具有相同的内参矩阵 K ，与世界坐标系之间的变换关系分别为 [R1  T1] 和 [R2  T2] ，那么我们可以得到下面两个等式 

s1x1=K(R1X+T1)s2x2=K(R2X+T2)

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

s1K−1x1=R1X+T1s2K−1x2=R2X+T2

设 K−1x1=x′1 ， K−1x2=x′2 ，则有 

s1x′1=R1X+T1s2x′2=R2X+T2

我们一般称 x′1 和 x′2 为归一化后的像坐标，它们和图像的大小没有关系，且原点位于图像中心。 
由于世界坐标系可以任意选择，我们将世界坐标系选为第一个相机的相机坐标系，这时 R1=I, T1=0 。上式则变为 

s1x′1=Xs2x′2=R2X+T2

将第一式带入第二式，有 

s2x′2=s1R2x′1+T2

x′2 和 T2 都是三维向量，它们做外积（叉积）之后得到另外一个三维向量 T2ˆx′2 （其中 T2ˆ 为外积的矩阵形式， T2ˆx′2 代表 T2×x′2 ），且该向量垂直于 x′2 和 T2 ，再用该向量对等式两边做内积，有 

0=s1(T2ˆx′2)TR2x′1

即 

x′2T2ˆR2x′1=0

令 E=T2ˆR2  有 

x′2Ex′1=0

可以看出，上式是同一点在两个相机中的像所满足的关系，它和点的空间坐标、点到相机的距离均没有关系，我们称之为极线约束，而矩阵 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" 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> extract_features(
    <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-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">string</span>></span>& image_names,
    <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><Mat></span>& descriptor_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
    )
{
    key_points_for_all.clear();
    descriptor_for_all.clear();
    Mat image;

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//读取图像，获取图像特征点，并保存</span>
    Ptr<Feature2D> sift = xfeatures2d::SIFT::create(<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>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.04</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">10</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;">auto</span> it = image_names.begin(); it != image_names.end(); ++it)
    {
        image = imread(*it);
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (image.empty()) <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</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> key_points;
        Mat descriptor;
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//偶尔出现内存分配失败的错误</span>
        sift->detectAndCompute(image, noArray(), key_points, descriptor);

        <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;">if</span> (key_points.size() <= <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">10</span>) <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;

        key_points_for_all.push_back(key_points);
        descriptor_for_all.push_back(descriptor);

        <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(key_points.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.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" style="color: rgb(0, 0, 136); box-sizing: border-box;">void</span> match_features(Mat& query, Mat& train, <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-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> knn_matches;
    BFMatcher matcher(NORM_L2);
    matcher.knnMatch(query, train, knn_matches, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>);

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//获取满足Ratio Test的最小匹配的距离</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> min_dist = FLT_MAX;
    <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> r = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; r < knn_matches.size(); ++r)
    {
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//Ratio Test</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>].distance > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.6</span>*knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>].distance)
            <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">continue</span>;

        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">float</span> dist = knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>].distance;
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (dist < min_dist) min_dist = dist;
    }

    matches.clear();
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> (size_t r = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>; r < knn_matches.size(); ++r)
    {
        <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//排除不满足Ratio Test的点和匹配距离过大的点</span>
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (
            knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>].distance > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.6</span>*knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>].distance ||
            knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>].distance > <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">5</span> * max(min_dist, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">10.0f</span>)
            )
            <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>
        matches.push_back(knn_matches[r][<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</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><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><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</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><li style="box-sizing: border-box; padding: 0px 5px;">60</li><li style="box-sizing: border-box; padding: 0px 5px;">61</li><li style="box-sizing: border-box; padding: 0px 5px;">62</li><li style="box-sizing: border-box; padding: 0px 5px;">63</li><li style="box-sizing: border-box; padding: 0px 5px;">64</li><li style="box-sizing: border-box; padding: 0px 5px;">65</li><li style="box-sizing: border-box; padding: 0px 5px;">66</li><li style="box-sizing: border-box; padding: 0px 5px;">67</li><li style="box-sizing: border-box; padding: 0px 5px;">68</li><li style="box-sizing: border-box; padding: 0px 5px;">69</li><li style="box-sizing: border-box; padding: 0px 5px;">70</li><li style="box-sizing: border-box; padding: 0px 5px;">71</li></ul>

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

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

<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;">bool</span> find_transform(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><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, Mat& R, Mat& T, Mat& mask)
{
    <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;">double</span> focal_length = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.5</span>*(K.at<<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>) + K.at<<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">4</span>));
    Point2d principle_point(K.at<<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>), K.at<<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span>>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">5</span>));

    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//根据匹配点求取本征矩阵，使用RANSAC，进一步排除失配点</span>
    Mat E = findEssentialMat(p1, p2, focal_length, principle_point, RANSAC, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.999</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1.0</span>, mask);
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (E.empty()) <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span> feasible_count = countNonZero(mask);
    <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">cout</span> << (<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">int</span>)feasible_count << <span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">" -in- "</span> << p1.size() << endl;
    <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">//对于RANSAC而言，outlier数量大于50%时，结果是不可靠的</span>
    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">if</span> (feasible_count <= <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">15</span> || (feasible_count / p1.size()) < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.6</span>)
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</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> pass_count = recoverPose(E, p1, p2, R, T, focal_length, principle_point, mask);

    <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;">if</span> (((<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">double</span>)pass_count) / feasible_count < <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.7</span>)
        <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">false</span>;

    <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">return</span> <span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">true</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></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></ul>

三维重建

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

s2x2=K(R2X+T2)

这个等式中有两个未知量，分别是 s2 和 X 。用 x2 对等式两边做外积，可以消去 s2 ，得 

0=x2ˆK(R2X+T2)

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

x2ˆKR2X=−x2ˆT2

解上述方程，即可求取X。其几何意义相当于分别从两个相机的光心作过 x1 和 x2 的延长线，延长线的焦点即为方程的解，如文章最上方的图所示。由于这种方法和三角测距类似，因此这种重建方式也被称为三角化（triangulate）。OpenCV提供了该方法，可以直接使用。

<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& R, Mat& 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, <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, Mat& 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);

    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>)) = 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_32FC1);
    proj1.col(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>) = 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_32FC1);

    R.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);
    T.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>
    triangulatePoints(proj1, proj2, p1, p2, structure);
}</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></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></ul>

测试

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

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

 

图片中的两个彩色坐标系分别代表两个相机的位置。 
在接下来的文章中，会将相机的个数推广到任意多个，成为一个真正的SfM系统。

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

代码下载