個人博客：http://www.chenjianqu.com/
原文鏈接：http://www.chenjianqu.com/show-94.html
在多種應用場景中，我們需要根據運動中相鄰的兩張圖像恢復相機的運動，並計算空間點的深度信息，這個問題可以由對極約束和三角測量解決。
對極約束
對極約束可以根據兩張圖像的匹配的一組特徵點計算出相機從第一張圖像到第二張的運動。關於計算特徵匹配可以使用特徵點法、光流法、直接法求解出，比如LK光流跟蹤，ORB特徵匹配。下面開始介紹對極幾何，其示意圖如下：
我們希望求取兩幀圖像 I1, I2 之間的運動，設第一幀到第二幀的運動爲 R, t，兩個相機中心分別爲 O1, O2，考慮 I1 中有一個特徵點 p1，它在 I2 中對應着特徵點 p2。連線O1p1和連線O2p2在三維空間相較於點 P，O1O2 P三個點的平面稱爲極平面(Epipolar plane)。O1O2連線稱爲基線(Baseline)，基線與像平面I1, I2的交點分別爲e1, e2，稱爲極點(Epipoles)。極平面與兩個像平面I1, I2之間的相交線l1, l2爲極線(Epipolar line)。
在第一幀的座標系下，設 P=[X,Y,Z]，根據相機模型，有 s1p1=KP，s2p2=K(RP+t)，s1,s2是P點在兩個座標系的z軸座標，K爲相機內參矩陣，R, t爲第一個座標系到第二個座標系的運動。在使用齊次座標時，一個向量將等於它自身乘上任意的非零常數，這通常用於表達一個投影關係，稱爲尺度意義下相等(equal up to a scale)。此時，有sp≃p。因此有：p1≃KP，p2≃K(RP+t)。
根據相機模型，我們知道 z = 1 平面是歸一化平面，歸一化座標 p 和像素座標 x 的關係爲p=Kx ,x=P/s，即x1=K-1p1，x2=K-1p2。將x1 x2代入上式，得 x2≃Rx1+t。兩邊與平移向量t 做外積：t^ x2≃t^ Rx1+t^ t，t^ t=0，得：t^ x2≃t^ Rx1。等式兩側同時左乘x2T，得 x2T t^ x2≃x2T t^ Rx1。由於t^ x2與x2垂直，故：x2T t^ x2=0 ，得：
公式一：x2T t^ Rx1 = 0。
代入p1,p2，而且 x2T=(K-1p2)T = p2T K-T，得：
公式二：p2T K-T x2T t^ R K-1p1 = 0 。
記基礎矩陣(Fundamental Matrix) F=K-T E K-1 和本質矩陣(Essential Matrix) E=t^ R，最終得：
對極約束：x2T E x1 = p2T F p1 = 0 。
於是，相機位姿估計問題變爲以下兩步：1.根據配對點的像素位置求出E或者F。2.根據E或F求出R,t。下面介紹E的求解方法。
本質矩陣的求解
本質矩陣E=t^ R，是一個3x3的矩陣，後面會證明，奇異值必定是 [σ, σ, 0]T 的形式，這稱爲本質矩陣的內在性質。由於平移和旋轉各有 3 個自由度，故 t^ R 共有 6 個自由度。由於對極約束是等式爲零的約束，所以對 E 乘以任意非零常數後，對極約束依然滿足，即E在不同尺度下是等價的，故 E 實際上有 5 個自由度。這表明我們最少可以用 5 對點來求解 E。但是，E 的內在性質是一種非線性性質，在估計時會帶來麻煩，因此，也可以只考慮它的尺度等價性，使用 8 對點來估計 E——這就是經典的八點法（Eight-point-algorithm）。
設一對匹配點的歸一化座標爲x1=[u1,v1,1]，x2=[u2,v2,1]，對極約束爲：x2T E x1=0，矩陣形式爲：
將上式展開，將本質矩陣E展開，e=[e1,e2,e3,e4,e5,e6,e7,e8,e9]T，則對極約束寫成e有關的形式：[u2u1,u2v1,u2,v2u1,v2v1,v2,u1,v1,1]*e=0。將8對匹配點放在同一方程組裏，得：
如果 8 對匹配點組成的矩陣滿足秩爲 8 的條件，那麼 E 的各元素就可由上述方程解得。
本質矩陣的分解
得到E之後，由E=t^ R，我們想要通過E恢復得到R,t。
本質矩陣的性質
證明：一個3×3的矩陣是本質矩陣的充要條件是它的奇異值中有兩個相等而第三個是0。 本質矩陣的定義：E=t^ R=SR，其中 t^=S是反對稱矩陣。根據反對稱矩陣的基本性質，有S=UBUT，其中B是diag(a1Z, a2Z, ..., amZ, 0, ..., 0)的分塊對角陣，Z=[[0,1],[-1,0]]。反對稱矩陣的特徵矢量都是純虛數並且奇數階的反對稱矩陣必是奇異的。則可以將S寫成S=kUZUT，其中Z爲：
Z可以寫成Z=diag(1,1,0)W，其中W爲：
因此E矩陣可以分解爲：E = SR = U diag(1,1,0) (WUTR)。由此證明E的奇異值必定是 [σ, σ, 0]T 的形式。
SVD分解
上面說過，S=kUZUT，在相差一個常數因子的情況下S=UZUT。設旋轉矩陣R的SVD分解爲R=UXVT。則有：
U diag(1,1,0) VT= E = SR = (UZUT)(UXVT) = U(ZX)VT
即ZX=diag(1,1,0)，因此X=W或X=WT。因此：
E的SVD兩種分解形式爲：E=SR，S=UZUT，R=UWVT或R=UWTVT
又因爲St=0，|t|=1，因此t=U(0,0,1)T=u3，即矩陣U的最後一列。因爲t的符號不確定，R也有兩種情況，故分解有四種情況：
變換矩陣 T = [UWVT∣+u3] or [UWVT∣−u3]or[UWTVT∣+u3] or [UWTVT∣−u3]
因此，從 E 分解到 t, R 時，一共存在 4 個可能的解。
上面只有第一種解中 P 在兩個相機中都具有正的深度。因此，只要把任意一點代入 4 種解中，檢測該點在兩個相機下的深度，就可以確定哪個解是正確的了。
根據線性方程解出的 E，可能不滿足 E 的內在性質：奇異值爲 (σ, σ, 0)。通常的做法是，對八點法求得的 E 進行 SVD 分解後，會得到奇異值矩陣 Σ = diag(σ1, σ2, σ3)，設 σ1 ⩾ σ2 ⩾ σ3，取：
這相當於是把求出來的矩陣投影到了 E 所在的流形上。更簡單的做法是將奇異值矩陣取成diag(1, 1, 0)，因爲 E 具有尺度等價性，所以這樣做也是合理的。
單應矩陣的求解
單應矩陣(Homography)H，描述了兩個平面之間的映射關係。若場景中的特徵點都落在同一平面上（比如牆、地面等），則可以通過單應性來進行運動估計。這種情況在無人機攜帶的俯視相機或掃地機攜帶的頂視相機中比較常見。
單應矩陣通常描述處於共同平面上的一些點在兩張圖像之間的變換關係。考慮在圖像 I1, I2有一對匹配好的特徵點 p1, p2。這些特徵點落在平面 P上，設這個平面滿足方程：nT P+d=0，整理得 -nTP/d = 1。由尺度意義下相等，有：
於是得到了一個直接描述圖像座標 p1, p2 之間的變換，把中間這部分記爲 單應矩陣H，於是：p2≃Hp1，它與旋轉、平移及平面的參數有關。與基礎矩陣 F 類似，單應矩陣 H 也是一個 3 × 3 的矩陣，求解時的思路也和 F 類似，同樣可以先根據匹配點計算 H，然後將它分解以計算旋轉和平移。把上式展開，得：
展開有：
在實際處理中通常乘以一個非零因子使得h9 = 1（在它取非零值時），得：
這樣一組匹配點對就可以構造出兩項約束，於是自由度爲 8 的單應矩陣可以通過 4 對匹配特徵點算出（注意，這些特徵點不能有三點共線的情況），即求解以下的線性方程組（當 h9 = 0 時，右側爲零）：
這種做法把 H 矩陣看成了向量，通過解該向量的線性方程來恢復H，又稱直接線性變換法(Direct Linear Transform)。與本質矩陣相似，求出單應矩陣以後需要對其進行分解，纔可以得到相應的旋轉矩陣 R 和平移向量 t。分解的方法包括數值法與解析法。單應矩陣的分解同樣會返回 4 組旋轉矩陣與平移向量，並且同時可以計算出它們分別對應的場景點所在平面的法向量。如果已知成像的地圖點的深度全爲正值（即在相機前方），則又可以排除兩組解。最後僅剩兩組解，這時需要通過更多的先驗信息進行判斷。
單應性在 SLAM 中具有重要意義。當特徵點共面或者相機發生純旋轉時，基礎矩陣的自由度下降，這就出現了所謂的退化（degenerate）。現實中的數據總包含一些噪聲，這時候如果繼續使用八點法求解基礎矩陣，基礎矩陣多餘出來的自由度將會主要由噪聲決定。爲了能夠避免退化現象造成的影響，通常我們會同時估計基礎矩陣 F 和單應矩陣 H，選擇重投影誤差比較小的那個作爲最終的運動估計矩陣。
三角測量
在單目 SLAM 中，僅通過單張圖像無法獲得像素的深度信息，需要通過三角測量(Triangulation)的方法來估計地圖點的深度，如下圖：
三角測量是指，通過在兩處觀察同一個點的夾角，從而確定該點的距離。考慮圖像 I1, I2，以左圖爲參考，右圖的變換矩陣爲 T。相機光心爲O1, O2。在I1 中有特徵點 p1，對應 I1 中有特徵點 p2。理論上直線 O1p1 與 O2p2 在場景中會相交於一點 P，該點即兩個特徵點所對應的地圖點在三維場景中的位置。然而由於噪聲的影響，這兩條直線往往無法相交。因此，可以通過最二小乘法求解。
設 x1, x2爲兩個特徵點的歸一化座標，那麼它們滿足：s1x1 = s2Rx2 + t。現在已經知道了 R, t，想要求解的是兩個特徵點的深度 s1, s2。可以對上式兩側左乘一個 x1^ ，得：
求解公式：s1 x1^ x1 = 0 = s2 x1^ R x2 + x1^ t。
上式右側看成s2的一個方程，可以求得s2。有了s2，就可以求出s1。由於噪聲的存在，更常見的做法是求最小二乘解而不是零解。
純旋轉是無法使用三角測量的，因爲對極約束將永遠滿足。下面是三角測量的矛盾：
當平移很小時，像素上的不確定性將導致較大的深度不確定性。也就是說，如果特徵點運動一個像素 δx，使得視線角變化了一個角度 δθ，那麼將測量到深度值有 δd 的變化。從幾何關係可以看到，當 t 較大時，δd 將明顯變小，這說明平移較大時，在同樣的相機分辨率下，三角化測量將更精確。
因此，要提高三角化的精度，其一是提高特徵點的提取精度，也就是提高圖像分辨率——但這會導致圖像變大，增加計算成本。另一方式是使平移量增大。但是，這會導致圖像的外觀發生明顯的變化，比如箱子原先被擋住的側面顯示出來，又比如反射光發生變化，等等。外觀變化會使得特徵提取與匹配變得困難。總而言之，再增大平移，會導致匹配失效；而平移太小，則三角化精度不夠——這就是三角化的矛盾。
如果假設特徵點服從高斯分佈，並且不斷地對它進行觀測，在信息正確的情況下，我們就能夠期望它的方差會不斷減小乃至收斂。這就得到了一個濾波器，稱爲深度濾波器（Depth Filter）。
代碼實現
代碼用到的OpenCV函數：
1.在圖片上顯示文字 void cv::putText( cv::Mat& img, // 待繪製的圖像 const string& text, // 待繪製的文字 cv::Point origin, // 文本框的左下角 int fontFace, // 字體 (如cv::FONT_HERSHEY_PLAIN) double fontScale, // 尺寸因子，值越大文字越大 cv::Scalar color, // 線條的顏色（RGB） int thickness = 1, // 線條寬度 int lineType = 8, // 線型（4鄰域或8鄰域，默認8鄰域） bool bottomLeftOrigin = false // true='origin at lower left' ); 2.本質矩陣求解函數 Mat findEssentialMat( InputArray points1, //第一幀中特徵匹配點， InputArray points2, //第二幀中特徵匹配點 double focal = 1.0, //相機焦距 Point2d pp = Point2d(0, 0), //相機主點 int method = RANSAC, //這裏採用RANSAC算法計算本質矩陣，還有LMEDS算法 double prob = 0.999, double threshold = 1.0, OutputArray mask = noArray() ); 3.從本質矩陣中恢復旋轉和平移信息. int recoverPose( InputArray E, //本質矩陣 InputArray points1, //第一幀中特徵匹配點， InputArray points2, //第二幀中特徵匹配點， OutputArray R, //旋轉矩陣 OutputArray t, //平移向量 double focal = 1.0, //相機焦距 Point2d pp = Point2d(0, 0), //相機主點 InputOutputArray mask = noArray() ); 4.三角測量函數 void triangulatePoints( InputArray projMatr1, //從源座標系到第一張圖片的變換矩陣，在這裏以該圖像座標系爲原點，因此設爲單位陣 InputArray projMatr2, //從源座標系到第而張圖片的變換矩陣，這裏爲第一幀到第二幀的變換矩陣 InputArray projPoints1, //第一幀中特徵匹配點，歸一化座標 InputArray projPoints2, //第二幀中特徵匹配點，歸一化座標 OutputArray points4D //測量好的三維點，採取4維齊次座標的形式 );
下面的代碼首先匹配ORB特徵，然後計算本質矩陣，求解處兩幀圖像之間的運動，最後採用三角測量計算每個特徵點的深度，代碼如下：
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;
//特徵匹配
void find_feature_matches(
const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches);
//運動估計
void pose_estimation_2d2d(
const std::vector<KeyPoint> &keypoints_1,
const std::vector<KeyPoint> &keypoints_2,
const std::vector<DMatch> &matches,
Mat &R, Mat &t);
//三角測量
void triangulation(
const vector<KeyPoint> &keypoint_1,
const vector<KeyPoint> &keypoint_2,
const std::vector<DMatch> &matches,
const Mat &R, const Mat &t,
vector<Point3d> &points
);
//根據深度返回不同的顏色
inline cv::Scalar get_color(float depth) {
float up_th = 50, low_th = 10, th_range = up_th - low_th;
if (depth > up_th) depth = up_th;
if (depth < low_th) depth = low_th;
return cv::Scalar(255 * depth / th_range, 255 * depth / th_range, 255 * depth / th_range);
}
// 像素座標轉相機歸一化座標
Point2f pixel2cam(const Point2d &p, const Mat &K);
int main(int argc, char **argv)
{
//讀取圖像
Mat img_1 = imread("1.png", CV_LOAD_IMAGE_COLOR);
Mat img_2 = imread("2.png", CV_LOAD_IMAGE_COLOR);
//特徵匹配
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
cout << "一共找到了" << matches.size() << "組匹配點" << endl;
//估計兩張圖像間運動
Mat R, t;
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
//三角化
vector<Point3d> points;
triangulation(keypoints_1, keypoints_2, matches, R, t, points);
//驗證三角化點與特徵點的重投影關係
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
Mat img1_plot = img_1.clone();
Mat img2_plot = img_2.clone();
for (int i = 0; i < matches.size(); i++) {
// 第一個圖
float depth1 = points[i].z;
cout << "depth: " << depth1 << endl;
Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);
if(i%5==0)
cv::putText(img1_plot,to_string(int(depth1)), keypoints_1[matches[i].queryIdx].pt, cv::FONT_HERSHEY_PLAIN, 1,(255,255,255), 1);
// 第二個圖
Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;
float depth2 = pt2_trans.at<double>(2, 0);
cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);
}
cv::imshow("img 1", img1_plot);
cv::imshow("img 2", img2_plot);
cv::waitKey();
return 0;
}
void find_feature_matches(const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches) {
//-- 初始化
Mat descriptors_1, descriptors_2;
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:檢測 Oriented FAST 角點位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根據角點位置計算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離
vector<DMatch> match;
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配點對篩選
double min_dist = 10000, max_dist = 0;
//找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
//當描述子之間的距離大於兩倍的最小距離時,即認爲匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作爲下限.
for (int i = 0; i < descriptors_1.rows; i++)
if (match[i].distance <= max(2 * min_dist, 30.0))
matches.push_back(match[i]);
}
void pose_estimation_2d2d(const std::vector<KeyPoint> &keypoints_1,
const std::vector<KeyPoint> &keypoints_2,
const std::vector<DMatch> &matches,
Mat &R,
Mat &t)
{
// 相機內參,TUM Freiburg2
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point2f> points1,points2;
for (int i = 0; i < (int) matches.size(); i++) {
int index1=matches[i].queryIdx;
int index2=matches[i].trainIdx;
points1.push_back(keypoints_1[index1].pt);
points2.push_back(keypoints_2[index2].pt);
}
//計算本質矩陣
Point2d principal_point(325.1, 249.7); //相機主點, TUM dataset標定值
int focal_length = 521; //相機焦距, TUM dataset標定值
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
//-- 從本質矩陣中恢復旋轉和平移信息.
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
}
void triangulation(
const vector<KeyPoint> &keypoint_1,
const vector<KeyPoint> &keypoint_2,
const std::vector<DMatch> &matches,
const Mat &R, const Mat &t,
vector<Point3d> &points)
{
Mat T1 = (Mat_<float>(3, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
Mat T2 = (Mat_<float>(3, 4) <<
R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
);
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point2f> pts_1, pts_2;
for (DMatch m:matches) {
pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));// 將像素座標轉換至相機座標
pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
}
Mat pts_4d;
cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
// 轉換成非齊次座標
for (int i = 0; i < pts_4d.cols; i++) {
Mat x = pts_4d.col(i);
x /= x.at<float>(3, 0); // 歸一化
Point3d p(
x.at<float>(0, 0),
x.at<float>(1, 0),
x.at<float>(2, 0)
);
points.push_back(p);
}
}
Point2f pixel2cam(const Point2d &p, const Mat &K) {
return Point2f
(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
);
}
參考文獻
[0]高翔.視覺SLAM14講
[1]Jichao_Peng.多視圖幾何總結——從本質矩陣恢復攝像機矩陣.https://blog.csdn.net/weixin_44580210/article/details/90344511. 2019-05-20