激光SLAM从理论到实践学习——第四节（激光SLAM的前端配准方法）

最新推荐文章于 2024-08-22 09:35:56 发布
最新推荐文章于 2024-08-22 09:35:56 发布
阅读量3.4k 收藏 40
点赞数 8
分类专栏： SLAM 机器人
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42263553/article/details/103172430
版权
SLAM 同时被 2 个专栏收录
13 篇文章 34 订阅
订阅专栏
机器人
11 篇文章 1 订阅
订阅专栏

激光的前端配准算法（帧间匹配算法）

  • 前端配准在视觉内叫做Tracking或者帧间匹配，对激光SLAM是有非常大的影响的
  • 帧间匹配不一定说的是前后两帧进行匹配，也可以是任意帧之间进匹配
  • 是一个Map—>Scan的过程，一个Scan和一个Map去匹配的过程
  • 常用的方法有下面四种
    • ICP匹配方法（对上节ICP算法的证明）
    • PL-ICP匹配方法（与ICP算法相似，只是说误差是之间的）
    • 基于优化的匹配方法（Hector和cartographer两种算法是基于优化的方法）
    • 相关匹配方法及分支定界加速（CSM算法）

ICP匹配方法

  • ICP方法的目的：就是两帧3D点云的匹配，求解一个R和t使得下列式（误差）最小。

    在这里插入图片描述

  • 已知对应点的求解方法

    就是先求一个平均点，每一个点云都求一个平均点，然后对任何一个点云进行平移操作，把它移到以P点为圆心的地方，然后构造一个矩阵W，然后对这个矩阵进行SVD分解，然后得到ICP的解R与t

    在这里插入图片描述

    下面对ICP的解 （R与t）进行证明
    在这里插入图片描述

    对加号左边与加号右边分别取极小值，所以加号右边=0时，即可得t=ux-Rup
    在这里插入图片描述

    现在来求R，现在加号左边是这样的式子

    在这里插入图片描述

    我们知道（红色字）

    在这里插入图片描述

    所以可以将加号左边写成

    在这里插入图片描述

    把两式展开

    在这里插入图片描述

    R的转置* R是=I 一个常数，a²那项也是常数，所以最后留下

    在这里插入图片描述

    所以最后就是

在这里插入图片描述

​ 所以要求的就是上面这个式子的最小值，也就是求下面式子的最大值，然后得R

在这里插入图片描述
在这里插入图片描述

  • 未知对应点的求解方法

    上节已经推理
    在这里插入图片描述

ICP实现过程

也就是求一个R和t，使得两个点云间的距离最短（下式（误差）最小），求解下式
在这里插入图片描述

t和R的求解过程忽略，得到R和t的解

在这里插入图片描述

然后就可以计算得两个点云间的最短距离（使得（误差）最小）

PL-ICP方法（已有开源代码—indigo下的CSM）

  • 上节ICP其实别名是PP-ICP，Point-to-Point ，点到点距离的ICP匹配方法

  • PL-ICP是Point-to-Line，点到线距离的ICP匹配方法
    在这里插入图片描述

  • 他的算法流程（和PP-ICP没什么区别，就是误差形式不同，到后面也是求R和t）
    在这里插入图片描述

基于优化的匹配方法（基于梯度，HectorSLAM是纯梯度优化方法、Cartogerepher用的是CSM+梯度优化）

  • 基于优化的匹配方法实际上就是基于梯度的方法

    在这里插入图片描述

    • 示意图：其实就是求极值，按照梯度下降方向一步一步求极值

      在这里插入图片描述

    • 数学描述：T表示我们要求解的机器人的位姿，Si（T）表示把激光数据用位姿T进行转换，也就是说把激光数据（Pix、Piy）用一个T转换到世界坐标系下（地图坐标系下）得到一个二维的坐标（x，y）,i表示第i个激光束。

      M(x)表示得到坐标x的地图占用概率（激光数据与地图最大匹配程度），激光数据与地图匹配程度最高是1，M(x)表示最大匹配程度，那么也就是1-minM(x)，所以有

      在这里插入图片描述

      这个M(x)一般会是个似然场，就是一个似然得分，目标函数就定义成如上图那样。所以我们只需要求得一个T，使得M(x)最大。

      M（Si(t)）表示的是我们把每个激光束都统一到我们的世界坐标系下，然后求它占用的概率（与地图的最大匹配程度）越大代表激光与地图重合程度越好，机器人的位姿也就越准确。

      目标函数E(T)这个式子他是非线性的，因为我们的旋转矩阵T，要求它的梯度，但因为地图是栅格化的也就是离散的，无法进行求解梯度，所以只能对地图进行插值。

      求解这个式子还有一个前提就是需要知道梯度，所以他里面肯定是包含一个梯度项的，那为了求解这个梯度项，我们就必须对地图进行插值，那怎么对地图进行插值？这就是主要要讲的内容

    • 基于优化方法的求解

      目标函数依然是E(T)，设pi=(pix,piy)表示第i个激光点的坐标，Si(T)表示将激光点的坐标转换到世界坐标，M（Si(t)）是个非线性函数，对其进行一节泰勒展开，转换成线性函数E(T+△T)，下图中▽的意思是对△T进行求偏导，M(Si(T))是个常数项。得到线性函数E(T+△T)，对于线性系统，求其对△T的导数，并令其等于0得到下式

      在这里插入图片描述

      然后求解上式即可得到△T，令𝑇=𝑇+Δ𝑇，不断进行迭代即可

      在这里插入图片描述

      求解过程：（用符号去代替某些式子就可简化求解）

      在这里插入图片描述

      在这里插入图片描述

      ▽M(Si(T))中的Si(T)可以看作是地图中的一个坐标，然后▽M(Si(T))就表示对地图上的这个坐标求梯度，要求它的梯度，但因为地图是栅格化的也就是离散的，无法进行求解梯度，所以只能对地图进行插值。

    • 地图双线性插值（X,Y两个方向进行插值，是一维线性插值的推广）

      在这里插入图片描述

      • 拉格朗日插值法

        插值的定义：使得n次多项式在每个插值点都成立，那么称Ln(x)为f(x)的插值多项式

        • 一维线性插值

        在这里插入图片描述

        我们想求解这个插值方法的话，只要构造一个基函数li(xk)满足他的条件即可。

        在这里插入图片描述

        基函数求出来了，那么插值方法也就解出来了。

        • 双线性插值（二维）

          和一维差不多，我们有四个点就构造四个基函数就行。

          在这里插入图片描述

          在这里插入图片描述

        然后对应之前说的▽M(Si(T))中的Si(T)可以看作是地图中的一个坐标，然后▽M(Si(T))就表示对地图上的这个坐标求梯度，然后现在▽M(Si(T))也求解出来了，△T的所有项都已知了在这里插入图片描述

        然后之前的方程也就解了，然后令𝑇=𝑇+Δ𝑇，不断进行迭代即可。

简化实现过程：

令目标函数（地图占用概率（激光数据与地图最大匹配程度））

在这里插入图片描述

目标函数是E(T)，设pi=(pix,piy)表示第i个激光点的坐标，Si(T)表示将激光点的坐标转换到世界坐标，M（Si(t)）表示的是我们把每个激光束都统一到我们的世界坐标系下，然后求它占用的概率（与地图的最大匹配程度）越大代表激光与地图重合程度越好，机器人的位姿也就越准确。 M（Si(t)）是个非线性函数，对其进行一阶泰勒展开，转换成线性函数E(T+△T)

在这里插入图片描述

对于线性系统，对T求偏导等于0，就得到了最大匹配程度。求其对△T的导数，并令其等于0得到下式

在这里插入图片描述

但▽M(Si(T)) 表示的是对地图上的这个坐标求梯度，但地图是栅格化的也就是离散的，所以无法求梯度，只能对地图进行插值（方法有一维线性插值和双线性插值），求出插值函数之后就可以求梯度，▽M(Si(T)) 这个式子也就已知了。

插值之后即可对上图式子求解（所以位置量全部已知了）得到△T，令𝑇=𝑇+Δ𝑇，不断进行迭代即可。就是求极值，按照梯度下降方向一步一步求极值。

难点是推导的过程（插值、求解），过程就是这样。

相关匹配方法及分枝定界加速（Cartographer用的）

  • 帧间匹配似然场（CSM）

    暴力匹配不会出现局部极值，他的暴力之处在于三层for循环x，y，θ，计算量巨大

    在这里插入图片描述

    • 算法流程

      在这里插入图片描述

    • 构造似然场（就是高斯模糊的过程），然后在制定的搜索空间内，进行搜索，计算每一个位姿的得分（找出最优得分），根据每个位姿的最优得分，计算本次位姿匹配的方差。

    • 我们不能直接使用暴力匹配，因为他的计算量太大，所以得对其进行加速

  • 位姿搜索

    在这里插入图片描述

    • 1.暴力搜索

      使用三层for循环（x,y,θ）枚举每一个位姿，分别计算每个位姿的得分，计算量巨大（指的不是循环，是他每个位姿都要进行投影，而投影则需要计算sin和cos函数两次），然后把每个位姿都投影到世界坐标系中，累加得分就得到了似然场。

    • 2.预先投影搜索

      先循环θ，再y，再循环x，然后我们世界坐标系转换矩阵是

      cosθ -sinθ x

      sinθ cosθ y

      0 0 1

      我们第一次循环θ之后，这个矩阵中的sinθ和cosθ就只需要计算一遍，其余的就是计算x，y了，这样就大大减少了计算量。

    • 3.多分辨率搜索（CSM用的比较多的方法）

      • 构造粗分辨率和细分辨率两个似然场，然后在粗分辨率似然场上先进行搜索获取最优位姿，然后再把粗分辨率的最优位姿对应栅格进行细分辨率划分，然后再进行细分辨率搜索，再次得到最优位姿。粗分辨率地图的栅格的似然值（最优值）为对应的细分辨率地图对应空间的所有栅格的最大值（上界），也就是说粗分辨率的位姿得分是细分辨率的位姿得分的上界。这个搜索方法可能会错过最优解，但最终解也一定是很接近最优解的。

        在这里插入图片描述

  • 分枝定界算法

    他其实和栅格搜索很相近，栅格搜索就是分栅格，然后栅格内再分栅格，然后栅格是不能一直往下分的，就得到最终解了。而分枝定界算法就是不断地分树枝，这就是分枝部分。然后定界就是确定一个界，搜索树中的每一个节点，确定以该节点为根节点的子树的界（最小值问题确定下界，最大值问题确定上界）。

    （看红色部分）在这里插入图片描述

    他是一个常用的树形搜索剪枝算法，它主要是为了求解整数规划的问题，解的数量为有限个。他就是把最优解求解问题转换为树形搜索问题，根节点表示整个解空间，叶子节点表示最优解，中间的节点表示解空间的某一部分子空间。

    剪枝是怎么个剪枝法呢？

    他就是确定了一个目前的最大得分bestscore，比如说到达了某个节点，假设此节点的上界是A，但这个A小于最大得分basescore，那么这个节点下的子树小面的所有节点都会被剪掉。

    就好比搜栅格，最优解在左下角，如果从右下角开始搜，那么得分自然会很低，既然得分低也就没必要进行细分了。也就相当于剪枝了。

    粗分辨率高的不一定保证细分辨率也高，然后按照得分该剪的剪，没必要细分的就不要细分了。

    关键是如何定界，如何完成算法中的定界函数。
    在这里插入图片描述

    看算法代码可知，首先是选择一个节点，知判断是不是叶子节点，如果不是就score一下，看是否比最大得分还大，如果没有就是剪枝。

    在这里插入图片描述

    • 分枝定界在相关方法的加速作用

      • 搜索树中的节点表示一个正方形的搜索范围
      • 分枝：对于节点（cx，cy，cθ，ch），分枝为4个子节点
      • 定界：其实就是score函数
      • 在这里插入图片描述

      在这里插入图片描述

  • 作业

    在这里插入图片描述

    • PL-ICP匹配方法（与ICP算法相似，只是说误差是之间的）
    • 基于优化的匹配方法（Hector和cartographer两种算法是基于优化的方法）
    • 相关匹配方法及分支定界加速（CSM算法）
激光SLAM的前端配准算法
weixin_46777885的博客
05-16 1250
四、激光的前端配准算法 Tracking , 激光slam里为 帧间匹配算法 帧间匹配算法 ---- map和scan匹配，不一定是前后两帧。 1、ICP匹配方法 2、PL-ICP匹配方法 3、基于优化的匹配方法 Hectorslam , cartographer 4、相关匹配方法及分支定界加速 暴力匹配 CSM correlation 目的 给定两个点云集合: 𝑋 = {𝑥1, 𝑥2, ⋯ , 𝑥𝑁𝑥} 𝑃 = {𝑝1, 𝑝2, ⋯ , 𝑝𝑁𝑝} X和P为 图中黄色和白色形成的点云集
激光SLAM理论与实践(四)--激光SLAM的前端配准方法
weixin_43619346的博客
06-10 761
本章主要介绍激光SLAM的前端配准方法。 主要从以下几个具体的方法来介绍： 首先就是最基础最简单的ICP方法： 有关ICP方法的相关介绍网上有很多，具体的可以参考以下几篇博客： ...
激光SLAM前端方法比较
weixin_44035919的博客
05-27 368
概述 主要对比NDT和ICP的方法 关于NDT的原理可以参考论文：The Three-Dimensional Normal-Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection 的第六章 关于ICP-SVD的方法可以参考我的发文：https://blog.csdn.net/weixin_44035919/article/details/12500
【激光SLAM】激光的前端配准算法
PuddleRubbish的博客
02-18 1218
实现上述插值的一种方法，主要特点为把插值多项式表示成基函数的线性组合：基函数l。
激光扫描匹配SLAM（Laser SLAM）项目指南
最新发布
gitblog_00572的博客
08-22 947
#激光扫描匹配SLAM（Laser SLAM）项目指南 laser_slamThis package provides an end-to-end system to laser-based graph SLAM using laser point clouds.项目地址:https://gitcode.com/gh_mirrors/la/laser_slam 项目介绍 激光扫描匹配SLAM（...
激光slam学习日记——激光的前端配准算法
weixin_51326570的博客
03-11 892
激光slam学习日记——激光的前端配准算法 一、ICP匹配方法’ 简单回顾一下：ICP算法实际就是两个位置观测到了同一块点云，然后把其中一个位置观测到的点云用我们估计的R 、t 投影到第二个位置观测的地方，因为R 、t 是不准确的，所以会与第二个位置的观测产生误差，ICP要做的就是是这个误差尽可能的小，使得R、t 变得很准确。 这里就是先求出两块点云的中心点坐标，分别是每个点与中心点做差（这样做可使得计算量变小，同时也方便计算），然后X与P的转置想乘实际就是两个点云的位姿关系：X = T * P；然后SV
激光SLAM--(4)前端配准方法
qq_42692305的博客
10-09 674
激光SLAM--(4)前端配准方法
激光SLAM——第四节（激光SLAM的前端配准方法）
老李的博客
03-06 8526
@[目录] ICP变种方法 – 第四章 激光的前端...
激光SLAM入门笔记(四):前端配准 I
u010507357的博客
08-28 1021
前端配准 I1.ICP目的数学表述求解方法已知对应点步骤推导未知对应点2.PL-ICP3.NICP4.IMLS-ICP 内容基于深蓝学院课件，如有错误，烦请斧正，不胜感激。 1.ICP 属于point-to-point ICP，掌握思想。 目的 求解两坨点云之间的转换关系。 数学表述 给定两坨点云 X={x1,x2,...,xN}P={p1,p2,...,pN} X=\{x_1,x_2,...,x_{N}\} \\ P=\{p_1,p_2,...,p_{N}\} X={x1​,x2​,...,xN​}P=
深蓝激光slam理论与实践-第四节笔记（激光slam的前端配准方法）
weixin_46417419的博客
09-19 219
PL-ICP的原理：激光的离散导致激光前后两帧打到的不是同一物理实体，或是同一个实体的不同部位，总之它们在空间中对应的不是同一个点，然后通过匹配算法PP-ICP强行对准相邻的任意点，那么自然会引入误差（点与点进行了匹配），PL-ICP发现这个问题后，通过点与该点相邻两点的连线进行匹配，即pj1与piw线段(向量)到pj1与pj2法向量上的投影。
激光SLAM理论与实践第四章
01-20
《激光SLAM理论与实践第四章》主要涵盖了激光SLAM（Simultaneous Localization and Mapping，同时定位与建图）技术中的关键概念、算法及其在C++编程中的实现。本章节着重讲解了基于IMLS（Iterative Closest Point ...
激光slam理论与实践
热门推荐
qq_29230261的博客
10-22 1万+
本篇是记录曾书格老师的课程《激光slam理论与实践》 先贴一下个人总结（有理解的不正确的，麻烦指出来）： 第一章：激光SLAM简要介绍 1、输出Metrical map尺度地图，slam分为两种：基于滤波的 filter-based 的SLAM，和Graph-based的SLAM。 2、 （1）基于Graph-based的代表是cartographer，可以修复 t 时刻之前的误差分...
激光 SLAM 算法
04-18
激光SLAM（Simultaneous Localization And Mapping，...通过深入学习和实践开源的激光SLAM代码，不仅可以了解基本原理，还能掌握实际应用中的技巧和优化方法。这些知识对于提升机器人系统的自主性和鲁棒性至关重要。
matlab的slam代码-NDTLaserSLAM:代码没优化，速度巨慢，仅用了激光的信息，NDT配准做前端，后端批量优化，并添加图的边
06-03
代码没优化，速度巨慢，仅用了激光的信息，NDT配准做前端，后端位姿图批量优化，并添加图的边 地图更新也没写，就用的matlab的insertRay添加的，为了地图好看点还是个全SLAM，在线slam的效果8太行
激光SLAM入门笔记(五):前端配准 II
u010507357的博客
09-11 999
前端配准 II1.爬山法(拟梯度法)1.1 示意图1.2 基本思想1.3 得分函数定义1.4 算法流程1.5 伪代码2.高斯牛顿优化方法2.1 示意图2.2 数学描述2.3 优化方法的求解2.4 地图插值2.4.1 拉格朗日插值2.4.2 进行地图插值直接求简化求2.4.3 用插值函数对坐标求导3.NDT方法4.相关匹配方法及分支定界加速 内容基于深蓝学院课件，如有错误，烦请斧正，不胜感激。 1.爬山法(拟梯度法) 1.1 示意图 1.2 基本思想 搜索方向固定(只在有限的几个方向中选取，平移8个，旋
激光SLAM理论与实践 笔记 - 激光前端配准算法（基于优化/势场方法） 第四期
少磊的博客
01-28 1580
– 第五章 激光前端配准算法（基于优化/势场方法） 以下为基于势场的方法，与ICP算法不一样，以下算法都不需要匹配点；简单来说就是对某段连续的激光点（障碍物）进行高斯平滑/膨胀，再用分数评价，离激光点（障碍物）越近，得分越高（障碍物本身得分为零），人为地构造出一个得分势场； 这样只要比较得分即可，不需要匹配点。 基本数学概念： a.梯度gradient，由多元函数的各个偏导数组成的向量 以二元...
ransac算法_无人驾驶算法学习（一）：激光里程计之帧间匹配算法
weixin_39856265的博客
11-27 890
转自我的csdn博客​blog.csdn.net1. 引言激光slam主要包括前端激光里程计和后端非线性优化，由于激光雷达测量精度很高，就算没做回环和后端优化定位精度也很高。高精度地图中，里程计也是考当前帧和地图参考帧的帧间匹配算法完成位姿估计的，本文主要研究3d里程计常用的匹配算法，包括ICP算法和NDT算法。如图所示，高精度地图定位：一般利用PCL（点云库）进行帧间匹配（点云配准，regist...
[slam]点云帧间匹配
weixin_55533898的博客
01-23 1582
通过两帧数据的匹配计算两帧间的变换矩阵​。
激光slam课程学习笔记－－第６课：激光的前端配准算法
qq_45701501的博客
04-23 1241
前言：这系列笔记是学习曾书格老师的激光slam课程所得，这里分享只是个人理解，有误之处，望大佬们赐教。这节课主要介绍激光点云的前端配准算法。 １．帧间匹配算法 ［帧间匹配算法，不一定指的是前后帧情况，也可能是好几帧情况］ 1.1 ICP匹配算法 已知点的求解方法 求解证明： ［没理解，上图左侧的底部式子为零］ [正定对称举矩阵：矩阵对称，并且元素都是大于０]［正交矩阵，其逆矩阵和转置矩阵相等］ ［右侧的Track拼写有误，应该写成Ｔrace］ [老师说，slam更偏向于一个工程问题而不是一个理论问题
3D激光SLAM的帧间匹配方法： Point-to-Plane ICP、NDT、Feature-based Method
努力努力努力
10-18 3411
0、前言 在说Point-to-Plane ICP 之前，先介绍一下ICP： 经典ICP 形如求相机的旋转矩阵一样，ICP的目的很简单，就是求解两堆点云之间的变换关系。具体的做法是，既然不知道 R和t (针对刚体运动)，那我们就假设为未知量呗，然后通过某些方法求解。 假设有两堆点云，分别记为两个集合 X=x1,x2,…,xm 和 Y=y1,y2,…,ym（m并不总是等于n）。然后呢，我们不失一般性的，假设两个点云之间的变换为 R（旋转变换）和t（平移变换），这个问题描述成最小化均方误差： ...
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值