4. 障碍物检测实战：从点云到目标跟踪

好，咱们终于到了最激动人心的部分——障碍物检测实战。前面几章我们聊了滤波、分割、聚类，说白了都是在为这一步做准备。今天我要带你走完一整套流程：从生成Bounding Box开始，到分类、跟踪，最后聊聊LiDAR和Camera怎么融合。

我个人习惯把障碍物检测拆成四个模块：检测 → 分类 → 跟踪 → 融合。每个模块都有坑，也有技巧。咱们一个一个来。

4.1 Bounding Box生成：从点云到3D框

聚类完成后，我们得到了一堆点云簇。每个簇代表一个潜在的障碍物。接下来要做的，就是给每个簇套上一个3D框——也就是Bounding Box。

生成Bounding Box的方法有很多。最简单的就是轴对齐包围盒（AABB），直接取点云在X、Y、Z三个方向上的最大最小值。但说实话，AABB有个致命问题：它不随物体旋转。你想想看，一辆斜着停的车，AABB框出来会多出一大块空白区域。

所以我更推荐方向包围盒（OBB）。OBB会计算点云的主方向，然后沿着这个方向生成框。怎么做？

1. 计算协方差矩阵：对簇内所有点的坐标求协方差
2. 特征值分解：得到三个主方向（特征向量）
3. 投影计算：将点云投影到主方向上，得到长宽高
4. 生成框体：用中心点、尺寸、方向角表示

// C++示例：基于PCA的OBB生成
Eigen::Vector3d computeOBB(const std::vector<PointXYZ>& cluster,
                           Eigen::Vector3d& center,
                           Eigen::Vector3d& size,
                           Eigen::Matrix3d& rotation) {
    // 1. 计算质心
    center = Eigen::Vector3d::Zero();
    for (const auto& p : cluster) {
        center += Eigen::Vector3d(p.x, p.y, p.z);
    }
    center /= cluster.size();
    
    // 2. 计算协方差矩阵
    Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
    for (const auto& p : cluster) {
        Eigen::Vector3d diff(p.x - center.x(), 
                             p.y - center.y(), 
                             p.z - center.z());
        cov += diff * diff.transpose();
    }
    cov /= cluster.size();
    
    // 3. 特征值分解
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov);
    rotation = solver.eigenvectors();
    
    // 4. 投影计算尺寸
    Eigen::Vector3d min_pt = Eigen::Vector3d::Constant(1e10);
    Eigen::Vector3d max_pt = Eigen::Vector3d::Constant(-1e10);
    for (const auto& p : cluster) {
        Eigen::Vector3d local = rotation.transpose() * 
                               (Eigen::Vector3d(p.x, p.y, p.z) - center);
        min_pt = min_pt.cwiseMin(local);
        max_pt = max_pt.cwiseMax(local);
    }
    size = max_pt - min_pt;
    
    return center;
}

4.2 障碍物分类：车辆、行人、自行车

框出来了，接下来要判断框里是什么。是车？是人？还是自行车？这一步叫障碍物分类。

我记得刚入行时，团队用的是手工特征+分类器。比如提取点云的尺寸特征（长宽高比）、形状特征（点云分布方差）、反射强度特征（平均强度、强度方差）。然后用SVM或随机森林做分类。

但说实话，手工特征上限有限。现在主流做法是用PointNet++或VoxelNet这类深度学习模型。不过，如果你资源有限，或者想快速上线，手工特征依然是个不错的选择。

特征类型	具体特征	区分能力
尺寸特征	长、宽、高、体积、长宽比	车辆 vs 行人（车辆更大）
形状特征	点云分布方差、主成分占比	车辆（扁平） vs 行人（细长）
强度特征	平均反射强度、强度直方图	金属（车） vs 非金属（人）
运动特征	速度、加速度、运动方向	静止车辆 vs 行人（速度不同）

4.3 基于点云的目标跟踪

分类完了，但单帧检测是不够的。你想想看，如果每帧都独立检测，那同一个目标在前后帧的ID会变来变去。这就是目标跟踪要解决的问题。

跟踪的核心是数据关联和状态估计。我常用的组合是：

• 状态估计：卡尔曼滤波（Kalman Filter）——预测目标下一帧的位置和速度
• 数据关联：匈牙利算法（Hungarian Algorithm）——将当前帧的检测框和已有的跟踪轨迹匹配

卡尔曼滤波的模型很简单：

// 状态向量: [x, y, z, vx, vy, vz, l, w, h]
// 观测向量: [x, y, z, l, w, h]
// 预测方程: x_k = F * x_{k-1} + w
// 更新方程: z_k = H * x_k + v

// 关键参数设置
float dt = 0.1;  // 帧间隔，10Hz LiDAR
Eigen::MatrixXf F(9, 9);  // 状态转移矩阵
F.setIdentity();
F(0, 3) = dt;  // x = x + vx * dt
F(1, 4) = dt;  // y = y + vy * dt
F(2, 5) = dt;  // z = z + vz * dt

Eigen::MatrixXf H(6, 9);  // 观测矩阵
H.setZero();
H(0, 0) = 1;  // 观测x
H(1, 1) = 1;  // 观测y
H(2, 2) = 1;  // 观测z
H(3, 6) = 1;  // 观测l
H(4, 7) = 1;  // 观测w
H(5, 8) = 1;  // 观测h

4.4 多传感器融合：LiDAR + Camera

LiDAR精度高，但稀疏；Camera信息丰富，但受光照影响大。两者融合，取长补短。

融合分前融合和后融合。我个人的经验是：

• 前融合：在数据层面融合。比如把图像特征投影到点云上，或者把点云投影到图像上。好处是信息损失少，但计算量大。
• 后融合：在目标层面融合。LiDAR和Camera各自检测，然后用匈牙利算法匹配。好处是模块解耦，但可能漏掉一些信息。

我项目中用得最多的是后融合，因为调试方便。具体做法：

1. 时间同步：确保LiDAR和Camera的时间戳对齐。一般用硬件触发或软件插值。
2. 空间对齐：通过标定参数，将LiDAR点云投影到图像坐标系。
3. 目标匹配：用IoU或中心距离匹配LiDAR框和Camera框。
4. 融合输出：匹配上的目标，取置信度高的分类结果；未匹配的目标，保留单传感器的结果。

// 后融合伪代码
void fuseDetections(const std::vector<DetectedObject>& lidar_objs,
                    const std::vector<DetectedObject>& camera_objs,
                    std::vector<FusedObject>& fused_objs) {
    // 1. 构建代价矩阵
    Eigen::MatrixXf cost(lidar_objs.size(), camera_objs.size());
    for (int i = 0; i < lidar_objs.size(); i++) {
        for (int j = 0; j < camera_objs.size(); j++) {
            float iou = computeIoU3D(lidar_objs[i].bbox, camera_objs[j].bbox);
            cost(i, j) = 1.0 - iou;  // 匈牙利算法求最小代价
        }
    }
    
    // 2. 匈牙利匹配
    std::vector<std::pair<int, int>> matches;
    hungarianMatch(cost, matches);
    
    // 3. 融合策略
    for (auto& match : matches) {
        FusedObject fused;
        fused.bbox = lidar_objs[match.first].bbox;  // 位置用LiDAR的
        // 分类用置信度高的
        if (lidar_objs[match.first].confidence > camera_objs[match.second].confidence) {
            fused.label = lidar_objs[match.first].label;
        } else {
            fused.label = camera_objs[match.second].label;
        }
        fused_objs.push_back(fused);
    }
}

好了，这一章的内容就到这里。从Bounding Box生成到多传感器融合，每一步都有细节和技巧。希望你能动手跑一遍代码，遇到问题随时回来翻翻这一章。