這個章節主要講解：
圖像特徵提取；
多幅圖像匹配特徵點；
對極幾何；
PNP問題；
ICP問題；
三角化原理；
一. 特徵點提取和匹配
工程實踐需要你事先安裝了opencv3； 由於opencv3中提供了由本質矩陣E 恢復R，t的接口。
opencv2和opencv3提取特徵點時有些語法寫法不太一樣~
用CLion打開slambook2-master的ch7工程；
其中舊的slambook的ch7中有如下的文件：
而slambook2中變成如下的了：
其中 feature_extraction.cpp 變成了orb_cv.cpp
本文針對orb_cv.cpp做實踐；
其代碼內容如下：
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp> //特徵提取必須的庫
#include <opencv2/highgui/highgui.hpp>
#include <chrono> //計時用
using namespace std;
using namespace cv;
int main(int argc, char **argv) {
if (argc != 3) {
cout << "usage: feature_extraction 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 != nullptr && img_2.data != nullptr); //確保圖像正確讀取
//-- 初始化
std::vector<KeyPoint> keypoints_1, keypoints_2; //兩張圖像的特徵點向量
Mat descriptors_1, descriptors_2; //特徵描述器
Ptr<FeatureDetector> detector = ORB::create(); //構建ORB特徵檢測器
Ptr<DescriptorExtractor> descriptor = ORB::create(); //構建ORB特徵描述器
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming"); //創建匹配，選擇暴力匹配，計算描述器的漢明距離，ORB無法用歐式距離計算
//-- 第一步:檢測 Oriented FAST 角點位置
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
detector->detect(img_1, keypoints_1); //提取圖1的關鍵點
detector->detect(img_2, keypoints_2); //提取圖2的關鍵點
//-- 第二步:根據角點位置計算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1); //計算描述子
descriptor->compute(img_2, keypoints_2, descriptors_2);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
Mat outimg1;
drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
imshow("ORB features", outimg1); //顯示圖1提取的ORB特徵
//-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離
vector<DMatch> matches;
t1 = chrono::steady_clock::now();
matcher->match(descriptors_1, descriptors_2, matches); //進行匹配，飯後DMatch結構的匹配結果，該結構中包含圖1，2的關鍵點ID，兩者的漢明距離
t2 = chrono::steady_clock::now();
time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
//-- 第四步:匹配點對篩選
// 計算最小距離和最大距離
auto min_max = minmax_element(matches.begin(), matches.end(),
[](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });
double min_dist = min_max.first->distance;
double max_dist = min_max.second->distance;
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//當描述子之間的距離大於兩倍的最小距離時,即認爲匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作爲下限.
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 matches", img_match);
imshow("good matches", img_goodmatch);
waitKey(0);
return 0;
}
針對上面這個cpp,自己寫個CMakeLists.txt (由於提供的CMakeLists.txt是針對ch7下面所有的cpp的)，這個代碼主要是用到了OpenCV3 內容如下：
cmake_minimum_required(VERSION 2.8)
project(orb_cv) #我將其重命名了下
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
)
add_executable(orb_cv orb_cv.cpp)
target_link_libraries(orb_cv ${OpenCV_LIBS})
然後就是編譯了：
//新建了個文件夾，命名爲feature-extracture
//將上面新建的CMakeLists.txt和orb_cv.cpp拷貝進來，切到這個目錄下
mkdir build
cd build
cmake ..
make
得到可執行文件orb_cv，我將圖像1.png和2.png拷貝到這build下，終端運行：
./orb_cv 1.png 2.png
顯示ORB提取特徵花費時間2.46495秒；
匹配時間8.00450712秒；
暴力匹配結果如下（每個特徵點都會尋找匹配關係）：
匹配漢明距離最大95，最小7；
刷選匹配結果如下：
特徵點之間的匹配關係可爲計算相機姿態提供必要信息。
特徵匹配後得到了特徵點之間的對應關係
如果只有兩個單目圖像，得到了2D-2D間的關係 ——對極幾何
如果匹配的是幀和地圖，得到3D-2D間的關係 —— PnP
如果匹配的是RGB-D圖，得到3D-3D間的關係 —— ICP
二.對極幾何
概念部分請參考博客：視覺里程計之對極幾何
引用博主的一幅圖
兩個圖像中的p1和p2可以通過特徵匹配得到，P未知，e1和e2未知，T12（相機2到相機1的變換）待求；
對極約束爲：
本質矩陣E（Essential Matrix）：
基礎矩陣F（Fundamental Matrix）：
相機位姿估計問題變爲如下兩步：
- 根據配對點的像素位置求出E或者F；
- 根據E或者F求出R，t
E與F只相差內參，由於內參通常爲已知的，所以往往使用更簡單的E。用經典的8點法求E；
從E計算R,t ：用奇異值分解；
2D-2D情況下，只知道圖像座標之間的對應關係
- 當特徵點在平面上時（例如俯視或仰視），使用H恢復R,t;
- 否則，使用E或者F恢復R,t;
求得R,t後：
- 利用三角化計算特徵點的3D位置（即深度）
針對ch7中的pose_estimation_2d2d.cpp 代碼進行分析；
在ch7中新建個文件夾叫pose-estimation2d2d，然後將pose_estimation_2d2d.cpp 和 CMakeLists.txt 拷貝進來，其中CMakeLists.txt內容修改爲：
cmake_minimum_required(VERSION 2.8)
project(pose_estimation_2d2d)
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
)
# add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp extra.cpp ) # use this if in OpenCV2
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS})
終端切到這個pose-estimation2d2d目錄下，編譯：
mkdir build
cd build
cmake ..
make -j2
將圖像1.png和2.png拷貝到build中，執行：
./pose_estimation_2d2d 1.png 2.png
輸出結果如下：
匹配漢明距離最小7，最大95；
找到81組匹配點，調用opencv3的接口，計算得到基礎矩陣F,本質矩陣E, 單應矩陣H,相機旋轉矩陣R,平移向量t（已經長度歸一化爲1了）,以及打印81組匹配點之間的對極約束值（理論上應該是0的）
pose_estimation_2d2d.cpp源碼分析如下：
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
// #include "extra.h" // use this if in OpenCV2
using namespace std;
using namespace cv;
/****************************************************
* 本程序演示瞭如何使用2D-2D的特徵匹配估計相機運動
* **************************************************/
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); //像素座標轉爲歸一化的相機座標Zc=1
int main(int argc, char **argv) {
if (argc != 3) { //需要傳入兩幅圖像
cout << "usage: pose_estimation_2d2d 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 << "一共找到了" << matches.size() << "組匹配點" << endl; //打印匹配結果
//-- 估計兩張圖像間運動
Mat R, t; //（圖像2相對圖像1的運動）
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t); //估計相機運動，通過2D-2D點恢復相機旋轉與平移R,t
//-- 驗證E=t^R*scale
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); //將匹配的點轉變到歸一化後的相機座標，其Zc=1
Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1); //點pt1的歸一化相機座標，3x1維度
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; //這個計算公式是對極約束的公式，判斷其是否爲0；
cout << "epipolar 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;
// used in OpenCV3
Ptr<FeatureDetector> detector = ORB::create(); //創建特徵檢測器
Ptr<DescriptorExtractor> descriptor = ORB::create(); //構建特徵描述器
// use this if you are in OpenCV2
// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming"); //暴力匹配法，漢明距離
//-- 第一步:檢測 Oriented FAST 角點位置
detector->detect(img_1, keypoints_1); //計算圖1，圖2的特徵點
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;
//BFMatcher matcher ( NORM_HAMMING );
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;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_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]); //刷選較好的匹配結果
}
}
}
Point2d pixel2cam(const Point2d &p, const Mat &K) {
return Point2d //返回歸一化後的相機座標（Xc,Yc,1）
(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0), //(u-u0)/fx
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1) //(v-v0)/fy
);
}
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1, //傳入圖像1，2的特徵點，以及刷選後的匹配
std::vector<KeyPoint> keypoints_2,
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>的形式
vector<Point2f> points1;
vector<Point2f> points2;
for (int i = 0; i < (int) matches.size(); i++) {
points1.push_back(keypoints_1[matches[i].queryIdx].pt); //將匹配點對中圖1特徵點ID對應的點存到向量points1
points2.push_back(keypoints_2[matches[i].trainIdx].pt); //將匹配點對中圖2特徵點ID對應的點存到向量points2
}
//-- 計算基礎矩陣
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT); //調用了opencv中計算基礎矩陣的接口，如果用eigen編寫估計比opencv的Mat實現快些；傳入匹配的2D點座標，用8點法計算
cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;
//-- 計算本質矩陣
Point2d principal_point(325.1, 249.7); //相機光心, TUM dataset標定值（在圖像平面中的像素位置）
double focal_length = 521; //相機焦距, TUM dataset標定值
Mat essential_matrix; //定義本質矩陣
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point); //根據匹配點，光軸，光心調用opencv的接口計算本質矩陣
cout << "essential_matrix is " << endl << essential_matrix << endl;
//-- 計算單應矩陣，單應矩陣主要處理目標點位於一個平面上
//-- 但是本例中場景不是平面，單應矩陣意義不大
Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3); //當目標點對多於4個點對時，用到RANSAC，由於單應矩陣是8自由度（9個位置量減去一個尺度不變性，一對點構建兩個等式，求8個位置參數需要至少4個點對）
cout << "homography_matrix is " << endl << homography_matrix << endl;
//-- 從本質矩陣中恢復旋轉和平移信息.
// 此函數僅在Opencv3中提供
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point); //輸出R,t
cout << "R is " << endl << R << endl;
cout << "t is " << endl << t << endl;
}
代碼中相機平移向量t=[Tx,Ty,Tz]T的矩陣表示爲S,其表達式爲：
其中findHomography() 函數形式和參數說明如下：
Mat cv::findHomography ( InputArray srcPoints,
InputArray dstPoints,
int method = 0,
double ransacReprojThreshold = 3,
OutputArray mask = noArray(),
const int maxIters = 2000,
const double confidence = 0.995 ）
參數詳解：
其中 recoverPose（）函數原型和參數解析如下：
如上，由2D-2D特徵點匹配計算E,F等，然後恢復R,t。接下來用三角測量則可以計算點的空間位置了。
三. 三角測量，空間點位置求解
單目SLAM中，無法由一張圖像獲取像素深度信息，需要用三角測量法估計。假設x1與x2爲兩個特徵點歸一化相機座標，則：
其中s1和s2是深度信息，兩邊同時左乘x2的反對稱矩陣可得：
其中R和t已經可以求出，則可直接求出s1，然後算出s2， 由於噪聲的存在，求出的R和t不一定使上式爲0，所以更常見的做法是求出最小二乘解而不是零解。
上上面式子可以寫成：
![\left [ -Rx_{1} ,x_{2}\right ]\begin{bmatrix} s_{1}\\ s_{2} \end{bmatrix}=t](https://pic1.xuehuaimg.com/proxy/csdn/https://private.codecogs.com/gif.latex?%5Cleft%20%5B%20-Rx_%7B1%7D%20%2Cx_%7B2%7D%5Cright%20%5D%5Cbegin%7Bbmatrix%7D%20s_%7B1%7D%5C%5C%20s_%7B2%7D%20%5Cend%7Bbmatrix%7D%3Dt)
即求Ax=b形式的最小二乘解問題，則
以下爲工程實踐：
在slambook2-master/ch7中新建個文件夾叫 triangulation，然後將triangulation.cpp和CMakeLists.txt拷貝進來，修改CMakeLists.txt的內容如下：
cmake_minimum_required(VERSION 2.8)
project(triangulation)
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
)
# # add_executable( triangulation triangulation.cpp extra.cpp) # use this if in opencv2
add_executable(triangulation triangulation.cpp)
target_link_libraries(triangulation ${OpenCV_LIBS})
然後終端進到這個目錄下，執行如下指令：
mkdir build
cd build
cmake ..
make
會在build中生成可執行文件triangulation，將1.png和2.png拷貝到build下，終端執行：
./triangulation 1.png 2.png
終端輸出結果如下：打印了81個圖1情況下目標點相對相機的距離Zc（即depth）
打印出匹配點 的距離值depth
triangulation.cpp 的源碼分析如下：
#include <iostream>
#include <opencv2/opencv.hpp>
// #include "extra.h" // used in opencv2
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( //位姿估計分裝成一個函數，得到相機的R,t
const std::vector<KeyPoint> &keypoints_1,
const std::vector<KeyPoint> &keypoints_2,
const std::vector<DMatch> &matches,
Mat &R, Mat &t);
void triangulation( //三角測量計算3D座標封裝成函數
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; //距離閾值區間[10,50]
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); //將像素座標轉爲歸一化後的相機座標，其Zc=1
int main(int argc, char **argv) {
if (argc != 3) {
cout << "usage: triangulation 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); //計算2D-2D的E,然後恢復得到R,t
//-- 三角化
vector<Point3d> points; //3D點向量
triangulation(keypoints_1, keypoints_2, matches, R, t, points); //傳入圖1,2的特徵點，刷選後的匹配結果，由本質矩陣恢復得到的R和t,輸出圖1中目標在其相機姿態中的3D實際相機座標（Xc,Yc,Zc）
//-- 驗證三角化點與特徵點的重投影關係
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; //圖1中第i和匹配的距離
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); //在圖1中畫出刷選後的匹配點（顏色與距離相關）
// 第二個圖
Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t; //根據公式x2=Rx1+t, 其中x1和x2都是相機座標，這個式子是將圖1中計算得到的P的實際座標投影到圖2中相機座標下；
float depth2 = pt2_trans.at<double>(2, 0); //理論計算得到P在圖2相機下的相機座標
cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2); //在圖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;
// used in OpenCV3
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
// use this if you are in OpenCV2
// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
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;
// BFMatcher matcher ( NORM_HAMMING );
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;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_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>的形式
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); //相機主點, 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); //輸出R,t，這個R,t計算的是圖2相對圖1的旋轉和平移
}
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); //這塊表示圖1的相機作爲參考，變換矩陣中沒有相機旋轉和平移
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)
); //T2中傳入R,t，即圖2相對圖1這個參考位置的變換矩陣
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; //圖1和圖1上的2D圖像像素點轉換後的歸一化相機座標點，對應的Zc=1,所以只要計算Xc和Yc
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); //傳入兩個圖像對應相機的變化矩陣，各自相機座標系下歸一化相機座標，輸出的3D座標是齊次座標，共四個維度，因此需要將前三個維度除以第四個維度以得到非齊次座標xyz
// 轉換成非齊次座標
for (int i = 0; i < pts_4d.cols; i++) { //遍歷所有的點，列數表述點的數量
Mat x = pts_4d.col(i); //x爲4x1維度
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); //將圖1測得的目標相對相機實際位置（Xc,Yc,Zc）存入points
}
}
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)
);
}
其中triangulatePoints 的函數原型爲：
void triangulatePoints(InputArray projMatr1, InputArray projMatr2, InputArray projPoints1, InputArray projPoints2, OutputArray points4D)
參數說明：
即需要第一個相機的變換矩陣，第2個相機的變換矩陣，圖1中計算得到的點，圖2中計算得到的點，返回重構的4xN 維度點集，其中N是點的數量。
通過三角測量及像素上的匹配點，可以求出路標即P點基於第一個相機座標系的三維座標點，然後通過對極約束求出的R，t可以求出在第二個相機座標系上路標P點的三維座標值。
三角平移是由平移得到的，有平移纔會有對極幾何約束的三角形。純旋轉是無法使用三角測量的，對極約束將永遠滿足。
在平移時，三角測量有不確定性，會引出三角測量的矛盾。這個矛盾就是：當平移很小時，像素上的不確定性將導致較大的深度不確定性，平移較大時，在相同的相機分辨率下，三角化測量將更精確。三角化測量的矛盾：平移增大，會導致匹配失效，平移太小，三角化精度不夠。
參考：
opencv相機標定與3D重構（官方文檔） 推薦好好看這篇官方手冊，對每個函數都有詳細解釋。