目录

1.ikd-tree的优点? 以及在其他项目中如何使用?

2.iKFOM

3.lidar和IMU外参初始化与时间同步工具包

4.FAST-LIO-LOCALIZATION

5.UAV Avoiding Dynamic Obstacles 

---

1.ikd-tree的优点? 以及在其他项目中如何使用?

在 FAST-LIO2笔记（六）总结 中总结了ikd-tree的优点。

相比kd-tree，实现了增量更新（插入和删除）。

• 当插入新点后，自底向上更新节点属性，找到最后一个不平衡的节点，提取该子树所有点，重建一个平衡的 KD-Tree（对局部子树重建），重建过程中会把被标记为deleted  的节点（懒删除节点）真正删除。
• 当需要删除某个点时，不立即从树中移除，将节点属性 deleted = true，更新父节点属性；在进行搜索的时候跳过这个点；在每次增量操作后，主动判断平衡性，不平衡则进行重建，懒删除节点（deleted = true）被 真正删除。

根据ikd-tree源码了解具体的使用方法

• 构建平衡的 k-d 树：Build() 构建一棵结构平衡的 k-d 树，初始化使用。
• 动态插入或删除点：Add_Points() / Delete_Points() 支持在运行时动态地向树中添加点或从树中删除点。
• 删除位于指定区域中的点：Delete_Point_Boxes() 删除在指定区域内的所有点。
• K 近邻搜索（带距离限制）：Nearest_Search() 在给定范围内执行 K 近邻搜索，用于查找最近的 K 个点。
• 查找位于指定区域内的点：Box_Search() 获取树中所有位于指定区域内的点，常用于空间查询。
• 查找指定半径球体内的点：Radius_Search() 获取所有落在指定球形范围内的点，用于局部区域感知。

1.1 ikd-tree的构建  Build()

void Build(PointVector point_cloud)

// fast-lio2代码
// 初始化 ikd-tree
if(ikdtree.Root_Node == nullptr)
{
    if(feats_down_size > 5)
    {
         // 设置 KD 树的降采样参数
         ikdtree.set_downsample_param(filter_size_map_min);
         // 去畸变点云坐标(lidar)转到世界坐标系
         feats_down_world->resize(feats_down_size);
         for(int i = 0; i < feats_down_size; i++)
         {
              pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
         }
         // 使用世界坐标系的点构建ikd-tree
         ikdtree.Build(feats_down_world->points);
     }
     continue;
}

根据代码，开始的是ikd-tree是空的（没有点，根节点为空指针），用每个点对应的世界系坐标构造ikd-tree，类似于pcl的setInputCloud()

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(feats_down_world);  // feats_down_world是点云

1.2 ikd-tree添加点云  Add_Points()

// 将新的点云 增量式地插入到 IKD-Tree 中，可以选择是否在插入前进行降采样
// PointToAdd: 要插入的新点集合（vector 类型，点为 PointType）。
// downsample_on: 是否在插入前进行降采样。为 true 时，会使用之前设定的 voxel 尺寸进行体素降采样。
// 返回值是 实际插入或更新到 IKD-Tree 中的点的个数
int Add_Points(PointVector &PointToAdd, bool downsample_on)

// 代码中的使用
add_point_size = ikdtree.Add_Points(PointToAdd, true)

1.3 ikd-tree 添加区域点云 Add_Point_Boxes()

// 把区域BoxPoints内的点添加到ikd-tree
void Add_Point_Boxes(vector<BoxPointType> &BoxPoints)

1.4 ikd-tree 删除点  Delete_Points()

// 从 IKD-Tree 中删除指定的一组点
void Delete_Points(PointVector &PointToDel)

1.5 ikd-tree 删除区域点 Delete_Point_Boxes()

// 从ikd-tree 删除区域BoxPoints内的点，BoxPointType结构体表示该区域的最小、最大点
// 返回 被删除的点的总数
int Delete_Point_Boxes(vector<BoxPointType> &BoxPoints)

// 代码 动态调整地图区域lasermap_fov_segment()会用到这个，删除区域的点
// cub_needrm 根据lidar的移动情况进行修改
kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm);

删除一个节点时，先将其标记为删除，被标记为删除的节点在 kNN 搜索时会被跳过，但在树形结构中它还是存在的。只有当tree结构被彻底重构时，才会借机真的把这些节点从tree结构中删除。

1.6 在 ikd-Tree 中搜索给定目标点的 k 个最近邻点  Nearest_Search()

// 参数：目标点 point, 最近邻点的数量k_nearest, 返回找到的k 个最近邻点集合Nearest_Points, 返回每个最近邻点到目标点的距离平方值 Point_Distance
// 搜索范围限制，仅在该**最大半径（单位：米）**内查找最近邻点 max_dist

void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> &Point_Distance, float max_dist = INFINITY)

类似于PCL的nearestKSearch()函数，只不过这个是返回近邻点的索引，还需要通过索引才能找到对应的近邻点。

1.7 demo

ikd_Tree_demo这个比较简单，随机生成点，演示如何插入、删除点的操作。

ikd_Tree_Search_demo 搜索区域 和 指定半径球体内的点。

ikd_Tree_Async_demo 在ikd-Tree中，点的删除是通过在树节点上标记"deleted"来实现的，而不是立即从ikd-Tree中物理移除。这些被标记的节点会在后续重建过程中被实际移除。

注：基于FAST-LIO2,高博团队发表了Faster-LIO，把ikd-tree替换成了iVox，实现了更快的LIO。在典型的32线激光雷达中可以取得100-200Hz左右的计算频率，在固态雷达中甚至可以达到1000-2000Hz，能够达到FastLIO2的1.5-2倍左右的速度。

关于ikd-tree注释参考 FAST_LIO_SLAM 和 ikd-Tree-detailed

2.iKFOM

IKFoM（Manifold 上的迭代卡尔曼滤波器）是一个计算高效且使用方便的工具包，用于在各类机器人系统中部署迭代卡尔曼滤波器，尤其适用于运行在高维流形上的系统。它实现了一种流形嵌入式的卡尔曼滤波器，将系统的流形结构与系统描述本身进行了分离，用户只需以标准形式定义系统，并按步骤调用相应函数即可使用。

注：流行的迭代卡尔曼滤波器没有在欧式空间的好理解，相关的论文参考[2]

3.lidar和IMU外参初始化与时间同步工具包

LI-Init 是一种鲁棒的实时激光-惯性系统初始化方法。该方法可校准激光雷达与IMU之间的时间偏移和外参，同时标定重力向量和IMU零偏。本方案无需任何标定靶、辅助传感器、特定结构化环境、先验环境点云图或外参与时间偏移的初始值。技术方案解决了以下核心问题：

• 基于FAST-LIO2改进的鲁棒激光雷达里程计（FAST-LO）；
• 无需硬件同步装置即可快速稳健地完成激光-惯性标定；
• 支持多类型激光雷达：机械旋转式（Hesai/Velodyne/Ouster）与固态雷达（Livox Avia/Mid360）；
• 可作为初始化模块无缝集成至FAST-LIO2系统

注：论文翻译参考[3]

4.FAST-LIO-LOCALIZATION

 基于 FAST-LIO 构建的地图中进行重新定位，具体流程：

1）使用FAST-LIO算法得到一个pcd地图，运行重定位的时候需要载入这个地图

roslaunch fast_lio_localization localization_avia.launch map:=/path/to/your/map.pcd

这个pcd地图通过功能包pcl_ros的节点 pcd_to_pointcloud 以话题的形式发布

<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
		  args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />

 2）重定位需要提供一个初始位姿估计，也可以通过 RVIZ 中的 “2D Pose Estimate” 工具来提供这个初始猜测。

// 前三维是位置,后三维是旋转(欧拉角)
rosrun fast_lio_localization publish_initial_pose.py 14.5 -7.5 0 -0.25 0 0 

在 publish_initial_pose.py 中将这个初始位姿以话题形式发布 /initialpose

3）然后是定位 global_localization.py，计算当前帧到地图的位姿 T_map_odom

订阅FAST-LIO发布的每帧去畸变后的点云，在回调函数中将这个话题换了个名字发布/cur_scan_in_map；然后取出每个点的xyz坐标(world系)，转为 Open3D 格式点云 cur_scan；

订阅FAST-LIO发布的里程计话题，将msg赋值给cur_odom，当前帧的位姿；

初始化全局地图initialize_global_map()：rospy.wait_for_message会一直阻塞，直到收到/map的消息(第一步的结果)，其实就是获取传入的pcd文件的xyz坐标系，然后降采样，作为全局地图global_map；

rospy.logwarn('Waiting for global map......')
initialize_global_map(rospy.wait_for_message('/map', PointCloud2))



# todo:将载入地图的点云下采样存到global_map  
# 得到了一个全局地图
def initialize_global_map(pc_msg):
    global global_map

    global_map = o3d.geometry.PointCloud()
    global_map.points = o3d.utility.Vector3dVector(msg_to_array(pc_msg)[:, :3])
    global_map = voxel_down_sample(global_map, MAP_VOXEL_SIZE)
    rospy.loginfo('Global map received.')

等待初始位姿，和上一步类似，会一直阻塞直到接收到 /initialpose 消息（第二步的结果），其实就是将初始位姿赋值给pose_msg，将位姿pose_msg 转为4x4的矩阵形式表示；

开始定位global_localization() ：

已知量有哪些? 首先是全局地图global_map ，然后是当前帧的点云cur_scan，当前帧的里程计信息cur_odom，初始的位姿估计pose_msg

代码里有个trick，根据当前雷达的位姿和视野（FOV），从全局地图中裁剪出视野范围内的点云数据，作为新的全局地图global_map_in_FOV（减少后续ICP匹配的计算量）。然后用ICP计算当前帧cur_scan到新的全局地图global_map_in_FOV的位姿变换，以话题的形式发布结果 /map_to_odom

4）transform_fusion.py

订阅FAST-LIO发布的里程计话题/Odometry和定位的 /map_to_odom。transform_fusion()融合两个变换并发布当前帧的定位结果。

注：这里的的github提到两个相关的项目

• FAST-LIO-SLAM ：融合FAST-LIO与Scan-Context闭环检测模块的SLAM系统；
• LIO-SAM_based_relocalization：基于LIO-SAM实现的轻量级地图重定位系统。

前者是在fast-lio基础上新增回环(scon context)和优化(gtsam)，使其变为完整的SLAM；

后者是基于lio-sam的重定位，rviz提供初始位姿，当前帧和全局地图ICP从而实现重定位

5.UAV Avoiding Dynamic Obstacles 

无人机动态障碍物规避：FAST-LIO 在机器人路径规划中规避动态障碍的一个实际应用案例。

参考:

1.ikd-tree使用 https://zhuanlan.zhihu.com/p/529926254

2.IKFOM论文翻译 https://zhuanlan.zhihu.com/p/649097264

3.LI-Init论文翻译 https://zhuanlan.zhihu.com/p/643584117