算法"/>
点云配准及运动估计算法
文章目录
- 点云配准及运动估计算法
- 1. 基于距离判断:
- 2. 基于概率似然模型判断:
点云配准及运动估计算法
虽然激光slam的前端配准,主流思路是基于交叉迭代优化算法,即分为两步优化,优化点云配准与优化运动估计。但是关于点云匹配的评价函数有不同的思路。评价函数用于评价运动估计(R,t)的可靠性。就是估计的(R,t)对不对。
根据匹配点关系,建立目标评价函数函数,把激光的帧间匹配的运动估计问题转换为求解目标函数的极值问题。目前个人见过两种:
1. 基于距离判断:
思路:将两帧点云数据在同一个坐标系下,一帧数据中的点找到另一帧数据最近的点,就作为一对匹配点。
代表算法:ICP类算法:ICP/PLICP/NICP/IMLS等
评价方程:多种,点到点(ICP)/点到线(PLICP)/点到面的距离等,详细解释查看ICP相关介绍
优化目标:最小化评价方程,得到R,t
-
点到点距离作为评价方程(ICP):
ICP算法介绍
匹配点在同一坐标下的范数(距离平方)的大小,即评价匹配点的距离。
点到点距离作为约束方程,容易卡在局部最优解。
-
NICP:
NICP算法介绍
加入特征点所在曲面法向量的约束,去掉一些错误匹配的点。
匹配评价方程仍使用距离匹配。 -
点到线的距离作为评价方程(PLICP):
paper: 《An ICP variant using a point-to-line metric》
paper中介绍的point-to-point约束方程:
S r e f S^{ref} Sref表示点所在曲面
Π { S r e f , p i } \Pi\{S^{ref}, p_i\} Π{Sref,pi}表示激光点 p i p_i pi在曲面上的投影为什么使用投影点与原始点距离作为约束,因为激光在相邻两帧之间有激光打到同一点云的概率很小,因此在ICP算法中,上述的约束方程是实际比较常用的匹配算法。而不是在两帧点云中找到匹配点。
paper中提出的约束方程:
分析,相比之前的方程多乘了一个曲面法向量,因此将会提高逼近速度。
ICP中的得到的激光点连线来近似曲面,上图(a),(b)为文中说的曲面匹配的点到点算法。©为本文的约束实际实现方法,找到上一帧与目标点最近的两个相邻点,计算这两点与目标点的距离。
带w表示世界坐标系下的点的坐标描述。以下为算法的实现步骤,感觉论文在(4)那里没有描述好
-
点到面的评价方法:Point-to-Plane ICP
评价方程:点到(对应点所在)面的距离
M o p t = a r g m i n M ∑ i ( ( M ∙ s i − d i ) ∙ n i ) 2 M_{opt}={\rm argmin}_M\sum_{i}{((M\bullet s_i-d_i)\bullet n_i)}^2 Mopt=argminM∑i((M∙si−di)∙ni)2
s i s_i si指第I个source point
d i d_i di指第i个destination point
M M M为变换矩阵 -
IMLS:
paper:《IMLS-SLAM: scan-to-model matching based on 3D data》
计算当前扫描中一点 x i x_i xi到曲面的距离 I P k ( x i ) I^{P_k}(x_i) IPk(xi),曲面是前n次扫描计算得到的环境曲面。 P k P_k Pk为当前得到的所有点集。
则当前帧的评价方程:
由于 I P k ( x i ) I^{P_k}(x_i) IPk(xi)含有指数,因此不好迭代下降优化,因此用以下方程近似点到面的距离:
其中 n j n_j nj为曲面上 ( x i ) (x_i) (xi)最近点的曲面法向量。
y j y_j yj为 ( x i ) (x_i) (xi)在曲面上的投影。
通过评价方程可以看出,IMLS是一种点到面的约束方法,评价函数上与Point-to-Plane的差异也比较小:
Point-to-Plane:目标对应点的法向量*原始点与原始点在平面上的投影点的连线
IMLS:目标对应点的法向量*目标点与原始点连线
2. 基于概率似然模型判断:
papers:
《Real-Time Correlative Scan Matching》
《The Normal Distributions Transform_A New Approach to Laser Scan Matching》
主要是针对栅格地图的slam算法,计算匹配点落在同一个栅格中的概率。
目前见过两种评价函数方法:
思路1:网格使用高斯概率模型,实现连续可优化
代表算法:NDT,CSM等
评价函数1:详细介绍查看NDT相关介绍
解释:评价两帧点云的分布情况
x i = ( x , y ) T x_i=(x,y)^T xi=(x,y)T表示激光点坐标
q i , Σ i q_i, \Sigma _i qi,Σi表示点 x i x_i xi对应的高斯分布的均值和方差。
Score函数:
优化目标1:
思路2:计算目标匹配点转换后占用地图的概率,用‘1-‘变成最小化函数。
评价函数2:
解释:匹配点的地图占用概率
其中:
表示 S i ( T ) S_i(T) Si(T)激光点i用T转换之后的坐标
M ( x ) M(x) M(x)表示得到的坐标x的地图占用概率,可利用双线性插值得到cell的概率拟合,即使用插值法可以拟合得到参考点云的概率分布函数 M ( x ) M(x) M(x)。
优化目标2:最小化目标评价函数
参考链接:
更多推荐
点云配准及运动估计算法
发布评论