本文將介紹如何根據(jù)相機在不同位置拍攝的兩張圖片恢復(fù)出相機的運動。在多視圖幾何學(xué)中，這被稱為對極幾何。
一、對極幾何
如下圖所示，相機在兩個不同的位置（藍色位置和綠色位置）同時觀測到了同一個點X。根據(jù)上一篇文章《OpenCV提取ORB特征并匹配》，我們可以將X在兩個相機平面上的投影x點和x'點的像素坐標(biāo)匹配起來。此時，x和x'便滿足一個約束，稱為對極約束。
對極約束的數(shù)學(xué)推導(dǎo)并不復(fù)雜，本文為求簡明和實用，對此不做細究。更多內(nèi)容可以參考文末列出的參考資料。這里只給出直觀的解釋。
相機在兩個位置的光心分別為O點和O'點，連線OO'與兩個成像平面的交點分別為e點和e'點。我們把線段xe和x'e'稱為極線。下面我們來分析為什么利用兩張圖片中特征點對的像素坐標(biāo)可以恢復(fù)相機運動和地圖點X的深度（X點的深度估計將在下篇文章中介紹）。
假設(shè)左側(cè)相機固定，右側(cè)相機位姿待估計。那么OX射線方向確定，同時，OX在右側(cè)相機平面上的投影也隨之確定，即直線l'。這一投影關(guān)系便稱為對極約束。不過，單個對極約束并不能完全確定右側(cè)相機的位置，大家可以想象右側(cè)相機平面如何在保持x'坐標(biāo)不變的情況下調(diào)整自己的姿態(tài)。換句話說，對極約束是有多個自由度的，一對匹配點并不能唯一確定兩個相機的位姿。那到底需要多少對點呢？數(shù)學(xué)上可以證明，至少需要5對。而實際應(yīng)用中通常使用8對以簡化計算過程，稱為“八點法”。下面就進入實際代碼環(huán)節(jié)，不在理論上過多糾纏了。
二、2D-2D相機位姿估計
從兩張2D圖像估計相機位姿的流程如下圖所示。
其中有一處分叉點，分別適用于不同的情況。左側(cè)“計算基礎(chǔ)矩陣或本質(zhì)矩陣”適用于特征點不共面的情況；右側(cè)“計算單應(yīng)矩陣”適用于特征點共面的情況（比如墻壁、地面、航拍等場合）。
下面只給出2D-2D相機位姿估計函數(shù)的代碼，特征點匹配部分的代碼在上一篇文章中可以找到。
完整代碼的下載地址：https://github.com/jingedawang/FeatureMethod
void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector< DMatch > matches,
Mat& R, Mat& t )
{
// 相機內(nèi)參,TUM Freiburg2
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
//-- 把匹配點轉(zhuǎn)換為vector<Point2f>的形式
vector<Point2f> points1;
vector<Point2f> points2;
for ( int i = 0; i < ( int ) matches.size(); i++ )
{
points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
}
//-- 計算基礎(chǔ)矩陣
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;
//-- 計算本質(zhì)矩陣
Point2d principal_point ( 325.1, 249.7 ); //相機光心, TUM dataset標(biāo)定值
double focal_length = 521; //相機焦距, TUM dataset標(biāo)定值
Mat essential_matrix;
essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;
//-- 計算單應(yīng)矩陣
Mat homography_matrix;
homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;
//-- 從本質(zhì)矩陣中恢復(fù)旋轉(zhuǎn)和平移信息.
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
cout<<"R is "<<endl<<R<<endl;
cout<<"t is "<<endl<<t<<endl;
}
這里需要解釋一下，基礎(chǔ)矩陣和本質(zhì)矩陣都是3×3的矩陣，它們之間不過是差了個相機內(nèi)參，因此使用時效果完全一樣。上邊的代碼使用了本質(zhì)矩陣來恢復(fù)相機運動，而沒有用單應(yīng)矩陣，這是因為示例圖片中的特征點并不共面。在實際應(yīng)用中，如果事先無法知道特征點是否共面，則應(yīng)當(dāng)同時計算本質(zhì)矩陣和單應(yīng)矩陣，選擇重投影誤差比較小的那個作為最終的運動估計矩陣，具體操作敬請期待后續(xù)系列文章。
三、進一步討論
2D-2D相機位姿估計是單目SLAM初始化時的關(guān)鍵技術(shù)。初始化成功之后，后續(xù)的視頻幀就可以采用3D-2D匹配來簡化計算過程。因此初始化成功與否對SLAM至關(guān)重要。
從單目SLAM的角度考慮，2D-2D相機位姿估計存在以下三個敏感的問題：
尺度不確定性
用上面的方法估計出的相機平移向量t的值并沒有單位，也就是說相機移動的距離只有相對值，沒有絕對值。這是單目相機固有的尺度不確定性問題，無法從根本上解決。因此單目SLAM中一般把初始化后的t歸一化，即把初始化時移動的距離默認為1，此后的距離都以這個1為單位。初始化的純旋轉(zhuǎn)問題
單目初始化不能只有旋轉(zhuǎn)，必須要有一定程度的平移，否則由于t趨近于0，導(dǎo)致無從求解R或者誤差非常大。多于8對點的情況
如果匹配的點對數(shù)多于8（大多數(shù)情況都是這樣），可以考慮充分利用這些點，而不是只從中選擇8對用于計算。推薦的算法是隨機采樣一致性（Random Sample Consensus，RANSAC），該算法可以有效地避免錯誤數(shù)據(jù)對整體結(jié)果的影響。在代碼中，只需要將findFundamentalMat函數(shù)的第三個參數(shù)從CV_FM_8POINT換成CV_FM_RANSAC就可以了。
四、參考資料
《視覺SLAM十四講》第7講 視覺里程計1 高翔
本質(zhì)矩陣和基礎(chǔ)矩陣的區(qū)別是什么？ 知乎
