4. 传感器数据处理：激光雷达点云数据读取与可视化、相机图像数据读取与畸变校正、IMU数据融合与滤波

各位同学，欢迎来到第四章。说实话，传感器数据处理是整个自动驾驶感知系统的基石。你想想看，如果原始数据都处理不好，后面那些花里胡哨的算法再牛也白搭。我在项目里见过太多因为传感器标定没做好、数据同步出问题，导致整个系统崩掉的案例。今天我们就来把这几个核心传感器——激光雷达、相机、IMU——的数据处理流程彻底讲透。

4.1 激光雷达点云数据读取与可视化

激光雷达，说白了就是给车装了一双「激光眼」。它通过发射激光束并测量反射时间，来获取周围物体的三维坐标。我个人习惯用 Velodyne 或 Livox 的驱动，但 ROS2 里其实已经封装好了通用的接口。

4.1.1 点云数据读取

在 ROS2 中，激光雷达数据通常以 sensor_msgs::msg::PointCloud2 消息格式发布。读取它其实很简单，核心就是订阅话题。嗯，这里要注意：不同雷达的帧率、坐标系定义可能不同，一定要先看文档。

// C++ 示例：订阅点云话题
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

class LidarSubscriber : public rclcpp::Node
{
public:
    LidarSubscriber() : Node("lidar_subscriber")
    {
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/livox/lidar", 10,
            std::bind(&LidarSubscriber::topic_callback, this, std::placeholders::_1));
    }

private:
    void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "收到点云数据，宽度: %d, 高度: %d", msg->width, msg->height);
        // 这里可以进一步处理点云数据
    }
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
};

4.1.2 点云可视化

光读取还不够，你得看到点云长什么样。ROS2 里最常用的可视化工具是 RViz2。但如果你想在代码里直接渲染，我推荐用 PCL（Point Cloud Library） 配合 pcl_ros 桥接。

# Python 示例：使用 PCL 可视化点云
import pcl
import pcl.pcl_visualization

# 假设你已经有了点云数据 cloud
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowMonochromeCloud(cloud, b"cloud")
while True:
    pass  # 保持窗口打开

不过说实话，实际工程中我更推荐直接用 RViz2 做可视化。它支持点云着色、距离测量、多传感器叠加显示，调试起来非常方便。你只需要在 RViz2 中添加一个 PointCloud2 显示插件，选择对应的话题即可。

4.2 相机图像数据读取与畸变校正

相机是感知系统的另一只眼睛。但相机有个天生的毛病——畸变。尤其是广角镜头，边缘的物体都变形了。你想想看，如果不对图像做畸变校正，后面的目标检测、车道线识别全都会偏掉。

4.2.1 图像数据读取

ROS2 中图像数据通过 sensor_msgs::msg::Image 消息传递。读取方式跟点云类似，但要注意图像编码格式（RGB8、BGR8、MONO8 等）。

# Python 示例：订阅图像话题并转换为 OpenCV 格式
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        self.subscription = self.create_subscription(
            Image, '/camera/image_raw', self.listener_callback, 10)
        self.bridge = CvBridge()

    def listener_callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        self.get_logger().info(f'收到图像，尺寸: {cv_image.shape}')

4.2.2 畸变校正

畸变校正的原理其实不复杂。相机标定后会得到内参矩阵和畸变系数，然后用 OpenCV 的 cv2.undistort() 函数就能搞定。但这里有个坑：校正后的图像可能会丢失边缘像素。

# 畸变校正示例
import cv2
import numpy as np

# 假设你已经标定得到了内参矩阵 mtx 和畸变系数 dist
def undistort_image(img, mtx, dist):
    h, w = img.shape[:2]
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
    dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
    # 裁剪掉黑边
    x, y, w, h = roi
    dst = dst[y:y+h, x:x+w]
    return dst

4.3 IMU数据融合与滤波

IMU（惯性测量单元）提供加速度和角速度数据。但 IMU 有个致命弱点——漂移。你拿着手机原地转一圈，IMU 算出来的角度可能已经偏了十几度。所以必须做融合滤波。

4.3.1 IMU数据读取

ROS2 中 IMU 数据通过 sensor_msgs::msg::Imu 消息发布。它包含线加速度、角速度以及方向四元数（如果有磁力计辅助）。

// C++ 示例：读取 IMU 数据
void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    // 获取线加速度 (m/s^2)
    double ax = msg->linear_acceleration.x;
    double ay = msg->linear_acceleration.y;
    double az = msg->linear_acceleration.z;
    
    // 获取角速度 (rad/s)
    double gx = msg->angular_velocity.x;
    double gy = msg->angular_velocity.y;
    double gz = msg->angular_velocity.z;
    
    // 注意：这里的加速度包含了重力分量，使用前需要去除
}

4.3.2 数据融合与滤波

IMU 数据融合最经典的方法就是 卡尔曼滤波 和 互补滤波。我个人更推荐互补滤波，因为它计算量小、实时性好，适合嵌入式平台。

互补滤波的核心思想很简单：加速度计提供长期稳定的姿态参考（但短期噪声大），陀螺仪提供短期精确的角度变化（但长期会漂移）。把两者按比例融合，就能得到既稳定又精确的姿态估计。

# 互补滤波示例（简化版）
def complementary_filter(accel, gyro, dt, alpha=0.98):
    """
    accel: 从加速度计计算的俯仰/横滚角 (rad)
    gyro: 陀螺仪角速度 (rad/s)
    dt: 时间间隔 (s)
    alpha: 融合系数，通常取 0.98
    """
    # 陀螺仪积分得到角度变化
    gyro_angle = gyro * dt
    # 互补融合
    fused_angle = alpha * (fused_angle + gyro_angle) + (1 - alpha) * accel
    return fused_angle

4.4 本章知识体系总览

为了让你更直观地理解这三个传感器数据处理的关系，我画了一张流程图。它展示了从原始数据到可用信息的完整链路。

你看，这三个传感器各有各的脾气。激光雷达精度高但稀疏，相机信息丰富但受光照影响大，IMU 实时性好但会漂移。把它们的数据处理好、融合好，才是自动驾驶感知系统的真正功底。

好了，这一章的内容就到这里。记住我强调的几个关键点：点云读取注意 QoS 设置、畸变校正记得保留视野、IMU 融合选互补滤波更实用。下一章我们会把这些处理好的数据送到感知算法里，开始真正的目标检测和跟踪。