目录fastlio2介绍fastlio2依赖库ikd-tree流形计算库IKFOMeigen3sophuspcl1.12fastlio2编译方式fastlio2代码框架论文内容介绍代码总体框架数据预处理和打包IMU初始化与状态传播误差状态卡尔曼滤波器ESKFESKF代码解析建图与⾥程计发布ikdtree介绍kd-tree的近邻搜索kd-tree的特点地图管理点的插⼊点的删除再平衡fastlio2介绍香港大学MaRS实验室论文(IEEE-RAL,TRO) 高计算效率、高鲁棒性的雷达惯性里程计 紧耦合迭代卡尔曼滤波融合IMU和LiDAR FAST-LIO2在FAST-LIO基础上提出了ikdtree 数据结构 实现增量式的地图更新 代码已开源至Github。与其它开源框架相比具有更优越的表 现精度、计算速度 是目前最先进的开源LIO框架之一 涵盖流形、李群李代数、IMU积分、雷达残差、卡尔曼滤波等多方位的知识 学会FAST-LIO有助于理解其它多传感器融合框架 代码可读性较好适合初学者上手。原版代码https://github.com/hku-mars/FAST_LIOfastlio2依赖库ikd-treehttps://github.com/hku-mars/ikd-Tree下载源码放在/include/ikdtree目录下。流形计算库IKFOM局部具有欧⼏⾥得空间性质的空间,在数学中⽤于描述⼏何形体,我们所能观察到的数据实际上是由⼀个低维流形映射到⾼维空间上的即这些数据所在的空间是“嵌⼊在⾼维空间的低维流形”,流形是⼀个“空间”⽽不是⼀个“形状”.eigen3sudo apt-get updatesudo apt install libeigen3-dev安装后文件通常位于/usr/local/include/eigen3sophus在Eigen基础上提供李群、李代数运算安装方法如下git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout a621ff mkdir build cd build cmake ../ -DUSE_BASIC_LOGGINGON make sudo make installpcl1.12sudo apt update sudo apt upgrade -ysudo apt install cmake git libpcl-devfastlio2编译方式ros1的catkin_make编译后续需要修改为cmakelists.txt的传统通用编译方式便于定制化管理与部署。cd ~/$A_ROS_DIR$/src git clone https://github.com/hku-mars/FAST_LIO.git cd FAST_LIO git submodule update --init cd ../.. catkin_make source devel/setup.bashfastlio2代码框架论文内容介绍1 雷达点云预处理点云累积、特征提取2 IMU数据前向传播、反向传播、运动补偿3 状态估计模块残差计算、迭代更新4 发布⾥程计、更新地图Fast-lio2与Fast-lio1的区别1 省略了特征提取模块2 只计算⾯点残差3 添加了对外参的优化4 添加ikdtree数据结构代码总体框架laserMapping.cpp: 主函数负责统筹所有进程接收话题建图可视化IMU_Processing.cpp: IMU前向传播、反向传播把传播后的状态量、协方差矩阵、点云返回laserMappingpreprocess.cpp: 对点云进行预处理格式转换use-ikfom.hpp: 状态量的定义生成前向传播的状态转移矩阵esekfom.hpp: 广义加减法定义前向传播函数计算残差以及雅可比eskf主函数。数据预处理和打包IMU初始化与状态传播误差状态卡尔曼滤波器ESKF传统EKF的运动⽅程和观测⽅程xkf(xk−1,uk) wkzkh(xk) vk误差状态卡尔曼滤波器的运动⽅程和观测⽅程kf(k−1 ,uk) wkzkh(k) vk即上式把x⽤代替。有了误差量的估计再通过x⊞计算最优估计。这将带来以下好处ESKF的状态变量可以采⽤最⼩化的参数表达⽽传统KF需要⽤到四元数(4维)或者更⾼维的表达(旋 转矩阵9维) ESKF总是在原点附近离奇异点较远并且也不会由于离⼯作点太远⽽导致线性化近似不够的问题 ESKF的状态量为⼩量其⼆阶变量相对来说可以忽略。⼤多数雅可⽐矩阵在⼩量情况下变得⾮常简单甚⾄可以⽤单位阵代替。ESKF代码解析建图与⾥程计发布ikdtree介绍Kd-TreeK-dimensional tree是⼀种⾼维索引树形数据结构经常使⽤于在⼤规模的⾼维数据空间进⾏最近邻查找。 kd树结构类似于⾼维的⼆叉树树中存储的是⼀些K维数据。kd-tree的构建步骤1 对数据在每个维度的⽅差进⾏计算选取⽅差最⼤的维度作为划分维度kv;2 计算数据在kv维度的中位数根据中位数将数据划分为2个⼦集在kv维度上⼩于等于中位数的3 数据放⼊左边⼦集⼤于中位数的数据放⼊右边⼦集;4 对每个⼦集重复1,2步骤直⾄⼦集不能再划分为⽌kd-tree的近邻搜索1 从根节点开始对各个节点进⾏访问直⾄访问⾄叶⼦结点计算与叶⼦节点内数据的距离2 访问过程在节点的维度与节点的划分⼤⼩进⾏⽐较3 因为每次访问时只在⼀个维度⽐较⼤⼩所以此时得到的最近点并不⼀定为最⼩距离的点未被访 问的分⽀内话可能存在距离更⼩的点。还需要对未被访问的分⽀进⾏回溯4 进⾏回溯操作判断其⽗节点下未被访问过的分⽀⾥是否还有更近的点如果有进⾏同样操作并更新距离回溯的判断过程是从下往上进⾏的直到回溯到根结点时已经不存在与P更近的分⽀为⽌kd-tree的特点数据为多维数据不同树节点根据不同维度进⾏数据划分⽗节点将所有数据划分⾄左右⼦树叶⼦节点包含所有数据适⽤于动态程度不⾼的数据插⼊删除不⽅便不是动态增量式的结构每次需要重新构建kdtreeA-LOAMkdtree的深度会影响搜索速度在建图过程中树的结构可能不平衡地图管理地图由ikd-tree组成仅保留当⻓度为L的⼤⽴⽅体的点云当探测球体触碰到边界时地图沿接触的边界⽅向移动橙⾊部分的点云被删除并新增点云点的插⼊1 ⾸先将整个空间体素化并明确新点落⼊哪个体素2 然后向ikd-Tree查询⽬标体素内是否已经有点以及有哪些点3 如果已经有点了将已有点和新点⼀起排序找出离体素中⼼最近的那个点然后做判断如果最 近的点是已有点意味着新点⽆必要再插⼊了结束处理如果最近的点是新点则把已有点全部 标记删除并插⼊新点如果体素内尚不存在点则直接插⼊新点。点的删除1 从根节点开始在每个节点处⾸先判断节点⾃⾝是否在灰⾊区域内若是则标记删除deletedtrue2 判断两个⼦节点的range是否与删除区域有交叉若⽆则直接剪枝3 若有则重复上述过程4 直⾄搜索完所有的⼦节点完成所有被删除节点的标记删除5 每次增量操作之后对treesizeinvalidnumrangetreedeleted进⾏更新6 被删除的节点不会⽴即删除⽽是令deletedtrue最后在re-balancing环节删掉再平衡如果需要重建的(sub)tree规模很⼤,重建时间花销不可忽略如果在主线程中执⾏重建就会导致ikd-Tree⼀直被占⽤外部的SLAM/LIO算法⽆法访问ikd-Tree执⾏查询或增删操作,导致算法阻塞。 针对这种情况ikd-Tree采取了⼀种双线程策略。耗时的重建过程在额外的线程中执⾏主线程仍 旧对外提供查询和增删服务其中涉及到“正在重建的(sub)tree”的增删操作会被额外缓存到⼀个叫 作OperationLogger的容器中待额外线程中完成了重建后会从OperationLogger把增删操作“补作 业”到新的(sub)tree上这样就确保了新的(sub)tree没有漏掉任何过程最后把新⼦树替换到相应位置。