本文是《视觉SLAM十四讲》第7讲的个人读书笔记，为防止后期记忆遗忘写的。
本讲关注基于特征点 方式的视觉里程计算法。我们将介绍什么是特征点，如何提取和匹配特征点，以及如何根 据配对的特征点估计相机运动。
7.1 特征点法
前端也称为 视觉里程计（VO）。它根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。VO 的实现方法，按是否需要提取特征，分为特征点法的前端以及不提特征的直接法前端。优点：基于特征点法的前端，长久以来（直到现在）被认为是视觉里程计的主流方法。 它运行稳定，对光照、动态物体不敏感，是目前比较成熟的解决方案。
7.1.1 特征点
VO需要对图像提取特征点， 如何根据图像来估计相机运动。然后，在这些点（路标）的基础上，讨论相机位姿估计问题，以及这些点的定位问题。
特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。关键点 是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个 向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量 空间上的距离相近，就可以认为它们是同样的特征点。
普通 PC 的 CPU 还无法实时地计算 SIFT 特征，计算代价大。FAST 关键点适当降低精度和鲁棒性，计算特别快的一种特征点。ORB （Oriented FAST and Rotated BRIEF）特征它改进了 FAST 检测子不具有方向性的问题，并采用速度极快的二进制描述子 BRIEF，使整个图像特征提取的环节大大加速。ORB 在保持了特征子具有旋转，尺度不变性的同时，速度方面提升明显，对于实时性要求很高的 SLAM 来说是一个很好的选择。
7.1.2 ORB 特征
ORB 特征的关键点称为“Oriented FAST”，是 一种改进的 FAST 角点。它的描述子称为 BRIEF （Binary Robust Independent Elementary Features）。
FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思想是：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是角点。但是，FAST 特征点数量很大且不确定。其次，FAST 角点不具有方向信息。而且，由于它固定取半径为 3 的圆，存在尺度问题： 远处看着像是角点的地方，接近后看可能就不是角点了。
所以ORB改进了FAST，对原始 FAST 角点分别计算 Harris 响应值，然后选取前 N 个具有最大响应值的角点， 作为最终的角点集合。ORB 添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字 塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法实现的。从而，大大提升了FAST在不同图像之间 表述的鲁棒性。
BRIEF 描述子是一种二进制描述子。编码了关键点附近两个像素（比如说 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就 取 0。原始的 BRIEF 描述子在图像发生旋转时容易丢失。而 ORB 在 FAST 特征点提取阶段计算了关键 点的方向，使 ORB 的 描述子具有较好的旋转不变性。
通过对图像与图像，或者图像与地图之间的描述子进行准确的匹配，我们可以为后续的姿态估计，优化等操作减轻大量负担。然而，由于图像特征的局部特性，误匹配的情况广泛存在，目前已经成为视觉 SLAM 中制约性能提升的一大瓶颈。
如何匹配？测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示了两个特征之 间的相似程度对于浮点类型的描述子， 使用欧氏距离进行度量即可。而对于二进制的描述子（比如 BRIEF 这样的），我们往往使用汉明距离做为度量——两个二进制串之间的汉明距离，指的是它们不同位数的个数。
7.2 实践：特征提取和匹配
这里，我们筛选的依据是汉 明距离小于最小距离的两倍，这是一种工程上的经验方法，不一定有理论依据。
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
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 );
//-- 初始化图像匹配需要的相关对象
vector<KeyPoint> keypoints_1, keypoints_2; //特征点对象数组
Mat descriptors_1, descriptors_2;
Ptr<FeatureDetector> detector = ORB::create(); //检测器
Ptr<DescriptorExtractor> descriptor = ORB::create(); //描述子对象
// Ptr<FeatureDetector> detector = FeatureDetector::create(detector_name);
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create(descriptor_name);
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 );
//outimg1上标出特征点
Mat outimg1;
drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
imshow("ORB特征点",outimg1);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离,
vector<DMatch> matches;
//BFMatcher matcher ( NORM_HAMMING );
matcher->match ( descriptors_1, descriptors_2, matches );
//-- 第四步:匹配点对筛选
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;
printf ( "-- Max dist : %f \n", max_dist );
printf ( "-- Min dist : %f \n", min_dist );
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值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] );
}
}
//-- 第五步:绘制匹配结果
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;
}我们希望根据匹配的点对，估计相机的运动。然而，因为相机模型的不同，我们采用的方法也是不同的。
- 1. 当相机为单目时，问题是根据两组 2D 点估计运动。该问题用对极几何来解决。
- 2. 当相机为双目、RGB-D 时，或者我们通过某种方法得到了距离信息，那问题就是根据两组 3D 点估计运动，通常用 ICP 来解决。
- 3. 如果我们有 3D 点和它们在相机的投影位置，也能估计相机的运动。该问题通过 PnP 求解。
7.3 2D-2D: 对极几何
7.3.1 对极约束（重点理解）
设空间中一点P存在这么一种空间约束关系：
这时候相机中心点 O1, O2和 P 三个点可以 确定一个平面，称为极平面。O1 O2 连线与像平面 I1, I2 的交点分别 为 e1, e2。e1, e2，称为极点，O1 O2 被称为基线。称极平面与 两个像平面 I1, I2 之间的相交线 l1, l2 为极线。连线e2p2（也就是第二个图像中的极线）就是 P 可能出现的投影 的位置，也就是射线 O1p1 在第二个相机中的投影（极线的几何意义）。
设 P = [X, Y, Z] T。根据第 5 讲，两个像素点 p1, p2 的像素位置
这里 K 为相机内参矩阵，R, t 为两个坐标系的相机运动。使用齐次坐标形式表示，则为：
下面进行对对极几何的推导过程：
-------》
t ∧ x2 是一个与 t 和 x2 都垂直的向量。把它再和 x2 做内积时，将得 到 0。因此：
（重要）其中P1和P2是像素坐标，x1和x2是相机归一化坐标。
公式（7.9）便是对极约束。所以，怎么求出相机的变化外参：R和t呢？分成两步来解：
- 找出多对正确的匹配像素点，带入公式（7.9--3）求出E和F；
- 根据（7.9--1 和 7.9---2）求出R和t。
7.3.2 本质矩阵
E 具有五个自由度的事实，表明我们最少可以用五对点来求解 E。但是，E 的内在 性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等 价性，使用八对点来估计 E——这就是经典的八点法。八点法只利用了 E 的线性性质，因此可以在线性代数框架下求解。
------》单个方程
-----》求解目标方程组
通过上述方程组求出E矩阵，然后根据
求出R和t。接下来的问题是如何根据已经估得的本质矩阵 E，恢复出相机的运动 R, t。这个过程 是由奇异值分解（SVD）得到的。
在进行奇异分解过程中，发现一个E对应4个可能解，但是只要通过深度验证，便可求出真正的唯一解。
7.3.3 单应矩阵
单应矩阵H 描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙，地面等）（无人机始终俯视大地），则可以通过单应性来进行运动估计。
考虑在图 像 I1 和 I2 有一对匹配好的特征点 p1 和 p2。这些特征点落在某平面上。设这个平面满足方程（两点都满足这个方程）：
根据（7.8）求解出H的过程和上述求解本质矩阵的过程一致。通过对H进行分解，便可求出R和t（不赘述）。
单应性在 SLAM 中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩 阵的自由度下降，这就出现了所谓的退化。现实中的数据总包含一些噪声，这时候如果我们继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 F 和单应矩阵 H，选择重投影误差比较小的那个作为最终的运动估计矩阵。
7.4 实践：对极约束求解相机运动
下面的代码摘抄之《SLAM十四讲》7.4节。
代码思路：根据7.2节的思路，找出图片中匹配ORB特征点对----》利用8点法求出本质矩阵----》对本质矩阵进行分解，求出R和t---》对求得的结果R和t进行验证。
#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 );
//根据匹配点使用对极约束求R和t
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<<"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 );
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<<"验证E=t^R*scale 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 = 0? d=" << d << endl;
}
return 0;
}
// 像素坐标转 相机归一化坐标
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 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 ( std::vector<KeyPoint> keypoints_1, 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 );
points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
}
//-- 传递匹配像素点对,计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT ); //8点法
cout<<"1. 计算基础矩阵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 );
cout<<"2. 计算本质矩阵essential_matrix is "<<endl<< essential_matrix<<endl;
//-- 计算单应矩阵
Mat homography_matrix;
homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
cout<<"3. 计算单应矩阵homography_matrix is "<<endl<<homography_matrix<<endl;
//-- 从本质矩阵中恢复旋转和平移信息.
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
cout<<"4. 从本质矩阵中恢复旋转 R is "<<endl<<R<<endl;
cout<<"5. 从本质矩阵中恢复平移信息 t is "<<endl<<t<<endl;
}从 E,F 和 H 都可以分解出运动，不过 H 需 要假设特征点位于平面上。对于本实验的数据，这个假设是不好的，所以我们这里主要用 E 来分解运动。
尺度不确定性：
例 如，程序中输出的 t 第一维约 0.822。这个 0.822 究竟是指 0.822 米呢，还是 0.822 厘米 呢，我们是没法确定的。换言之， 在单目 SLAM 中，对轨迹和地图同时缩放任意倍数，我们得到的图像依然是一样的。
初始化的纯旋转问题：
从 E 分解到 R, t 的过程中，如果相机发生的是纯旋转，导致 t 为零，那么，得到的 E 也将为零，这将导致我们无从求解 R。不过，此时我们可以依靠 H 求取旋转，但仅有 旋转时，我们无法用三角测量估计特征点的空间位置，于是，另一个 结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，单目将 无法初始化。因而有经验的 SLAM 研究人员，在单目 SLAM 情况下，经常选择让相机进行左右平 移以顺利地进行初始化。
多于八对点的情况：
不过，当可能存在误匹配的情况时，我们会更倾向于使用随机采样一致性来求，而不是最小二乘。
7.5 三角测量
在单目 SLAM 中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量或三 角化的方法来估计地图点的深度。在 SLAM 中，我们主要用三角化来估计像素点的距离。
按照对极几何中的定义，设 x1, x2 为两个特征点的归一化坐标，那么它们满足：
我们已经知道了 R, t，想要求解的是两个特征点的深度 s1, s2。如果要算 s2，那么先对上式两侧左乘一个 x ∧（X1和X2是相机坐标），得： 
该式左侧为零，右侧可看成 s2 的一个方程，可以根据它直接求得 s2。有了 s2，s1 也 非常容易求出。于是，我们就得到了两个帧下的点的深度，确定了它们的空间坐标。当然， 由于噪声的存在，我们估得的 R, t，不一定精确使式为零，所以更常见的做法求最小二乘解而不是零解。
7.6 实践：三角测量
#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" // 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 (
const std::vector<KeyPoint>& keypoints_1, const std::vector<KeyPoint>& keypoints_2,
const std::vector< DMatch >& matches,
Mat& R, Mat& t );
void triangulation (
const vector<KeyPoint>& keypoint_1, const vector<KeyPoint>& keypoint_2,
const std::vector< DMatch >& matches,
const Mat& R, const Mat& t,
vector<Point3d>& points
);
// 像素坐标转相机归一化坐标
Point2f pixel2cam( const Point2d& p, const Mat& K );
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 ); //keypoints_1 keypoints_2 像素坐标
//-- 三角化
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 );
for ( int i=0; i<matches.size(); i++ )
{
Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );//像素坐标转相机归一化坐标
Point2d pt1_cam_3d(
points[i].x/points[i].z,
points[i].y/points[i].z
);
cout<<"point in the first camera frame: "<<pt1_cam<<endl;
cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points[i].z<<endl; //从3D投射的点
// 第二个图 像素坐标转相机归一化坐标
Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t; //s2*x2-->S1*R*x1+t
cout<<"point in the second camera frame: "<<pt2_cam<<endl;
double distance=pt2_trans.at<double>(2,0);
pt2_trans /= pt2_trans.at<double>(2,0);
cout<<"point reprojected from second frame: "<<pt2_trans.t()<<", d="<<distance<<endl; //从第二帧重新投射的点：
cout<<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 );
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 );
}
//-- 计算基础矩阵
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 ); //相机主点, TUM dataset标定值
int focal_length = 521; //相机焦距, TUM dataset标定值
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 "<<endl<<R<<endl;
cout<<"t is "<<endl<<t<<endl;
}
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)
);
}
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 )
{
//第一台摄像机的3x4投影矩阵
Mat T1 = (Mat_<float> (3,4) <<
1,0,0,0,
0,1,0,0,
0,0,1,0);
//第二台摄像机的3x4投影矩阵。
Mat T2 = (Mat_<float> (3,4) <<
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0)
);
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point2f> pts_1, pts_2;
for ( DMatch m:matches )
{
// 将像素坐标转换至相机归一化坐标(X1 and X2)
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; //均匀坐标中的4xN重建点阵列,每一列是4维度齐次坐标
cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );//s1*T1*p1=s2*T2*p2 T1没有实际坐标变换作用
// 转换成非齐次坐标
for ( int i=0; i<pts_4d.cols; i++ )
{
Mat x = pts_4d.col(i);
x /= x.at<float>(3,0); // 根据第4维度归一化-->求出s1-->求出P1相机坐标(这里不是归一化平面的相机坐标)
Point3d p (
x.at<float>(0,0),
x.at<float>(1,0),
x.at<float>(2,0)
);
points.push_back( p );
}
}
存在的矛盾：
三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈的上三角测量。因 此，纯旋转是无法使用三角测量的，因为对极约束将永远满足。当平移很小时，像素上的不确定性将导致较大的深度不确定性。从几何关系可以看到，当 t 较大时，δd 将明显变小，这说明平移较大时， 在同样的相机分辨率下，三角化测量将更精确。
因此，要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率 ——但这会导致图像变大，提高计算成本。另一方式是使平移量增大。但是，平移量增大，会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来了，比如反射光 发生变化了，等等。外观变化会使得特征提取与匹配变得困难。总而言之，在增大平移，会 导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
