注意：本文中的代碼必須使用OpenCV3.0或以上版本進(jìn)行編譯，因?yàn)楹芏嗪瘮?shù)是3.0以后才加入的。

目錄：

• SfM介紹
• 小孔相機(jī)模型
• 坐標(biāo)系
• 內(nèi)參矩陣
• 外參矩陣
• 相機(jī)的標(biāo)定

SfM介紹

SfM的全稱為Structure from Motion，即通過相機(jī)的移動(dòng)來確定目標(biāo)的空間和幾何關(guān)系，是三維重建的一種常見方法。它與Kinect這種3D攝像頭最大的不同在于，它只需要普通的RGB攝像頭即可，因此成本更低廉，且受環(huán)境約束較小，在室內(nèi)和室外均能使用。但是，SfM背后需要復(fù)雜的理論和算法做支持，在精度和速度上都還有待提高，所以目前成熟的商業(yè)應(yīng)用并不多。?
本系列介紹SfM中的基本原理與算法，借助OpenCV實(shí)現(xiàn)一個(gè)簡易的SfM系統(tǒng)。

小孔相機(jī)模型

在計(jì)算機(jī)視覺中，最常用的相機(jī)模型就是小孔模型（小孔成像模型），它將相機(jī)的透鏡組簡化為一個(gè)小孔，光線透過小孔在小孔后方的像面上成像，如下圖所示。?
?
由上圖可知，小孔模型成的是倒像，為了表述與研究的方便，我們常常將像面至于小孔之前，且到小孔的距離仍然是焦距f，這樣的模型與原來的小孔模型是等價(jià)的，只不過成的是正像，符合人的直觀感受。在這種情況下，往往將小孔稱作光心（Optical Center）。?
?
小孔模型是一種理想相機(jī)模型，沒有考慮實(shí)際相機(jī)中存在的場曲、畸變等問題。在實(shí)際使用時(shí)，這些問題可以通過在標(biāo)定的過程中引入畸變參數(shù)解決，所以小孔模型仍然是目前最廣泛使用的相機(jī)模型。

坐標(biāo)系

為了用數(shù)學(xué)研究SfM，我們需要坐標(biāo)系。在SfM中主要有兩類坐標(biāo)系，一類為相機(jī)坐標(biāo)系，一類為世界坐標(biāo)系。在本系列中，所以坐標(biāo)系均為右手坐標(biāo)系。?
相機(jī)坐標(biāo)系以相機(jī)的光心（小孔）作為原點(diǎn)，X軸為水平方向，Y軸為豎直方向，Z軸指向相機(jī)所觀察的方向。?
世界坐標(biāo)系的原點(diǎn)可以任意選擇，與相機(jī)的具體位置無關(guān)。?

內(nèi)參矩陣

設(shè)空間中有一點(diǎn)P，若世界坐標(biāo)系與相機(jī)坐標(biāo)系重合，則該點(diǎn)在空間中的坐標(biāo)為(X, Y, Z)，其中Z為該點(diǎn)到相機(jī)光心的垂直距離。設(shè)該點(diǎn)在像面上的像為點(diǎn)p，像素坐標(biāo)為(x, y)，那么(X, Y, Z)和(x, y)有什么關(guān)系呢？?
?
由上圖可知，這是一個(gè)簡單的相似三角形關(guān)系，從而得到?

x=fXZ,???y=fYZ
但是，圖像的像素坐標(biāo)系原點(diǎn)在左上角，而上面公式假定原點(diǎn)在圖像中心，為了處理這一偏移，設(shè)光心在圖像上對應(yīng)的像素坐標(biāo)為 (cx,cy) ，則?
x=fXZ+cx,???y=fYZ+cy
將以上關(guān)系表示為矩陣形式，有?
Zxy1=f000f0cxcy1XYZ
其中，將矩陣?
K=f000f0cxcy1
稱為內(nèi)參矩陣，因?yàn)樗缓拖鄼C(jī)自身的內(nèi)部參數(shù)有關(guān)（焦距，光心位置）。

外參矩陣

一般情況下，世界坐標(biāo)系和相機(jī)坐標(biāo)系不重合，這時(shí)，世界坐標(biāo)系中的某一點(diǎn)P要投影到像面上時(shí)，先要將該點(diǎn)的坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)系下。設(shè)P在世界坐標(biāo)系中的坐標(biāo)為X，P到光心的垂直距離為s（即上文中的Z），在像面上的坐標(biāo)為x，世界坐標(biāo)系與相機(jī)坐標(biāo)系之間的相對旋轉(zhuǎn)為矩陣R（R是一個(gè)三行三列的旋轉(zhuǎn)矩陣），相對位移為向量T（三行一列），則?

sx=K[RX+T]
其中 RX+T ?即為P在相機(jī)坐標(biāo)系下的坐標(biāo)，使用齊次坐標(biāo)改寫上式，有?
sx=K[RT][X1]
其中 [RT] 是一個(gè)三行四列的矩陣，稱為外參矩陣，它和相機(jī)的參數(shù)無關(guān)，只與相機(jī)在世界坐標(biāo)系中的位置有關(guān)。?

相機(jī)的標(biāo)定

相機(jī)的標(biāo)定，即為通過某個(gè)已知的目標(biāo)，求取相機(jī)內(nèi)參矩陣的過程。最常用的標(biāo)定目標(biāo)就是棋盤格。用相機(jī)對棋盤格從不同角度拍攝多張照片，然后將這些照片導(dǎo)入標(biāo)定程序或算法，即可自動(dòng)求出相機(jī)的內(nèi)參。?

相機(jī)標(biāo)定的方法和工具，我在這篇文章中已有詳細(xì)的介紹，這里就不再細(xì)述了。在此提醒一下，之后的文章中若無特殊說明，所有相機(jī)均假定內(nèi)參已知。

目錄：

• 極線約束與本征矩陣
• 特征點(diǎn)提取與匹配
• 三維重建
• 測試

極線約束與本征矩陣

在三維重建前，我們先研究一下同一點(diǎn)在兩個(gè)相機(jī)中的像的關(guān)系。假設(shè)在世界坐標(biāo)系中有一點(diǎn)p，坐標(biāo)為X，它在1相機(jī)中的像為x1，在2相機(jī)中的像為x2（注意x1和x2為齊次坐標(biāo)，最后一個(gè)元素是1），如下圖。?
?
設(shè)X到兩個(gè)相機(jī)像面的垂直距離分別為s1和s2，且這兩個(gè)相機(jī)具有相同的內(nèi)參矩陣K，與世界坐標(biāo)系之間的變換關(guān)系分別為[R1??T1]和[R2??T2]，那么我們可以得到下面兩個(gè)等式?

s1x1=K(R1X+T1)s2x2=K(R2X+T2)
由于K是可逆矩陣，兩式坐乘K的逆，有?
1
設(shè) K1x1=x′1 ， K1x2=x′2 ，則有?
s1x′1=R1X+T1s2x′2=R2X+T2
我們一般稱 x′1 和 x′2 為歸一化后的像坐標(biāo)，它們和圖像的大小沒有關(guān)系，且原點(diǎn)位于圖像中心。?
由于世界坐標(biāo)系可以任意選擇，我們將世界坐標(biāo)系選為第一個(gè)相機(jī)的相機(jī)坐標(biāo)系，這時(shí) R1=I,?T1=0 。上式則變?yōu)?
s1x′1=Xs2x′2=R2X+T2
將第一式帶入第二式，有?
s2x′2=s1R2x′1+T2
x′2 和 T2 都是三維向量，它們做外積（叉積）之后得到另外一個(gè)三維向量 T2x′2 （其中 為外積的矩陣形式， T2x′2 代表 T2×x′2 ），且該向量垂直于 x′2 和 T2 ，再用該向量對等式兩邊做內(nèi)積，有?
0=s1(T2x′2)TR2x′1
即?
x′2T2R2x′1=0
令 E=T2R2 ?有?
x′2Ex′1=0
可以看出，上式是同一點(diǎn)在兩個(gè)相機(jī)中的像所滿足的關(guān)系，它和點(diǎn)的空間坐標(biāo)、點(diǎn)到相機(jī)的距離均沒有關(guān)系，我們稱之為極線約束，而矩陣 E 則稱為關(guān)于這兩個(gè)相機(jī)的本征矩陣。如果我們知道兩幅圖像中的多個(gè)對應(yīng)點(diǎn)（至少5對），則可以通過上式解出矩陣 E ，又由于 E 是由 T2 和 R2 構(gòu)成的，可以從E中分解出 T2 和 R2 。?
如何從 E 中分解出兩個(gè)相機(jī)的相對變換關(guān)系（即 T2 和 R2 ），背后的數(shù)學(xué)原理比較復(fù)雜，好在OpenCV為我們提供了這樣的方法，在此就不談原理了。

特征點(diǎn)提取與匹配

從上面的分析可知，要求取兩個(gè)相機(jī)的相對關(guān)系，需要兩幅圖像中的對應(yīng)點(diǎn)，這就變成的特征點(diǎn)的提取和匹配問題。對于圖像差別較大的情況，推薦使用SIFT特征，因?yàn)镾IFT對旋轉(zhuǎn)、尺度、透視都有較好的魯棒性。如果差別不大，可以考慮其他更快速的特征，比如SURF、ORB等。?
本文中使用SIFT特征，由于OpenCV3.0將SIFT包含在了擴(kuò)展部分中，所以官網(wǎng)上下載的版本是沒有SIFT的，為此需要到這里下載擴(kuò)展包，并按照里面的說明重新編譯OpenCV（哎~真麻煩，-_-!）。如果你使用其他特征，就不必為此辛勞了。?
下面的代碼負(fù)責(zé)提取圖像特征，并進(jìn)行匹配。

void extract_features(vector<string>& image_names,vector<vector<KeyPoint>>& key_points_for_all,vector<Mat>& descriptor_for_all,vector<vector<Vec3b>>& colors_for_all) {key_points_for_all.clear();descriptor_for_all.clear();Mat image;//讀取圖像，獲取圖像特征點(diǎn)，并保存Ptr<Feature2D> sift = xfeatures2d::SIFT::create(0, 3, 0.04, 10);for (auto it = image_names.begin(); it != image_names.end(); ++it){image = imread(*it);if (image.empty()) continue;vector<KeyPoint> key_points;Mat descriptor;//偶爾出現(xiàn)內(nèi)存分配失敗的錯(cuò)誤sift->detectAndCompute(image, noArray(), key_points, descriptor);//特征點(diǎn)過少，則排除該圖像if (key_points.size() <= 10) continue;key_points_for_all.push_back(key_points);descriptor_for_all.push_back(descriptor);vector<Vec3b> colors(key_points.size());for (int i = 0; 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);} }void match_features(Mat& query, Mat& train, vector<DMatch>& matches) {vector<vector<DMatch>> knn_matches;BFMatcher matcher(NORM_L2);matcher.knnMatch(query, train, knn_matches, 2);//獲取滿足Ratio Test的最小匹配的距離float min_dist = FLT_MAX;for (int r = 0; r < knn_matches.size(); ++r){//Ratio Testif (knn_matches[r][0].distance > 0.6*knn_matches[r][1].distance)continue;float dist = knn_matches[r][0].distance;if (dist < min_dist) min_dist = dist;}matches.clear();for (size_t r = 0; r < knn_matches.size(); ++r){//排除不滿足Ratio Test的點(diǎn)和匹配距離過大的點(diǎn)if (knn_matches[r][0].distance > 0.6*knn_matches[r][1].distance ||knn_matches[r][0].distance > 5 * max(min_dist, 10.0f))continue;//保存匹配點(diǎn)matches.push_back(knn_matches[r][0]);} }

需要重點(diǎn)說明的是，匹配結(jié)果往往有很多誤匹配，為了排除這些錯(cuò)誤，這里使用了Ratio Test方法，即使用KNN算法尋找與該特征最匹配的2個(gè)特征，若第一個(gè)特征的匹配距離與第二個(gè)特征的匹配距離之比小于某一閾值，就接受該匹配，否則視為誤匹配。當(dāng)然，也可以使用Cross Test（交叉驗(yàn)證）方法來排除錯(cuò)誤。

得到匹配點(diǎn)后，就可以使用OpenCV3.0中新加入的函數(shù)findEssentialMat()來求取本征矩陣了。得到本征矩陣后，再使用另一個(gè)函數(shù)對本征矩陣進(jìn)行分解，并返回兩相機(jī)之間的相對變換R和T。注意這里的T是在第二個(gè)相機(jī)的坐標(biāo)系下表示的，也就是說，其方向從第二個(gè)相機(jī)指向第一個(gè)相機(jī)（即世界坐標(biāo)系所在的相機(jī)），且它的長度等于1。

bool find_transform(Mat& K, vector<Point2f>& p1, vector<Point2f>& p2, Mat& R, Mat& T, Mat& mask) {//根據(jù)內(nèi)參矩陣獲取相機(jī)的焦距和光心坐標(biāo)（主點(diǎn)坐標(biāo)）double focal_length = 0.5*(K.at<double>(0) + K.at<double>(4));Point2d principle_point(K.at<double>(2), K.at<double>(5));//根據(jù)匹配點(diǎn)求取本征矩陣，使用RANSAC，進(jìn)一步排除失配點(diǎn)Mat E = findEssentialMat(p1, p2, focal_length, principle_point, RANSAC, 0.999, 1.0, mask);if (E.empty()) return false;double feasible_count = countNonZero(mask);cout << (int)feasible_count << " -in- " << p1.size() << endl;//對于RANSAC而言，outlier數(shù)量大于50%時(shí)，結(jié)果是不可靠的if (feasible_count <= 15 || (feasible_count / p1.size()) < 0.6)return false;//分解本征矩陣，獲取相對變換int pass_count = recoverPose(E, p1, p2, R, T, focal_length, principle_point, mask);//同時(shí)位于兩個(gè)相機(jī)前方的點(diǎn)的數(shù)量要足夠大if (((double)pass_count) / feasible_count < 0.7)return false;return true; }

三維重建

現(xiàn)在已經(jīng)知道了兩個(gè)相機(jī)之間的變換矩陣，還有每一對匹配點(diǎn)的坐標(biāo)。三維重建就是通過這些已知信息還原匹配點(diǎn)在空間當(dāng)中的坐標(biāo)。在前面的推導(dǎo)中，我們有?

s2x2=K(R2X+T2)
這個(gè)等式中有兩個(gè)未知量，分別是 s2 和 X 。用 x2 對等式兩邊做外積，可以消去 s2 ，得?
0=x2K(R2X+T2)
整理一下可以得到一個(gè)關(guān)于空間坐標(biāo)X的線性方程?
x2KR2X=x2T2
解上述方程，即可求取X。其幾何意義相當(dāng)于分別從兩個(gè)相機(jī)的光心作過 x1 和 x2 的延長線，延長線的焦點(diǎn)即為方程的解，如文章最上方的圖所示。由于這種方法和三角測距類似，因此這種重建方式也被稱為三角化（triangulate）。OpenCV提供了該方法，可以直接使用。

void reconstruct(Mat& K, Mat& R, Mat& T, vector<Point2f>& p1, vector<Point2f>& p2, Mat& structure) {//兩個(gè)相機(jī)的投影矩陣[R T]，triangulatePoints只支持float型Mat proj1(3, 4, CV_32FC1);Mat proj2(3, 4, CV_32FC1);proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);T.convertTo(proj2.col(3), CV_32FC1);Mat fK;K.convertTo(fK, CV_32FC1);proj1 = fK*proj1;proj2 = fK*proj2;//三角化重建triangulatePoints(proj1, proj2, p1, p2, structure); }

測試

我用了下面兩幅圖像進(jìn)行測試?

得到了著色后的稀疏點(diǎn)云，是否能看出一點(diǎn)輪廓呢？！

?

圖片中的兩個(gè)彩色坐標(biāo)系分別代表兩個(gè)相機(jī)的位置。?
在接下來的文章中，會(huì)將相機(jī)的個(gè)數(shù)推廣到任意多個(gè)，成為一個(gè)真正的SfM系統(tǒng)。

關(guān)于源代碼的使用?
代碼是用VS2013寫的，OpenCV版本為3.0且包含擴(kuò)展部分，如果不使用SIFT特征，可以修改源代碼，然后使用官方未包含擴(kuò)展部分的庫。軟件運(yùn)行后會(huì)將三維重建的結(jié)果寫入Viewer目錄下的structure.yml文件中，在Viewer目錄下有一個(gè)SfMViewer程序，直接運(yùn)行即可讀取yml文件并顯示三維結(jié)構(gòu)。

目錄：

• 問題簡化
• 求第三個(gè)相機(jī)的變換矩陣
• 加入更多圖像
• 代碼實(shí)現(xiàn)
• 測試
• 思考
• 下載

問題簡化

終于有時(shí)間來填坑了，這次一口氣將雙目重建擴(kuò)展為多目重建吧。首先，為了簡化問題，我們要做一個(gè)重要假設(shè)：用于多目重建的圖像是有序的，即相鄰圖像的拍攝位置也是相鄰的。多目重建本身比較復(fù)雜，我會(huì)盡量說得清晰，如有表述不清的地方，還請見諒并歡迎提問。

---

求第三個(gè)相機(jī)的變換矩陣

由前面的文章我們知道，兩個(gè)相機(jī)之間的變換矩陣可以通過findEssentialMat以及recoverPose函數(shù)來實(shí)現(xiàn)，設(shè)第一個(gè)相機(jī)的坐標(biāo)系為世界坐標(biāo)系，現(xiàn)在加入第三幅圖像（相機(jī)），如何確定第三個(gè)相機(jī)（后面稱為相機(jī)三）到到世界坐標(biāo)系的變換矩陣呢？

最簡單的想法，就是沿用雙目重建的方法，即在第三幅圖像和第一幅圖像之間提取特征點(diǎn)，然后調(diào)用findEssentialMat和recoverPose。那么加入第四幅、第五幅，乃至更多呢？隨著圖像數(shù)量的增加，新加入的圖像與第一幅圖像的差異可能越來越大，特征點(diǎn)的提取變得異常困難，這時(shí)就不能再沿用雙目重建的方法了。

那么能不能用新加入的圖像和相鄰圖像進(jìn)行特征匹配呢？比如第三幅與第二幅匹配，第四幅與第三幅匹配，以此類推。當(dāng)然可以，但是這時(shí)就不能繼續(xù)使用findEssentialMat和recoverPose來求取相機(jī)的變換矩陣了，因?yàn)檫@兩個(gè)函數(shù)求取的是相對變換，比如相機(jī)三到相機(jī)二的變換，而我們需要的是相機(jī)三到相機(jī)一的變換。有人說，既然知道相機(jī)二到相機(jī)一的變換，又知道相機(jī)到三到相機(jī)二的變換，不就能求出相機(jī)三到相機(jī)一的變換嗎？實(shí)際上，通過這種方式，你只能求出相機(jī)三到相機(jī)一的旋轉(zhuǎn)變換（旋轉(zhuǎn)矩陣R），而他們之間的位移向量T，是無法求出的。這是因?yàn)樯厦鎯蓚€(gè)函數(shù)求出的位移向量，都是單位向量，丟失了相機(jī)之間位移的比例關(guān)系。

說了這么多，我們要怎么解決這些問題？現(xiàn)在請出本文的主角——solvePnP和solvePnPRansac。根據(jù)opencv的官方解釋，該函數(shù)根據(jù)空間中的點(diǎn)與圖像中的點(diǎn)的對應(yīng)關(guān)系，求解相機(jī)在空間中的位置。也就是說，我知道一些空間當(dāng)中點(diǎn)的坐標(biāo)，還知道這些點(diǎn)在圖像中的像素坐標(biāo)，那么solvePnP就可以告訴我相機(jī)在空間當(dāng)中的坐標(biāo)。solvePnP和solvePnPRansac所實(shí)現(xiàn)的功能相同，只不過后者使用了隨機(jī)一致性采樣，使其對噪聲更魯棒，本文使用后者。

好了，有這么好的函數(shù)，怎么用于我們的三維重建呢？首先，使用雙目重建的方法，對頭兩幅圖像進(jìn)行重建，這樣就得到了一些空間中的點(diǎn)，加入第三幅圖像后，使其與第二幅圖像進(jìn)行特征匹配，這些匹配點(diǎn)中，肯定有一部分也是圖像二與圖像一之間的匹配點(diǎn)，也就是說，這些匹配點(diǎn)中有一部分的空間坐標(biāo)是已知的，同時(shí)又知道這些點(diǎn)在第三幅圖像中的像素坐標(biāo)，嗯，solvePnP所需的信息都有了，自然第三個(gè)相機(jī)的空間位置就求出來了。由于空間點(diǎn)的坐標(biāo)都是世界坐標(biāo)系下的（即第一個(gè)相機(jī)的坐標(biāo)系），所以由solvePnP求出的相機(jī)位置也是世界坐標(biāo)系下的，即相機(jī)三到相機(jī)一的變換矩陣。

---

加入更多圖像

通過上面的方法得到相機(jī)三的變換矩陣后，就可以使用上一篇文章提到的triangulatePoints方法將圖像三和圖像二之間的匹配點(diǎn)三角化，得到其空間坐標(biāo)。為了使之后的圖像仍能使用以上方法求解變換矩陣，我們還需要將新得到的空間點(diǎn)和之前的三維點(diǎn)云融合。已經(jīng)存在的空間點(diǎn)，就沒必要再添加了，只添加在圖像二和三之間匹配，但在圖像一和圖像三中沒有匹配的點(diǎn)。如此反復(fù)。?
?
為了方便點(diǎn)云的融合以及今后的擴(kuò)展，我們需要存儲(chǔ)圖像中每個(gè)特征點(diǎn)在空間中的對應(yīng)點(diǎn)。在代碼中我使用了一個(gè)二維列表，名字為correspond_struct_idx，correspond_struct_idx[i][j]代表第i幅圖像第j個(gè)特征點(diǎn)所對應(yīng)的空間點(diǎn)在點(diǎn)云中的索引，若索引小于零，說明該特征點(diǎn)在空間當(dāng)中沒有對應(yīng)點(diǎn)。通過此結(jié)構(gòu)，由特征匹配中的queryIdx和trainIdx就可以查詢某個(gè)特征點(diǎn)在空間中的位置。

---

代碼實(shí)現(xiàn)

前一篇文章的很多代碼不用修改，還可以繼續(xù)使用，但是程序的流程有了較大變化。首先是初始化點(diǎn)云，也就是通過雙目重建方法對圖像序列的頭兩幅圖像進(jìn)行重建，并初始化correspond_struct_idx。

void init_structure(Mat K,vector<vector<KeyPoint>>& key_points_for_all, vector<vector<Vec3b>>& colors_for_all,vector<vector<DMatch>>& matches_for_all,vector<Point3f>& structure,vector<vector<int>>& correspond_struct_idx,vector<Vec3b>& colors,vector<Mat>& rotations,vector<Mat>& motions) {//計(jì)算頭兩幅圖像之間的變換矩陣vector<Point2f> p1, p2;vector<Vec3b> c2;Mat R, T; //旋轉(zhuǎn)矩陣和平移向量Mat mask; //mask中大于零的點(diǎn)代表匹配點(diǎn)，等于零代表失配點(diǎn)get_matched_points(key_points_for_all[0], key_points_for_all[1], matches_for_all[0], p1, p2);get_matched_colors(colors_for_all[0], colors_for_all[1], matches_for_all[0], colors, c2);find_transform(K, p1, p2, R, T, mask);//對頭兩幅圖像進(jìn)行三維重建maskout_points(p1, mask);maskout_points(p2, mask);maskout_colors(colors, mask);Mat R0 = Mat::eye(3, 3, CV_64FC1);Mat T0 = Mat::zeros(3, 1, CV_64FC1);reconstruct(K, R0, T0, R, T, p1, p2, structure);//保存變換矩陣rotations = { R0, R };motions = { T0, T };//將correspond_struct_idx的大小初始化為與key_points_for_all完全一致correspond_struct_idx.clear();correspond_struct_idx.resize(key_points_for_all.size());for (int i = 0; i < key_points_for_all.size(); ++i){correspond_struct_idx[i].resize(key_points_for_all[i].size(), -1);}//填寫頭兩幅圖像的結(jié)構(gòu)索引int idx = 0;vector<DMatch>& matches = matches_for_all[0];for (int i = 0; i < matches.size(); ++i){if (mask.at<uchar>(i) == 0)continue;correspond_struct_idx[0][matches[i].queryIdx] = idx;correspond_struct_idx[1][matches[i].trainIdx] = idx;++idx;} }

初始點(diǎn)云得到后，就可以使用增量方式重建剩余圖像，注意，在代碼中為了方便實(shí)現(xiàn)，所有圖像之間的特征匹配已經(jīng)事先完成了，并保存在matches_for_all這個(gè)列表中。增量重建的關(guān)鍵是調(diào)用solvePnPRansac，而這個(gè)函數(shù)需要空間點(diǎn)坐標(biāo)和對應(yīng)的像素坐標(biāo)作為參數(shù)，有了correspond_struct_idx，實(shí)現(xiàn)這個(gè)對應(yīng)關(guān)系的查找還是很方便的，如下。

void get_objpoints_and_imgpoints(vector<DMatch>& matches,vector<int>& struct_indices, vector<Point3f>& structure, vector<KeyPoint>& key_points,vector<Point3f>& object_points,vector<Point2f>& image_points) {object_points.clear();image_points.clear();for (int i = 0; i < matches.size(); ++i){int query_idx = matches[i].queryIdx;int train_idx = matches[i].trainIdx;int struct_idx = struct_indices[query_idx];if (struct_idx < 0) continue;object_points.push_back(structure[struct_idx]);image_points.push_back(key_points[train_idx].pt);} }

之后調(diào)用solvePnPRansac得到相機(jī)的旋轉(zhuǎn)向量和位移，由于我們使用的都是旋轉(zhuǎn)矩陣，所以這里要調(diào)用opencv的Rodrigues函數(shù)將旋轉(zhuǎn)向量變換為旋轉(zhuǎn)矩陣。之后，使用上一篇文章中用到的reconstruct函數(shù)對匹配點(diǎn)進(jìn)行重建（三角化），不過為了適用于多目重建，做了一些簡單修改。

void reconstruct(Mat& K, Mat& R1, Mat& T1, Mat& R2, Mat& T2, vector<Point2f>& p1, vector<Point2f>& p2, vector<Point3f>& structure) {//兩個(gè)相機(jī)的投影矩陣[R T]，triangulatePoints只支持float型Mat proj1(3, 4, CV_32FC1);Mat proj2(3, 4, CV_32FC1);R1.convertTo(proj1(Range(0, 3), Range(0, 3)), CV_32FC1);T1.convertTo(proj1.col(3), CV_32FC1);R2.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);T2.convertTo(proj2.col(3), CV_32FC1);Mat fK;K.convertTo(fK, CV_32FC1);proj1 = fK*proj1;proj2 = fK*proj2;//三角重建Mat s;triangulatePoints(proj1, proj2, p1, p2, s);structure.clear();structure.reserve(s.cols);for (int i = 0; i < s.cols; ++i){Mat_<float> col = s.col(i);col /= col(3); //齊次坐標(biāo)，需要除以最后一個(gè)元素才是真正的坐標(biāo)值structure.push_back(Point3f(col(0), col(1), col(2)));} }

最后，將重建結(jié)構(gòu)與之前的點(diǎn)云進(jìn)行融合。

void fusion_structure(vector<DMatch>& matches, vector<int>& struct_indices, vector<int>& next_struct_indices,vector<Point3f>& structure, vector<Point3f>& next_structure,vector<Vec3b>& colors,vector<Vec3b>& next_colors) {for (int i = 0; i < matches.size(); ++i){int query_idx = matches[i].queryIdx;int train_idx = matches[i].trainIdx;int struct_idx = struct_indices[query_idx];if (struct_idx >= 0) //若該點(diǎn)在空間中已經(jīng)存在，則這對匹配點(diǎn)對應(yīng)的空間點(diǎn)應(yīng)該是同一個(gè)，索引要相同{next_struct_indices[train_idx] = struct_idx;continue;}//若該點(diǎn)在空間中已經(jīng)存在，將該點(diǎn)加入到結(jié)構(gòu)中，且這對匹配點(diǎn)的空間點(diǎn)索引都為新加入的點(diǎn)的索引structure.push_back(next_structure[i]);colors.push_back(next_colors[i]);struct_indices[query_idx] = next_struct_indices[train_idx] = structure.size() - 1;} }

整個(gè)增量方式重建圖像的代碼大致如下。

//初始化結(jié)構(gòu)（三維點(diǎn)云） init_structure(K,key_points_for_all,colors_for_all,matches_for_all,structure,correspond_struct_idx,colors,rotations,motions);//增量方式重建剩余的圖像 for (int i = 1; i < matches_for_all.size(); ++i) {vector<Point3f> object_points;vector<Point2f> image_points;Mat r, R, T;//Mat mask;//獲取第i幅圖像中匹配點(diǎn)對應(yīng)的三維點(diǎn)，以及在第i+1幅圖像中對應(yīng)的像素點(diǎn)get_objpoints_and_imgpoints(matches_for_all[i], correspond_struct_idx[i], structure,key_points_for_all[i+1], object_points,image_points);//求解變換矩陣solvePnPRansac(object_points, image_points, K, noArray(), r, T);//將旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣Rodrigues(r, R);//保存變換矩陣rotations.push_back(R);motions.push_back(T);vector<Point2f> p1, p2;vector<Vec3b> c1, c2;get_matched_points(key_points_for_all[i], key_points_for_all[i + 1], matches_for_all[i], p1, p2);get_matched_colors(colors_for_all[i], colors_for_all[i + 1], matches_for_all[i], c1, c2);//根據(jù)之前求得的R，T進(jìn)行三維重建vector<Point3f> next_structure;reconstruct(K, rotations[i], motions[i], R, T, p1, p2, next_structure);//將新的重建結(jié)果與之前的融合fusion_structure(matches_for_all[i], correspond_struct_idx[i], correspond_struct_idx[i + 1],structure, next_structure,colors,c1); }

---

測試

我用了八幅圖像進(jìn)行測試，正如問題簡化中所要求的那樣，圖像是有序的。?
?
程序的大部分時(shí)間花在特征提取和匹配上，真正的重建過程耗時(shí)很少。最終結(jié)果如下。?
?
圖中每個(gè)彩色坐標(biāo)系都代表一個(gè)相機(jī)。

---

思考

• 這個(gè)多目三維重建程序，要求圖像必須是有序的，如果圖像無序，比如只是對某個(gè)目標(biāo)在不同角度的隨意拍攝，程序應(yīng)該如何修改？
• 增量式三維重建方法，有一個(gè)很大的缺點(diǎn)——隨著圖像的不斷增加，誤差會(huì)不斷累積，最后誤差過大以至于完全偏離重建的目標(biāo)，怎么解決？

有興趣的讀者可以思考一下上面兩個(gè)問題，第二個(gè)問題比較難，我會(huì)在下一篇文章中詳細(xì)介紹。

---

下載

程序使用VS2015開發(fā)，OpenCV版本為3.1且包含擴(kuò)展部分，如果不使用SIFT特征，可以修改源代碼，然后使用官方未包含擴(kuò)展部分的庫。軟件運(yùn)行后會(huì)將三維重建的結(jié)果寫入Viewer目錄下的structure.yml文件中，在Viewer目錄下有一個(gè)SfMViewer程序，直接運(yùn)行即可讀取yml文件并顯示三維結(jié)構(gòu)。

總結(jié)

以上是生活随笔為你收集整理的三维重建-opencv实现sfm的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。