3. 点云预处理（下）：半径滤波、点云配准初探（ICP算法原理）、点云坐标系变换
好，咱们接着聊点云预处理的下半部分。上一章我们把直通滤波、体素滤波和统计滤波都过了一遍，今天要聊的三个东西——半径滤波、ICP配准、坐标系变换——说实话，在实际工程里出现的频率一点都不比前面那些低。
我个人习惯把半径滤波和统计滤波放在一起用。统计滤波管全局离群点，半径滤波管局部稀疏点。两者配合，效果往往比单独用任何一个都好。
3.1 半径滤波：简单粗暴但有效
半径滤波的思路特别直白：对于每个点，看看它周围指定半径内有多少个邻居。如果邻居数量少于阈值，就把它干掉。
你想想看，一个点如果孤零零地待着，周围半径R内连K个点都没有，那它大概率是噪声。这个逻辑在工程上非常实用。
核心参数就两个：
- 搜索半径 r：多大范围内找邻居
- 邻居数量阈值 k：少于k个邻居就剔除
我在项目中遇到过这样一个场景：用16线激光雷达扫描一个仓库，地面点云很密，但空中飘着一些灰尘点。统计滤波因为整体分布还算均匀，没能完全滤掉。换成半径滤波，设半径0.1米、阈值3个点，那些孤立的灰尘点瞬间就被清干净了。
代码实现也很简单，PCL里直接调：
// C++ PCL 半径滤波示例
pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_filter;
radius_filter.setInputCloud(cloud);
radius_filter.setRadiusSearch(0.1); // 半径0.1米
radius_filter.setMinNeighborsInRadius(3); // 至少3个邻居
radius_filter.filter(*cloud_filtered);我的经验：半径滤波的参数设置跟点云密度强相关。远距离点云密度低，半径要适当放大。我一般先跑一下体素滤波，再用半径滤波，这样参数好调很多。
3.2 点云配准初探：ICP算法原理
点云配准，说白了就是把两片点云拼到一起。比如你用一个激光雷达扫了一圈，不同时刻扫到的点云需要对齐到同一个坐标系下。ICP（Iterative Closest Point）就是干这个活最经典的算法。
ICP的核心思想其实很简单：
- 找对应点：对源点云中的每个点，在目标点云中找到最近的点作为匹配对
- 算变换：根据这些匹配对，计算一个旋转平移矩阵，让源点云向目标点云靠拢
- 迭代：重复1和2，直到收敛
嗯，这里要注意：ICP对初始位置很敏感。如果两片点云初始位置差太远，它很容易掉进局部最优。我刚开始做配准时吃过这个亏——两片点云差了半米，ICP直接跑飞了。
避坑指南：ICP不是万能的。它要求两片点云有足够的重叠区域，而且初始位姿不能差太远。我建议先用NDT（正态分布变换）做粗配准，再用ICP做精配准。这个组合我在实际项目中用了很多次，效果很稳。
ICP的数学本质是最小化一个目标函数：
min(R,t) Σ || R·p_i + t - q_i ||²
其中：
- p_i 是源点云中的点
- q_i 是目标点云中对应的最近点
- R 是旋转矩阵
- t 是平移向量这个优化问题可以用SVD分解来求解，效率很高。PCL里已经封装好了：
// PCL ICP 配准示例
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source_cloud);
icp.setInputTarget(target_cloud);
icp.setMaxCorrespondenceDistance(0.5); // 最大匹配距离
icp.setMaximumIterations(50); // 最大迭代次数
icp.setTransformationEpsilon(1e-8); // 收敛阈值
icp.align(*aligned_cloud);
if (icp.hasConverged()) {
Eigen::Matrix4f transform = icp.getFinalTransformation();
// 拿到变换矩阵，后续就可以用了
}我的习惯：ICP跑完后一定要检查一下匹配误差。如果均方根误差大于0.05米，我一般会重新调整参数或者检查一下初始配准是否到位。
3.3 点云坐标系变换
坐标系变换，这是点云处理里绕不开的基础操作。你想想看，激光雷达有自己的坐标系，IMU有自己的坐标系，车体也有自己的坐标系。要把这些数据融合到一起，坐标系变换就是基本功。
说白了就是把一个坐标系下的点，通过旋转和平移，映射到另一个坐标系下。
数学表达很简单：
P' = R · P + t
其中：
- P 是原始坐标系下的点
- R 是3x3旋转矩阵
- t 是3x1平移向量
- P' 是目标坐标系下的点实际工程中，我们通常用4x4齐次变换矩阵来统一表示：
| R t |
| 0 1 |这样旋转和平移就可以用一个矩阵乘法搞定，代码写起来也清爽：
// 齐次变换示例
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
transform.block<3,3>(0,0) = rotation_matrix; // 旋转部分
transform.block<3,1>(0,3) = translation_vector; // 平移部分
// 对点云做变换
pcl::transformPointCloud(*input_cloud, *output_cloud, transform);常见坐标系变换场景：
- 激光雷达 → 车体坐标系：标定外参，把点云从雷达安装位置转到车辆中心
- 车体坐标系 → 全局坐标系：结合GPS/IMU数据，把局部点云拼到全局地图
- 不同传感器之间：比如激光雷达和相机之间的坐标对齐
我在实际项目中踩过一个坑：坐标系变换的顺序搞反了。先旋转后平移和先平移后旋转，结果天差地别。记住，标准流程是先旋转，再平移。齐次变换矩阵天然保证了这个顺序，所以用4x4矩阵是最安全的做法。
注意：坐标系变换时，旋转矩阵必须是正交矩阵（R·Rᵀ = I）。如果标定出来的矩阵不正交，需要做正交化处理，否则点云会变形。我曾经因为标定板没放平，导致旋转矩阵不正交，点云拼出来全是扭曲的，排查了半天才发现问题。
3.4 本章知识体系
为了让你更直观地理解这三块内容的关系，我画了一张流程图：
这三块内容在实际工程中是串联使用的。先做半径滤波把噪声干掉，然后用ICP把不同时刻或不同视角的点云对齐，最后通过坐标系变换统一到同一个参考系下。每一步都踩过坑，也都有优化空间。
好了，点云预处理的部分就聊到这里。下一章我们开始进入真正的目标检测环节，到时候会用到今天讲的这些预处理结果。记住，预处理做得好，后面的算法才能跑得稳。