【6】lightning_lm项目-LIO 前端 -点云预处理模块

阶段二：LIO 前端 - 深入学习点云预处理模块

来看点云预处理模块，这是 LIO 前端的第一步！

先看结构很清晰，这个模块的功能：把不同厂家的雷达数据统一处理！

一、PointCloudPreprocess 类的功能

看 pointcloud_preprocess.h

这个类支持 4 种常见的激光雷达：

核心的成员变量：

LidarType lidar_type_=LidarType::AVIA;// 雷达类型intpoint_filter_num_=1;// 抽稀点数（比如设为 2，每 2 个点取 1 个）intnum_scans_=6;// 扫描线数doubleblind_=0.01;// 盲区（太近的点去掉）floattime_scale_=1e-3;// 时间戳缩放floatheight_min_=-1.0;// 高度 ROI 最小值floatheight_max_=1.0;// 高度 ROI 最大值

二、核心函数 1：通用 Process() - 处理标准 ROS PointCloud2

看 pointcloud_preprocess.cc

voidPointCloudPreprocess::Process(constsensor_msgs::msg::PointCloud2::SharedPtr&msg,PointCloudType::Ptr&pcl_out)

很简单，根据 lidar_type_ 调用对应的 Handler：

switch(lidar_type_){caseLidarType::OUST64:Oust64Handler(msg);break;caseLidarType::VELO32:VelodyneHandler(msg);break;caseLidarType::ROBOSENSE:RoboSenseHandler(msg);break;}

三、核心函数 2：Livox 专用 Process()

看 pointcloud_preprocess.cc

Livox 雷达的特点 ：非重复扫描，自带高精度时间戳！

步骤 ：

1. 数据准备

   cloud_out_.clear();cloud_full_.clear();intplsize=msg->point_num;cloud_out_.reserve(plsize);cloud_full_.resize(plsize);

2. 并行处理每个点 ， 用了并行！

   std::for_each(std::execution::par_unseq,index.begin(),index.end(),[&](constuint&i){// 抽稀if(i%point_filter_num_!=0)return;// 赋值坐标cloud_full_[i].x=msg->points[i].x;cloud_full_[i].y=msg->points[i].y;cloud_full_[i].z=msg->points[i].z;// 赋值强度cloud_full_[i].intensity=msg->points[i].reflectivity;// 赋值时间戳（从 ns 转成 ms）cloud_full_[i].time=msg->points[i].offset_time/double(1000000);// 高度 ROI 滤波if(cloud_full_[i].z<height_min_||cloud_full_[i].z>height_max_)return;// 盲区滤波 + 去重if(...&&(x²+y²+z²>blind²)){is_valid_pt[i]=1;}});

3. 收集有效点

   for(uint i=1;i<plsize;i++){if(is_valid_pt[i])cloud_out_.points.push_back(cloud_full_[i]);}

四、核心函数 3：VelodyneHandler() - 处理 Velodyne 雷达

看 pointcloud_preprocess.cc

Velodyne 雷达的特点 ：旋转扫描，有些旧款不自带时间戳！

这个函数的重点 ：如果没有时间戳， 根据旋转角度估计时间戳！

VelodyneHandler() 步骤详解：

1. 检查是否有自带时间戳

   if(pl_orig.points[plsize-1].time>0){given_offset_time_=true;// 太好了，有时间戳！}else{given_offset_time_=false;// 没有，需要自己算！// 计算第一个点和最后一个点的 yawdoubleyaw_first=atan2(pl_orig.points[0].y,pl_orig.points[0].x)*57.29578;// ...}

2. 如果没有时间戳，根据角度估计

   这是重点！

   if(!given_offset_time_){intlayer=pl_orig.points[i].ring;// 第几根线doubleyaw_angle=atan2(added_pt.y,added_pt.x)*57.2957;// 当前点的航向角if(is_first[layer]){// 这根线的第一个点yaw_fp[layer]=yaw_angle;is_first[layer]=false;added_pt.time=0.0;continue;}// 计算偏移时间：根据 yaw 差和角速度！if(yaw_angle<=yaw_fp[layer]){added_pt.time=(yaw_fp[layer]-yaw_angle)/omega_l;}else{added_pt.time=(yaw_fp[layer]-yaw_angle+360.0)/omega_l;}// 处理跨越 360 度的情况if(added_pt.time<time_last[layer]){added_pt.time+=360.0/omega_l;}}

   原理 ：

   • Velodyne 雷达是旋转的，角速度是固定的（比如 10Hz）

   • 知道了角度差，就知道时间差！

   • time = delta_angle / omega

3. 抽稀和盲区滤波

   if(i%point_filter_num_==0){if(x²+y²+z²>blind²){cloud_out_.points.push_back(added_pt);}}

五、Oust64Handler() 和 RoboSenseHandler()

看 pointcloud_preprocess.cc

这两个比较简单，因为都自带时间戳！

共同点 ：

1. 从 ROS 消息转成 PCL 点云
2. 抽稀（每 N 个点取一个）
3. 盲区滤波
4. 高度 ROI 滤波
5. 提取时间戳

六、预处理模块总结

七、为什么抽稀？

原因 ：

1. 雷达点太多了（比如 32 线雷达每秒 10 万点）
2. 太多点处理不过来
3. 相邻点信息冗余，去掉一些影响不大

抽稀方法 ：

• point_filter_num_ = 1 → 不抽稀
• point_filter_num_ = 2 → 每 2 个点取 1 个
• point_filter_num_ = 3 → 每 3 个点取 1 个

重点总结

1. 支持 4 种常见雷达 ：Livox、Velodyne、Ouster、RoboSense
2. Livox 处理 ：并行处理，速度快
3. Velodyne 特殊处理 ：没有时间戳时根据角度估计
4. 预处理的 4 个步骤 ：抽稀、盲区滤波、高度 ROI、时间戳处理

下一步

现在 LIO 前端的主要模块都学完了！

• LaserMapping 主流程
• ESKF 滤波器
• IMU 处理
• 点云预处理

接下来可以学习：

1. IVox 局部地图 （ ivox3d.h ）
2. 回环检测模块 （ loop_closing.h/cc ）
3. 定位模块 （ loc_system.h/cc ）