点云配准

摘要：本文整理 ICP、点到平面 ICP 与 NDT 点云配准，从刚体变换、误差模型和初始位姿理解算法流程。各方法分别配合 PCL 核心代码，讲解输入准备、参数配置、迭代求解与配准结果检查。

1. 点云配准的目标

配准估计两份点云之间的空间变换，使同一场景的表面尽可能重合。源点云（source）是待变换的点云，目标点云（target）是参考点云。帧间配准可用于里程计，当前帧与地图配准可用于定位，但动态物体和不充分的几何结构可能干扰位姿估计。

本文采用列向量表示，源点 $p_i$ 变换到目标坐标系：

q_i\approx Rp_i+t,\qquad T_{\mathrm{target}\leftarrow\mathrm{source}}= \begin{bmatrix}R&t\\0&1\end{bmatrix}.

$R\in SO(3)$ 表示旋转， $t\in\mathbb{R}^3$ 表示平移。明确变换方向可以避免把返回矩阵直接当成其逆矩阵使用。

下文代码为处理函数内的教学片段，点云非空、已去除 NaN/Inf，长度单位为米。参数是示例值，实际需要结合场景与分辨率设置。

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <Eigen/Geometry>
#include <cmath>

using PointT = pcl::PointXYZ;
using Cloud = pcl::PointCloud<PointT>;
// source、target 是已加载的 Cloud::Ptr。

1.1 代码讲解：提供初始位姿

const float yaw = 0.05f;  // 弧度：示例初始偏航角
Eigen::Affine3f initial = Eigen::Translation3f(0.5f, 0.0f, 0.0f)
    * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ());
Eigen::Matrix4f guess = initial.matrix();

guess 也应表示源到目标的变换。工程中初值可来自上一帧位姿、IMU、轮速计或其他定位结果。ICP 与 NDT 都是局部优化方法，初值偏差较大时可能落入错误的局部最优。

2. ICP（点到点迭代最近点）

参考：PCL ICP 教程。

流程：按当前位姿变换源点 → 搜索目标中的对应点 → 剔除过远对应 → 求解增量变换 → 更新位姿 → 检查停止条件。

设 $\pi(i)$ 为源点当前匹配到的目标点索引，点到点误差为：

\min_{R,t}\sum_{i\in\mathcal{C}} \|Rp_i+t-q_{\pi(i)}\|_2^2.

对应集合 $\mathcal{C}$ 会随迭代改变，最近邻不一定是真实的同一表面点。通常需要足够重叠和较好的初始位姿。

配置项	示例值	作用
`setMaxCorrespondenceDistance`	`1.0`	允许对应点的最大距离 [m]
`setMaximumIterations`	`50`	最大迭代次数
`setTransformationEpsilon`	`1e-6`	相邻变换的平移变化停止阈值，PCL 使用平方平移量
`setEuclideanFitnessEpsilon`	`1e-6`	配准误差变化的停止阈值

2.1 代码讲解：设置输入与对应范围

#include <pcl/registration/icp.h>

pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputSource(source);
icp.setInputTarget(target);
icp.setMaxCorrespondenceDistance(1.0);
icp.setMaximumIterations(50);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);

setInputSource 指定待移动点云，setInputTarget 指定参考。对应距离太小可能找不到足够匹配，太大则可能连接错误结构。增加迭代次数无法补救完全错误的初值，也不能解决几何退化。

2.2 代码讲解：执行迭代并读取变换

Cloud aligned;
icp.align(aligned, guess);
if (!icp.hasConverged()) return false;  // 位于返回 bool 的处理函数中
const Eigen::Matrix4f T = icp.getFinalTransformation();
const double score = icp.getFitnessScore();

align 从 guess 开始迭代，aligned 是变换后的源点云。getFinalTransformation 返回包含初值作用的最终源到目标变换，不应再乘一次 guess。默认 getFitnessScore 衡量变换后源点与目标最近邻的平均平方距离，不是精度百分比。

2.3 代码讲解：将结果应用到原始分辨率

Cloud aligned_full;
pcl::transformPointCloud(*source_full, aligned_full, T);

若 source 是下采样点云，source_full 是同一坐标系的原始点云，直接用最终矩阵变换即可恢复完整输出，无需维护下采样索引。先把源点云按初值手动变换后，又将同一初值传给 align，会重复施加初始变换。

3. Point-to-Plane ICP（点到平面 ICP）

原理：使用目标表面法向量，最小化源点到目标局部切平面的距离：

\min_{R,t}\sum_{i\in\mathcal{C}} \left[n_{\pi(i)}^T(Rp_i+t-q_{\pi(i)})\right]^2.

$n_{\pi(i)}$ 是目标对应点处的单位法向量。它强调法向方向的残差，在初值较好、局部表面平滑时常有较好的局部收敛表现；单个大平面不能充分约束平面内平移及绕法向的旋转。

配置项	示例值	作用
法向估计半径	`0.5 m`	影响法向稳定性与边缘细节
最大对应距离	`1.0 m`	限制错误对应
最大迭代次数	`50`	限制计算量

3.1 代码讲解：估计目标法向量

#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/io.h>

pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
ne.setInputCloud(target);
ne.setSearchMethod(tree);
ne.setRadiusSearch(0.5);
ne.compute(*normals);

pcl::PointCloud<pcl::PointNormal>::Ptr target_n(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*target, *normals, *target_n);

邻域估计法向量后，concatenateFields 将位置与法向字段组合。半径太小，稀疏区域可能无法估计可靠法向；太大则会跨越边缘混合不同表面。参与配准前，应删除无效法向、保留一致的点与法向对应关系，并检查剩余点数。

3.2 代码讲解：显式选择点到平面误差

下面假设 target_n 已移除无效法向。源使用位置点类型，目标使用带法向的点类型，明确指定线性最小二乘的点到平面估计器。

#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>

using TargetT = pcl::PointNormal;
using Estimator = pcl::registration::TransformationEstimationPointToPlaneLLS<
    PointT, TargetT>;
pcl::IterativeClosestPoint<PointT, TargetT> plane_icp;
Estimator::Ptr estimator(new Estimator);
plane_icp.setTransformationEstimation(estimator);
plane_icp.setInputSource(source);
plane_icp.setInputTarget(target_n);
plane_icp.setMaxCorrespondenceDistance(1.0);
plane_icp.setMaximumIterations(50);
plane_icp.setTransformationEpsilon(1e-6);
Cloud plane_aligned;
plane_icp.align(plane_aligned, guess);

更换点类型本身不等于更换优化目标，setTransformationEstimation 才明确指定点到平面求解器。LLS 使用小增量近似，仍然依赖初值；最近邻搜索和迭代框架由 ICP 提供。完成后也应检查 hasConverged、对应数量和最终几何重合，不能只看残差下降。

参考：PCL 点到平面估计器接口。

4. NDT（正态分布变换）

参考：PCL NDT 教程。

原理：将目标点云组织成网格，在有效网格中用均值与协方差描述点的分布。源点变换后，通过所在区域的分布评价匹配程度。

\mu_k=\frac{1}{n_k}\sum_{q_i\in V_k}q_i,\qquad \Sigma_k=\frac{1}{n_k-1}\sum_{q_i\in V_k} (q_i-\mu_k)(q_i-\mu_k)^T.

用简化的高斯模型表达局部评价：

P(x)\propto\exp\left[-\frac12 (x-\mu_k)^T\Sigma_k^{-1}(x-\mu_k)\right].

这是解释 NDT 的模型表达，并非 PCL 完整目标函数。有效网格需要足够点数，协方差还需处理退化情况；无需手动对每个源点建立一组显式点到点对应。

配置项	示例值	作用
`setResolution`	`1.0`	目标分布网格尺寸 [m]
`setStepSize`	`0.1`	优化线搜索的最大步长，不是车辆行驶距离
`setTransformationEpsilon`	`0.01`	NDT 的变换变化停止阈值，不可直接视为定位误差上限
`setMaximumIterations`	`35`	最大迭代次数

4.1 代码讲解：准备源点与目标分布

#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ndt.h>

Cloud::Ptr source_filtered(new Cloud);
pcl::VoxelGrid<PointT> voxel;
voxel.setInputCloud(source);
voxel.setLeafSize(0.2f, 0.2f, 0.2f);
voxel.filter(*source_filtered);

pcl::NormalDistributionsTransform<PointT, PointT> ndt;
ndt.setInputSource(source_filtered);
ndt.setInputTarget(target);
ndt.setResolution(1.0);

源点下采样减少每次迭代的评价点数。setResolution 控制 NDT 的目标分布网格，与源点的下采样叶尺寸是两个不同参数。目标不能过度稀疏，否则很多网格缺少足够样本来形成稳定分布。

4.2 代码讲解：优化位姿并变换原始源点

ndt.setStepSize(0.1);
ndt.setTransformationEpsilon(0.01);
ndt.setMaximumIterations(35);
Cloud ndt_aligned;
ndt.align(ndt_aligned, guess);
if (!ndt.hasConverged()) return false;
const Eigen::Matrix4f T_ndt = ndt.getFinalTransformation();
Cloud ndt_full;
pcl::transformPointCloud(*source, ndt_full, T_ndt);

align 优化初值附近的位姿，返回最终源到目标矩阵。求解时用下采样源点，输出时用该矩阵变换原始源点。NDT 对初值、重叠率、网格分辨率和场景结构都有要求，不能认为使用分布模型就能保证全局正确配准。

4.3 autoware中的ndt重定位

作用：把当前 LiDAR 扫到的点云，和已有地图点云进行 NDT 匹配，求出车辆/机器人当前在 map 坐标系下的位姿。地址：src/universe/autoware.universe/localization/ndt_scan_matcher 流程：

接收当前 LiDAR 点云
把点云从 sensor_frame 转到 base_link
从 EKF 位姿队列里插值得到当前时刻的初始位姿
如果有 regularization_pose，则加入正则化约束
检查 NDT 地图是否存在
用初始位姿作为初值，执行 NDT align
得到 NDT 匹配后的位姿
检查是否收敛
发布 TF、ndt_pose、对齐点云、调试信息

关键程序：

5. 配准结果检查与方法选择

hasConverged 仅表示满足算法的停止判据，不保证位姿正确。getFitnessScore 属于几何误差统计，不能直接当作 NDT 的概率目标值；不同点数、过滤范围或评分距离下的数值也不宜直接比较。

方法	匹配依据	重点检查
点到点 ICP	对应点的坐标差	初值、重叠、错误最近邻
点到平面 ICP	目标表面法向残差	法向有效性、平面退化、小增量假设
NDT	目标局部分布	网格样本数、分辨率、初值

验证时观察点云重合、有效对应比例、位姿变化是否合理，并记录耗时。高速运动中的扫描畸变应考虑先去畸变，动态物体也应按任务需要处理。长走廊、单平面或重复结构可能导致部分自由度约束不足。

若分阶段配准，NDT 输出的完整源到目标变换可以作为 ICP 的初值；最终 ICP 返回的也是完整变换，不能直接将两个完整矩阵再次相乘。若显式估计的是目标坐标系中的增量 $\Delta T$ ，才采用：

T_{\mathrm{new}}=\Delta T\,T_{\mathrm{old}}.

无效点清理、范围裁剪与下采样见点云过滤。代码片段未包含数据加载与构建配置，尚需在安装了对应版本 PCL 的 C++ 工程中集成。

本篇文章受 CC BY-NC-SA 4.0 协议保护，转载请注明出处。

1. 点云配准的目标​

1.1 代码讲解：提供初始位姿​

2. ICP（点到点迭代最近点）​

2.1 代码讲解：设置输入与对应范围​

2.2 代码讲解：执行迭代并读取变换​

2.3 代码讲解：将结果应用到原始分辨率​

3. Point-to-Plane ICP（点到平面 ICP）​

3.1 代码讲解：估计目标法向量​

3.2 代码讲解：显式选择点到平面误差​

4. NDT（正态分布变换）​

4.1 代码讲解：准备源点与目标分布​

4.2 代码讲解：优化位姿并变换原始源点​

4.3 autoware中的ndt重定位​

5. 配准结果检查与方法选择​