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

文章目录

- [阶段二：LIO 前端 - 深入学习点云预处理模块](#阶段二：LIO 前端 - 深入学习点云预处理模块)
- - [一、PointCloudPreprocess 类的功能](#一、PointCloudPreprocess 类的功能)
  - [二、核心函数 1：通用 Process() - 处理标准 ROS PointCloud2](#二、核心函数 1：通用 Process() - 处理标准 ROS PointCloud2)
  - [三、核心函数 2：Livox 专用 Process()](#三、核心函数 2：Livox 专用 Process())
  - [四、核心函数 3：VelodyneHandler() - 处理 Velodyne 雷达](#四、核心函数 3：VelodyneHandler() - 处理 Velodyne 雷达)
  - [五、Oust64Handler() 和 RoboSenseHandler()](#五、Oust64Handler() 和 RoboSenseHandler())
  - 六、预处理模块总结
  - 七、为什么抽稀？
- 重点总结
- 下一步

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

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

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

一、PointCloudPreprocess 类的功能

看 pointcloud_preprocess.h

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

核心的成员变量：

cpp 复制代码

LidarType lidar_type_ = LidarType::AVIA;  // 雷达类型
int point_filter_num_ = 1;  // 抽稀点数（比如设为 2，每 2 个点取 1 个）
int num_scans_ = 6;  // 扫描线数
double blind_ = 0.01;  // 盲区（太近的点去掉）
float time_scale_ = 1e-3;  // 时间戳缩放
float height_min_ = -1.0;  // 高度 ROI 最小值
float height_max_ = 1.0;  // 高度 ROI 最大值

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

看 pointcloud_preprocess.cc

cpp 复制代码

void PointCloudPreprocess::Process(const sensor_msgs::msg::PointCloud2::SharedPtr &msg,
                                   PointCloudType::Ptr &pcl_out)

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

cpp 复制代码

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

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

看 pointcloud_preprocess.cc

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

步骤：

数据准备

cpp 复制代码

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

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

cpp 复制代码

std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const uint &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;
  }
});

收集有效点

cpp 复制代码

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() 步骤详解：

检查是否有自带时间戳

cpp 复制代码

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

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

这是重点！

cpp 复制代码

if (!given_offset_time_) {
  int layer = pl_orig.points[i].ring;  // 第几根线
  double yaw_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

抽稀和盲区滤波

cpp 复制代码

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、RoboSense
Livox 处理：并行处理，速度快
Velodyne 特殊处理：没有时间戳时根据角度估计
预处理的 4 个步骤：抽稀、盲区滤波、高度 ROI、时间戳处理

下一步

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

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

接下来可以学习：

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