——20.11.27

在CP2裡面講到，視覺SLAM主要分為視覺前端以及優化後端，後端也被稱為視覺里程計（VO）。根據相鄰影象的資訊，估計粗略的相機運動，給後端提供較好的初始值。VO的實現方法按是否需要提取特徵，分為特徵點的前端以及不提特徵的直接法前端。特徵點的前端執行穩定，對光照和動態物體不敏感。

一、特徵點法

1.特徵點

VO的主要問題是如何根據影象來估計相機運動，影象本身是一個由亮度以及色彩組成的矩陣，如果從矩陣層面考慮運動估計，將會十分困難。採用另一做法。1.選取比較有代表性的點（-視角發生少量變化後，保持不變 -各個影象中相同的點）2.在這些點的基礎上，討論相機位姿估計問題，以及這些點的定位問題。在SLAM中又稱這些點是位標，在視覺SLAM中路標則指影象特徵。

特徵是影象資訊的另一種數字表達形式。數字影象在計算機中以灰度值矩陣的方式儲存。在視覺里程計中，我們希望特徵點在相機運動後保持穩定。但是灰度值受光照、形變、物體材質的影響嚴重，不同影象之間變化很大，不夠穩定。理想情況下在相機發生少許變化之後，我們還能從影象中分辨出此時與上一幀對應的相同點。因此僅憑灰度值是不夠的，我們需要對影象提取特徵點。

我們通常可以把角點、邊緣和區塊作為影象中具有代表的地方。但在影象中很明顯分辨度是：角點>邊緣>區塊。所以一種直觀的提取方式就是在不用的影象中提取角點之後兩個影象一一對應，此時角點就是特徵。但我們找出的角點不一定符合我們的需求，比如在放大後發現其並不是角點或是經過旋轉之後難以辨別出同一個角點。所以相對於樸素的角點，人工設計的特徵點因具有以下特徵：

1.可重複性，相同的區域可以在不同的影象識別出來。

2.可區別性，不同的區域有不同的表達。

3.高效率，特徵點的數量應遠小於影象的畫素。

4.本地性，特徵僅於一小片區有相關。

特徵點主要由關鍵點與描述子兩部分構成。關鍵點指的是特徵點在影象中的位置（還可以包括朝向、大小等資訊）。描述子通常是一個向量，描述了關鍵點周圍畫素的資訊。描述子是按照“外觀相似的特徵應該有相同的描述子”這個原則設計的。所以兩個特徵點的描述子在向量空間上的距離相近，就能認為是同一特徵點。

圖形特徵中SIFT（尺度不變特徵變換）是最為經典的一種。充分考慮了在影象變換中出現的光照、尺度、旋轉等變化，但隨之而來的是極大的計算量。另外一種就是在考慮適當放棄精度以及魯棒性，從而提升執行速度。就是FAST關鍵點（注意這裡沒有描述子，不能稱之為特徵點），而ORB特徵是目前非常具有代表性的實時影象特徵。它改進了FAST檢測子不具有方向性的問題，並採用二進位制描述子BRIEF，加速特徵提取。

2.ORB特徵

ORB特徵也是由關鍵點和描述子組成。關鍵點是一種改進的FAST角點。描述子是BRIEF。FAST角點提取相較於FAST，ORB計算了特徵點的主方向，為後續的BRIEF提供了旋轉不變性。

FAST關鍵點主要是檢測區域性畫素灰度明顯的地方，以速度著稱。思想是：如果一個畫素與它周圍的畫素差距較大（過亮或過暗），那麼它很可能是角點，FAST只關心畫素的亮度。步驟如下：

1.在影象中選取畫素p，設他的亮度為Ip

2.設定一個閾值T（比如Ip的20%）

3.以p為中心，選取半徑為3的圓上的16個點

4.假如在選取的圓上，有連續N個點大於Ip+T後小於Ip-T，那麼就能認為是特徵點（N通常為12，即FAST-12，常用的有FAST-9，FAST-11）

5.對影象上面每個畫素迴圈上述步驟

在FAST-12的演算法中，為了更加高效可以增加一個測試，來排除不是角點的畫素。就是直接檢測1，5，9，13的畫素亮度，只有這四個中有三個同時大於Ip+T或小於Ip-T，該畫素才有可能是角點。FAST還可能出現"扎堆"現象，所以在第一次檢測之後，還需要用非極大值抑制，在一定區域內僅保留響應最大值的角點。

FAST特徵點的缺點，第一，就是FAST特徵點數量大且不穩定，我們往往需要從影象中提取固定數量的特徵，因此，在ORB中進行了改進，我們可以設定角點的數量N，對初始FAST角點分別計算Harris響應值，然後提取前N個角點。第二，FAST不具有方向資訊，而且在它固定取半徑為3的圓存在尺度問題，遠處的角點到近處就可能不是角點了。ORB增加了尺度以及旋轉的描述，尺度不變性由構建影象金字塔（對影象進行不同層次的降取樣，以獲得不同解析度的影象），並在金字塔的每一次進行檢測角點。特徵的旋轉是通過灰度質心法。質心是指以影象塊灰度作為權重的中心。

通過上述方法，FAST角點具有了尺度與旋轉的描述，增加了影象表述的魯棒性。

BRIEF描述子是一種二進位制描述子，他的描述由許多的0，1組成，這裡的0，1編碼了關鍵點附近兩個畫素（q和p）的大小關係：如果q>p,則為1，反之則為0.如果我們取了128對qp，最後得到128維由0，1組成的向量。qp的選取是依據某種概率分佈，隨機的挑選其位置。BRIEF使用了隨機選點的方式，速度十分的快，並且使用二進位制方式儲存，適用於實時影象匹配。原始的BRIEF描述子不具有旋轉不變性，當影象進行旋轉的時候很容易資料丟失。ORB的FAST特徵點提取階段計算了關鍵點的方向，可以利用方向資訊計算，計算旋轉之後的“Steer BRIEF”特徵，使得ORB具有好的旋轉不變性（？這裡不太明白）。

3.特徵匹配

特徵匹配是視覺SLAM關鍵的一步，因為特徵匹配解決了SLAM中的資料關聯問題，即當前看到的路標和之前看到的路標之間的對應關係。通過對比影象與影象，或影象與地圖之間的描述子進行匹配，為我們後續的姿態估計，優化等操作減輕負擔。但是，由於影象特徵區域性特徵，誤匹配的情況廣泛存在，已經成為目前SLAM效能提升的一大瓶頸。部分原因是因為場景中經常存在大量的重複紋理，使得特徵描述十分相似。在這種情況之下，單純使用區域性特徵解決誤匹配是十分困難的。

在獲得兩個時刻的特徵點後，如何區找尋兩個特徵點集合元素之間的對應關係，最簡單的方法就是暴力匹配（Brute-Force Matcher），即第一幀每一個特徵點與第二幀所有特徵點測量描述子的距離，然後排序取最近一個作為匹配點。描述子距離表示了兩個特徵之間的相似程度，在實際情況中可以尋找不同的距離度量範數，對於浮點型別的描述子，使用歐氏距離（m維中的兩點之間的歐式距離就是兩點距離，或者是向量的自然距離）進行度量，對於二進位制的描述子，往往使用漢明距離（兩個二進位制串之間的漢明距離就是它們之間不同位數的個數）作為量度。

為了符合SLAM實時性需求，使用暴力匹配運算量會變得很大，此時快速近似最近鄰（FLANN）演算法更適合匹配點多的情況。

4.實踐

先上程式碼。CMakeLists裡面只需要包括OpenCV的庫就行。就不多複述。提取部分程式碼講解方法。報錯方面有一點要注意的就是CV_LOAD_IMAGE_COLOR 要使用#include<opencv2/imgcodecs/legacy/constants_c.h>或者還有其他方法就不多贅述。

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>

using namespace std;
using namespace cv;

int main(int argc, char** argv){
    if(argc!=3){
        cout<<"img"<<endl;
        return 1;
    }
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    
    std::vector<KeyPoint> keyPoints_1, keyPoints_2;
    Mat descriptors_1, descriptors_2;
    Ptr<ORB> orb = ORB::create(500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE, 31, 20);
    
    orb->detect(img_1, keyPoints_1);
    orb->detect(img_2, keyPoints_2);
    
    orb->compute(img_1, keyPoints_1, descriptors_1);
    orb->compute(img_2, keyPoints_2, descriptors_2);
    
    Mat outimg1;
    drawKeypoints(img_1, keyPoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
    imshow("ORB", outimg1);
    
    vector<DMatch> matches;
    BFMatcher matcher(NORM_HAMMING);
    matcher.match(descriptors_1, descriptors_2, matches);
    
    double min_dist = 1000, max_dist=0;
    for(int i=0; i<descriptors_1.rows; i++){
        double dist = matches[i].distance;
        if(dist<min_dist) min_dist=dist;
        if(dist<max_dist) max_dist=dist;
    }
    
    cout<<"min:"<<min_dist<<endl;
    cout<<"max:"<<max_dist<<endl;
    
    std::vector<DMatch> good_matches;
    for(int i=0; i<descriptors_1.rows; i++){
        if(matches[i].distance<=max(2*min_dist, 30.0)){
            good_matches.push_back(matches[i]);
        }
    }
    
    Mat img_match;
    Mat img_goodmatch;
    drawMatches(img_1, keyPoints_1, img_2, keyPoints_2, matches, img_match);
    drawMatches(img_1, keyPoints_1, img_2, keyPoints_2, good_matches, img_goodmatch);
    imshow("all", img_match);
    imshow("good", img_goodmatch);
    waitKey(0);
    
    return 0;
}

Ptr<ORB> orb = ORB::create(500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE, 31, 20);
    
orb->detect(img_1, keyPoints_1);
orb->detect(img_2, keyPoints_2);
    
orb->compute(img_1, keyPoints_1, descriptors_1);
orb->compute(img_2, keyPoints_2, descriptors_2);
BFMatcher matcher(NORM_HAMMING);
matcher.match(descriptors_1, descriptors_2, matches);
    
double min_dist = 1000, max_dist=0;
for(int i=0; i<descriptors_1.rows; i++){
    double dist = matches[i].distance;
    if(dist<min_dist) min_dist=dist;
    if(dist<max_dist) max_dist=dist;
}

std::vector<DMatch> good_matches;
for(int i=0; i<descriptors_1.rows; i++){
   if(matches[i].distance<=max(2*min_dist, 30.0)){
        good_matches.push_back(matches[i]);
    }
}

ORB::Create()對應的是建立一個ORB的方法，輸入對應的引數，比如最大特徵點保留，影象金字塔比例，影象金字塔級別，為檢測邊框大小等等，可以在OpenCV官網中查詢對應的內容。BFMatcher是使用暴力搜尋的方式進行特徵匹配，用漢明距離的方式，對應的到能夠存在BFMatcher也可以存在DMatch，因為對應的結構體中有封裝了一個 查詢集queryId 和 一個 訓練集trainId，對應的就是descriptors_1, descriptors_2，檢測次數和descriptors_1的個數，對應是查詢集的每一行對應訓練集中找到最匹配的那一行。後面對應的就是匹配點篩選，找出匹配點之間最大和最小距離即最相似和最不相似兩組點之間的距離，當描述子距離大於兩倍的最小距離當最小距離過小則設定一個經驗值。

min:4
max:94

上面是對應結果。

二、2D-2D:對極幾何

當相機為單目，我們只知道2d畫素座標，我們需要根據兩組2D點估計運動。則需要對極幾何。

1.對極約束

現在我們假設從兩張影象中獲得了一對匹配好的特徵點，如果我們我們有若干對匹配點，就能通過直接二維匹配點的關係恢復兩幀之間相機的運動過程。

比如說我們希望求L1，L2兩幀之間的運動，設第一幀和第二幀之間的運動為R，t，兩個相機中心分別為O1，O2。在L1內的一個特徵點p1對應L2內的一個特徵點p2，如果匹配正確，可以說他們是同一個空間點在兩個成像平面上的投影。首先連線O1p1和O2p2會在三維空間內交於P點，此時O1，O2，P可以確定一個平面，我們稱之為極平面，O1，O2和像平面L1，L2分別交於e1，e2稱之為極點，O1O2為基線，稱及平面與來兩個像平面L1，L2之間的相交線l1，l2為極線。

從第一幀看過去射線O1p1上的某點是影象L1畫素所在的空間位置——因為射線上的所有點都會投影到同一個畫素點，同時，不知道P的位置，我們在L2上看到e2p2（第二個影象的極線）就是P可能出現的投影位置，也就是O1p1射線在第二個相機的投影。我們現在通過特徵點匹配，確定p2的畫素位置，才能推斷正確的P點，以及相機運動。

對極約束檢閱了給出了兩個匹配點的空間位置關係，相機問題變成以下兩步：1.根據配對點畫素位置求出E或者F。（只差相機內參，而相機內參在SLAM問題中是已知的）2.根據E或F，求出R，t

2.本質矩陣

根據定義本質矩陣E=t^R，是一個3*3的矩陣，有六個未知數。特徵：1.本質矩陣是對極約束定義的，由於對極約束是等式為零的約束，所有對E乘以任意非零常數，對極約束依舊成立。所以E在不同尺度下是等價的2.根據本質矩陣的式子，本質矩陣的奇異值必定是[σ,σ,0]^T的形式，稱為本質矩陣的內在性質3.由於平移和旋轉各有三個自由度，所以t^R有六個自由度，但由於尺度等價性，E有五個自由度。

E具有五個自由度的事實，需要有五對點來求解R。但是，E的內在性質是一種非線性性質，在求解方程的時候會帶來麻煩，因此可以只考慮尺度等價性，使用八點法來估計E，只利用E的線性性質，可以線上性代數框架下求解。

上圖通過影象的方式表示分解本質矩陣得到的四個解，我們已知空間點在相機上的投影，想求解相機的運動，在保持紅點不變的情況，可以畫出四種解，幸運的是隻有第一種解中具有正的深度，只要把任意一點帶入四個解中，檢測該點的在兩個相機下的深度，就能知道那個是正確的。

3.單應矩陣

除了基本矩陣以及本質矩陣，還有一種單應矩陣H，描述了兩個平面之間的對映關係，若場景的特徵點都落在同一平面上，則可以使用單應性進行運動估計。

單應性在SLAM中十分重要，當特徵點共面或發生純旋轉的時候，基礎矩陣的自由度下降，此時就發生了退化，此時繼續使用八點法求解多餘出來的自由度，就會被噪聲所決定。為了防止退化現象的發生，我們會同時估計基礎矩陣以及單應矩陣，選擇重投影誤差（一個特徵點在歸一化相機座標系下估計值和觀測值的差）小的作為最後運動估計矩陣。

4.實踐

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/calib3d/calib3d.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>

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(std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2, std::vector<DMatch> matches,
    Mat &R, Mat &t);

Point2d pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char** argv){
    if(argc!=3){
        cout<<"img1 img2"<<endl;
        return 1;
    }
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");
    
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout<<"find matches count:"<<matches.size()<<endl;
    
    Mat R, t;
    pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
    
    Mat t_x=(Mat_<double>(3,3) << 0, -t.at<double>(2,0), t.at<double>(1,0),
        t.at<double>(2,0), 0, -t.at<double>(0,0),
        -t.at<double>(1,0), t.at<double>(0,0), 0);
    cout<<"t^R="<<endl<<t_x*R<<endl;
    
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    for(DMatch m:matches){
        Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        Mat y1 = (Mat_<double>(3,1) << pt1.x, pt1.y, 1);
        Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        Mat y2 = (Mat_<double>(3,1) << pt2.x, pt2.y, 1);
        Mat d = y2.t() * t_x * R * y1;
        cout<<"constraint"<<d<<endl;
    }
    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");
    
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
    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;
    }
    
    cout<<"max dist:"<<max_dist<<endl;
    cout<<"min dist:"<<min_dist<<endl;
    
    for(int i=0; i<descriptors_1.rows; i++){
        if(match[i].distance <= max(2*min_dist, 30.0)){
            matches.push_back(match[i]);
        }
    }
}

Point2d pixel2cam(const Point2d &p, const Mat &K){
    return Point2d((p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y - K.at<double>(1,2))/K.at<double>(1,1)
    );
}

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2, std::vector<DMatch> matches,
    Mat &R, Mat &t){
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    
    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);
    }
    
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT);
    cout<< "fundamental_matrix is "<<endl<<fundamental_matrix<<endl;
    
    Point2d principal_point(325.1, 249.7);
    double focal_length = 521;
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    cout<< "essential_matrix is"<< endl<< essential_matrix<<endl;
    
    Mat homography_matrix;
    homography_matrix = findHomography(points1, points2, RANSAC, 3);
    cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;
    
    recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    cout<<"R is"<<R<<endl;
    cout<<"t is"<<t<<endl;
}

Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//相機內參

for(int i=0; i<(int)matches.size(); i++){
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);//matches[i].queryIdx對應keypoints_1陣列內特徵點index，因為是通過keypoints_1特徵點的描述子得到的位置
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);//matches[i].trainIdx對應keypoints_2陣列內特徵點index，因為是通過keypoints_2特徵點的描述子得到的位置
}

Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT);
cout<< "fundamental_matrix is "<<endl<<fundamental_matrix<<endl;
    
Point2d principal_point(325.1, 249.7);//相機光心
double focal_length = 521;//相機焦距
Mat essential_matrix;
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
cout<< "essential_matrix is"<< endl<< essential_matrix<<endl;
    
Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3);
cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;
    
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);

函式find_feature_matches內的過程就不多贅述了，和前面的過程基本一致，就是換了對應查詢描述子以及特徵點的類，但都是衍生類（？）。我們可以看一下pose_estimation_2d2d函式裡面的內容，裡面添加了對應的註釋queryIdx，trainIdx在上面的程式碼中有提到是查詢集以及訓練集的關係。我們知道基礎矩陣和本質矩陣之間相差內參K。函式在OpenCVwiki都可以查得到。http://wiki.opencv.org.cn/index.php/Cv%E7%85%A7%E7%9B%B8%E6%9C%BA%E5%AE%9A%E6%A0%87%E5%92%8C%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA

在main函式內還有就是驗證對極約束的內容。

下面是對應的結果

max dist:94
min dist:4
find matches count:79
fundamental_matrix is
[4.54443750398184e-06, 0.0001333855576992603, -0.01798499246479044;
-0.0001275657012964255, 2.266794804645652e-05, -0.01416678429206633;
0.01814994639971766, 0.004146055870980492, 1]
essential_matrix is
[-0.008455114492964278, 0.05451570701059781, 0.1546375809484052;
-0.008287154708445212, 0.03351311565984172, -0.6896472136971504;
-0.1153993974485718, 0.6945899967012867, 0.02159624094256633]
homography_matrix is
[0.9261214237658335, -0.1445322040802305, 33.26921164265664;
0.04535424230636757, 0.9386696658342905, 8.570980713233848;
-1.006198269424755e-05, -3.008140685985328e-05, 1]
R is[0.9956584940813579, -0.05615340406690447, 0.07423582945816433;
0.05268846331440004, 0.9974645001566195, 0.04783823534446425;
-0.07673388428334535, -0.0437191735855581, 0.9960926386957119]
t is[-0.9726703113454949;
-0.2153829834753195;
0.08673313009645391]
t^R=
[0.01195733758736675, -0.07709685221674556, -0.2186905642298021;
0.01171980658216709, -0.04739470268352609, 0.9753084428633267;
0.1631993929614534, -0.9822985936236425, -0.03054169683725466]

對極約束是從x2=Rx1+t得到的，T是R，t組成的變換矩陣，x2=T21x1

討論

從E，F，H中都可以分解出運動，不過H要建立在平面上。由於E本身具有尺度等價性，它分解得到的t，R也有尺度等價性。在分解過程中，t乘以任意非零常數，分解是成立的，所以我們要對t進行歸一化。

尺度不確定性

因為對t進行歸一化，直接導致了單目視覺的尺度不確定性，因為對t乘以任意比例常數之後，對極約束依舊成立，在單目SLAM中，對軌跡以及地圖同時縮放任意倍數後，得到的影象是一樣的。在單目視覺中，我們對兩張影象的t進行歸一化後，相當於固定了尺寸，這被稱為單目SLAM的初始化，在初始化後，就可以用3d-2d來計算相機運動了，初始化後軌跡和地圖的單位就是初始化固定的尺度。因此，單目SLAM有一步不可避免的初始化，初始化的影象一定要有平移，此後的軌跡以及地圖都以此步平移為單位。

除了對t初始化，還可以令初始化的特徵點平均深度為1，也可以固定一個尺度。把深度歸一化可以控制場景的規模大小。

初始化的純旋轉問題

單目初始化不能只有純旋轉，必須要有一定程度的平移。因為從E分解到R，t如果t為零，E也會為零，

三、三角測量

前面我們通過對極幾何約束估計相機運動之後。我們需要相機的運動估計特徵點的空間位置，在單目相機中我們需要通過一張以上影象來獲得相機的深度資訊。

三角測量是指，通過兩處觀察同一點的夾角，來確定該點的距離。同樣使用上一節的幾何模型，左圖為參考，右圖的變換矩陣為T。理論上O1p1與O2p2會相交於一點P。但因為噪聲的影響，兩條直線往往無法相交，又需要最小二乘來求解。根據對極幾何裡面的定義，x1、x2為兩個特徵點的歸一化座標，深度對應s1，s2滿足：

s1x1=s2Rx2+t

s1x1^x1=0=s2x1^Rx2+x1^t

這樣就能求出對應的深度，但因為噪聲的存在，我們估計的R，t不一定能使上式等於零，更常見的做法是最小二乘而不是零解。

實踐

#include<iostream>
#include<opencv2/opencv.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>
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> &keypoint_2,
    std::vector<DMatch> &matches
);

void pose_estimation_2d2d(
    const std::vector<KeyPoint> &keypoints_1, const std::vector<KeyPoint> &keypoints_2,
    std::vector<DMatch> &matches, Mat &R, Mat &t
);

void triangulation(
    const vector<KeyPoint> &keypoint_1, const vector<KeyPoint> &keypoints_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, 0, 255 * (1-depth/th_range));
}

Point2f pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv){
    if(argc!=3){
        cout<<"img1 img2"<<endl;
        return 1;
    }
    
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], 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);

        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");
    
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
    
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
    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;
    }
    
    cout<<"max_dist :"<<max_dist<<endl;
    cout<<"min_dist :"<<min_dist<<endl;
    
    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,
    std::vector<DMatch> &matches, Mat &R, Mat &t
){
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    
    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);
    }
    
    Point2d principal_point(325.1,249.7);
    int focal_length = 521;
    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), R.at<double>(0,0),
        R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), R.at<double>(1,0),
        R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), R.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)
    );
}

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);
}

inline cv::Scalar get_color(float depth,float up_th = 50, float low_th=10){
 float th_range = up_th - low_th;
 depth-=low_th;
 //if(depth > up_th) depth = up_th;
 //if(depth < low_th) depth = low_th;
 return cv::Scalar(255 * depth / th_range, 0, 255 *(1- depth / th_range));
}

這部分是三角測量部分，以及對得到特徵點進行歸一化處理。在main程式碼中使用顏色來表示深度，我對程式碼顯示顏色稍作修改，對映到最大值和最小值區間內比較明顯，可以得到以下結果：

討論

三角測量是由平移得到的，有平移才有三角測量，當平移很小的時候，畫素上的不確定性將導致較大的深度不確定性，也就是說，如果特徵點運動一個畫素,使得視角移動了一個角度，那麼觀測到深度值就有δd的變化，從上圖來看，當t比較大的時候，δd將有明顯變化，相同的相機解析度下，三角化測量更加精確。因此要增加三角的精度，其一提高特徵點的提取精度（解析度），其二就是增大平移量，但又會導致影象外觀發生明顯變化。但只要我們願意，有也能夠定量的計算每個特徵點和位置的不確定性。所以，如果假設特徵點服從高斯分佈，我們就能夠期望他的方差不斷減小乃至收斂，這就得到了一個深度濾波器。

三、3D-2D PnP

這是求解3D到2D點運動的方法，描述了當我們知道n個3D點的空間點以及他的投影位置時，如何估計相機所在的位姿。然而，如果兩張影象中，其中一張的特徵點已知，那麼至少需要三個點對（需要一個附加點來驗證）就能估計相機運動。特徵點的3D位置可以通過三角化，或者RGB-D的深度圖來確定。因此，在雙目或RGB-D中我們能夠直接通過PnP估計相機運動。在單目相機中則需要先初始化，才能使用。3D-2D方法不需要對極約束。

PnP有多種求解方法，P3P、DLT、EPnP、UPnP等。此外還可以使用非線性優化的方法，構建最小二乘並迭代求解，就是萬金油的Bundle Adjustment。

1.直接線性變換

//空

2.P3P

輸入資料為三對3D-2D匹配點，記A、B、C，2D為a，b，c，其中小寫字母表示的點是大寫字母在相機成像平面的投影。此外P3P還需要一對驗證點D-d，相機光心為O。ABC是世界座標的座標。

式子內三個餘弦值已知，且u=BC²/AB²，w=AC²/AB²在世界座標的系計算出來的比例，變換到相機座標系中不會改變，此式中x，y是未知的，會根據相機的移動發生變化。解析的求解該方程組是一個複雜的過程。需要使用吳消元法。類似與求解E的過程，有四個解，我們可以通過驗證點，得到最有可能的的解，得到ABC在相機座標系的3D座標，再通過3D-3D的對點計算相機的運動R，t。但P3P存在一些問題：

1.P3P只利用三個點的資訊，當資訊多餘三個點的時候，能以提供更多資訊。

2.如果3D，2D點受噪聲影響，或誤匹配，則演算法失敗。

在SLAM中優先使用P3P/EPnP等方法估計相機位姿，然後構建最小二乘優化問題對估計值進行調整。

3.Bundle Adjustment

//空

實踐

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/calib3d/calib3d.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>
#include<Eigen/Core>
#include<g2o/core/base_vertex.h>
#include<g2o/core/base_unary_edge.h>
#include<g2o/core/sparse_optimizer.h>
#include<g2o/core/block_solver.h>
#include<g2o/core/solver.h>
#include<g2o/core/optimization_algorithm_gauss_newton.h>
#include<g2o/solvers/dense/linear_solver_dense.h>
#include<sophus/se3.hpp>
#include<chrono>

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);

Point2d pixel2cam(const Point2d &p, const Mat &K);

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundleAdjustmentG2O(const VecVector3d &points_3d,
    const VecVector2d &points_2d, const Mat &K, 
    Sophus::SE3d &pose);

void bundleAdjustmentGaussNewton(const VecVector3d &points_3d, const VecVector2d &points_2d, const Mat &K,
    Sophus::SE3d &pose);

int main(int argc, char **argv){
    if(argc!=5){
        cout<<"img_1, img_2, depth1, depth2"<<endl;
        return 1;
    }
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    assert(img_1.data && img_2.data && "Can not load images!");
    
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout<<"matches"<<matches.size()<<endl;
    
    Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for(DMatch m:matches){
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if(d==0) continue;
        float dd=d/5000.0;
        Point2d p1=pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(Point3f(p1.x*dd, p1.y * dd, dd));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);
    }
    
    cout<<"3d-2d pair:"<<pts_3d.size()<<endl;
    
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Mat r, t;
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
    Mat R;
    cv::Rodrigues(r,R);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"pnp in opencv time:"<<time_used.count()<<endl;
    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;
    
    VecVector3d pts_3d_eigen;
    VecVector2d pts_2d_eigen;
    for(size_t i=0; i<pts_3d.size(); ++i){
        pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
        pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
    }
    
    cout<<"call BA by gauss newton "<<endl;
    
    Sophus::SE3d pose_gn;
    t1 = chrono::steady_clock::now();
    bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"pnp by gauss newton time:"<<time_used.count()<<endl;
    
    cout<<"call BA by g2o";
    Sophus::SE3d pose_g2o;
    t1=chrono::steady_clock::now();
    bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
    t2=chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"pnp by g2o time:"<<time_used.count()<<endl;
    return 0;
}

void find_feature_matches(const cv::Mat& img_1, const cv::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");
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
    
    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;
    }
    
    cout<<"max_dist"<<max_dist<<endl;
    cout<<"min_dist"<<min_dist<<endl;
    
    for(int i=0; i<descriptors_1.rows; i++){
        if(match[i].distance <= max(2 * min_dist, 30.0)){
            matches.push_back(match[i]);
        }
    }
}

Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return Point2d(
        (p.x-K.at<double>(0,2))/K.at<double>(0,0),
        (p.y-K.at<double>(1,2))/K.at<double>(1,1)
    );
}

void bundleAdjustmentGaussNewton(const VecVector3d& points_3d, const VecVector2d& points_2d, const cv::Mat& K, Sophus::SE3d& pose)
{
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    const int iterations = 10;
    double cost = 0, lastCost = 0;
    double fx = K.at<double>(0,0);
    double fy = K.at<double>(1,1);
    double cx = K.at<double>(0,2);
    double cy = K.at<double>(1,2);
    
    for(int iter=0; iter<iterations; iter++){
        Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();
        
        cost = 0;
        for(int i=0; i<points_3d.size(); i++){
            Eigen::Vector3d pc = pose * points_3d[i];
            double inv_z = 1.0/pc[2];
            double inv_z2 = inv_z * inv_z;
            Eigen::Vector2d proj(fx * pc[0]/pc[2] + cx, fy * pc[1]/pc[2]+cy);
            Eigen::Vector2d e = points_2d[i] - proj;
            cost += e.squaredNorm();
            Eigen::Matrix<double, 2, 6> J;
            J<<-fx * inv_z,
               0,
               fx*pc[0]*inv_z2,
               fx * pc[0]*pc[1]*inv_z2,
               -fx - fx * pc[0]*pc[0] * inv_z2,
               fx * pc[1] * inv_z,
               0,
               -fy * inv_z,
               fy * pc[1] *inv_z2,
               fy + fy *pc[1]*pc[1]*inv_z2,
               -fy * pc[0]*pc[1]*inv_z2,
               -fy * pc[0] * inv_z;
            
            H+=J.transpose() * J;
            b+=-J.transpose() * e;
        }
        Vector6d dx;
        dx = H.ldlt().solve(b);
        
        if(isnan(dx[0])){
            cout<<"result is nan"<<endl;
            break;
        }
        
        if(iter>0 && cost>=lastCost){
            cout<<"cost:"<<cost<<" lastCost:"<<lastCost<<endl;
            break;
        }
        
        pose = Sophus::SE3d::exp(dx) * pose;
        lastCost = cost;
        
        cout<<"iterations"<<iter<<" cost="<<std::setprecision(12)<<cost<<endl;
        
        if(dx.norm() < 1e-6){
            break;
        }
    }
    
    cout<<"pose by g-n"<<endl<<pose.matrix()<<endl;
}

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    virtual void setToOriginImpl() override{
        _estimate = Sophus::SE3d();
    }
    
    virtual void oplusImpl(const double *update) override{
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen<<update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
    }
    
    virtual bool read(std::istream & in) override {}
    virtual bool write(std::ostream & out) const override{}
};

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
    
    virtual void computeError() override{
        const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Sophus::Vector3d pos_pixel = _K * (T * _pos3d);
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();
    }
    
    virtual void linearizeOplus() override{
        const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Eigen::Vector3d pos_cam = T * _pos3d;
        double fx = _K(0,0);
        double fy = _K(1,1);
        double cx = _K(0,2);
        double cy = _K(1,2);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Z2 = Z*Z;
        _jacobianOplusXi << -fx/Z, 0, fx*X/Z2, fx*X*Y/Z2, -fx - fx * X * X/Z2, fx * Y/Z, 0, -fy / Z, fy*Y/(Z * Z), fy + fy * Y * Y/Z2  ,-fy*X*Y/Z2, -fy*X/Z;
    }
    
    virtual bool read(istream &in) override {}
    virtual bool write(std::ostream & os) const override {}

private:
    Eigen::Vector3d _pos3d;
    Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(const VecVector3d& points_3d, const VecVector2d& points_2d, const cv::Mat& K, Sophus::SE3d& pose)
{
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);
    
    VertexPose *vertex_pose = new VertexPose();
    vertex_pose->setId(0);
    vertex_pose->setEstimate(Sophus::SE3d());
    optimizer.addVertex(vertex_pose);
    
    Eigen::Matrix3d K_eigen;
    K_eigen<<K.at<double>(0,0), K.at<double>(0,1), K.at<double>(0,2), K.at<double>(1,0), K.at<double>(1,1), K.at<double>(1,2), K.at<double>(2,0), K.at<double>(2,1), K.at<double>(2,2);
    
    int index = 1;
    for(size_t i = 0;i<points_2d.size(); ++i){
        auto p2d = points_2d[i];
        auto p3d = points_3d[i];
        EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
        edge->setId(index);
        edge->setVertex(0, vertex_pose);
        edge->setMeasurement(p2d);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        index++;
    }
    
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"cost time:"<<time_used.count()<<endl;
    cout<<"pose by g2o=\n"<<vertex_pose->estimate().matrix()<<endl;
    pose=vertex_pose->estimate();
}

    Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for(DMatch m:matches){
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if(d==0) continue;
        float dd=d/5000.0;
        Point2d p1=pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(Point3f(p1.x*dd, p1.y * dd, dd));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);
    }
    
    Mat r, t;
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
    Mat R;
    cv::Rodrigues(r,R);

Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
 return Point2d(
 (p.x-K.at<double>(0,2))/K.at<double>(0,0),
 (p.y-K.at<double>(1,2))/K.at<double>(1,1)
 );
}

我們可以先看pixel2cam這個函式，這個是將畫素座標系，轉化到影象座標系。[u,v,1]^T=K[x,y,1]^T.對應的u=xfu + uo V=yfv + Vo。然後從影象座標系轉到世界座標需要乘以深度Z。然後使用solvePnP函式得到旋轉向量以及平移向量。通過Rodrigues函式轉成矩陣。

max_dist94
min_dist4
matches79
3d-2d pair:75
pnp in opencv time:0.0300834
R=
[0.9979059095501289, -0.05091940089111062, 0.03988747043647115;
0.04981866254254162, 0.9983623157438141, 0.02812094175381178;
-0.04125404886071617, -0.02607491352889358, 0.9988083912027663]
t=
[-0.1267821389556796;
-0.008439496817594587;
0.06034935748886031]

可以與2d2d的結果進行比較，可以發現差距不大。

程式碼中同樣使用了非線性優化，L-M法以及G-N。後面有需要我會再詳細寫。

四、3D-3D：ICP

假設我們有一組配對好的3D點。我們需要找到一個歐氏變換R，t，使得：

這個問題可以用迭代最近點（ICP），在3D-3D相機位姿估計中沒有出現相機模型，僅考慮兩組3D點之間的變換。認為距離最近的兩個點為一個就是迭代最近點。

1.SVD

2.非線性優化

實踐

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/calib3d/calib3d.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<Eigen/Geometry>
#include<Eigen/SVD>
#include<g2o/core/base_vertex.h>
#include<g2o/core/base_unary_edge.h>
#include<g2o/core/block_solver.h>
#include<g2o/core/optimization_algorithm_gauss_newton.h>
#include<g2o/core/optimization_algorithm_levenberg.h>
#include<g2o/solvers/dense/linear_solver_dense.h>
#include<chrono>
#include<sophus/se3.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);

Point2d pixel2cam(const Point2d &p, const Mat &K);

void pose_estimation_3d3d(
    const vector<Point3f> &pts1,
    const vector<Point3f> &pts2,
    Mat &R, Mat &t);

void bundleAdjustment(
    const vector<Point3f> &points_3d,
    const vector<Point3f> &points_2d,
    Mat &R, Mat &t);

class VertexPose: public g2o::BaseVertex<6, Sophus::SE3d>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    virtual void setToOriginImpl() override{
        _estimate = Sophus::SE3d();
    }
    
    virtual void oplusImpl(const double *update) override{
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen<<update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
    }
    
    virtual bool read(istream & in) override{}
    virtual bool write(ostream & out) const override{}
};

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point):_point(point) {}
    
    virtual void computeError() override{
        const VertexPose *pose = static_cast<const VertexPose *>(_vertices[0]);
        _error = _measurement - pose->estimate() * _point;
    }
    
    virtual void linearizeOplus() override{
        VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
        Sophus::SE3d T = pose->estimate();
        Eigen::Vector3d xyz_trans = T * _point;
        _jacobianOplusXi.block<3,3>(0,0) = -Eigen::Matrix3d::Identity();
        _jacobianOplusXi.block<3,3>(0,3) = Sophus::SO3d::hat(xyz_trans);
    }
    
    bool read(std::istream &in) {}
    bool write(std::ostream & out) const {}
protected:
    Eigen::Vector3d _point;
};

int main(int argc, char **argv){
    if(argc != 5){
        cout<<"img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    
    Mat img_1 = imread(argv[1],CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], 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<<"matchs:"<<matches.size()<<endl;
    Mat depth1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
    Mat depth2 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);
    Mat K = (Mat_<double>(3,3)<<520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<Point3f> pts1, pts2;
    
    for(DMatch m:matches){
        ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
        if(d1==0||d2==0) continue;
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1)/5000.0;
        float dd2 = float(d2)/5000.0;
        pts1.push_back(Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    cout<<"3d-3d pairs: "<<pts1.size()<<endl;
    Mat R,t;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout<<"ICP via SVD result: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t()<<endl;
    cout<<"t_inv = "<<-R.t() * t<<endl;
    
    cout<<"calling bundle adjustment"<<endl;
    
    bundleAdjustment(pts1, pts2, R, t);
    for(int i=0; i<5; i++){
        cout<<"p1="<<pts1[i]<<endl;
        cout<<"p2="<<pts2[i]<<endl;
        cout<<"(R*p2+t)="<<R*(Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t<<endl<<endl;
    }
}

void find_feature_matches(const cv::Mat& img_1, const cv::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");
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);
    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>max_dist) max_dist=dist;
        if(dist<min_dist) min_dist=dist;
    }
    cout<<"max_dist = "<<max_dist<<endl;
    cout<<"min_dist = "<<min_dist<<endl;
    
    for(int i=0; i<descriptors_1.rows; i++){
        if(match[i].distance<=max(2*min_dist, 30.0)){
            matches.push_back(match[i]);
        }
    }
}

cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return Point2d((p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y-K.at<double>(1,2))/K.at<double>(1,1)
    );
}

void pose_estimation_3d3d(const vector<cv::Point3f>& pts1, const vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    Point3f p1, p2;
    int N = pts1.size();
    for(int i=0; i<N; i++){
        p1+=pts1[i];
        p2+=pts2[i];
    }
    p1=Point3f(Vec3f(p1)/N);
    p2=Point3f(Vec3f(p2)/N);
    vector<Point3f> q1(N), q2(N);
    for(int i=0; i<N; i++){
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }
    
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i=0;i<N;i++){
        W+=Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    cout<<"W="<<W<<endl;
    
    //SVD
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;
    Eigen::Matrix3d R_ = U * (V.transpose());
    if(R_.determinant()<0){
        R_ = -R_;
    }
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
    
    R = (Mat_<double>(3,3)<< R_(0,0), R_(0,1), R_(0,2),
        R_(1,0), R_(1,1), R_(1,2),
        R_(2,0), R_(2,1), R_(2,2)
    );
    t=(Mat_<double>(3,1)<<t_(0,0), t_(1,0), t_(2,0));
}

void bundleAdjustment(const vector<cv::Point3f>& pts1, const vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    typedef g2o::BlockSolverX BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);
    
    VertexPose *pose = new VertexPose();
    pose->setId(0);
    pose->setEstimate(Sophus::SE3d());
    optimizer.addVertex(pose);
    
    for(size_t i=0; i<pts1.size(); i++){
        EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
        edge->setVertex(0, pose);
        edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
        edge->setInformation(Eigen::Matrix3d::Identity());
        optimizer.addEdge(edge);
    }
    
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"optimization time :"<<time_used.count()<<endl;
    
    Eigen::Matrix3d R_ = pose->estimate().rotationMatrix();
    Eigen::Vector3d t_ = pose->estimate().translation();
    R=(Mat_<double>(3,3) << 
        R_(0,0), R_(0,1), R_(0,2),
        R_(1,0), R_(1,1), R_(1,2),
        R_(2,0), R_(2,1), R_(2,2)
    );
    
    t=(Mat_<double>(3,1) << t_(0,0), t_(1,0), t_(2,0));
}

下面是對應的結果，ICP問題是從第二步推回第一步。這裡作了逆變換。

max_dist = 94
min_dist = 4
matchs:79
3d-3d pairs: 72
W= 10.871 -1.01948 2.54771
-2.16033 3.85307 -5.77742
3.94738 -5.79979 9.62203
U= 0.558087 -0.829399 -0.0252034
-0.428009 -0.313755 0.847565
0.710878 0.462228 0.530093
V= 0.617887 -0.784771 -0.0484806
-0.399894 -0.366747 0.839989
0.676979 0.499631 0.540434
ICP via SVD result:
R = [0.9969452351705235, 0.0598334759429696, -0.05020112774999549;
-0.05932607556034211, 0.9981719680327525, 0.01153858709846634;
0.05079975225724825, -0.008525103530306, 0.9986724727258676]
t = [0.1441598281917405;
-0.06667849447794799;
-0.03009747343724256]
R_inv = [0.9969452351705235, -0.05932607556034211, 0.05079975225724825;
0.0598334759429696, 0.9981719680327525, -0.008525103530306;
-0.05020112774999549, 0.01153858709846634, 0.9986724727258676]
t_inv = [-0.1461462830262246;
0.0576744363694081;
0.03806387978797152]
calling bundle adjustment
iteration= 0 chi2= 1.816112 time= 6.0297e-05 cumTime= 6.0297e-05 edges= 72 schur= 0 lambda= 0.000758 levenbergIter= 1
iteration= 1 chi2= 1.815514 time= 3.1219e-05 cumTime= 9.1516e-05 edges= 72 schur= 0 lambda= 0.000505 levenbergIter= 1
iteration= 2 chi2= 1.815514 time= 2.7632e-05 cumTime= 0.000119148 edges= 72 schur= 0 lambda= 0.000337 levenbergIter= 1
iteration= 3 chi2= 1.815514 time= 2.6113e-05 cumTime= 0.000145261 edges= 72 schur= 0 lambda= 0.000225 levenbergIter= 1
iteration= 4 chi2= 1.815514 time= 2.6208e-05 cumTime= 0.000171469 edges= 72 schur= 0 lambda= 0.000150 levenbergIter= 1
iteration= 5 chi2= 1.815514 time= 2.7833e-05 cumTime= 0.000199302 edges= 72 schur= 0 lambda= 0.000299 levenbergIter= 1
optimization time :0.000497761
p1=[-0.243698, -0.117719, 1.5848]
p2=[-0.297211, -0.0956614, 1.6558]
(R*p2+t)=[-0.2409901495364604;
-0.1254270500587826;
1.609221205029395]

p1=[0.402045, -0.341821, 2.2068]
p2=[0.378811, -0.262859, 2.2196]
(R*p2+t)=[0.3946591022539743;
-0.3259188829495218;
2.20803983035825]

p1=[-0.522843, -0.214436, 1.4956]
p2=[-0.58581, -0.208584, 1.6052]
(R*p2+t)=[-0.532923946912698;
-0.2216052393093164;
1.54499035805527]

p1=[-0.627753, 0.160186, 1.3396]
p2=[-0.709645, 0.159033, 1.4212]
(R*p2+t)=[-0.6251478068660965;
0.1505624195985039;
1.351809862638435]

p1=[0.594266, -0.0256024, 1.5332]
p2=[0.514795, 0.0391393, 1.5332]
(R*p2+t)=[0.5827556962439571;
-0.04046060384335358;
1.526884519595548]

感謝你看到這裡，這部分的內容很多，有一些我看的不是很懂，還是帶入實際的專案中，從實踐裡面出真理，再返過來研究。Cheers！