文章目录阶段二LIO 前端 - 深入学习点云预处理模块一、PointCloudPreprocess 类的功能二、核心函数 1通用 Process() - 处理标准 ROS PointCloud2三、核心函数 2Livox 专用 Process()四、核心函数 3VelodyneHandler() - 处理 Velodyne 雷达五、Oust64Handler() 和 RoboSenseHandler()六、预处理模块总结七、为什么抽稀重点总结下一步阶段二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.ccvoidPointCloudPreprocess::Process(constsensor_msgs::msg::PointCloud2::SharedPtrmsg,PointCloudType::Ptrpcl_out)很简单根据 lidar_type_ 调用对应的 Handlerswitch(lidar_type_){caseLidarType::OUST64:Oust64Handler(msg);break;caseLidarType::VELO32:VelodyneHandler(msg);break;caseLidarType::ROBOSENSE:RoboSenseHandler(msg);break;}三、核心函数 2Livox 专用 Process()看 pointcloud_preprocess.ccLivox 雷达的特点 非重复扫描自带高精度时间戳步骤 数据准备cloud_out_.clear();cloud_full_.clear();intplsizemsg-point_num;cloud_out_.reserve(plsize);cloud_full_.resize(plsize);并行处理每个点 用了并行std::for_each(std::execution::par_unseq,index.begin(),index.end(),[](constuinti){// 抽稀if(i%point_filter_num_!0)return;// 赋值坐标cloud_full_[i].xmsg-points[i].x;cloud_full_[i].ymsg-points[i].y;cloud_full_[i].zmsg-points[i].z;// 赋值强度cloud_full_[i].intensitymsg-points[i].reflectivity;// 赋值时间戳从 ns 转成 mscloud_full_[i].timemsg-points[i].offset_time/double(1000000);// 高度 ROI 滤波if(cloud_full_[i].zheight_min_||cloud_full_[i].zheight_max_)return;// 盲区滤波 去重if(...(x²y²z²blind²)){is_valid_pt[i]1;}});收集有效点for(uint i1;iplsize;i){if(is_valid_pt[i])cloud_out_.points.push_back(cloud_full_[i]);}四、核心函数 3VelodyneHandler() - 处理 Velodyne 雷达看 pointcloud_preprocess.ccVelodyne 雷达的特点 旋转扫描有些旧款不自带时间戳这个函数的重点 如果没有时间戳 根据旋转角度估计时间戳VelodyneHandler() 步骤详解检查是否有自带时间戳if(pl_orig.points[plsize-1].time0){given_offset_time_true;// 太好了有时间戳}else{given_offset_time_false;// 没有需要自己算// 计算第一个点和最后一个点的 yawdoubleyaw_firstatan2(pl_orig.points[0].y,pl_orig.points[0].x)*57.29578;// ...}如果没有时间戳根据角度估计这是重点if(!given_offset_time_){intlayerpl_orig.points[i].ring;// 第几根线doubleyaw_angleatan2(added_pt.y,added_pt.x)*57.2957;// 当前点的航向角if(is_first[layer]){// 这根线的第一个点yaw_fp[layer]yaw_angle;is_first[layer]false;added_pt.time0.0;continue;}// 计算偏移时间根据 yaw 差和角速度if(yaw_angleyaw_fp[layer]){added_pt.time(yaw_fp[layer]-yaw_angle)/omega_l;}else{added_pt.time(yaw_fp[layer]-yaw_angle360.0)/omega_l;}// 处理跨越 360 度的情况if(added_pt.timetime_last[layer]){added_pt.time360.0/omega_l;}}原理 Velodyne 雷达是旋转的角速度是固定的比如 10Hz知道了角度差就知道时间差time delta_angle / omega抽稀和盲区滤波if(i%point_filter_num_0){if(x²y²z²blind²){cloud_out_.points.push_back(added_pt);}}五、Oust64Handler() 和 RoboSenseHandler()看 pointcloud_preprocess.cc这两个比较简单因为都自带时间戳共同点 从 ROS 消息转成 PCL 点云抽稀每 N 个点取一个盲区滤波高度 ROI 滤波提取时间戳六、预处理模块总结七、为什么抽稀原因 雷达点太多了比如 32 线雷达每秒 10 万点太多点处理不过来相邻点信息冗余去掉一些影响不大抽稀方法 point_filter_num_ 1 → 不抽稀point_filter_num_ 2 → 每 2 个点取 1 个point_filter_num_ 3 → 每 3 个点取 1 个重点总结支持 4 种常见雷达 Livox、Velodyne、Ouster、RoboSenseLivox 处理 并行处理速度快Velodyne 特殊处理 没有时间戳时根据角度估计预处理的 4 个步骤 抽稀、盲区滤波、高度 ROI、时间戳处理下一步现在 LIO 前端的主要模块都学完了LaserMapping 主流程ESKF 滤波器IMU 处理点云预处理接下来可以学习IVox 局部地图 ivox3d.h 回环检测模块 loop_closing.h/cc 定位模块 loc_system.h/cc