第四章來至運動的結(jié)構(gòu)

1、 探測來至運動的結(jié)構(gòu)在這一章，我們將討論來至運動的結(jié)構(gòu)的概念（SfM），或者從一個運動的相機拍攝到的圖像中更好的推測提取出來的幾何結(jié)構(gòu)，我們使用OpenCV的Api函數(shù)來幫我們完成。讓我們將我們使用的冗長的方法約束為使用單個攝像機，通常稱為單目方法，并且是一組分離的和稀疏的視頻幀而不是連續(xù)的視頻流。這兩個約束在很大程度上簡化了系統(tǒng)，這個系統(tǒng)我們將在接下來的頁碼中進(jìn)行描述，并且?guī)椭覀兝斫馊魏蜸fM方法的原理。為了實現(xiàn)我們的方法，我們將跟隨Hartley和Zisserman的腳步（后面稱作H和Z），伴隨著他們有創(chuàng)意的書計算機視覺中的多視覺幾何的第9章到第12章的記錄。在這一章，我們將涉及到以下內(nèi)

2、容：1、 來至運動結(jié)構(gòu)的概念2、 從一組圖像中估計攝像機的運動3、 重構(gòu)場景4、 從多個視圖中重構(gòu)5、 可視化3D點云自始自終本章假定使用一個標(biāo)定過的攝像機一個先前標(biāo)定過的攝像機。標(biāo)定是在計算機視覺中普遍存在的操作，在OpenCV中得到很好的支持，我們可以使用在前面一張討論的命令行方法來完成標(biāo)定。因此，我們假定攝像機內(nèi)參數(shù)存在，并且具體化到K矩陣中，K矩陣為攝像機標(biāo)定過程的一個結(jié)果輸出。為了在語言方面表達(dá)的清晰，從現(xiàn)在開始我們將單個攝像機稱為場景的單個視圖而不是光學(xué)和獲取圖像的硬件。一個攝像機在空間中有一個位置，一個觀察的方向。在兩個攝像機中，有一個平移成分（空間運動）和一個觀察方向的旋轉(zhuǎn)。我

3、們同樣對場景中的點統(tǒng)一一下術(shù)語，世界（world），現(xiàn)實（real），或者3D，意思是一樣的，即我們真實世界存在的一個點。這同樣適用于圖像中的點或者2D,這些點在圖像坐標(biāo)系中，是在這個位置和時間上一些現(xiàn)實的3D點投影到攝像機傳感器上形成的。在這一章的代碼部分，你將注意參考計算視覺中的多視覺幾何（Multiple View Geometry in Computer Vision），例如，/HZ9.12，這指的是這本書的第9章第12個等式。同樣的，文本僅包括代碼的摘錄，而完整的代碼包含在伴隨著這本書的材料中。來至運動結(jié)構(gòu)的概念第一個我們應(yīng)當(dāng)區(qū)別的是立體（Stereo or indeed any m

4、ultiview），使用標(biāo)準(zhǔn)平臺的三維重建和SfM之間的差異。在兩個或多個攝像機的平臺中，假定我們已經(jīng)知道了兩個攝像機之間的運動，而在SfM中，我們實際上不知道這個運動并且我們希望找到它。標(biāo)準(zhǔn)平臺，來至觀察的一個過分簡單的點，可以得到一個更加精確的3D幾何的重構(gòu)，因為在估計多個攝像機間距離和旋轉(zhuǎn)時沒有誤差（距離和旋轉(zhuǎn)已知）。實現(xiàn)一個SfM系統(tǒng)的第一步是找到相機之間的運動。OpenCV可以幫助我們在許多方式上獲得這個運動，特別地，使用findFundamentalMat函數(shù)。讓我們想一下選擇一個SfM算法背后的目的。在很多情況下，我們希望獲得場景的幾何。例如，目標(biāo)相對于相機的位置和他們的形狀是什

5、么。假定我們知道了捕獲同一場景的攝像機之間的運動，從觀察的一個合理的相似點，現(xiàn)在我們想重構(gòu)這個幾何。在計算視覺術(shù)語中稱為三角測量（triangulation），并且有很多方法可以做這件事。它可以通過 射線相交的方法完成，這里我們構(gòu)造兩個射線：來至于每個攝像機投影中心的點和每一個圖像平面上的點。理想上，這兩個射線在空間中將相交于真實世界的一個3D點（這個3D點在每個攝像機中成像），如下面圖表展示：實際上，射線相交非常不可靠。H和Z推薦不使用它。這是因為兩個射線通常不能相交，讓我們回到使用連接兩個射線的最短線段上的中間點。相反，H和Z建議一些方法來三角化3D點（trianglulate 3D po

6、ints），這些方法中我們將在重構(gòu)場景部分討論兩個。OpenCV現(xiàn)在的版本沒有包含三角測量的API，因此，這部分我們將自己編寫代碼。學(xué)習(xí)完如何從兩個視圖恢復(fù)3D幾何之后，我們將看到我們是怎么樣加入更多的同一個場景的視圖來獲得更豐富的重構(gòu)。在那時，大部分的SfM方法試圖依靠束調(diào)整（Bundle Adjustment）來優(yōu)化我們的攝像機和3D點一束估計位置（the bundle of estimated positon），這部分內(nèi)容在重構(gòu)提純部分（Refinement of the reconstruction section）。OpenCV在新的圖像拼接的工具箱內(nèi)包含了用來束調(diào)整的方法。然而，使

7、OpenCV和C+完美結(jié)合的工作是豐富的外部工具，這些工具可以很容易地整合到一起。因此，我們將看到如何如何整合一個外部的束調(diào)節(jié)器靈巧的SSBA庫。既然我們已經(jīng)描述了使用OpenCV實現(xiàn)我們的SfM方法的一個概括，我們將看到每個分成是如何實現(xiàn)的。使用一對圖像估計攝像機的運動事實上，在我們開始著手兩個攝像機之間的運動之前，讓我們檢查一下我們手邊用來執(zhí)行這個操作的輸入和工具。首先，我們有來至（希望并不是非常）不同空間位置的同一場景的兩個圖像。這是一個強大的資產(chǎn)，并且我們將確保使用它?，F(xiàn)在工具都有了，我們應(yīng)當(dāng)看一下數(shù)學(xué)對象，來對我們的圖像，相機和場景施加約束。兩個非常有用的數(shù)學(xué)對象是基礎(chǔ)矩陣（用F表示

8、）和本征矩陣（用E表示）。除了本征矩陣是假設(shè)使用的標(biāo)定的相機，它們非常相似。這種情況針對我們，所以我們將選著它。OpenCV函數(shù)僅允許我們通過findFundamentalMat函數(shù)找到基礎(chǔ)矩陣。然而，我們非常簡單地使用標(biāo)定矩陣（calibration matrix）K從本征矩陣中獲得基礎(chǔ)矩陣，如下：Mat_<double> E = K.t() * F * K; /according to HZ (9.12)本征矩陣，是一個3×3大小的矩陣，使用xEx=0在圖像中的一點和另外一個圖像中的一點之間施加了一個約束，這里x是圖像一中的的一點，x是圖像二中與之相對應(yīng)的一點。這非常

9、有用，因為我們將要看到。我們使用的另一個重要的事實是本征矩陣是我們用來為我們的圖像恢復(fù)兩個相機的所有需要，盡管只有尺度，但是我們以后會得到。因此，如果我們獲得了本征矩陣，我們知道每一個相機在空間中的位置，并且知道它們的觀察方向，如果我們有足夠這樣的約束等式，那么我們可以簡單地計算出這個矩陣。簡單的因為每一個等式可以用來解決矩陣的一小部分。事實上，OpenCV允許我們僅使用7個點對來計算它，但是我們希望獲得更多的點對來得到一個魯棒性的解。使用豐富的特征描述子進(jìn)行點匹配現(xiàn)在我們將使用我們的約束等式來計算本征矩陣。為了獲得我們的約束，記住對于圖像A中的每一個點我們必須在圖像B中找到一個與之相對應(yīng)的點

10、。我們怎樣完成這個匹配呢？簡單地通過使用OpenCV的廣泛的特征匹配框架，這個框架在過去的幾年了已經(jīng)非常成熟。在計算機視覺中，特征提取和描述子匹配是一個基礎(chǔ)的過程，并且用在許多方法中來執(zhí)行各種各樣的操作。例如，檢測圖像中一個目標(biāo)的位置和方向，或者通過給出一個查詢圖像在大數(shù)據(jù)圖像中找到相似的圖像。從本質(zhì)上講，提取意味著在圖像中選擇點，使得獲得好的特征，并且為它們計算一個描述子。一個描述子是含有多個數(shù)據(jù)的向量，用來描述在一個圖像中圍繞著特征點的周圍環(huán)境。不同的方法有不同的長度和數(shù)據(jù)類型來表示描述子矢量。匹配是使用它的描述子從另外一個圖像中找到一組與之對應(yīng)的特征。OpenCV提供了非常簡單和有效的方

11、法支持特征提取和匹配。關(guān)于特征匹配的更多信息可以在Chapter 3少量（無）標(biāo)記增強現(xiàn)實中找到。讓我們檢查一個非常簡單特征提取和匹配方案：/ detectingkeypointsSurfFeatureDetectordetector();vector<KeyPoint> keypoints1, keypoints2;detector.detect(img1, keypoints1);detector.detect(img2, keypoints2);/ computing descriptorsSurfDescriptorExtractor extractor;Mat descr

12、iptors1, descriptors2;pute(img1, keypoints1, descriptors1);pute(img2, keypoints2, descriptors2);/ matching descriptorsBruteForceMatcher<L2<float>> matcher;vector<DMatch> matches;matcher.match(descriptors1, descriptors2, matches);你可能已經(jīng)看到類似的OpenCV代碼，但是讓我們快速的回顧一下它。我們的目的是獲得三個元素：兩個圖像的特征

13、點，特征點的描述子，和兩個特征集的匹配。OpenCV提供了一組特征檢測器、描述子提取器和匹配器。在這個簡單例子中，我們使用SurfFeatureDetector函數(shù)來獲得SURF（Speeded-Up-Robust Features）特征的2D位置，并且使用SurfDescriptorExtractor函數(shù)來獲得SURF描述子。我們使用一個brute-force匹配器來獲得匹配，這是一個最直接方式來匹配兩個特征集，該方法是通過比較第一個集中的每一個特征和第一個集的每一個特征并且獲得最好的匹配來實現(xiàn)的。在下一個圖像中，我們將看到兩個圖像上的特征點的匹配：（這兩個圖像來至于the Fountain

14、-P11 sequence，可以在網(wǎng)址中找到：http:/cvlab.epfl.ch/strecha/multiview/denseMVS.html.）實際上，像我們執(zhí)行的原始匹配（raw matching），只有到達(dá)某一程度時執(zhí)行效果才比較好并且許多匹配可能是錯誤的。因此，大多數(shù)SfM方法對原始匹配進(jìn)行一些濾波方式來確保正確和減少錯誤。一種濾波方式叫做交叉檢驗濾波，它內(nèi)置于OpenCV的brute-force匹配器中。也就是說，如果第一個圖像的一個特征匹配第二個圖像的一個特征，并且通過反向檢查，第二個圖像的特征也和第一個圖像的特征匹配，那么這個匹配被認(rèn)為是正確的。另一個常見的濾波機制（該機制

15、用在提供的代碼中）是基于同一場景的兩個圖像并且在他們之間存在某一種立體視覺關(guān)系，這樣的一個事實基礎(chǔ)之上來濾波的。在實踐中，這個濾波器嘗試采用魯棒性的算法來計算這個基礎(chǔ)矩陣，我們將會在尋找相機矩陣（Finding camera matrices）部分學(xué)習(xí)這種計算方法，并且保留由該計算得到的帶有小誤差的特征對。使用光流進(jìn)行點匹配一個替代使用豐富特征（如SURF）匹配的方法，是使用optical flow（OF光流）進(jìn)行匹配。下面的信息框提供了光流的一個簡短的概述。最近的OpenCV為從兩個圖像獲得流場擴展了API，并且現(xiàn)在更快，更強大。我們將嘗試使用它作為匹配特征的一個替代品。【注釋：光流是匹配來

16、至一幅圖像選擇的點到另外一幅圖像選著點的過程，假定這兩個圖像是一個視頻序列的一部分并且它們彼此非常相近。大多數(shù)的光流方法比較一個小的區(qū)域，稱為搜索窗口或者塊，這些塊圍繞著圖像A中的每一點和同樣區(qū)域的圖像B中的每一點。遵循計算機視覺中一個非常普通的規(guī)則，稱為亮度恒定約束（brightness constancy constraint）（和其他名字），圖像中的這些小塊從一個圖像到另外一個圖像不會有太大的變化，因此，他們的幅值差接近于0。除了匹配塊，更新的光流方法使用一些額外的方法來獲得更好的結(jié)果。其中一個方法就是使用圖像金字塔，它是圖像越來越小的尺寸（大?。┌姹?，這考慮到了工作的從粗糙到精致計算機

17、視覺中一個非常有用的技巧。另外一個方法是定義一個流場上的全局約束，假定這些點相互靠近，向同一方向一起運動。在OpenCV中，一個更加深入的光流方法可以在Chapter Developing Fluid Wall Using the Microsoft Kinect中找到，這個書可以在出版社網(wǎng)站上訪問到?！吭贠penCV中，使用光流相當(dāng)?shù)暮唵?，可以通過調(diào)用calcOpticalFlowPyrLK函數(shù)來實現(xiàn)。我們想要保存光流的結(jié)果匹配，像使用豐富的特征那樣，在未來，我們希望兩種方法可以互換。為了這個目標(biāo)，我們必須安裝一個特殊的匹配方法可以與基于特征的方法互換，這將在下面的代碼中看到：Vector&

18、lt;KeyPoint>left_keypoints,right_keypoints;/ Detect keypoints in the left and right imagesFastFeatureDetectorffd;ffd.detect(img1, left_keypoints);ffd.detect(img2, right_keypoints);vector<Point2f>left_points;KeyPointsToPoints(left_keypoints,left_points);vector<Point2f>right_points(left

19、_points.size();/ making sure images are grayscaleMat prevgray,gray;if (img1.channels() = 3) cvtColor(img1,prevgray,CV_RGB2GRAY);cvtColor(img2,gray,CV_RGB2GRAY); else prevgray = img1;gray = img2;/ Calculate the optical flow field:/ how each left_point moved across the 2 imagesvector<uchar>vstat

20、us; vector<float>verror;calcOpticalFlowPyrLK(prevgray, gray, left_points, right_points, vstatus, verror);/ First, filter out the points with high errorvector<Point2f>right_points_to_find;vector<int>right_points_to_find_back_index;for (unsigned inti=0; i<vstatus.size(); i+) if (v

21、statusi &&verrori < 12.0) / Keep the original index of the point in the/ optical flow array, for future useright_points_to_find_back_index.push_back(i);/ Keep the feature point itselfright_points_to_find.push_back(j_ptsi); else vstatusi = 0; / a bad flow/ for each right_point see which de

22、tected feature it belongs toMat right_points_to_find_flat = Mat(right_points_to_find).reshape(1,to_find.size(); /flatten arrayvector<Point2f>right_features; / detected featuresKeyPointsToPoints(right_keypoints,right_features);Mat right_features_flat = Mat(right_features).reshape(1,right_featur

23、es.size();/ Look around each OF point in the right image/ for any features that were detected in its area/ and make a match.BFMatchermatcher(CV_L2);vector<vector<DMatch>>nearest_neighbors;matcher.radiusMatch(right_points_to_find_flat,right_features_flat,nearest_neighbors,2.0f);/ Check th

24、at the found neighbors are unique (throw away neighbors/ that are too close together, as they may be confusing)std:set<int>found_in_right_points; / for duplicate preventionfor(inti=0;i<nearest_neighbors.size();i+) DMatch _m;if(nearest_neighborsi.size()=1) _m = nearest_neighborsi0; / only on

25、e neighbor else if(nearest_neighborsi.size()>1) / 2 neighbors check how close they aredouble ratio = nearest_neighborsi0.distance / nearest_neighborsi1.distance;if(ratio < 0.7) / not too close/ take the closest (first) one_m = nearest_neighborsi0; else / too close we cannot tell which is bette

26、rcontinue; / did not pass ratio test throw away else continue; / no neighbors. :(/ prevent duplicatesif (found_in_right_points.find(_m.trainIdx) = found_in_right_points.end() / The found neighbor was not yet used:/ We should match it with the original indexing / ofthe left point_m.queryIdx = right_p

27、oints_to_find_back_index_m.queryIdx; matches->push_back(_m); / add this matchfound_in_right_points.insert(_m.trainIdx);cout<<"pruned "<< matches->size() <<" / "<<nearest_neighbors.size() <<" matches"<<endl;函數(shù)KeyPointsToPoints和Po

28、intsToKeyPoints是用來進(jìn)行cv:Point2f和cv:KeyPoint結(jié)構(gòu)體之間相互轉(zhuǎn)換的簡單方便的函數(shù)。從先前的代碼片段我們可以看到一些有趣的事情。第一個注意的事情是，當(dāng)我們使用光流時，我們的結(jié)果表明，一個特征從左手邊圖像的一個位置移動到右手邊圖像的另外一個位置。但是我們有一組在右手邊圖像中檢測到的新的特征，在光流中從這個圖像到左手邊圖像的特征不一定是對齊的。我們必須使它們對齊。為了找到這些丟失的特征，我們使用一個k鄰近（KNN）半徑搜索,這給出了我們兩個特征，即感興趣的點落入了2個像素半徑范圍內(nèi)。我們可以看得到另外一個事情是用來測試KNN的比例的實現(xiàn)，在SfM中這是一種常見的

29、減少錯誤的方法。實質(zhì)上，當(dāng)我們對左手邊圖像上的一個特征和右手邊圖像上的一個特征進(jìn)行匹配時，它作為一個濾波器，用來移除混淆的匹配。如果右手邊圖像中兩個特征太靠近，或者它們之間這個比例（the rate）太大（接近于1.0），我們認(rèn)為它們混淆了并且不使用它們。我們也安裝一個雙重防御濾波器來進(jìn)一步修剪匹配。下面的圖像顯示了從一幅圖像到另外一幅圖像的流場。左手邊圖像中的分紅色箭頭表示了塊從左手邊圖像到右手邊圖像的運動。在左邊的第二個圖像中，我們看到一個放大了的小的流場區(qū)域。粉紅色箭頭再一次表明了塊的運動，并且我們可以通過右手邊的兩個原圖像的片段看到它的意義。在左手邊圖像上可視的特征正在向左移動穿過圖像

30、，粉紅色箭頭的方向在下面的圖像中展示：使用光流法代替豐富特征的優(yōu)點是這個過程通常更快并且可以適應(yīng)更多的點，使重構(gòu)更加稠密。在許多光流方法中也有一個塊整體運動的統(tǒng)一模型，在這個模型中，豐富的特征匹配通常不考慮。使用光流要注意的是對于從同一個硬件獲取的連續(xù)圖像，它處理的很快，然而豐富的特征通常不可知。它們之間的差異源于這樣的一個事實：光流法通常使用非?；A(chǔ)的特征，像圍繞著一個關(guān)鍵點的圖像塊，然而，高階豐富特征（例如，SURF）考慮每一個特征點的較高層次的信息。使用光流或者豐富的特征是設(shè)計師根據(jù)應(yīng)用程序的輸入所做的決定。找到相機矩陣-Finding camera matrices 既然我們獲得了兩個

31、關(guān)鍵點之間的匹配，我們可以計算基礎(chǔ)矩陣并且從中獲得本征矩陣。然而，我們必須首先調(diào)整我們的匹配點到兩個數(shù)組，其中數(shù)組中的索引對應(yīng)于另外一個數(shù)組中同樣的索引。這需要通過findFundametalMat函數(shù)獲得。我們可能也需要轉(zhuǎn)換KeyPoint 結(jié)構(gòu)到Point2f結(jié)構(gòu)。我們必須特別注意DMatch的queryIdx和trainIdx成員變量，在OpenCV中DMatch是容納兩個關(guān)鍵點匹配的結(jié)構(gòu)，因為它們必須匹配我們使用matche.match()函數(shù)的方式。下面的代碼部分展示了如何調(diào)整一個匹配到兩個相應(yīng)的二維點集，以及如何使用它們來找到基礎(chǔ)矩陣：vector<Point2f>im

32、gpts1,imgpts2;for( unsigned inti = 0; i<matches.size(); i+ )/ queryIdx is the "left" imageimgpts1.push_back(keypoints1matchesi.queryIdx.pt);/ trainIdx is the "right" imageimgpts2.push_back(keypoints2matchesi.trainIdx.pt);Mat F = findFundamentalMat(imgpts1, imgpts2, FM_RANSAC,

33、0.1, 0.99, status);Mat_<double> E = K.t() * F * K; /according to HZ (9.12)稍后我們可能使用status二值向量來修剪這些點，使這些點和恢復(fù)的基礎(chǔ)矩陣匹配。看下面的圖像：用來闡述使用基礎(chǔ)矩陣修剪后的點匹配。紅色箭頭表示特征匹配在尋找基礎(chǔ)矩陣的過程中被移除了，綠色箭頭表示被保留的特征匹配?，F(xiàn)在我們已經(jīng)準(zhǔn)備好尋找相機矩陣。這個過程在H和Z書的第九章進(jìn)行了詳細(xì)的描述。然而，我們將使用一個直接和簡單的方法來實現(xiàn)它，并且OpenCV很容易的為我們做這件事。但是首先，我們將簡短地檢查我們將要使用的相機矩陣的結(jié)構(gòu)：我們的相機

34、使用該模型，它由兩個成分組成，旋轉(zhuǎn)（表示為R）和平移（表示為t）。關(guān)于它的一個有趣的事情是它容納一個非?；镜牡仁剑簒=PX，這里x是圖像上的二維點，X是空間上的三維點。還有更多，但是這個矩陣給我們一個非常重要的關(guān)系，即圖像中點和場景中點之間的關(guān)系。因此，既然我們有了尋找攝像機矩陣的動機，那么我們將看到這個過程是怎么完成的。下面的的代碼部分展示了如何將本征矩陣分解為旋轉(zhuǎn)和平移成分。SVD svd(E);Matx33d W(0,-1,0,/HZ 9.131,0,0,0,0,1);Mat_<double> R = svd.u * Mat(W) * svd.vt; /HZ 9.19Mat

35、_<double> t = svd.u.col(2); /u3Matx34d P1( R(0,0),R(0,1), R(0,2), t(0),R(1,0),R(1,1), R(1,2), t(1),R(2,0),R(2,1), R(2,2), t(2);非常簡單。我們需要做的工作僅是對我們先前獲得的本征矩陣進(jìn)行奇異值分解(SVD)，并且乘以一個特殊的矩陣W。對于我們所做的數(shù)學(xué)上的操作不進(jìn)行過深地闡述，我們可以說SVD操作將我們的矩陣E分解成為兩部分，一個旋轉(zhuǎn)成分和一個平移成分。實時上，本征矩陣起初是通過這兩個成分相乘構(gòu)成的。完全地是滿足我們的好奇心，我們可以看下面的本征矩陣的等式，

36、它在字面意義上表現(xiàn)為：E=txR（x是t的下標(biāo)）。我可以看到它是由一個平移成分和一個旋轉(zhuǎn)成分組成。我們注意到，我們所做的僅是得到了一個相機矩陣，因此另外一個相機矩陣呢？好的，我們在假定一個相機矩陣是固定的并且是標(biāo)準(zhǔn)的（沒有旋轉(zhuǎn)和平移）情況下進(jìn)行這個操作。下一個矩陣（這里的下一個矩陣表示相對于上面的P）也是標(biāo)準(zhǔn)的：我們從本征矩陣恢復(fù)的是另外一個相機相對于固定的相機進(jìn)行了移動和旋轉(zhuǎn)。這同樣意味著我們從這兩個相機矩陣中恢復(fù)的任何三維點都是擁有第一個相機在世界坐標(biāo)系中的原點（0，0，0）。然而，這不是一個完全解。H和Z在他們的書中展示了如何和為什么這樣的分解有四個可能的相機矩陣，但是僅有一個是正確的。

37、正確的相機矩陣將產(chǎn)生帶有一個正Z值（點在攝像機的前面）的重構(gòu)點。但是我們僅有當(dāng)學(xué)了下一步將要討論的三角測量和3維重建之后才能理解。還有一個我們可以考慮添加到我們的方法中的事情是錯誤檢測。多次從點的匹配中計算基礎(chǔ)矩陣是錯誤的，并影響相機矩陣。帶有錯誤相機矩陣的進(jìn)行三角測量是毫無意義的。我們可以安裝一個檢查來檢測是否旋轉(zhuǎn)成分是一個有效的旋轉(zhuǎn)矩陣。牢記旋轉(zhuǎn)矩陣必須是一個行列式值為1（或者-1），我們可以簡單地進(jìn)行如下做法：bool CheckCoherentRotation(cv:Mat_<double>& R) if(fabsf(determinant(R)-1.0 >

38、1e-07) cerr<<"det(R) != +-1.0, this is not a rotation matrix"<<endl;return false; return true;現(xiàn)在我們可以看到所有元素是如何組合到用來恢復(fù)P矩陣的函數(shù)中，如下：void FindCameraMatrices(const Mat& K,const Mat& Kinv,const vector<KeyPoint>& imgpts1,const vector<KeyPoint>& imgpts2,Matx34

39、d& P,Matx34d& P1,vector<DMatch>& matches,vector<CloudPoint>& outCloud)/Find camera matrices/Get Fundamental MatrixMat F = GetFundamentalMat(imgpts1,imgpts2,matches);/Essential matrix: compute then extract cameras R|tMat_<double> E = K.t() * F * K; /according to HZ (

40、9.12)/decompose E to P' , HZ (9.19)SVD svd(E,SVD:MODIFY_A);Mat svd_u = svd.u;Mat svd_vt = svd.vt;Mat svd_w = svd.w;Matx33d W(0,-1,0,/HZ 9.131,0,0,0,0,1);Mat_<double> R = svd_u * Mat(W) * svd_vt; /HZ 9.19Mat_<double> t = svd_u.col(2); /u3if (!CheckCoherentRotation(R) cout<<"

41、;resulting rotation is not coherentn"P1 = 0;return;P1 = Matx34d(R(0,0),R(0,1),R(0,2),t(0),R(1,0),R(1,1),R(1,2),t(1),R(2,0),R(2,1),R(2,2),t(2);此時，我們擁有兩個我們需要用來重建場景的相機。第一個相機是標(biāo)準(zhǔn)的，存儲在P變量中，第二個相機是我們計算得到的，構(gòu)成基礎(chǔ)矩陣，存儲在P1變量中。下一部分我們將揭示如何使用這些相機來獲得場景的三維重建。重構(gòu)場景接下來我們看一下從我們已經(jīng)獲得的信息中恢復(fù)場景的3D結(jié)構(gòu)的事情。像先前所做的，我們應(yīng)當(dāng)看一下，用來完

42、成這個事情我們手邊所擁有的工具和信息。在前一部分，我們從本征矩陣和矩陣矩陣中獲得了兩個相機矩陣。我們已經(jīng)討論了這些工具用來獲得空間中一點的3D位置是如何的有用。那么，我可以返回我們的匹配點來用數(shù)據(jù)填充等式。點對同樣對于計算我們獲得的近似計算的誤差有用?，F(xiàn)在我們看一些如何使用OpenCV執(zhí)行三角測量。這次我們將會按照Tartley和Strum的三角測量的文章的步驟，文章中他們實現(xiàn)和比較了一些三角剖分的方法。我們將實現(xiàn)他們的線性方法的一種，因為使用OpenCv非常容易編程?；貞浺幌?，我們有兩個由2D點匹配和P矩陣產(chǎn)生的關(guān)鍵等式：x=PX和x=PX，x和x是匹配的二維點，X是兩個相機進(jìn)行拍照的真實世

43、界三維點。如果我們重寫這個等式，我們可以公式化為一個線性方程系統(tǒng)，該系統(tǒng)可以解出X的值，X正是我們所期望尋找的值。假定X=(x,y,z,1)t（一個合理的點的假設(shè)，這些點離相機的中心不太近或者不太遠(yuǎn)）產(chǎn)生一個形式為AX=B的非齊次線性方程系統(tǒng)。我們可以編碼和解決這個問題，如下：Mat_<double> LinearLSTriangulation(Point3d u,/homogenous image point (u,v,1)Matx34d P,/camera 1 matrixPoint3d u1,/homogenous image point in 2nd cameraMatx3

44、4d P1/camera 2 matrix)/build A matrixMatx43d A(u.x*P(2,0)-P(0,0),u.x*P(2,1)-P(0,1),u.x*P(2,2)-P(0,2),u.y*P(2,0)-P(1,0),u.y*P(2,1)-P(1,1),u.y*P(2,2)-P(1,2),u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1),u1.x*P1(2,2)-P1(0,2),u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1),u1.y*P1(2,2)-P1(1,2);/build B vectorMa

45、tx41d B(-(u.x*P(2,3)-P(0,3),-(u.y*P(2,3)-P(1,3),-(u1.x*P1(2,3)-P1(0,3),-(u1.y*P1(2,3)-P1(1,3);/solve for XMat_<double> X;solve(A,B,X,DECOMP_SVD);return X;我們將獲得由兩個2維點產(chǎn)生的3D點的一個近似。還有一個要注意的事情是，2D點用齊次坐標(biāo)表示，意味著x和y的值后面追加一個1。我們必須確保這些點在標(biāo)準(zhǔn)化的坐標(biāo)系中，這意味著它們必須乘以先前的標(biāo)定矩陣K.我們可能注意到我們簡單地利用KP矩陣來替代k矩陣和每一個點相乘（每一點乘以KP)

46、,就是H和Z遍及第9章的做法那樣。我們現(xiàn)在可以寫一個關(guān)于點匹配的循環(huán)語句來獲得一個完整的三角測量，如下：double TriangulatePoints(const vector<KeyPoint>& pt_set1,const vector<KeyPoint>& pt_set2,const Mat&Kinv,const Matx34d& P,const Matx34d& P1,vector<Point3d>& pointcloud)vector<double> reproj_error;for (

47、unsigned int i=0; i<pts_size; i+) /convert to normalized homogeneous coordinatesPoint2f kp = pt_set1i.pt;Point3d u(kp.x,kp.y,1.0);Mat_<double> um = Kinv * Mat_<double>(u);u = um.at<Point3d>(0);Point2f kp1 = pt_set2i.pt;Point3d u1(kp1.x,kp1.y,1.0);Mat_<double> um1 = Kinv *

48、Mat_<double>(u1);u1 = um1.at<Point3d>(0);/triangulateMat_<double> X = LinearLSTriangulation(u,P,u1,P1);/calculate reprojection errorMat_<double> xPt_img = K * Mat(P1) * X;Point2f xPt_img_(xPt_img(0)/xPt_img(2),xPt_img(1)/xPt_img(2);reproj_error.push_back(norm(xPt_img_-kp1);/s

49、tore 3D pointpointcloud.push_back(Point3d(X(0),X(1),X(2);/return mean reprojection errorScalar me = mean(reproj_error);return me0;在下面的圖像中我們將看到一個兩個圖像三角測量的結(jié)果。這兩個圖像來至于P-11序列：http:/cvlab.epfl.ch/strecha/multiview/denseMVS.html.上面的兩個圖像是原始場景的兩個視圖，下面的一對圖像是從兩個視圖重構(gòu)得到的點云視圖，包含著估計相機朝向噴泉。我們可以看到右手邊紅色磚塊墻部分是如何重構(gòu)的，并

50、且也可以看到突出于墻的噴泉。然而，像我們前面提到的那樣，關(guān)于重構(gòu)存在著這樣的一個問題：重構(gòu)僅能達(dá)到尺度上的。我們應(yīng)當(dāng)花一些時間來理解達(dá)到尺度（up-to-scale）的意思。我們獲得的兩個攝像機之間的運動存在一個隨意測量的單元，也就是說，它不是用厘米或者英寸，而是簡單地給出尺度單位。我們重構(gòu)的相機將是尺度單元距離上的分開。這在很大程度上暗示我們應(yīng)當(dāng)決定過會重新獲得更多的相機，因為每一對相機都擁有各自的尺度單元，而不是一個一般的尺度。現(xiàn)在我們將討論我們建立的誤差測量是如何可能的幫助我們來找到一個更加魯棒性的重構(gòu)。首先，我們需要注意重投影意味著我們簡單地利用三角化的3D點并且將這些點重塑到相機上以

51、獲得一個重投影的2D點。如果這個距離很大，這意味著我們在三角測量中存在一個誤差，因此，在最后的結(jié)果中，我們可能不想包含這個點。我們的全局測量是平均重投影距離并且可能提供一個暗示我們的三角剖分總體執(zhí)行的怎么樣。高的重投影率可能表明P矩陣存在問題，因此該問題可能是存在于本征矩陣的計算或者匹配特征點中。我們應(yīng)當(dāng)簡短地回顧一下在前一部分我們討論的相機矩陣。我們提到相機矩陣P1的分解可以通過四個不同的方式進(jìn)行分解，但是僅有一個分解是正確的。既然，我們知道如何三角化一個點，我們可以增加一個檢測來看四個相機矩陣中哪一個是有效地的。我們應(yīng)當(dāng)跳過在一點實現(xiàn)上的細(xì)節(jié)，因為它們是本書隨書實例代碼中的精選（專題）。下

52、一步，我們將要看一下重新獲得直視場景的更多的相機，并且組合這些3D重建的結(jié)果。從多視圖中重建既然我們知道了如何從兩個相機中恢復(fù)運動和場景幾何，簡單地應(yīng)用相同的過程獲得額外相機的參數(shù)和更多場景點似乎不重要。事實上，這件事并不簡單因為我們僅可以獲得一個達(dá)到尺度的重構(gòu)，并且每對圖像給我們一個不同的尺度。有一些方法可以正確的從多個視場中重構(gòu)3D場景數(shù)據(jù)。一個方法是后方交會（resection）或者相機姿態(tài)估計（camera pose estimation），也被稱為PNP（Perspective-N-Point），這里我們將使用我們已經(jīng)找到的場景點來解決一個新相機的位置。另外一個方法是三角化更多的點并

53、且看它們是如何適應(yīng)于我們存在的場景幾何的。憑借ICP（Iterative Closest Point ）我們可以獲得新相機的位置。在這一章，我們將討論使用OpenCV的sovlePnP函數(shù)來完成第一個方法。第一步我們選擇這樣的重構(gòu)類型即使用相機后方交會的增加的3D重構(gòu)，來獲得一個基線場景結(jié)構(gòu)。因為我們將基于一個已知的場景結(jié)構(gòu)來尋找任何相機的位置，我們需要找到要處理的一個初始化的結(jié)構(gòu)和一條基線。我們可以使用先前討論的方法例如，在第一個視頻幀和第二個視頻幀之間，通過尋找相機矩陣（使用FindCameraMatrices函數(shù)）來獲得一條基線并且三角化幾何（使用TriangulatePoints函數(shù)）

54、。發(fā)現(xiàn)一個基礎(chǔ)結(jié)構(gòu)后，我們可以繼續(xù)。然而，我們的方法需要相當(dāng)多的數(shù)據(jù)記錄。首先，我們需要注意solvePnP函數(shù)需要兩個對齊的3D和2D點的矢量。對齊的矢量意味著一個矢量的第i個位置與另外一個矢量的第i位置對齊。為了獲得這些矢量，我們需要在我們早前恢復(fù)的3D點中找到這些點，這些點與在我們新視頻幀下的2D點是對齊的。完成這個的一個簡單的方式是，對于云中的每一個3D點，附加一個來至2D點的矢量。然后我們可以使用特征匹配來獲得一個匹配對。讓我們?yōu)橐粋€3D點引入一個新的結(jié)構(gòu)，如下：struct CloudPoint cv:Point3d pt;std:vector<int>index_of

55、_2d_origin;它容納，3D點和一個容器，容器內(nèi)的元素為每幀圖像上2D點的索引值，這些2D點用來計算3D點。當(dāng)三角化一個新的3D點時，index_of_2d_origin的信息必須被初始化，來記錄在三角化中哪些相機涉及到。然而，我們可以使用它來從我們的3D點云追溯到每一幀上的2D點，如下：std:vector<CloudPoint> pcloud; /our global 3D point cloud/check for matches between i'th frame and 0'th frame (and thus the current cloud)

56、std:vector<cv:Point3f> ppcloud;std:vector<cv:Point2f> imgPoints;vector<int> pcloud_status(pcloud.size(),0);/scan the views we already used (good_views)for (set<int>:iterator done_view = good_views.begin(); done_view != good_views.end(); +done_view) int old_view = *done_view;

57、/a view we already used for reconstrcution/check for matches_from_old_to_working between <working_view>'th frame and <old_view>'th frame (and thus the current cloud)std:vector<cv:DMatch> matches_from_old_to_working = matches_matrixstd:make_pair(old_view,working_view);/scan

58、the 2D-2D matched-pointsfor (unsigned int match_from_old_view=0; match_from_old_view<matches_from_old_to_working.size(); match_from_old_view+) / the index of the matching 2D point in <old_view>int idx_in_old_view = matches_from_old_to_workingmatch_from_old_view.queryIdx;/scan the existing cloud to see if this point from <old_view> exists for (unsigned int pcldp=0; pcldp<pcloud.size(); pcldp+) /