视觉slam学习之——ch7 视觉里程计（centos系统）
这个章节主要讲解：
图像特征提取；
多幅图像匹配特征点；
对极几何；
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
#include
#include //特征提取必须的库
#include
#include //计时用
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 keypoints_1, keypoints_2; //两张图像的特征点向量
Mat descriptors_1, descriptors_2; //特征描述器
Ptr detector = ORB::create(); //构建ORB特征检测器
Ptr descriptor = ORB::create(); //构建ORB特征描述器
Ptr 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 time_used = chrono::duration_cast>(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 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>(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 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
#include
#include
#include
#include
// #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 &keypoints_1, //特征点向量
std::vector &keypoints_2,
std::vector &matches); //匹配向量
void pose_estimation_2d2d( //位姿估计封装成一个函数
std::vector keypoints_1,
std::vector keypoints_2,
std::vector 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 keypoints_1, keypoints_2;
vector 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_(3, 3) << 0, -t.at(2, 0), t.at(1, 0),
t.at(2, 0), 0, -t.at(0, 0),
-t.at(1, 0), t.at(0, 0), 0);
cout << "t^R=" << endl << t_x * R << endl; //输出平移向量和旋转矩阵的外积
//-- 验证对极约束
Mat K = (Mat_(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_(3, 1) << pt1.x, pt1.y, 1); //点pt1的归一化相机坐标，3x1维度
Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
Mat y2 = (Mat_(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 &keypoints_1,
std::vector &keypoints_2,
std::vector &matches) {
//-- 初始化
Mat descriptors_1, descriptors_2;
// used in OpenCV3
Ptr detector = ORB::create(); //创建特征检测器
Ptr descriptor = ORB::create(); //构建特征描述器
// use this if you are in OpenCV2
// Ptr detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr 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 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(0, 2)) / K.at(0, 0), //(u-u0)/fx
(p.y - K.at(1, 2)) / K.at(1, 1) //(v-v0)/fy
);
}
void pose_estimation_2d2d(std::vector keypoints_1, //传入图像1，2的特征点，以及刷选后的匹配
std::vector keypoints_2,
std::vector matches,
Mat &R, Mat &t) {
// 相机内参,TUM Freiburg2
Mat K = (Mat_(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
//-- 把匹配点转换为vector的形式
vector points1;
vector 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](http://img.e-com-net.com/image/info8/13194b96c28c4c5e8265ffee48d01431.gif){kind=small}
即求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
#include
// #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 &keypoints_1,
std::vector &keypoints_2,
std::vector &matches);
void pose_estimation_2d2d( //位姿估计分装成一个函数，得到相机的R,t
const std::vector &keypoints_1,
const std::vector &keypoints_2,
const std::vector &matches,
Mat &R, Mat &t);
void triangulation( //三角测量计算3D坐标封装成函数
const vector &keypoint_1,
const vector &keypoint_2,
const std::vector &matches,
const Mat &R, const Mat &t,
vector &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 keypoints_1, keypoints_2;
vector 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 points; //3D点向量
triangulation(keypoints_1, keypoints_2, matches, R, t, points); //传入图1,2的特征点，刷选后的匹配结果，由本质矩阵恢复得到的R和t,输出图1中目标在其相机姿态中的3D实际相机坐标（Xc,Yc,Zc）
//-- 验证三角化点与特征点的重投影关系
Mat K = (Mat_(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_(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(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 &keypoints_1,
std::vector &keypoints_2,
std::vector &matches) {
//-- 初始化
Mat descriptors_1, descriptors_2;
// used in OpenCV3
Ptr detector = ORB::create();
Ptr descriptor = ORB::create();
// use this if you are in OpenCV2
// Ptr detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr 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 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 &keypoints_1,
const std::vector &keypoints_2,
const std::vector &matches,
Mat &R, Mat &t) {
// 相机内参,TUM Freiburg2
Mat K = (Mat_(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
//-- 把匹配点转换为vector的形式
vector points1;
vector 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_1,
const vector &keypoint_2,
const std::vector &matches,
const Mat &R, const Mat &t,
vector &points) {
Mat T1 = (Mat_(3, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0); //这块表示图1的相机作为参考，变换矩阵中没有相机旋转和平移
Mat T2 = (Mat_(3, 4) <<
R.at(0, 0), R.at(0, 1), R.at(0, 2), t.at(0, 0),
R.at(1, 0), R.at(1, 1), R.at(1, 2), t.at(1, 0),
R.at(2, 0), R.at(2, 1), R.at(2, 2), t.at(2, 0)
); //T2中传入R,t，即图2相对图1这个参考位置的变换矩阵
Mat K = (Mat_(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); //相机内参
vector 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(3, 0); // 归一化
Point3d p(
x.at(0, 0),
x.at(1, 0),
x.at(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(0, 2)) / K.at(0, 0),
(p.y - K.at(1, 2)) / K.at(1, 1)
);
}
其中triangulatePoints 的函数原型为：
void triangulatePoints(InputArray projMatr1, InputArray projMatr2, InputArray projPoints1, InputArray projPoints2, OutputArray points4D)参数说明：
即需要第一个相机的变换矩阵，第2个相机的变换矩阵，图1中计算得到的点，图2中计算得到的点，返回重构的4xN 维度点集，其中N是点的数量。
通过三角测量及像素上的匹配点，可以求出路标即P点基于第一个相机坐标系的三维坐标点，然后通过对极约束求出的R，t可以求出在第二个相机坐标系上路标P点的三维坐标值。
三角平移是由平移得到的，有平移才会有对极几何约束的三角形。纯旋转是无法使用三角测量的，对极约束将永远满足。
在平移时，三角测量有不确定性，会引出三角测量的矛盾。这个矛盾就是：当平移很小时，像素上的不确定性将导致较大的深度不确定性，平移较大时，在相同的相机分辨率下，三角化测量将更精确。三角化测量的矛盾：平移增大，会导致匹配失效，平移太小，三角化精度不够。
参考：
findHomography()函数详解
opencv相机标定与3D重构（官方文档） 推荐好好看这篇官方手册，对每个函数都有详细解释。