SLAM前端知识汇总

最新推荐文章于 2024-03-03 19:31:51 发布

家门Jm

最新推荐文章于 2024-03-03 19:31:51 发布

阅读量1k

点赞数 1

分类专栏： SLAM面试

本文链接：https://blog.csdn.net/qq_38167930/article/details/119379327

版权

SLAM面试专栏收录该内容

12 篇文章 27 订阅

订阅专栏

1、常用特征点汇总

1.1 Fast特征点

《SLAM14讲》中LK光流中用的特征点就是Fast特征点，然后对这些特征点进行光流追踪。

在8.3.2节的LK光流代码，第1帧提取的就是Fast特征点，注意代码的写法：

if ( 0 == index )
{
    // “只”对第一帧提取FAST特征点
    vector<cv::KeyPoint> kps;
    cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
    
    detector->detect(color, kps);
    for (auto kp : kps)
        keypoints.push_back(kp.pt);

    last_color = color;

    continue;
}

Fast特征点的原理如下：

FAST特征是基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，若该像素点圆形邻域圆周上有3/4的点和该像素点灰度值差足够大，则认为该像素点为一个特征点。如下入所示：

FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思
想是：如果一个像素与它邻域的像素差别较大（过亮或过暗） , 那它更可能是角点。相比于
其他角点检测算法， FAST 只需比较像素亮度的大小，十分快捷。它的检测过程如下：

Fast特征点也有一些加速操作，比如：

（1）在 FAST-12 算法中，为了更高效，可以添加一项预测试操作，以快速地排除绝大多数不是角点的像素。具体操作为，对于每个像素，直接检测邻域圆上的第 1， 5， 9， 13 个像素的亮度。只有当这四个像素中有三个同时大于 Ip + T 或小于 Ip − T 时，当前像素才有可能是一个角点，否则应该直接排除。

（2）第一遍检测之后，还需要用非极大值抑制（Non-maximal suppression），在一定区域内仅保留响应极大值的角点，避免角点集中的问题。

1.2 ORB特征点及描述子（基于Fast特征点的改进）

ORB是第7讲和第8讲中用的最多的，主要需要记住的就是改进的两个方面。

1.2.1 ORB特征点和BRIEF

ORB特征点：

Fast特征点的缺点也就连着ORB特征点的改进，总结如下：

（1）FAST 特征点数量很大且不确定，而我们往往希望对图像提取固定数量的特征。在 ORB 中，对原始的 FAST 算法进行了改进。我们可以指定最终要提取的角点数量N，对原始 FAST 角点分别计算 Harris 响应值，然后选取前 N 个具有最大响应值的角点，作为最终的角点集合。

（2）FAST 角点不具有方向信息。而且，由于它固定取半径为 3 的圆，存在尺度问题：远处看着像是角点的地方，接近后看可能就不是角点了。ORB的改进如下：

a. ORB的尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。

b. 旋转问题或者说方向信息是通过灰度质心法（Intensity Centroid）实现的：

BRIEF描述子：

BRIEF 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成，这里的 0 和 1 编码了关键点附近两个像素（比如说 p 和 q）的灰度值大小关系：如果 p 比 q 大，则取 1，反之就取 0。如果我们取了 128 个这样的 p，q，最后就得到 128 维由 0， 1 组成的向量。

p与q的挑选大体上都是按照某种概率分布，随机地挑选 p 和 q 的位置。

方向不变性：ORB 在 FAST 特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征，使 ORB 的描述子具有较好的旋转不变性。

ORB特征优点：

ORB算法的速度大约是SIFT的100倍，是SURF的10倍。

以上参考 https://zhuanlan.zhihu.com/p/36382429

1.2.2《SLAM 14讲》中第7讲中用的特征主要就是ORB

直接上代码：

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 )
{
    //-- 1、初始化：创建匹配描述子，特征点向量，
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );       // use this if you are in OpenCV2
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );

    // 选择匹配方法
    // Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    // Ptr<DescriptorMatcher> matcher = makePtr<FlannBasedMatcher>(makePtr<flann::LshIndexParams>(12, 20, 2));
    // FlannBasedMatcher matcher;
    cv::FlannBasedMatcher matcher(new cv::flann::LshIndexParams(20, 10, 2));

    //-- 2、检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 3、根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );


    //-- 4、对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    // matcher->match ( descriptors_1, descriptors_2, match );
    matcher.match ( descriptors_1, descriptors_2, match );

    //-- 5、匹配点对筛选
    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 );

    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

1.3 光流中的Shi-Tomas角点

VINS中特征点在光流追踪过程中会有损失，因此会使用goodFeaturesToTrack()提取Shi-Tomas角点，补充计算特征点。

代码如下，主要使用了cv::goodFeaturesToTrack

/ 比如每个待发布的帧都要保证150个特征点，经过跟踪之后的稳定点是100个，那么还要再提取50个点
ROS_DEBUG("detect feature begins");
TicToc t_t;
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
    if(mask.empty())
        cout << "mask is empty " << endl;
    if (mask.type() != CV_8UC1)
        cout << "mask type wrong " << endl;
    if (mask.size() != forw_img.size())
        cout << "wrong size " << endl;

    // 只有发布才可以提取更多特征点，同时避免提的点进mask
    // 会不会这些点集中？会，不过没关系，他们下一次作为老将就得接受均匀化的洗礼
    cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
    n_pts.clear();
ROS_DEBUG("detect feature costs: %fms", t_t.toc());

OpenCV也实现了shi-tomas算法：

CV_EXPORTS_W void goodFeaturesToTrack( InputArray image, OutputArray corners,
                                     int maxCorners, double qualityLevel, double minDistance,
                                     InputArray mask = noArray(), int blockSize = 3,
                                     bool useHarrisDetector = false, double k = 0.04 );

（API没有用人名，而是用了他们发表的论文名称，这个纪念也是有意思）

image（srcGray）：原灰度图像
corners：检测到的角点vector<Point>
maxCorners：设置最大角点数
qualityLevel：最小可接受的向量值，常取0.01
minDistance：两点之间的最小距离
Mat()：未设置感兴趣区域
blockSize：计算导数的窗口大小
useHarrisDetector：是否使用Harris角点检测
k：使用Harris角点检测时的k值，使用Harris角点检测时才有意义

在讲shi-tomas角点之前，要先说明一下Harris角点。

1.3.1 Harris角点

1、Harris角点：如下图所示，通过一个小的滑动窗口在邻域检测角点，在任意方向上移动窗口，若窗口内的灰度值都有剧烈的变化，则窗口的中心就是角点。转化为数学描述就是自相关矩阵M两个特征值大小。

Fast

2、Harris特征描述子：Harris角点的描述子通常是由周围图像像素块的灰度值，以及用于比较的归一化互相关矩阵构成的。图像的像素块由以该像素为中心的周围矩形部分图像构成。

3、优缺点

优点：计算简单；提取的点特征均匀且合理；稳定，稳定的Harris算子对图像旋转、亮度变化、噪声影响和视点变换不敏感。

缺点：对尺度很敏感，不具有尺度不变性；提取的角点精度是像素级的；需要设计角点匹配算法。

那我们怎么判断角点呢？如下图所示：

当R为大数值的正数时是角点
当R为大数值的负数时是边界
当R为小数是认为是平坦区域

1.3.2 shi-tomas角点

shi-Tomas角点检测算法是对Haris角点检测的改进算法。Harris是怎么做到的提取角点呢？

1. 先上数据集

2. 从数据集中，引申出Harris

核心思想是：

特征值都比较大，即窗口中含有角点
特征值一个较大，一个较小，窗口中含有边缘
特征值都比较小，窗口处在平坦区域

Harris依据归一化互相关矩阵引进如下公式：

3. shi-Tomas的简化算法

从数据集上看，角点的都是比较大的，如果我们选择特征值较小的一个大于最小阈值，也可以得到强角点。

Harris 算法最原始的定义是将黑塞矩阵的行列式值与M的迹相减，再将差值同预先给定的阈值进行比较。所以shi-tomas提出来的公式如下：

本部分内容参考自知乎文章：https://zhuanlan.zhihu.com/p/87745981

1.4 SIFT(Scale-Invariant Feature Transform)和SURF(Speed Up Robust Feature)

1.4.1 SIFT

1、性质

（1）SIFT特征是图像的局部特征，对旋转、尺度缩放、亮度变化保持不变性，对视角变化、仿射变换、噪声也保持一定程度的稳定性（其中尺度不变性主要是通过尺度空间保证的，旋转不变性主要是通过主方向保证的）
（2）信息量丰富（128维特征），适用于在海量特征数据库中进行匹配
（3）实时性不高，对边缘光滑的目标无法准确提取特征点

2、算法简述

SIFT特征点：利用高斯金字塔和DOG函数进行特征点提取。高斯金字塔的当前层图像是对其前一层图像先进行高斯低通滤波，然后做隔行和隔列的降采样(去除偶数行与偶数列)而生成的。DoG (Difference of Gaussian)是高斯函数的差分，具体到图像处理来讲，就是将同一幅图像经过两个不同高斯滤波得到两幅滤波图像，将这两幅图像相减，得到DoG图。DOG图上的邻域梯度方向直方图峰值即特征点的主方向。

在这里插入图片描述

SIFT特征描述子：以特征点为中心取窗口，通过高斯加权增强特征点附近像素梯度方向信息的贡献，即在4 × 4的小块上计算梯度方向直方图（取8个方向），计算梯度方向累加值，形成种子点，构成4× 4 × 8= 128维特征向量。然后进行统计。

在这里插入图片描述

SIFT提取步骤：

（1）高斯差分金字塔的构建
        使用组和层的结构构建了一个具有线性关系的金字塔（尺度空间），这样可以在连续的高斯核尺度上查找图像的特征点；另外，它使用一阶的高斯差分来近似高斯的拉普拉斯核，大大的减少了运算量。
（2）尺度空间的极值检测及特征点的定位
        搜索上一步建立的高斯尺度空间，通过高斯差分来识别潜在的对尺度和旋转不变的特征点。但是，在离散空间中，局部极值点可能并不是真正意义的极值点，真正的极值点有可能落在离散点的间隙中，SIFT通过尺度空间DoG函数进行曲线拟合寻找极值点。
（3）特征方向赋值
        基于图像局部的梯度方向，分配给每个关键点位置一个或多个方向，后续的所有操作都是对于关键点的方向、尺度和位置进行变换，从而提供这些特征的不变性。
（4）特征描述子的生成
        通过上面的步骤已经找到的SIFT特征点的位置、方向、尺度信息，最后使用一组向量来描述特征点及其周围邻域像素的信息。

3、优缺点

优点：SIFT特征对旋转、尺度缩放、亮度变化保持不变性，对视角变化、仿射变换、噪声也保持一定的稳定性

缺点：速度较慢

1.4.2 SURF

1、性质

SURF是对SIFT的改进，它们在思路上是一致的，只是采用的方法不同。

2、算法简述

SUFT特征点：基于Hessian矩阵构造金字塔尺度空间，利用箱式滤波器（box filter）简化二维高斯滤波
SUFT特征描述子：通过Haar小波特征设定特征点主方向，这样构建的特征描述子就是64维的

在这里插入图片描述

SURF步骤：

（1）尺度空间的极值检测

（2）特征点过滤并进行精确定位

（3）设定主方向

（4）生成描述子

SURF的流程和SIFT比较类似，这些改进体现在以下几个方面：

（1）特征点检测是基于Hessian矩阵，依据Hessian矩阵行列式的极值来定位特征点的位置。并且将Hession特征计算与高斯平滑结合在一起，两个操作通过近似处理得到一个核模板。
（2）在构建尺度空间时，使用box filter与源图像卷积，而不是使用DoG算子。
（3）SURF使用一阶Haar小波在x、y两个方向的响应作为构建特征向量的分布信息。

3、优缺点

缺点：在求主方向阶段太过于依赖局部区域像素的梯度方向；图像金字塔的层取得不足够紧密也会使得尺度有误差。

1.4.3 SIFT和SURT的区别

1、尺度不变性的解决方案不同：SIFT是构建图像金字塔，利用不同尺寸的图像与高斯差分滤波器进行卷积；SURF特征利用原图片与不同尺寸的方框滤波器卷积；

2、特征描述子维度不同：SIFT有4×4×8=128维描述子；SURF有4×4×4=64维描述子。

3、特征点检测方法不同：SIFT特征先进行非极大值抑制，再去除低对比度的点，再通过Hessian矩阵去除边缘响应过大的点；SURF特征先利用Hessian矩阵确定候选点，然后再进行非极大值抑制。

4、特征点主方向：SIFT特征在正方形区域内统计梯度幅值的直方图，直方图最大值对应主方向，可以有多个主方向；SURF特征在圆形区域内计算各个扇形范围内x、y方向的Haar小波响应，模最大的扇形方向作为主方向。

1.5 特征匹配

特征匹配的方法很多，这里不一一赘述，包括暴力匹配（Brute-Force Matcher）、快速近似最近邻（FLANN）等等，ORB SLAM2中采用词典加速匹配过程。

2、2D-2D：对极约束、基础矩阵、本质矩阵、单应矩阵

对极约束：

本质矩阵E：

基本矩阵F：

其中，p1，p2 是像素坐标，对极约束描述的是空间中两个匹配点的空间位置关系，本质矩阵的奇异值必定是[ δ , δ , 0 ] ，由于平移和旋转各三个自由度，因此本质矩阵有六个自由度。但通常采用八点法进行求解。由本质矩阵恢复R , t的过程通过SVD分解完成。

当场景中所有特征点都落到一个平面上时就可以通过单应性来进行运动估计。通过这个关系可以推导得

，其中H就是单应矩阵。可以通过四对点求解。单应性的重要性在于，当相机发生纯旋转或者特征点共面时，基础矩阵的自由度就会下降，就会出现退化，这时候如果我们继续用8点法求基础矩阵，基础矩阵多余出来的自由度将主要由噪声决定。

这里涉及一个问题，通过E矩阵或者F矩阵恢复出R|t，然后使用三角化恢复归一化点的深度，判断深度值大于0的匹配特征点对的个数。

3、3D-2D：PnP

PnP是求解3D到2D点对运动的方法，即问题描述为，我们的已知条件是n个3D空间点以及它们作为特征点的位置（以归一化平面其次坐标表示），我们求解的是相机的位姿R | t，如果是3D空间点的位置是世界坐标系的位置，那么这个R，t也是世界坐标系下的。特征点的3D位置可以通过三角化或者RGBD相机的深度图确定。

3.1 直接线性变换方法

根据推导，一对特征点（一个3D点加一个2D点）可以提供两个线性约束，因此12维的其次变换矩阵需要6对特征点。

3.2 P3P方法

P3P的作用是将利用三对特征点，讲空间点在世界坐标系下的坐标，转换到像极坐标系中的坐标，将PnP问题转化为ICP问题，推导过程是利用三角形特征完成的。

3.3 Bundle Adjustment方法

其本质是一个最小化重投影误差的问题，公式如下：

在这里插入图片描述

即理解为调整相机的位姿使得重投影误差变小，而最小的重投影误差就对应着实际的位姿。需要求这里要注意的是这里仅仅是采用了BA的方法，但是实际做BA优化的时候，是同时优化位姿和路点位置，因此有两个相关的雅克比矩阵，但这里仅仅优化位姿，因此所求的雅克比矩阵仅仅和位姿有关，可以直接求取J如下，然后就可以进行求解δξ进行迭代：

3.4 面试题：描述PnP算法

已知空间点世界坐标系和其像素投影，公式如下：

或者描述为：

目前一共有两种解法，直接线性变换方法（一对点能够构造两个线性约束，因此12个自由度一共需要6对匹配点），另外一种就是非线性优化的方法，假设空间坐标点准确，根据最小重投影误差优化相机位姿。

目前主要有两个应用场景：

1. 求解相机相对于某2维图像、3维物体的位姿；通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机坐标系相对于世界坐标系（Twc）的位姿。

2. SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。通常输入的是上一帧中的3D点（上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换。

4、3D-3D：ICP（Iterative closest point）

其本质是确定两个点集之间的匹配关系。ICP问题在激光SLAM和视觉SLAM中并不相同，因为激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。

视觉SLAM中，特征点为我们提供了较好的匹配关系。

4.1 SVD方法

其问题构建为：

在这里插入图片描述

求解过程是先计算两组点的质心位置p和p'，对每个点进行去质心坐标，然后根据优化问题计算旋转矩阵R（这里会用到SVD），最后求t。

为什么引入质心的操作？为了实现平移t和旋转R的解耦。

4.2 非线性优化方法

其问题构建为：

在这里插入图片描述

这里的推导和PnP的类似，即求位姿导数。

5、直接法和光流法

5.1 光流法

光流法的基本假设是灰度不变假设，即同一个空间点的像素灰度值，在各个图像中是固定不变的：

在LK光流中假设某一个窗口内的像素具有相同的运动，因此w×w大小的窗口内有w²个像素，即构成 w² 个方程，然后构成dx/dt，dy/dt的超定线性方程，求其最小二乘解，LK光流是得到特征之间的对应关系，如同描述子的匹配，之后还是需要通过对极几何、PnP等求解相机位姿。

5.2 直接法

直接法之所以称为直接法是因为它是直接获得相机位姿，而不需要通过匹配、求解矩阵等过程，直接法思路是根据当前相机的位估计值，来寻找p2的位置。但若相机位姿不够好，p2的外观和p1会有明显差别，然后通过优化光度误差来优化相机位姿。

非常快的框架SVO就是结合了直接法和特征点法，SVO采用的是提取稀疏特征点（类似特征点法），帧间VO用图像对齐（类似于直接法）。

直接法：1、非凸性；2、单个像素没有区分度；3、灰度值不变是很强的假设。

6、Bundle Adjustment

对后端进行了一个非常全面的总结，参考博客视觉SLAM总结——后端总结。

首先注意目标函数如下：

其中Fij为整个代价函数在当前状态下对相机位姿的偏导数，Eij表示该函数对路标点位置的偏导数。因此在非线性优化过程中获得的H矩阵为：

H矩阵最重要的一个特性就是它的稀疏性，其中左上角和右下角为对角阵且一般左上角较右下角较大。左下角和右上角可能稀疏也可能稠密，矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点，有时候称为共视（Co-visibility）。其对应关系如下：

其求解H△x=g的过程中，消元的过程即Marginalization（边缘化），首先消元的结果如下：

先求解：

再将解得的△ xc带入原方程，再求解△ xp，其优势在于：

所谓鲁棒核函数就是减小误匹配带来的误差，如Huber核，其实就是改变一下目标函数的定义:

当误差e 大于某个阈值δ 后,函数增长由二次形式变成了一次形式,相当于限制了梯度的最大值。

7、回环检测

7.1 词袋模型

SLAM中目前使用的较多的方法是词袋模型，词袋模型中涉及到字典的生成和使用问题，这一部分和机器人学习知识挂钩比较深。

字典的生成问题就是非监督聚类的问题，可以采用k-means对特征点进行聚类，然后通过K叉树进行表达。相似度判断采用的是TD-IDF（Term Frequency—Inverse Document Frequency，译频率—逆文档频率）方法。

词袋模型中的单词是聚类特征，字典就是聚类特征的集合。

K-means的主要步骤：

1. 随机选取k个中心点：c1，...，ck。

2. 对每一个样本，计算它与每个中心点之间的距离，取最小的作为它的归类。

3. 重新计算每个类的中心点。

4. 如果每个中心点都变化很小，则算法收敛，退出；否则返回第2步。

7.2 深度学习进行闭环检测

可以结合自己的研究方向基于点云的大规模场景识别谈，完成场景识别，再根据识别结果进行配准，得到当前帧与地图的变换关系R|t。

参考文章：

1、https://blog.csdn.net/weixin_44580210/article/details/89514870 2D特征点和3D特征点汇总

2、https://blog.csdn.net/weixin_44580210/article/details/87214464#t41 视觉SLAM总结——视觉SLAM笔记整理

家门Jm

关注

1
点赞
踩
13

收藏

觉得还不错? 一键收藏
打赏
0
评论
SLAM前端知识汇总

1、常用特征点汇总1.1 Fast特征点《SLAM14讲》中LK光流中用的特征点就是Fast特征点，然后对这些特征点进行光流追踪。在8.3.2节的LK光流代码，第1帧提取的就是Fast特征点，注意代码的写法：if ( 0 == index ){ // “只”对第一帧提取FAST特征点 vector<cv::KeyPoint> kps; cv::Ptr<cv::FastFeatureDetector> dete...
复制链接

扫一扫