ICP轨迹估计变换矩阵与Ground Truth不符问题排查

安全阿岳

2026-7-6

ICP计算车辆轨迹变换矩阵跳变问题排查

初始变换矩阵传递错误
若相邻帧ICP未以上一帧的估计变换作为初始值，而是每次重置为单位矩阵，当帧间存在一定运动时，ICP极易陷入局部最优，导致变换矩阵元素出现大幅跳变。检查代码逻辑，确保当前帧ICP的初始变换是上一帧的累积变换 @ 上一帧ICP的局部变换，而非每次用np.eye(4)初始化。
对应点匹配策略失效
过大的搜索半径、未做特征约束的暴力匹配会引入大量错误对应点，直接导致求解出的变换严重偏离真实值。可以可视化相邻帧点云的对应点对，排查是否存在距离过远的错误匹配；同时调整搜索阈值（比如将半径搜索阈值缩小到0.5米以内），或先提取点云特征（比如FPFH）进行特征匹配后再做ICP精配准。
迭代终止条件不合理
仅设置最大迭代次数而无收敛阈值，会导致算法在未收敛时停止，或过度迭代引发数值不稳定。需同时设置变换增量阈值（比如相邻两次迭代的变换矩阵元素变化小于1e-6）和最大迭代次数（如50次），确保算法在收敛后及时停止。
点云预处理缺失
未过滤的离群点、动态物体点会严重干扰配准结果。对每帧点云先执行统计滤波（去除离群点）和体素下采样，减少噪声点和冗余点的影响，提升配准的稳定性。
旋转矩阵正交性未修正
ICP通过SVD求解旋转矩阵时，可能因数值误差导致矩阵不满足正交性（行列式不为1），进而引发元素跳变。求解得到旋转矩阵后，需用QR分解做正交化修正：R = np.linalg.qr(R)[0]，再检查行列式，若为-1则取负修正，确保旋转矩阵的合法性。
单帧变换结果验证
对比Ground Truth中相邻帧的变换矩阵（元素以0.999...开头，运动幅度极小），检查单帧ICP输出的变换是否符合帧间真实运动规律。如果单帧结果就出现大幅跳变，说明问题出在单帧配准环节，而非累积误差。