轉載地址：http://blog.csdn.net/aichipmunk/article/details/48132109

SfM介紹

SfM的全稱爲Structure from Motion，即通過相機的移動來確定目標的空間和幾何關係，是三維重建的一種常見方法。它與Kinect這種3D攝像頭最大的不同在於，它只需要普通的RGB攝像頭即可，因此成本更低廉，且受環境約束較小，在室內和室外均能使用。但是，SfM背後需要複雜的理論和算法做支持，在精度和速度上都還有待提高，所以目前成熟的商業應用並不多。
本系列介紹SfM中的基本原理與算法，藉助OpenCV實現一個簡易的SfM系統。

小孔相機模型

在計算機視覺中，最常用的相機模型就是小孔模型（小孔成像模型），它將相機的透鏡組簡化爲一個小孔，光線透過小孔在小孔後方的像面上成像，如下圖所示。

由上圖可知，小孔模型成的是倒像，爲了表述與研究的方便，我們常常將像面至於小孔之前，且到小孔的距離仍然是焦距f，這樣的模型與原來的小孔模型是等價的，只不過成的是正像，符合人的直觀感受。在這種情況下，往往將小孔稱作光心（Optical Center）。

小孔模型是一種理想相機模型，沒有考慮實際相機中存在的場曲、畸變等問題。在實際使用時，這些問題可以通過在標定的過程中引入畸變參數解決，所以小孔模型仍然是目前最廣泛使用的相機模型。

座標系

爲了用數學研究SfM，我們需要座標系。在SfM中主要有兩類座標系，一類爲相機座標系，一類爲世界座標系。在本系列中，所以座標系均爲右手座標系。
相機座標系以相機的光心（小孔）作爲原點，X軸爲水平方向，Y軸爲豎直方向，Z軸指向相機所觀察的方向。
世界座標系的原點可以任意選擇，與相機的具體位置無關。

內參矩陣

設空間中有一點P，若世界座標系與相機座標系重合，則該點在空間中的座標爲(X, Y, Z)，其中Z爲該點到相機光心的垂直距離。設該點在像面上的像爲點p，像素座標爲(x, y)，那麼(X, Y, Z)和(x, y)有什麼關係呢？

由上圖可知，這是一個簡單的相似三角形關係，從而得到

x = f X Z, y = f Y Z

但是，圖像的像素座標系原點在左上角，而上面公式假定原點在圖像中心，爲了處理這一偏移，設光心在圖像上對應的像素座標爲

(cx,cy)，則

x = f X Z + c x, y = f Y Z + c y

將以上關係表示爲矩陣形式，有

Z ⎡ ⎣ ⎢ x y 1 ⎤ ⎦ ⎥ = ⎡ ⎣ ⎢ f 00 0 f 0 c x c y 1 ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ X Y Z ⎤ ⎦ ⎥

其中，將矩陣

K = ⎡ ⎣ ⎢ f 00 0 f 0 c x c y 1 ⎤ ⎦ ⎥

稱爲內參矩陣，因爲它只和相機自身的內部參數有關（焦距，光心位置）。

外參矩陣

一般情況下，世界座標系和相機座標系不重合，這時，世界座標系中的某一點P要投影到像面上時，先要將該點的座標轉換到相機座標系下。設P在世界座標系中的座標爲X，P到光心的垂直距離爲s（即上文中的Z），在像面上的座標爲x，世界座標系與相機座標系之間的相對旋轉爲矩陣R（R是一個三行三列的旋轉矩陣），相對位移爲向量T（三行一列），則

s x = K [R X + T]

其中

RX+T 即爲P在相機座標系下的座標，使用齊次座標改寫上式，有

s x = K [R T] [X 1]

其中

[RT]是一個三行四列的矩陣，稱爲外參矩陣，它和相機的參數無關，只與相機在世界座標系中的位置有關。

相機的標定

相機的標定，即爲通過某個已知的目標，求取相機內參矩陣的過程。最常用的標定目標就是棋盤格。用相機對棋盤格從不同角度拍攝多張照片，然後將這些照片導入標定程序或算法，即可自動求出相機的內參。
相機標定的方法和工具，我在這篇文章中已有詳細的介紹，這裏就不再細述了。在此提醒一下，之後的文章中若無特殊說明，所有相機均假定內參已知。

極線約束與本徵矩陣

在三維重建前，我們先研究一下同一點在兩個相機中的像的關係。假設在世界座標系中有一點p，座標爲X，它在1相機中的像爲x1，在2相機中的像爲x2（注意x1和x2爲齊次座標，最後一個元素是1），如下圖。

設X到兩個相機像面的垂直距離分別爲s1和s2，且這兩個相機具有相同的內參矩陣K，與世界座標系之間的變換關係分別爲[R1 T1]和[R2 T2]，那麼我們可以得到下面兩個等式

s 1 x 1 = K (R 1 X + T 1) s 2 x 2 = K (R 2 X + T 2)

由於K是可逆矩陣，兩式坐乘K的逆，有

s 1 K - 1 x 1 = R 1 X + T 1 s 2 K - 1 x 2 = R 2 X + T 2

設

K−1x1=x′1，

K−1x2=x′2，則有

s 1 x' 1 = R 1 X + T 1 s 2 x' 2 = R 2 X + T 2

我們一般稱

x′1和

x′2爲歸一化後的像座標，它們和圖像的大小沒有關係，且原點位於圖像中心。
由於世界座標系可以任意選擇，我們將世界座標系選爲第一個相機的相機座標系，這時

R1=I, T1=0。上式則變爲

s 1 x' 1 = X s 2 x' 2 = R 2 X + T 2

將第一式帶入第二式，有

s 2 x' 2 = s 1 R 2 x' 1 + T 2

x′2和

T2都是三維向量，它們做外積（叉積）之後得到另外一個三維向量

T2ˆx′2（其中

T2ˆ爲外積的矩陣形式，

T2ˆx′2代表

T2×x′2），且該向量垂直於

x′2和

T2，再用該向量對等式兩邊做內積，有

0 = s 1 (T 2 ˆ x' 2) T R 2 x' 1

即

x' 2 T 2 ˆ R 2 x' 1 = 0

令

E=T2ˆR2 有

x' 2 E x' 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"><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>

三維重建

現在已經知道了兩個相機之間的變換矩陣，還有每一對匹配點的座標。三維重建就是通過這些已知信息還原匹配點在空間當中的座標。在前面的推導中，我們有

s 2 x 2 = K (R 2 X + T 2)

這個等式中有兩個未知量，分別是

s2和

X。用

x2對等式兩邊做外積，可以消去

s2，得

0 = x 2 ˆ K (R 2 X + T 2)

整理一下可以得到一個關於空間座標X的線性方程

x 2 ˆ K R 2 X = - x 2 ˆ K T 2

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

x 2 ˆ K (R 2 T) (X 1) = 0

用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>

測試

用了下面兩幅圖像進行測試

得到了着色後的稀疏點雲，是否能看出一點輪廓呢？！

圖片中的兩個彩色座標系分別代表兩個相機的位置。
在接下來的文章中，會將相機的個數推廣到任意多個，成爲一個真正的SfM系統。

問題簡化

終於有時間來填坑了，這次一口氣將雙目重建擴展爲多目重建吧。首先，爲了簡化問題，我們要做一個重要假設：用於多目重建的圖像是有序的，即相鄰圖像的拍攝位置也是相鄰的。多目重建本身比較複雜，我會盡量說得清晰，如有表述不清的地方，還請見諒並歡迎提問。

求第三個相機的變換矩陣

由前面的文章我們知道，兩個相機之間的變換矩陣可以通過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>

測試

我用了八幅圖像進行測試，正如問題簡化中所要求的那樣，圖像是有序的。

程序的大部分時間花在特徵提取和匹配上，真正的重建過程耗時很少。最終結果如下。

圖中每個彩色座標系都代表一個相機。

SFM：從運動中恢復結構

轉載地址：http://blog.csdn.net/aichipmunk/article/details/48132109

SfM介紹

小孔相機模型

座標系

內參矩陣

外參矩陣

相機的標定

極線約束與本徵矩陣

特徵點提取與匹配

三維重建

測試

問題簡化

求第三個相機的變換矩陣

加入更多圖像

代碼實現

測試

MySQL 核心模塊揭祕 | 18 期 | 鎖在內存里長什麼樣*

使用perf工具生成火焰圖

大齡程序員思考

響應式界面控件DevExtreme * 更強的數據分析和可視化功能

HttpSecurity 是如何組裝過濾器鏈的

數說海南——近6年海南各市縣人口簡單看

長序列中Transformers的高級注意力機制總結

WebStorm 創建 Vue 項目

Macbook安裝mysql+php+phpmyadming進行網站構建筆記

anaconda+ pytorch+torchvision安裝

VS2017+cmake3.13+opencv4+opencv-contrib4編譯生成opencv

sublime 2中快捷鍵的彙總

MacBook 安裝 win10 系統的方法

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

SFM：從運動中恢復結構

轉載地址 ：http://blog.csdn.net/aichipmunk/article/details/48132109

SfM介紹

小孔相機模型

座標系

內參矩陣

外參矩陣

相機的標定

極線約束與本徵矩陣

特徵點提取與匹配

三維重建

測試

問題簡化

求第三個相機的變換矩陣

加入更多圖像

代碼實現

測試

轉載地址：http://blog.csdn.net/aichipmunk/article/details/48132109