最簡單的OPENCV實現求解單目相機位姿

單目相機透過對極約束來求解相機運動的位姿。參考了ORBSLAM中單目實現的程式碼，這裡用opencv來實現最簡單的位姿估計。

對極約束的概念可以參考我的這篇

簡單的說就是對於單目相機，在不同的位置下我們看到同一個物體P，在兩個影象中分別是歸一化畫素P1（U1，V1）和P2（U2，V2），我們可以透過

來計算出E，E是本質矩陣。

， 這裡的tx就是Transition的反對稱矩陣。同時也就算出了T和R， 用的是8點法。

這裡我透過opencv來計算E：

mLeftImg

=

cv

：：

imread

（

lImg

，

cv

：：

IMREAD_GRAYSCALE

）；

mRightImg

=

cv

：：

imread

（

rImg

，

cv

：：

IMREAD_GRAYSCALE

）；

cv

：：

Ptr

<

ORB

>

OrbLeftExtractor

=

cv

：：

ORB

：：

create

（）；

cv

：：

Ptr

<

ORB

>

OrbRightExtractor

=

cv

：：

ORB

：：

create

（）；

OrbLeftExtractor

->

detectAndCompute

（

mLeftImg

，

noArray

（），

mLeftKps

，

mLeftDes

）；

OrbRightExtractor

->

detectAndCompute

（

mRightImg

，

noArray

（），

mRightKps

，

mRightDes

）；

Ptr

<

DescriptorMatcher

>

matcher

=

DescriptorMatcher

：：

create

（

DescriptorMatcher

：：

BRUTEFORCE_HAMMING

）；

matcher

->

match

（

mLeftDes

，

mRightDes

，

mMatches

）；

首先透過ORB特徵提取，獲取兩幅影象的匹配度，我將其連線出來的效果：

可以看到，裡面有很多的誤匹配，先使用RANSAC演算法，選出儘可能多的匹配準確的特徵點，這樣在之後的位姿計算時可以更精準。

RANSAC的演算法原理可以google，很容易理解。先看看ORBSLAM中的實現：

bool Initializer：：Initialize（const Frame &CurrentFrame， const vector &vMatches12， cv：：Mat &R21， cv：：Mat &t21，

vector &vP3D， vector &vbTriangulated）

{

// Fill structures with current keypoints and matches with reference frame

// Reference Frame： 1， Current Frame： 2

// Frame2 特徵點

mvKeys2 = CurrentFrame。mvKeysUn；

// mvMatches12記錄匹配上的特徵點對

mvMatches12。clear（）；

mvMatches12。reserve（mvKeys2。size（））；

// mvbMatched1記錄每個特徵點是否有匹配的特徵點，

// 這個變數後面沒有用到，後面只關心匹配上的特徵點

mvbMatched1。resize（mvKeys1。size（））；

// 步驟1：組織特徵點對

for（size_t i=0， iend=vMatches12。size（）；i

{

if（vMatches12［i］>=0）

{

mvMatches12。push_back（make_pair（i，vMatches12［i］））；

mvbMatched1［i］=true；

}

else

mvbMatched1［i］=false；

}

// 匹配上的特徵點的個數

const int N = mvMatches12。size（）；

// Indices for minimum set selection

// 新建一個容器vAllIndices，生成0到N-1的數作為特徵點的索引

vector vAllIndices；

vAllIndices。reserve（N）；

vector vAvailableIndices；

for（int i=0； i

{

vAllIndices。push_back（i）；

}

// Generate sets of 8 points for each RANSAC iteration

// **步驟2：在所有匹配特徵點對中隨機選擇8對匹配特徵點為一組，共選擇mMaxIterations組 **

// 用於FindHomography和FindFundamental求解

// mMaxIterations：200

mvSets = vector< vector >（mMaxIterations，vector（8，0））；

DUtils：：Random：：SeedRandOnce（0）；

for（int it=0； it

{

vAvailableIndices = vAllIndices；

// Select a minimum set

for（size_t j=0； j<8； j++）

{

// 產生0到N-1的隨機數

int randi = DUtils：：Random：：RandomInt（0，vAvailableIndices。size（）-1）；

// idx表示哪一個索引對應的特徵點被選中

int idx = vAvailableIndices［randi］；

mvSets［it］［j］ = idx；

// randi對應的索引已經被選過了，從容器中刪除

// randi對應的索引用最後一個元素替換，並刪掉最後一個元素

vAvailableIndices［randi］ = vAvailableIndices。back（）；

vAvailableIndices。pop_back（）；

}

}

// Launch threads to compute in parallel a fundamental matrix and a homography

// 步驟3：呼叫多執行緒分別用於計算fundamental matrix和homography

vector vbMatchesInliersH， vbMatchesInliersF；

float SH， SF； // score for H and F

cv：：Mat H， F； // H and F

// ref是引用的功能：http：//en。cppreference。com/w/cpp/utility/functional/ref

// 計算homograpy並打分

thread threadH（&Initializer：：FindHomography，this，ref（vbMatchesInliersH）， ref（SH）， ref（H））；

// 計算fundamental matrix並打分

thread threadF（&Initializer：：FindFundamental，this，ref（vbMatchesInliersF）， ref（SF）， ref（F））；

// Wait until both threads have finished

threadH。join（）；

threadF。join（）；

// Compute ratio of scores

// 步驟4：計算得分比例，選取某個模型

float RH = SH/（SH+SF）；

// Try to reconstruct from homography or fundamental depending on the ratio （0。40-0。45）

// 步驟5：從H矩陣或F矩陣中恢復R，t

if（RH>0。40）

return ReconstructH（vbMatchesInliersH，H，mK，R21，t21，vP3D，vbTriangulated，1。0，50）；

else //if（pF_HF>0。6）

return ReconstructF（vbMatchesInliersF，F，mK，R21，t21，vP3D，vbTriangulated，1。0，50）；

return false；

}

orbslam首先是從配對特徵中隨機迭代mMaxIterations次，每一次都從配對點中選出8個點用來計算homography和fundamental矩陣，都是用SVD來計算的，如下：

FindFundamental：

void Initializer：：FindFundamental（vector &vbMatchesInliers， float &score， cv：：Mat &F21）

{

// Number of putative matches

const int N = vbMatchesInliers。size（）；

// 分別得到歸一化的座標P1和P2

vector vPn1， vPn2；

cv：：Mat T1， T2；

Normalize（mvKeys1，vPn1， T1）；

Normalize（mvKeys2，vPn2， T2）；

cv：：Mat T2t = T2。t（）；

// Best Results variables

score = 0。0；

vbMatchesInliers = vector（N，false）；

// Iteration variables

vector vPn1i（8）；

vector vPn2i（8）；

cv：：Mat F21i；

vector vbCurrentInliers（N，false）；

float currentScore；

// Perform all RANSAC iterations and save the solution with highest score

for（int it=0； it

{

// Select a minimum set

for（int j=0； j<8； j++）

{

int idx = mvSets［it］［j］；

vPn1i［j］ = vPn1［mvMatches12［idx］。first］；

vPn2i［j］ = vPn2［mvMatches12［idx］。second］；

}

cv：：Mat Fn = ComputeF21（vPn1i，vPn2i）；

F21i = T2t*Fn*T1； //解除歸一化

// 利用重投影誤差為當次RANSAC的結果評分

currentScore = CheckFundamental（F21i， vbCurrentInliers， mSigma）；

if（currentScore>score）

{

F21 = F21i。clone（）；

vbMatchesInliers = vbCurrentInliers；

score = currentScore；

}

}

}

透過ComputeF21計算本質矩陣，

cv：：Mat Initializer：：ComputeF21（const vector &vP1，const vector &vP2）

{

const int N = vP1。size（）；

cv：：Mat A（N，9，CV_32F）； // N*9

for（int i=0； i

{

const float u1 = vP1［i］。x；

const float v1 = vP1［i］。y；

const float u2 = vP2［i］。x；

const float v2 = vP2［i］。y；

A。at（i，0） = u2*u1；

A。at（i，1） = u2*v1；

A。at（i，2） = u2；

A。at（i，3） = v2*u1；

A。at（i，4） = v2*v1；

A。at（i，5） = v2；

A。at（i，6） = u1；

A。at（i，7） = v1；

A。at（i，8） = 1；

}

cv：：Mat u，w，vt；

cv：：SVDecomp（A，w，u，vt，cv：：SVD：：MODIFY_A | cv：：SVD：：FULL_UV）；

cv：：Mat Fpre = vt。row（8）。reshape（0， 3）； // v的最後一列

cv：：SVDecomp（Fpre，w，u，vt，cv：：SVD：：MODIFY_A | cv：：SVD：：FULL_UV）；

w。at（2）=0； // 秩2約束，將第3個奇異值設為0 //強迫約束

return u*cv：：Mat：：diag（w）*vt；

}

看到用的是線性SVD解。

透過重投影來評估本質矩陣的好壞。

float Initializer：：CheckFundamental（const cv：：Mat &F21， vector &vbMatchesInliers， float sigma）

{

const int N = mvMatches12。size（）；

const float f11 = F21。at（0，0）；

const float f12 = F21。at（0，1）；

const float f13 = F21。at（0，2）；

const float f21 = F21。at（1，0）；

const float f22 = F21。at（1，1）；

const float f23 = F21。at（1，2）；

const float f31 = F21。at（2，0）；

const float f32 = F21。at（2，1）；

const float f33 = F21。at（2，2）；

vbMatchesInliers。resize（N）；

float score = 0；

// 基於卡方檢驗計算出的閾值（假設測量有一個畫素的偏差）

const float th = 3。841； //置信度95%，自由度1

const float thScore = 5。991；//置信度95%，自由度2

const float invSigmaSquare = 1。0/（sigma*sigma）；

for（int i=0； i

{

bool bIn = true；

const cv：：KeyPoint &kp1 = mvKeys1［mvMatches12［i］。first］；

const cv：：KeyPoint &kp2 = mvKeys2［mvMatches12［i］。second］；

const float u1 = kp1。pt。x；

const float v1 = kp1。pt。y；

const float u2 = kp2。pt。x；

const float v2 = kp2。pt。y；

// Reprojection error in second image

// l2=F21x1=（a2，b2，c2）

// F21x1可以算出x1在影象中x2對應的線l

const float a2 = f11*u1+f12*v1+f13；

const float b2 = f21*u1+f22*v1+f23；

const float c2 = f31*u1+f32*v1+f33；

// x2應該在l這條線上：x2點乘l = 0

const float num2 = a2*u2+b2*v2+c2；

const float squareDist1 = num2*num2/（a2*a2+b2*b2）； // 點到線的幾何距離 的平方

const float chiSquare1 = squareDist1*invSigmaSquare；

if（chiSquare1>th）

bIn = false；

else

score += thScore - chiSquare1；

// Reprojection error in second image

// l1 =x2tF21=（a1，b1，c1）

const float a1 = f11*u2+f21*v2+f31；

const float b1 = f12*u2+f22*v2+f32；

const float c1 = f13*u2+f23*v2+f33；

const float num1 = a1*u1+b1*v1+c1；

const float squareDist2 = num1*num1/（a1*a1+b1*b1）；

const float chiSquare2 = squareDist2*invSigmaSquare；

if（chiSquare2>th）

bIn = false；

else

score += thScore - chiSquare2；

if（bIn）

vbMatchesInliers［i］=true；

else

vbMatchesInliers［i］=false；

}

return score；

}

最後回到Initializer：：Initialize，將單映矩陣和本質矩陣的得分進行比對，選出最合適的，就求出RT了。

ORBSLAM2同時考慮了單應和本質，SLAM14講中也說到，工程實踐中一般都講兩者都計算出來選擇較好的，不過效率上會影響比較多感覺。

opencv實現就比較簡單了，思路和上面的類似，只是現在只考慮本質矩陣。在之前獲取到特徵點之後，

/*add ransac method for accurate match*/

30 vector vLeftP2f；

31 vector vRightP2f；

32 for（auto& each：mMatches）

33 {

34 vLeftP2f。push_back（mLeftKps［each。queryIdx］。pt）；

35 vRightP2f。push_back（mRightKps［each。trainIdx］。pt）；

36 }

37 vector vTemp（vLeftP2f。size（））；

/*計算本質矩陣，用RANSAC*/

38 Mat transM = findEssentialMat（vLeftP2f， vRightP2f， cameraMatrix，RANSAC， 0。999， 1。0， vTemp）；

39 vector optimizeM；

40 for（int i = 0； i < vTemp。size（）； i++）

41 {

42 if（vTemp［i］）

43 {

44 optimizeM。push_back（mMatches［i］）；

45 }

46 }

47 mMatches。swap（optimizeM）；

48 cout << transM<

49 Mat optimizeP；

50 drawMatches（mLeftImg， mLeftKps， mRightImg， mRightKps， mMatches， optimizeP）；

51 imshow（“output5”， optimizeP）；

看下結果：

確實效果好多了，匹配準確度比之前的要好，之後我們就可以透過這個本質矩陣來計算RT了。

Mat R， t， mask；

recoverPose（transM， vLeftP2f， vRightP2f， cameraMatrix， R， t， mask）；

一個介面搞定。最後我們可以透過驗證對極約束，來看看求出的位姿是否準確。

定義檢查函式：

11

Mat

cameraMatrix

=

（

Mat_

<

double

>

（

3

，

3

）

<<

CAM_FX

，

0。0

，

CAM_CX

，

0。0

，

CAM_FY

，

CAM_CY

，

0。0

，

0。0

，

1。0

）；

12

13

bool

epipolarConstraintCheck

（

Mat

CameraK

，

vector

<

Point2f

>&

p1s

，

vector

<

Point2f

>&

p2s

，

Mat

R

，

Mat

t

）

14

{

15

for

（

int

i

=

0

；

i

<

p1s

。

size

（）；

i

++

）

16

{

17

Mat

y1

=

（

Mat_

<

double

>

（

3

，

1

）

<<

p1s

［

i

］。

x

，

p1s

［

i

］。

y

，

1

）；

18

Mat

y2

=

（

Mat_

<

double

>

（

3

，

1

）

<<

p2s

［

i

］。

x

，

p2s

［

i

］。

y

，

1

）；

19

//T 的凡對稱矩陣

20

Mat

t_x

=

（

Mat_

<

double

>

（

3

，

3

）

<<

0

，

-

t

。

at

<

double

>

（

2

，

0

），

t

。

at

<

double

>

（

1

，

0

），

21

t

。

at

<

double

>

（

2

，

0

），

0

，

-

t

。

at

<

double

>

（

0

，

0

），

22

-

t

。

at

<

double

>

（

1

，

0

），

t

。

at

<

double

>

（

0

，

0

），

0

）；

23

Mat

d

=

y2

。

t

（）

*

cameraMatrix

。

inv

（）。

t

（）

*

t_x

*

R

*

cameraMatrix

。

inv

（）

*

y1

；

24

cout

<<

“epipolar constraint = ”

<<

d

<<

endl

；

25

}

26

}

最後可以看到結果都是趨近於0的，證明位姿還是比較準確的。