1 基本方法

1.1 点云表示

点云数据中包含了每个点的经纬度坐标、强度、多次回波、颜色等信息。3D激光点云数据里所包含的信息：
（1）x.y.z坐标信息
根据采集的x.y.z坐标，可直接获取被测物体的三维结构信息，三维结构信息是其他地理信息的载体。
（2）回波次数
回波数是某个给定脉冲的回波总数。从激光雷达系统发射的激光脉冲会从地表面和地表上的物体反射：汽车、行人、以及桥梁等等。
发射出的一个激光脉冲可能会以一个或多个回波的形式返回到激光雷达传感器。任何发射出的激光脉冲在向地面传播时，如果遇到多个反射表面则会被分割成与反射表面一样多的回波。
(3) 强度信息
强度是反映生成某点的激光雷达脉冲回波强度的一种测量指标(针对每个点而采集)。不同物体对于激光雷达的反射程度不同，所以通过强度信号可以对物体进行区分。
(4) 类别
每个经过处理的激光雷达点可拥有定义反射激光雷达脉冲对象类型的分类。可将激光雷达点分成多个类别，包括汽车、路面等。
(5) RGB
可以将RGB(红、绿、蓝)波段作为激光雷达数据的属性。此属性通常来自激光雷达测量时采集的影像。
(6) 扫描方向
扫描方向是激光脉冲向外发射时激光扫描镜的行进方向。

用数学的方式描述：
(1) 空间中的一个点：

（2）所有的点构成点云：

点云质心:
点云的重心是一个点坐标，计算出云中所有点的平均值, 可以说它是“质量中心”，认为是物体质量集中于此点的假想点。
物体质心公式：

在点云中，各点质量为1:

1.2 ICP方法

1.2.1 问题建模

问题模型：求解两帧点云(P和Q)之间的变换关系，找到最佳的旋转R和平移t, 使得经过变换后两帧点云之间的距离误差最小。
假设3D传感器在不同位置下采集两帧点云：

转化为最小二乘优化问题：

在误差函数中加入质心：

1.2.2 算法流程

计算流程如下：

1.2.3 旋转计算(SVD)

按照上面过程，只需要重点解决R的计算，就可以解决平移，接下对R的优化函数进行展开简化：

关于SVD分解的原理可以看《矩阵论》里面的矩阵分解章节，不进行具体介绍。

1.3 NDT方法

NDT是正态分布变换算法，基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。
有原始两帧点云数据，其中参考数据点云为X：

1.3.1 预处理

划分网格，计算统计特性

1.3.2 问题建模

构造最小误差问题模型

从形式来看，NDT形式是多维正态分布的极大似然函数，NDT匹配计算的本质就是一个多维正态分布的极大似然估计。

1.3.3 优化求解

优化方法在实现上采用了牛顿法，但是为了增加鲁棒性，只采用了牛顿法计算出来的迭代方向作为步长搜索的初始方向。
优化问题属于非线性优化的基本问题，可以参照相关大学或研究生优化课程，不详细介绍。非线性优化方法小结

改进NDT
在点云预处理阶段，采用不同的采用策略，比如随机采用，均匀采样，法空间采样等，提高效率。
在栅格策阶段，采用从固定尺寸栅格到基于八叉树的变化尺寸策略。

参考资料

[1] https://www.cnblogs.com/lvchaoshun/p/7823306.html
[2] https://zhuanlan.zhihu.com/p/56598915
[3] https://www.cnblogs.com/YongQiVisionIMAX/p/13871721.html

SLAM算法 - 3D激光匹配算法相关推荐

SLAM学习--2D激光SLAM--入门学习
一.学习心得记录(不一定对):转载了很多博主的网站,若侵权,告知必删 1. 激光slam 和视觉slam 实际在实现的时候完全是两码事,可能根本思想差不多,特别是非线性优化方面,但是实现方案差距较大, ...
开源3D激光（视觉）SLAM算法汇总（持续更新）
目录一.Cartographer 二.hdl_graph_slam 三.LOAM 四.LeGO-LOAM 五.LIO-SAM 六.S-LOAM 七.M-LOAM 八.livox-loam 九.Liv ...
3D激光雷达SLAM算法学习01——3D激光SLAM整体框架
#1.3D激光雷达系列主要内容结合个人毕业设计和日后发展方向,开始学习3D激光雷达的相关感知算法,预计SLAM(定位和建图)开始,本篇给出简单的框架和绪论,后边会按照图优化/滤波原理.传感器基础.数 ...
3D激光SLAM:LOAM 论文--算法详细解读
3D激光SLAM:LOAM 论文--算法详细解读 LOAM简介论文里面的符号表示算法部分激光雷达里程计 A 特征点提取 B 找特征点的匹配对 C 运动估计 lidar 建图测试结果 LOAM是 ...
激光SLAM算法学习（三）——3D激光SLAM
3D激光SLAM 1.3D激光SLAM的介绍 3D激光SLAM的输入: IMU数据 3D激光雷达数据里程计数据 3D激光SLAM的输出: 3D点云地图机器人的轨迹 or PoseGraph 2.3 ...
MULLS：一种基于多尺度线性最小二乘的激光SLAM算法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨paopaoslam 来源丨泡泡机器人SLAM 标题:MULLS: Versatile LiD ...
SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中. 由于参与分享汇报的同事有许多是做其他方向的机器人工程师(包括硬件.控制等各方面并 ...
激光slam课程学习笔记－－第11课：3D激光SLAM介绍
前言:这系列笔记是学习曾书格老师的激光slam课程所得,这里分享只是个人理解,有误之处,望大佬们赐教.这节课主要介绍一些3d slam数学知识,以及典型代表loam. 1. 3d激光SLAM 1.1 ...
彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总！
地图定位算法是自动驾驶模块的核心,而激光SLAM则是地图定位算法的关键技术,其重要性不言而喻,在许多AI产品中应用非常多(包括但不限于自动驾驶.移动机器人.扫地机等).相比于传统的视觉传感器,激光传感 ...
激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
应用背景介绍自主导航是机器人与自动驾驶的核心功能,而SLAM技术是实现自主导航的前提与关键.现有的机器人与自动驾驶车辆往往会安装激光雷达,相机,IMU,GPS等多种模态的传感器,而且已有许多优秀的激 ...

SLAM算法 - 3D激光匹配算法

3D激光匹配算法