点云配准及运动估计算法

编程入门 行业动态 更新时间:2024-10-26 20:30:29

点云配准及运动估计<a href=https://www.elefans.com/category/jswz/34/1770096.html style=算法"/>

点云配准及运动估计算法

文章目录

    • 点云配准及运动估计算法
      • 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:最小化目标评价函数

参考链接:

更多推荐

点云配准及运动估计算法

本文发布于:2024-02-25 20:11:26,感谢您对本站的认可!
本文链接:https://www.elefans.com/category/jswz/34/1700157.html
版权声明:本站内容均来自互联网,仅供演示用,请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系,我们将在24小时内删除。
本文标签:算法   点云配准

发布评论

评论列表 (有 0 条评论)
草根站长

>www.elefans.com

编程频道|电子爱好者 - 技术资讯及电子产品介绍!