📚 课程目录第4章基于ICP的激光雷达里程计

4. 基于ICP的激光雷达里程计

任务目标

基于连续两帧3D激光雷达点云，通过ICP（Iterative Closest Point）配准估计帧间刚体变换，进而累积得到车辆里程计轨迹。本案例聚焦ICP核心闭环：输入两帧点云，输出相对位姿变换（R, t），并验证配准精度。

环境与依赖

Ubuntu 20.04 + ROS 2 Foxy（或纯C++环境）
PCL 1.10+（点云处理与ICP）
Eigen 3.3+（线性代数）
测试数据：连续两帧Velodyne VLP-16点云（PCD格式），可从KITTI或自采数据获取

核心实现

以下代码实现基于PCL的ICP配准，输入两帧点云，输出变换矩阵。核心步骤：体素滤波降采样 → 初始对齐（可选） → ICP迭代 → 输出变换。

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/Dense>
#include <iostream>

using PointT = pcl::PointXYZI;
using Cloud = pcl::PointCloud<PointT>;

// ICP配准：返回源点云到目标点云的变换矩阵
Eigen::Matrix4f icp_registration(const Cloud::Ptr& source,
                                 const Cloud::Ptr& target,
                                 float leaf_size = 0.5f) {
    // 1. 体素滤波降采样（加速并抑制噪声）
    Cloud::Ptr src_filtered(new Cloud);
    Cloud::Ptr tgt_filtered(new Cloud);
    pcl::VoxelGrid<PointT> voxel;
    voxel.setLeafSize(leaf_size, leaf_size, leaf_size);
    voxel.setInputCloud(source);
    voxel.filter(*src_filtered);
    voxel.setInputCloud(target);
    voxel.filter(*tgt_filtered);

    // 2. 配置ICP
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setInputSource(src_filtered);
    icp.setInputTarget(tgt_filtered);
    icp.setMaxCorrespondenceDistance(2.0);   // 最大对应距离(m)
    icp.setMaximumIterations(50);            // 最大迭代次数
    icp.setTransformationEpsilon(1e-8);      // 变换增量阈值
    icp.setEuclideanFitnessEpsilon(0.01);    // 均方误差阈值

    // 3. 执行配准
    Cloud aligned;
    icp.align(aligned);

    if (!icp.hasConverged()) {
        std::cerr << "ICP未收敛!" << std::endl;
        return Eigen::Matrix4f::Identity();
    }

    std::cout << "ICP收敛，分数: " << icp.getFitnessScore() << std::endl;
    return icp.getFinalTransformation();
}

// 里程计累积：将当前帧变换累积到全局位姿
Eigen::Matrix4f accumulate_odometry(const Eigen::Matrix4f& delta,
                                    Eigen::Matrix4f& global_pose) {
    global_pose = global_pose * delta;  // 右乘：当前帧在世界坐标系下的位姿
    return global_pose;
}

int main(int argc, char** argv) {
    if (argc < 3) {
        std::cerr << "用法: " << argv[0] << " <source.pcd> <target.pcd>" << std::endl;
        return -1;
    }

    // 加载两帧点云
    Cloud::Ptr source(new Cloud);
    Cloud::Ptr target(new Cloud);
    if (pcl::io::loadPCDFile(argv[1], *source) == -1 ||
        pcl::io::loadPCDFile(argv[2], *target) == -1) {
        std::cerr << "PCD文件加载失败" << std::endl;
        return -1;
    }

    // 执行ICP
    Eigen::Matrix4f delta = icp_registration(source, target);

    // 输出帧间变换
    std::cout << "帧间变换矩阵:\n" << delta << std::endl;

    // 提取平移和旋转（用于验证）
    Eigen::Vector3f translation = delta.block<3,1>(0,3);
    Eigen::Matrix3f rotation = delta.block<3,3>(0,0);
    std::cout << "平移 (x,y,z): " << translation.transpose() << std::endl;
    std::cout << "旋转矩阵:\n" << rotation << std::endl;

    // 里程计累积示例（若处理连续帧序列）
    Eigen::Matrix4f global_pose = Eigen::Matrix4f::Identity();
    accumulate_odometry(delta, global_pose);
    std::cout << "累积位姿:\n" << global_pose << std::endl;

    return 0;
}

编译与运行

# 编译（CMakeLists.txt需链接PCL和Eigen）
mkdir -p build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4

# 运行（提供两帧PCD点云）
./icp_odometry ../data/frame_0.pcd ../data/frame_1.pcd

CMakeLists.txt关键配置：

find_package(PCL 1.10 REQUIRED COMPONENTS common io registration filters)
find_package(Eigen3 3.3 REQUIRED)
add_executable(icp_odometry icp_odometry.cpp)
target_link_libraries(icp_odometry ${PCL_LIBRARIES} Eigen3::Eigen)

验证方法与判定标准

收敛性检查：程序输出ICP收敛且分数（fitness score）小于0.1，表示配准成功。
变换合理性：若两帧来自车辆直线前进，平移向量的x分量应显著大于y、z分量；旋转矩阵应接近单位阵（小角度假设）。
可视化验证：将配准后的源点云与目标点云叠加显示，观察两者是否重合。可使用pcl_viewer：pcl_viewer aligned.pcd target.pcd（需在代码中保存aligned点云）。
里程计轨迹验证：对连续10帧以上序列重复上述过程，累积位姿应形成平滑轨迹，无跳变。

常见问题

ICP不收敛：增大setMaxCorrespondenceDistance（如5.0），或提供初始位姿估计（如使用IMU/轮速计先验）。
配准结果漂移：降采样体素尺寸过大（>1.0m）会丢失细节，建议0.3~0.5m；确保两帧间重叠度>50%。
计算耗时：若单帧点数>10万，建议先进行ROI裁剪或更粗的体素滤波（leaf_size=1.0）做粗配准，再精细配准。

本实现提供ICP里程计的最小闭环：两帧点云 → 帧间变换 → 位姿累积。实际自动驾驶系统中，还需结合IMU进行运动预测初始化、加入地面分割与动态物体剔除，但核心配准逻辑与本案例一致。