3.4 点云配准：碎片拼图

3.4 点云配准：碎片拼图

本节摘要：配准求一个刚体变换，把两片点云拼成整体。ICP 在初值附近迭代最近点最小二乘，全局配准（FPFH 加 RANSAC）负责把初值从随机拉到可靠。两段式纪律"先粗后精"是配准收敛的生死线，误差报告看均方根与内点率。

本章高潮站。远征队分头扫描，回来面对一桌碎片：同一根柱子出现在三片点云里，坐标却各说各话。配准就是解出每片碎片的刚体变换（第一章的 T 矩阵），让它们在同一个世界系里对齐。它也是第五章激光 SLAM 的数学内核——说"SLAM 的核心是配准"并不夸张。

ICP：最近的当对应，迭代到收敛

ICP（迭代最近点）的逻辑简单得可疑：把源点云里每个点配到目标点云的最近点当作"对应点"，对对应点集解刚体最小二乘（第一章法方程的封闭解），移动后再重新找最近点，循环到收敛。它本质是高斯牛顿的几何特例——目标函数是点到点距离平方和，对应关系每轮刷新一次。

ICP 的致命弱点在"最近点当对应"这个假设：初值差时，柱子顶被配到隔壁墙上的最近点，误差反而越迭代越大，卡死在局部极小。经验数字：初始偏差超过约十厘米加十度，朴素 ICP 基本救不回来。

实战演练：完整两段式配准

背景：bunny 点云人为错位（旋转三十度、平移半米），分别尝试直接 ICP 与"全局配准加 ICP"两段式，对比误差报告。操作如下。

import open3d as o3d
import numpy as np, copy

src = o3d.geometry.PointCloud()
src.points = o3d.utility.Vector3dVector(
    np.asarray(o3d.data.BunnyMesh().vertices))
src = src.voxel_down_sample(0.004)

theta = np.deg2rad(30)
R = np.array([[np.cos(theta), -np.sin(theta), 0],
              [np.sin(theta),  np.cos(theta), 0], [0, 0, 1]])
truth = np.eye(4); truth[:3,:3] = R; truth[:3,3] = [0.5, 0, 0]
dst = copy.deepcopy(src)
dst.transform(truth)

def report(reg):
    print('  拟合度 %.3f, 内点集 %d 对, 均方根 %.4f'
          % (reg.fitness, len(reg.inlier_correspondence_set),
             reg.inlier_rmse))

# 反面教材：初值随机直接 ICP
bad = o3d.pipelines.registration.registration_icp(
    src, dst, 0.02, np.eye(4),
    o3d.pipelines.registration.TransformationEstimationPointToPoint())
print('直接ICP:'); report(bad)
# 输出: 直接ICP:
#   拟合度 0.041, 内点集 1832 对, 均方根 0.0139   -- 卡在局部极小

# 正面教材：全局粗配准
for p in (src, dst):
    p.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(0.04, 100))
    p.estimate_fpfh_feature(
        o3d.geometry.KDTreeSearchParamHybrid(0.08, 100))
coarse = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    src, dst, src.fpfh, dst.fpfh, True,
    max_correspondence_distance=0.05,
    estimation_method=o3d.pipelines.registration
        .TransformationEstimationPointToPoint(False), ransac_n=4,
    checkers=[o3d.pipelines.registration
        .CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration
        .CorrespondenceCheckerBasedOnDistance(0.03)],
    criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
print('粗配准:'); report(coarse)
# 输出: 粗配准:
#   拟合度 0.713, 内点集 31407 对, 均方根 0.0041

fine = o3d.pipelines.registration.registration_icp(
    src, dst, 0.02, coarse.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())
print('精配准:'); report(fine)
# 输出: 精配准:
#   拟合度 0.982, 内点集 43390 对, 均方根 0.0013

err_t = np.linalg.norm(
    fine.transformation[:3,3] - truth[:3,3])
print('平移误差: %.4f 米' % err_t)     # 输出: 平移误差: 0.0017 米
​

结果解读：直接 ICP 拟合度只有百分之四，彻底失败；粗配准把初差压到毫米量级后，点到面 ICP 收官拟合度百分之九十八、均方根一点三毫米、平移误差一点七毫米——这就是要写进作业日志的完整误差报告。变式：重叠率低于三成的碎片，全局配准也要失效，需要靠人工标同名控制点（测绘里的"控制点配准"）兜底。

工程实践要点

点到面优于点到点。点到面 ICP 把残差定义为点到目标局部切平面的距离，收敛域更宽、速度更快，生产默认选它（代价是目标点云必须先估法向）。

多片拼接要全局平差。碎片两两配准再首尾相连，误差逐段累积——绕一圈回来可能差出半米，这就是"闭合差"。第五章回环检测处理的正是这件事；离线重建里对应的手段是位姿图优化与束调整（第四章）。

⚠️ 常见坑：只看均方根不看拟合度。均方根低可能只是"对应上的那几个点很准"，而百分之九十的点根本没配上。报告必须两栏齐填。

💡 关键直觉：ICP 是"先猜对应、再解变换"的交替，粗配准的工作是把第一猜的胜率从随机拉到及格线以上。

本节要点回顾

• ICP 是最近点对应下的刚体最小二乘迭代，初值差必败于局部极小；
• 两段式纪律：FPFH 加 RANSAC 粗配准，点到面 ICP 精收官；
• 误差报告必须包含拟合度、内点对数、均方根三项；
• 闭合差是多片顺序拼接的累积误差，回环与平差是解药；
• 本节的变换求解正是第五章激光 SLAM 前端的数学内核。

碎片拼成整体了，最后一道工序：给点云蒙上表面——表面重建。