目录
1、什么是slam
slam主要用于解决机器人在未知环境运动时的定位与地图构建问题,其研究主要可以分为Basic Theory(数学建模)、Sensor、Mapping(2D黑白/3D点云地图、吃存储)、Loop Detection(减小累积误差)、Advanced Topic(实际运用)。前面四种只有定位和建图俩件事,高级话题是slam技术主要发展方向,例如动态场景、语义地图、多机器人协作等等。
2、slam基本问题
视觉导航的方法化为三类:
(1)已知地图的导航(Map-BasedNavigation):表示地图的方法几何特征(GeometricPrimitives)、拓扑特征(TopologicalFeatures)或占据栅格(OccupancyGrids)移动机器人依据这些已知的环境地图进行导航。
(2)地图建立的导航(Map-Building-BasedNavigation):在没有已知环境地图的情况下,移动机器人通过自身的导航运动和传感器的不断感知更新来进行导航。
(3)未知环境的导航(MaplessNavigation):相对于上面两种方法,在实时的动态环境中无法建立明确的地图表达形式,更多的是通过传感器获得的观测信息用来识别或者跟踪环境中的物体来导航。
由于感知信息的不确定性,在未知环境中的定位成为最关键的问题。
(1)相对定位(RelativePositionMeasurements):主要依靠内部本体感受传感器如里程计(Odometry)、陀螺仪(Gyroscopes)等,通过给定初始位姿,来测量相对于机器人初始位姿的距离和方向来确定当前机器人的位姿,也叫做航迹推测(DeadReckoning, DR)。
(2)绝对定位(AbsolutePosition Measurements):主要采用主动或被动标识(Activeor Passive Beacons)、地图匹配(MapMatching)、全球定位系统(GlobalPositioning System,GPS)、或导航信标(LandmarkNavigation)进行定位。位置的计算方法包括有三角测量法(Triangulation)、三边测量法(Trilateration)和模型匹配算法(ModelMatching)等。
(3)组合定位(CombinedPositionMethod):虽然相对定位这种方法能够根据运动学模型的自我推算移动机器人的位姿和轨迹而且具有自包含的有点。但是不可避免地会存在随时间的增加和距离的增加而增加的累积航迹误差。在绝对定位中,地图匹配技术处理数据速度较慢,而信标或标识牌的建设和维护成本太高,GPS又只能在室外使用。由于单一定位的方法的缺陷,移动机器人定位仍然是基于航迹的推算与绝对位姿和轨迹矫正相结合起来。
3、slam框架
传感器数据:主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。
视觉里程计:主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。
后端:主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用。
回环检测:主要用于空间累积误差消除
建图:用于三维地图构建。
传感器读取数据后,视觉里程计估计两个时刻的相对运动(Ego-motion),后端处理视觉里程计估计结果的累积误差,建图则根据前端与后端得到的运动轨迹来建立地图,回环检测考虑了同一场景不同时刻的图像,提供了空间上约束来消除累积误差。
4、基于传感器的slam分类
1)激光slam
激光SLAM采用2D或3D激光雷达(也叫单线或多线激光雷达),2D激光雷达一般用于室内机器人上(如扫地机器人),而3D激光雷达一般使用于无人驾驶领域。激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点,被称为点云。激光雷达测距比较准确,误差模型简单,在强光直射以外的环境中运行稳定,点云的处理也比较容易。
2)视觉slam
可以从环境中获取海量的、富于冗余的纹理信息,拥有超强的场景辨识能力。
5、SIFT特征提取
尺度不变特征转换(Scale-invariant feature transform 或 SIFT)是一种电脑视觉的算法用来侦测与描述影像中的局部性特征,它在空间尺度中寻找极值点,并提取出其位置、尺度、旋转不变量。Sift算法就是用不同尺度(σ)的高斯函数对图像进行平滑,然后比较平滑后图像的差别,差别大的像素就是特征明显的点。
sift对旋转、尺度缩放、亮度变化保持不变性,对视角变化、仿射变换、噪声也保持一定程度的稳定性;信息量丰富,适用于在海量特征数据库中进行快速、准确的匹配;即使少数的几个物体也可以产生大量的SIFT特征向量;经优化的SIFT匹配算法甚至可以达到实时的要求。
SIFT特征的生成一般包括以下几个步骤:
(1) 构建尺度空间,检测极值点,获得尺度不变性。
尺度空间获取:
其中是高斯函数,I是输入图形,σ的值表示图片的清晰程度,σ 值越大,标准差越大,浮动范围也就越大,原始图像改变越明显。
DOG(高斯差分图像金字塔):,为高斯模糊后的图像相减得到。灰度变化大的地方,差分值大。k=2时,图像缩小为1/2,σ不变。
图像金字塔用以解决高斯核无法处理的外层像素,例如3*3的掩膜最外层无法处理。
DOG空间极值检测:某像素与周围的26个像素进行比较,为最大/最小值时就是极值点。
检测的极值点是离散的,是否为关键点要进行下一步。
(2) 特征点过滤并进行精确定位。
对尺度空间DOG函数用泰勒展开公式进行曲线拟合,计算极值点,从而实现关键点的精确定位。
近似值:
f(x)的极值就是近似值。此外DOG算法对边界非常敏感,可以模仿海森矩阵算法中的M矩阵去消除。
(3)为特征点分配方向值。
经过(1)、(2)得到了具有尺度不变性的关键点,为得到旋转不变性,需要根据关键点所在高斯尺度图像的邻域结构中求得一个方向基准。
特征点即可得到(x,y,σ,θ),即位置、尺度、方向。具有多个方向的关键点可以被复制成多份,然后将方向值分别赋予复制后的特征点,一个特征点就产生了多个坐标、尺度相等,但方向不同的特征点。
(4)生成特征描述子。
半径r=3*1.5σ(高斯金字塔r区域内的所有像素的梯度特征),使用直方图统计关键点邻域内像素的梯度幅值和方向。具体是将360°分为36柱,下图简化为8柱。
取16×16的窗口,箭头方向代表梯度方向,长度代表梯度幅值,用高斯窗口进行加权运算(σ=0.5d),每个特征由4×4个种子点组成,即一个关键点会产生128维的SIFT特征向量。
(5)代码
import cv2
import numpy as np
import matplotlib.pyplot as plt
#读取图像
img1 = cv2.imread('p1.png', 0)
#使用sift算法,计算特征向量与特征点
sift = cv2.SIFT_create() #创建一个sift算子
kp1, des1 = sift.detectAndCompute(img1, None) #计算特征向量与特征点
img1 = cv2.drawKeypoints(img1, kp1, img1) #绘制关键点
cv2.imshow("sift.png",img1)
cv2.waitKey(0)
cv2.destroyAllWindows()
6、特征匹配
Brute-Force特征点匹配总是尝试所有可能的匹配,从而使得它总能够找到最佳匹配的方法。
1)bf.match一对一匹配
BF匹配首先对互相匹配的两个特征点创建BFmatcher对象,其原理是在第一幅图像中选择一个关键点然后依次与第二幅图像的每个关键点进行(改变)距离测试,最后返回距离最近的关键点。然后根据匹配的质量对其进行排序。最后绘制前10个最佳匹配。
import cv2
#读取图像
img1 = cv2.imread('p1.png', 0)
img2 = cv2.imread('p2.png', 0)
#使用sift算法,计算特征向量与特征点
sift = cv2.SIFT_create() #创建一个sift算子
kp1, des1 = sift.detectAndCompute(img1, None) #计算特征点与特征向量
kp2, des2 = sift.detectAndCompute(img2, None)
bf = cv2.BFMatcher(crossCheck=True)
matches = bf.match(des1, des2)
#对点的距离的排序
matches = sorted(matches, key=lambda x: x.distance) #根据匹配质量排序,选择前n个
img3 = cv2.drawMatches(img1, kp1, img2, kp2, matches[:10], None,flags=2) #绘制前10个最佳匹配
#图片展示
cv2.imshow("pipei.png",img3)
cv2.waitKey(0)
cv2.destroyAllWindows()
2)bf.knnmatch一对多匹配
knnmatch匹配的参数k,表示每个关键点保留的最佳即最短距离匹配的最大数量。下例取k=2,即每个查询关键点有两个最佳匹配。
次近邻的距离乘以一个小于1的值,获得的值称为阈值,只有当距离分值小于阈值时,则称之为“好点”。最近邻和次近邻大小差不多的点,更容易出现错误的匹配,即“坏点”。这种方法时比率检验。
应用比率检验,下例代码将阈值设置为次优匹配距离分值的0.75倍,如果knnmatch不满足次优匹配,则该点为“坏点”,舍弃。
import cv2
#读取图像
img1 = cv2.imread('p1.png', 0)
img2 = cv2.imread('p2.png', 0)
#使用sift算法,计算特征向量与特征点
sift = cv2.SIFT_create() #创建一个sift算子
kp1, des1 = sift.detectAndCompute(img1, None) #计算特征点与特征向量
kp2, des2 = sift.detectAndCompute(img2, None)
bf = cv2.BFMatcher()
#添加knn算法
matches = bf.knnMatch(des1, des2, k=2)
good = []
for m, n in matches:
if m.distance < 0.75 * n.distance:
good.append([m])
img3 = cv2.drawMatchesKnn(img1,kp1,img2,kp2,good,None,flags=2)
#图片展示
cv2.imshow("pipei.png",img3)
cv2.waitKey(0)
cv2.destroyAllWindows()
显示前10个点:
img3 = cv2.drawMatchesKnn(img1,kp1,img2,kp2,good[:10],None,flags=2)
可知,一对一匹配没有过滤掉“坏点”。
3)FLANN匹配
FLANN(Fast_Library_for_Approximate_Nearest_Neighbors)快速最近邻搜索,是一个对大数据集和高维特征进行最近邻搜索的算法的集合,比蛮力匹配更快。
Flann要用两个字典作为参数设置,第一个是index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5) 。trees指定待处理核密度树的数量(理想的数量在1-16)。对于SIFT算法,FLANN_INDEX_KDTREE = 1
第二个是search_params = dict(checks=50),是递归次数,值越大效果越好,消耗时间越长。
代码取5kd-trees和50checks,其总能取得合理精度,而且短时间完成。舍弃距离大于0.7的点,可以避免90%的错误匹配,但是好的匹配结果也会很少。
import cv2
#读取图像
img1 = cv2.imread('p1.png', 0)
img2 = cv2.imread('p2.png', 0)
#使用sift算法,计算特征向量与特征点
sift = cv2.SIFT_create() #创建一个sift算子
kp1, des1 = sift.detectAndCompute(img1, None) #计算特征点与特征向量
kp2, des2 = sift.detectAndCompute(img2, None)
#设置参数
FLANN_INDEX_KDTREE = 1 #建立FLANN匹配器的参数
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50) #递归次数
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1, des2, k=2)
good = [] # 这里主要是匹配的更加精确,可以修改ratio值得到不同的效果,即下面的0.7
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append([m])
img3 = cv2.drawMatchesKnn(img1, kp1, img2, kp2, good, None, flags=2)
cv2.imshow("Sift", img3)
cv2.waitKey(0)
cv2.destroyAllWindows()
参考来自:即时定位与地图构建(SLAM)与基于视觉的SLAM(VSLAM)_曼陀罗彼岸花的博客-CSDN博客_slam即时定位与地图构建