4. 点云特征提取：法向量估计、PFH与FPFH特征、点云配准初探（ICP）
点云处理里，特征提取是个绕不开的坎。说白了，就是给每个点或者每个局部区域算一个“身份证”，让算法能认出它来。我刚开始做这行的时候，觉得点云不就是一堆坐标嘛，直接算距离不就完了？后来发现，没有特征，你连两个点云谁是谁都分不清。
这一章，我们重点聊三个东西：法向量、PFH/FPFH 描述子，还有经典的 ICP 配准。嗯，这三样东西是点云分割和配准的基石。
4.1 法向量估计：点云的“朝向”
法向量，就是点云表面那个小切面的垂直方向。你想想看，一个点孤零零的，它哪来的方向？所以法向量估计，本质上是在局部邻域里做平面拟合。
我个人习惯用 PCA（主成分分析）来做。具体步骤很简单：
- 对于每个点 p，找到它的 k 个最近邻。
- 计算这 k 个点的质心。
- 构建协方差矩阵。
- 对这个矩阵做特征值分解。
- 最小特征值对应的特征向量，就是法向量方向。
这里有个坑，我踩过好几次。法向量的方向是模糊的，它可能指向物体内部，也可能指向外部。如果你不做一致性调整，后面算特征全乱套。
代码实现也不复杂，用 PCL 的话几行就搞定：
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setRadiusSearch(0.05); // 半径搜索，单位米
ne.compute(*normals);搜索半径怎么选？我一般先看点云密度。如果点间距是 1cm，半径设 3-5cm 比较稳妥。设太大，法向量会被平滑掉；设太小，噪声会很大。
4.2 PFH 与 FPFH：点特征直方图
法向量只是基础。真正用来做匹配的，是 PFH（点特征直方图）和它的加速版 FPFH。
PFH 的思路很巧妙。它不光看一个点的法向量，还看它邻域内所有点对之间的法向量关系。具体来说，它计算每个点对的四个角度特征：
| 特征 | 含义 | 范围 |
|---|---|---|
| α | 法向量 n1 与连线方向 u 的夹角 | 0° ~ 180° |
| φ | 法向量 n2 在 u 方向上的投影 | -1 ~ 1 |
| θ | 法向量 n1 与 n2 的夹角 | 0° ~ 180° |
| d | 两点之间的欧氏距离 | 视尺度而定 |
这四个值统计成直方图，就是 PFH 描述子。但 PFH 有个问题——计算量太大。每个点都要算 O(k²) 个点对，k 是邻域点数。如果 k=50，那就是 2500 对，点云一多直接卡死。
所以后来有了 FPFH（快速点特征直方图）。它做了两个简化：
- 只算查询点与邻域点之间的点对，复杂度降到 O(k)。
- 再对邻域点的 SPFH 做加权求和，弥补丢失的信息。
我在项目里几乎只用 FPFH。速度比 PFH 快一个数量级，精度损失很小。PCL 里调用也很直接：
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
fpfh.setInputCloud(cloud);
fpfh.setInputNormals(normals);
fpfh.setRadiusSearch(0.1);
fpfh.compute(*fpfh_features);4.3 点云配准初探：ICP
配准，就是把两个点云对齐。最经典的算法就是 ICP（迭代最近点）。
ICP 的核心思想很简单：
- 找对应点：源点云中的每个点，在目标点云中找最近邻。
- 算变换：根据这些对应点，解出一个刚体变换（旋转+平移）。
- 迭代：用这个变换更新源点云，重复步骤 1-2，直到收敛。
听起来很完美对吧？但实际用起来，坑比想象的多。
我曾经在一个项目里，用 ICP 配准两个扫描的墙面点云。结果怎么都收敛不了，最后发现初始位置差了 30 度。ICP 对初值非常敏感，它只能做精细对齐，不能做粗配准。
PCL 里的 ICP 实现：
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source_cloud);
icp.setInputTarget(target_cloud);
icp.setMaxCorrespondenceDistance(0.1);
icp.setMaximumIterations(50);
icp.setTransformationEpsilon(1e-8);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.align(*aligned_cloud);参数怎么调？我一般这样设：
- MaxCorrespondenceDistance： 先设大一点（比如 0.5m），迭代几次后逐渐缩小到 0.05m。这叫“粗到细”策略。
- MaximumIterations： 50 次够了。如果 50 次还不收敛，说明初值有问题。
- TransformationEpsilon： 变换矩阵变化小于这个值就停止。1e-8 是个比较稳妥的值。
4.4 本章知识结构
下面这张图，是我自己梳理的本章核心逻辑。从原始点云到配准结果，每一步都离不开特征。
从图上你能看到，法向量是中间桥梁。没有它，FPFH 算不出来，ICP 也没法用点对面距离。所以，我建议你先把法向量估计吃透，再往后走。
4.5 小结
这一章我们聊了三个核心内容：
- 法向量估计： 用 PCA 拟合局部平面，注意方向一致性。
- PFH 与 FPFH： 用直方图描述局部几何，FPFH 是工程首选。
- ICP 配准： 迭代最近点，对初值敏感，需要粗配准打底。