本文为视觉 SLAM 学习笔记,讲解视觉里程计中的特征点法。
本讲内容概要
- 图像特征的意义,在单幅及多幅图像中提取特征点。
- 对极几何的原理,利用对极几何的约束恢复图像间相机的三维运动
- PnP 问题,利用已知三维结构与图像的对应关系求解相机的三维运动
- ICP 问题,利用点云的匹配关系求解相机的三维运动
- 通过三角化获得二维图像上对应点的三维结构
特征点法
经典 SLAM 模型中以位姿——路标(Landmark)来描述 SLAM 过程。我们在经典 SLAM 系统中说的运动方程是位姿与位姿之间的关系,而观测方程说的是位姿与路标的关系。
路标是三维空间中固定不变的点,且需满足以下的特点:
- 数量充足,以实现良好的定位
- 较好的区分性,以实现数据关联
在视觉 SLAM 中,·通常利用图像特征点作为 SLAM 中的路标,称为特征法。
特征点
特征点是图像中比较有代表性的部分,例如下图中的角点、边缘、区块:
好的特征点需要满足以下几个特点:
- 可重复性:在不同地方观察同一特征点,相差不应该过大
- 可区分性:不同特征点应该易于辨别
- 高效:特征点的提取和匹配效率要高
- 本地:特征只与图像局部性质有关
我们很容易看出,区分性排序为:角点>边缘>区块。因此在视觉 SLAM 中,我们通常使用角点作为特征点,边缘在某些情况下会用到,区块一般不用。
特征点的信息
特征点自己的信息有位置、大小、方向、评分等,称为关键点。
我们一般不通过特征点本身来区分特征点,而是通过点周围的图像来区分,因为同一特征点可能因为光照等因素的变化亮度差很大,误被识别为不同特征点。特征点周围的图像信息被称为描述子。描述子随着相机角度或光照的变化而变化不大。
当我们要找到表现好的描述子,其计算量会变大。SIFT 具有平移、缩放和旋转的不变性,性能高但计算量很大。比如 ORB 描述子为简单描述子,只有平移或缩放(尺度)不变性,性能不高但计算量小,可以满足实时性。目前 ORB 描述子在 SLAM 中是一种很好的描述。
如果你对各种关键点和描述子感兴趣,建议参考 OpenCV features2d 模块。
ORB 特征
因为 ORB 是 SLAM 中较为成功的一种描述,我们以它为代表介绍特征。SIFT 的相关资料已经有很多介绍,可自行查阅。
ORB 的关键点是一个 Oriented FAST,即带旋转的 FAST ,其描述也是带旋转的 BRIEF。FAST 和 BRIEF 都是在特征描述中属于较为简单实时性好,但精度不好的方式,SLAM 中图像位移不大时也是可以匹配到的。
FAST 是一种关键点。其思想为:以一个点为中心,周围取像素,如果连续有 n 个点的灰度值与中心点相差一个阈值以上,我们就认为该点为关键点。这种方式计算量很小,只是比大小,因此提取 FAST 点速度很快,还可以使用一些加速手段。如取中心点周围 10 个点进行比较,则称为 FAST10。但是这样提取到 FAST 点太多,我们还需要网格等计算评分来筛选出好的特征点,然后特征点就可以使用了。
但是这样提出的 FAST 只是一个位置,我们还需要计算一个 FAST 旋转。旋转相当于图像的重心,如果左边为暗右边为亮则指向右边,最后计算得到的是图像梯度指向的角度。旋转的计算过程如下:
-
在一个小的图像块 B B B 中,定义图像块的矩为:
m p q = ∑ x , y ∈ B x p y q I ( x , y ) , p , q = { 0 , 1 } m_{pq}=\sum\limits_{x,y∈B}x^py^qI(x,y),\quad p,q=\{0,1\} mpq=x,y∈B∑xpyqI(x,y),p,q={ 0,1}
-
通过矩可以找到图像块的质心:
C = ( m 10 m 00 , m 01 m 00 ) C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}) C=(m00m10,m00m01) -
连接图像块的几何中心 O O O 与质心 C C C,得到一个方向向量 O C ⃗ \vec{OC} OC,于是特征点的方向可以定义为:
θ = a r c t a n ( m 01 / m 10 ) \theta=arctan(m_{01}/m_{10}) θ=arctan(m01/m10)
FAST 只有平移不变性和旋转不变性,但没有尺度(缩放)不变性。尺度不变性为:当从远处和近处看向 FAST 是否还是角点,或者分辨率不同的情况下 FAST 角点还是不是角点。解决这个问题,我们一般会将图像做一个图像金字塔。最底层为图像最原始的分辨率,上面几层为原始图像缩小后的图像,最终构成一个分辨率不同大小的图像为金字塔。我们可以给每层提取一个 FAST,这样就获得了各种尺度下的 FAST 特征,我们可认为其具有一定程度上的尺度不变性。
BRIEF
BRIEF 是一种二进制的描述子,如 BRIEF-128 为 128 位,每一位表示附近每个点对 A、B 的大小关系,使用汉明距离进行计算。BRIEF 描述了附近图像的信息,可作为 FAST 点的描述。A、B 点对的选取可随机均匀分布、满足高斯分布或满足特定 pattern。已有文献对不同的选取方式进行了测试,结果相差不大,我们可以随机选取。在选定某种固定的 pattern 后不能再变,否则失去了比较的意义。
在计算描述的时候,需要先将图像按照 FAST 中计算的角度进行旋转,再去进行比较,称为旋转后的 BRIEF。下图为 ORB-SLAM 的 pattern:
特征匹配
我们已经有了两张图的特征点,现在需要通过判断描述子的差异来判断哪些特征为同一个点。
最简单的特征匹配方法为暴力匹配,即 A 中一特征点与 B 中所有特征点之间计算描述子的距离,哪个最小就匹配哪个。描述子距离表示了两特征间的相似程度,可取不同的距离量度范数。对于浮点类型的描述子,使用欧氏距离,对于二进制的描述子(如 BRIEF),使用汉明距离。
当想要某帧和一张地图时,暴力匹配运算量过大,需要**快速近似最近邻(FLANN)**算法来加速。因为其过程较为复杂,且实现已经集成到 OpenCV,这里不进行介绍,如果感兴趣可自行百度。
实践:特征提取和匹配
OpenCV 的安装见 Ubuntu 安装 OpenCV。
首先读取两张图像,在这两张图像间进行特征匹配:
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 );
然后申请关键点、描述子和匹配:
//-- 初始化
std::vector<KeyPoint> keypoints_1, keypoints_2;
Mat descriptors_1, descriptors_2;
Ptr<FeatureDetector> detector = ORB::create(); // 在这里可以设置提取特征点的数量
Ptr<DescriptorExtractor> descriptor = ORB::create();
// 不能使用欧氏距离,要声明汉明距离
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 );
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作为下限.
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 ( "所有匹配点对", img_match );
imshow ( "优化后匹配点对", img_goodmatch );
waitKey(0);
如果此时发生报错:
pose_estimation_3d2d.cpp:151:50: error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::BlockSolver(g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> >::LinearSolverType*&)’
Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
^
In file included from /usr/local/include/g2o/core/block_solver.h:199:0,
from /home/xin/Slam/slambook/slambook-master/ch7/pose_estimation_3d2d.cpp:10:
出现这种错误的原因是 g2o 的新旧版本间指针类型不匹配。我们需要进行修改。
在 pose_estimation_3d2d.cpp 中,第 151-152 行,修改前代码如下:
Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
修改后代码(修改的是 151-152 行):
Block* solver_ptr = new Block ( std::unique_ptr<Block::LinearSolverType>(linearSolver) ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );
在 pose_estimation_3d3d.cpp 中,第 279-280 行,修改前代码如下:
Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
修改后代码:
Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) ); // 矩阵块求解器
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::unique_ptr<Block>(solver_ptr) );
最后在 1.png, 2.png 下执行:
./build/feature_extraction 1.png 2.png
提取的特征点如下图,可见特征点都集中在灰度变化明显的地方:
暴力匹配得到的匹配结果,存在大量误匹配:
经过筛选之后,剩下的匹配大多都是正确的匹配:
特征点匹配后,得到了特征点之间的关系:
- 若只有两个单目图像,得到 2D-2D 间的关系——对极约束
- 若匹配的是帧和地图,得到 3D-2D 间的关系——PnP
- 若匹配的是 RGB-D 图,得到 3D-3D 间的关系——ICP
2D-2D:对极几何
对极约束
现在,我们以及找到了很对匹配的点,下面为其中的一个点对:
几何关系:
-
两个相机中心分别为 O 1 , O 2 O_1,O_2 O1,O2, P P P 在两个图像的投影为 p 1 , p 2 p_1,p_2 p1,p2,连线 O 1 p 1 ⃗ , O 2 p 2 ⃗ \vec{O_1p_1},\vec{O_2p_2} O1p1,O2p2 在三维空间中相交于点 P P P,为路标点
-
l 1 l_1 l1 为 O 2 p 2 ⃗ \vec{O_2p_2} O2p2 在平面 I 1 I_1 I1 的投影,称为极线, e 1 , e 2 e_1,e_2 e1,e2 称为极点
-
两个相机之间的变换为 T 12 T_{12} T12
实践中:
- p 1 , p 2 p_1,p_2 p1,p2 通过特征匹配得到, P P P 未知, e 1 , e 2 e_1,e_2 e1,e2 未知, T 12 T_{12} T12 待求。
- 这是一个通过匹配点建图和求解位姿的问题,是经典的 SLAM 问题
我们设世界坐标为 P = [ X , Y , Z ] T P=[X,Y,Z]^T P=[X,Y,Z]T。由相机模型,两个像素点 p 1 , p 2 p_1,p_2 p1,p2 的像素位置为:
s 1 p 1 = K P , s 2 p 2 = K ( R P + t ) s_1p_1=KP, \quad s_2p_2=K(RP+t) s1p1=KP,s2p2=K(RP+t)
使用归一化坐标(去掉内参):
x 1 = K − 1 p 1 , x 2 = K − 1 p 2 x_1=K^{-1}p_1, \quad x_2=K^{-1}p_2 x1=K−1p1,x2=K−1p2
x 1 , x 2 x_1,x_2 x1,x2 是两个像素点的归一化平面上的坐标,带入上式得齐次关系:
x 2 = R x 1 + t x_2=Rx_1+t x2=Rx1+t
两侧左乘 t ^ \hat t t^,相当于两边同时与 t t t 叉乘:
t ^ x 2 = t ^ R x 1 \hat t x_2=\hat t Rx_1 t^x2=t^Rx1
两边左乘 x 2 T x_2^T x2T:
x 2 T t ^ x 2 = x 2 T t ^ R x 1 x_2^T\hat t x_2=x_2^T\hat t Rx_1 x2Tt^x2=x2Tt^Rx1
因为 t ^ x 2 \hat t x_2 t^x2 是一个与 t t t 和 x 2 x_2