小白（我）本着学习，提高自我，增加知识的想法，决定认真分析高翔博士slam。在此立下一个flag：
特征点是由关键点和描述子两部分组成，关键点是指该特征点在图像里面的位置，描述子通常是一个向量，描述了该关键点周围像素的信息。到目前为止ORB特征则是目前非常具有代表性的实时图像特征。
ORB特征由关键点和描述子两部分组成，因此提取ORB特征分为两个步骤：
1、FAST角点提取；2、BRIEF描述子。
FAST角点不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法实现的。
特征匹配解决了SLAM中的数据关联问题，即确定当前看到的路标与之前看到的路标之间的对应关系。
汉明距离：表示了两个特征之间的相似程度。
实践：特征提取和匹配（代码详解）
#include <iostream>
//core模块包含了OpenCV库的基础结构以及基本操作
#include <opencv2/core/core.hpp>
//features2d包含了用于检测，描述以及匹配特征点的算法
#include <opencv2/features2d/features2d.hpp>
//highgui模块包含可以用来显示图像或者简单的输入的用户交互函数
#include <opencv2/highgui/highgui.hpp>
using namespace std;
using namespace cv;
int main(int argc, char ** argv) {
//argc :表示可执行程序输入参数的个数，argv：表示输入参数的字符串名称
//argc为1时，argv[0]是程序本身（默认的）
if(argc != 3)
{
cout << "usage: feture_ectraction 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);
//--初始化(为什么要定义以下变量，请查看detect()函数和compute()函数）
std::vector<KeyPoint> keypoints_1, keypoints_2;
Mat descriptors_1,descriptors_2;
Ptr<ORB> orb = ORB::create();//请查看ORB的构造函数
//Ptr<ORB> orb = ORB::create(500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE, 31,20);
//ORB::create都是默认值
//--第1步：检测Oriented FAST角点位置（并存储到vector中）
orb -> detect (img_1, keypoints_1);
orb -> detect (img_2, keypoints_2);
//--第2步：根据角点位置计算BRIEF描述子
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);
//-- 第3步：对两幅图像中的BRIEF描述子进行匹配，使用汉明距离
vector<DMatch> matches;
BFMatcher matcher (NORM_HAMMING);
matcher.match(descriptors_1, descriptors_2, matches);
//--第4步：匹配点对筛选
double min_dist = 10000, 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;
}
/*
min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
*/
cout << "-- Max dist : " << max_dist << endl;
cout << "-- Min dist : " << min_dist << endl;
//当描述子之间的距离大于两倍的最小距离时，即认为匹配有误。
//但有时候最小距离会非常小，设置一个经验值30作为下限。
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]);
}
}
//注意注意注意：筛选的依据是汉明距离小于最小距离的两倍，这是一种工程上的经验的方法，不一定有理论依据
//尽管在示例图像中能够筛选出正确的匹配，但我们仍然不嫩高保真挂在所有其他图像中得到的匹配都是正确的。
//因此，在后面的运动估计中，还需要使用去除误匹配的算法。
//--第5步：绘制匹配结果
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("所有匹配点对", img_match);
imshow("优化后匹配点对", img_goodmatch);
waitKey(0);
return 0;
}
cmake_minimum_required(VERSION 3.16)
project(feature2d)
set(CMAKE_CXX_STANDARD 11)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(feature2d main.cpp)
target_link_libraries(feature2d ${OpenCV_LIBS})
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d_c.h>
#include <cv.hpp>
using namespace std;
using namespace cv;
// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );
//两张图像得到关键点（keypoints）和描述子之间匹配关系（matches）
void find_feature_matches(
const Mat& img_1, const Mat& img_2,
vector<KeyPoint>& keypoints_1,
vector<KeyPoint>& keypoints_2,
vector<DMatch>& matches
);
//从特征点(keypoints)和匹配关系（matches）求解位姿(R,t)
void pose_estimation_2d2d(
const vector<KeyPoint> keypoints_1,
const vector<KeyPoint> keypoints_2,
const vector<DMatch> matches,
Mat& R, Mat& t
);
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);
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);
//--验证 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);
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 << "epipolar constraint = " << d << endl;
}
return 0；
}
void find_feature_matches(
const Mat& img_1, const Mat& img_2,
vector<KeyPoint>& keypoints_1,
vector<KeyPoint>& keypoints_2,
vector<DMatch>& matches
){
//--初始化
Mat descriptors_1, descriptors_2;
Ptr<ORB> orb = ORB::create();
//-- 第1步：检测Oriented FAST角点位置
orb -> detect( img_1, keypoints_1);
orb -> detect( img_2, keypoints_2);
//-- 第2步：根据角点位置计算BRIEF描述子
orb -> compute(img_1, keypoints_1, descriptors_1);
orb -> compute(img_2, keypoints_2, descriptors_2);
//--第3步：对两幅图像中的BRIEF描述子进行匹配，使用汉明距离
vector<DMatch> ngmatches;
BFMatcher matcher(NORM_HAMMING);
matcher.match(descriptors_1, descriptors_2, ngmatches);
//--第4步：匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离，即最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++){
double dist = ngmatches[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);
for (int i = 0; i < descriptors_1.rows; i++)
{
if (ngmatches[i].distance <= max( 2 * min_dist, 30.0))
{
matches.push_back(ngmatches[i]);
}
}
}
void pose_estimation_2d2d(
const vector<KeyPoint> keypoints_1,
const vector<KeyPoint> keypoints_2,
const vector<DMatch> matches,
Mat& R, Mat& t
){
// 相机内参， TUM Freiburg2
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 249.7, 0, 0, 1);
//-- 对齐匹配的点对，并用.pt转化为像素坐标。把匹配点转换为简单的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);//queryIdx第一个图像索引
points2.push_back( keypoints_2[matches[i].trainIdx].pt);//trainIdx第二个图像索引
}
//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat( points1, points2, CV_FM_8POINT);
cout << "fundamental_matrix is" << endl << fundamental_matrix << endl;
//-- 计算本质矩阵
Point2d principal_point(325.1, 249.7); // 光心，ＴＵＭ ｄａｔａｓｅｔ标定值
int 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);
cout << "homography_matrix is " << endl << homography_matrix << endl;
//-- 从本质矩阵中恢复旋转矩阵和平移信息
recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
cout << "R is " << endl << R << endl;
cout << "t is " << endl << t << endl;
}
//像素坐标系p到相机坐标系x
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 )
);
}
像素坐标系p到相机坐标系的转换函数
注意：最后得到的坐标是归一化平面上的坐标（位于相机前方z=1出的平面）
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 )
);
}
//函数原型
Mat findFundamentalMat(InputArray points1, InputArray points2, int method = FM_RANSAC, double ransacReprojThreshold = 3.,
double confidence = 0.99, OutputArray mask = noArray())
//函数原型
Mat findEssentialMat(InputArray points1, InputArray points2, double focal = 1.0, Point_<double> pp = Point2d(0, 0),
int method = RANSAC, double prob = 0.999, double threshold = 1.0, OutputArray mask = noArray())
//函数原型
Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method = 0, double ransacReprojThreshold = 3,
OutputArray mask = noArray(), const int maxIters = 2000, const double confidence = 0.995)
int recoverPose(InputArray E, InputArray points1, InputArray points2, OutputArray R, OutputArray t, double focal = 1.0, Point_<double> pp = Point2d(0, 0), InputOutputArray mask = noArray())
从E分解到R,t的过程，如果相机发生的是纯旋转，导致t为零，那么，得到的E也将为零，这将导致我们无法求解R。不过，此时我们可以依靠H求取像旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置。
结论：单目初始化不能只有纯旋转，必须要有一定程度的平移。
在实践当中，如果初始换时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。相对地，如果把相机左右移动而不是原地旋转，就容易让单目SLAM初始化。
当可能存在误匹配的情况时，更倾向于使用随机采样一致性（Random Sample Concensus, RANSAC）来求，这个方法是一种通用的方法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。
三角测量（高斯提出）是指，通过在两处观察同一个点的夹角，从而确定该点的距离。
三角化的矛盾：增大平移，会导致匹配失效，平移太小，则三角化精度不够
void triangulation(
const vector< KeyPoint > & keypoints_1,
const vector< KeyPoint > & keypoints_2,
const vector< DMatch > & matches,
const Mat & R, const Mat & t,
vector<Point3d> & points
){
Mat T1 = (Mat_<double> (3,4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);//这块表示图1的相机作为参考，变换矩阵中没有相机旋转和平移
Mat T2 = (Mat_<double> (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<Point2d> pts_1, pts_2;
//图1和图2上的2D图像像素点转换后的归一化相机坐标点，对应的Zc=1,所以只要计算Xc和Yc
for (DMatch m: matches)
{
//--将像素坐标转换至相机坐标
pts_1.push_back( pixel2cam( keypoints_1[m.queryIdx].pt, K));
pts_2.push_back( pixel2cam( keypoints_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
}
}
PnP(Perspective-n-Point) 是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
如果两张图像中其中一张特征点的3D位置已知，那么最少只需3个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此，在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。3D-2D方法不要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。
为了求解PnP，我们利用了三角形相似性质，P3P存在的问题：
在SLAM当中，通常的做法是先使用P3P/EPnP等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。