Home / 机器人经典基础 / Horn 配准：把旋转最小二乘变成四维特征值问题

021｜Horn 配准：把旋转最小二乘变成四维特征值问题

论文：Berthold K. P. Horn，Closed-Form Solution of Absolute Orientation Using Unit Quaternions，1987，JOSA A 4(4):629–642。作者原始全文 · DOI。

先接住前作的问题

RANSAC需要一个从候选对应拟合模型的内部求解器，尚未规定三维旋转怎样保持合法。Horn解决已知对应下的最小二乘配准，可作为这个模块；它自身不负责挑出正确对应。

五个前置概念

五个前置概念：刚体变换只含旋转和平移；中心化去掉质心；单位四元数用四参数加一约束表示旋转；Rayleigh 商联系二次型与特征值；几何退化表示数据缺少某些约束。

一、问题设定：已知对应的两组三维点怎样满足刚体约束地对齐

同一组三维标记点，在相机坐标系与机器人坐标系下各有一组坐标。怎样找一个旋转和平移，让两组尽量重合？点测量有误差，通常不存在完全重合的变换，因此要最小化整体残差。

问题定义与过去的困难

给定点对 $a_i,b_i$ ，先考虑 $b_i\approx Ra_i+t$ ， $R^TR=I$ 且 $\det R=1$ 。目标为 $E=\sum_i\|Ra_i+t-b_i\|^2$ 。机器人中的例子包括传感器坐标对齐与三维模型配准。

如果直接把旋转当成任意 $3\times3$ 矩阵求最小二乘，结果可能包含拉伸或反射；事后再强行正交化，也不一定保留原目标的最优性。原文引言针对这类问题讨论闭式方法的优势。

已知三个不共线点，在无噪声情况下能确定刚体姿态。点数增加的作用是综合更多有噪声信息，不是越多越能修复系统性偏差。所有点共线时，绕该直线的旋转仍不受约束。

二、先中心化，再把合法旋转转成四元数特征值解

方法：先平移，再旋转

令质心 $\bar a=\frac1N\sum a_i$ 、 $\bar b=\frac1N\sum b_i$ 。固定 $R$ 时，对 $t$ 求导得到 $t=\bar b-R\bar a$ 。因此先中心化 $a'_i=a_i-\bar a$ 、 $b'_i=b_i-\bar b$ ，平移问题就被消去。

展开平方项后，只需最大化 $\sum_i b_i'^TRa_i'$ 。构造交叉矩阵 $H=\sum_i a_i'b_i'^T$ ，令 $z=(H_{23}-H_{32},H_{31}-H_{13},H_{12}-H_{21})^T$ 。采用标量在前的四元数 $q$ ，定义

$N=\begin{bmatrix}\operatorname{tr}H&z^T\\z&H+H^T-\operatorname{tr}(H)I\end{bmatrix}.$

对应旋转目标为 $q^TNq$ ，约束 $q^Tq=1$ 。若将 $q$ 展开在 $N$ 的正交特征向量基上，目标就是各特征值按非负平方系数的加权平均，权重和为 1。因此最大值是最大特征值，取其特征向量即可达到。

这段推导解释了为什么选最大特征值，不是选绝对值最大。一个很负的特征值即使绝对值更大，也不会最大化二次型。四元数 $q$ 与 $-q$ 表示同一旋转，所以符号不唯一不代表姿态不唯一。

现代实现可直接调用对称特征值求解器；原文还讨论四次多项式的闭式求根。使用数值线性代数库并不改变配准目标，但库内部是否迭代与“无需在姿态空间反复优化”是不同层次。

用一个完整的原创点集走一遍四元数解，能同时检查旋转方向与平移恢复。源点取 $(1,0,0),(-1,0,0),(0,1,0),(0,-1,0)$ ，目标点由绕 $z$ 轴正转90度，再平移 $t=(3,-1,2)$ 得到。源质心为零，目标质心就是 $t$ ；中心化之后有

$H=\begin{bmatrix}0&2&0\\-2&0&0\\0&0&0\end{bmatrix},\qquad N=\begin{bmatrix}0&0&0&4\\0&0&0&0\\0&0&0&0\\4&0&0&0\end{bmatrix}.$

$N$ 的最大特征值为4，对应单位四元数 $q=(1,0,0,1)/\sqrt2$ ，即预设的90度旋转。最小特征值为−4，两者绝对值相同，但取后者会得到错误旋转。目标函数最小值也能检查：两组中心化点的平方模之和共8，而最优交叉项为4，因此 $E_{\min}=8-2\times4=0$ 。四点虽然共面，却不共线，足够确定这里的三维正常旋转；“必须不共面”是额外且不必要的要求。

如果实现把 $H$ 换成 $H^\top$ 却沿用同一四元数转换约定，结果通常会反向。应始终检验实际映射 $Ra_i+t\approx b_i$ ，而不仅看 $R^\top R=I$ 。RANSAC可以筛选可靠点对后调用本篇，是模块组合；手眼标定也用四元数处理旋转，但约束是 $AX=XB$ 的运动共轭关系，不能把点配准的矩阵直接套上去，二者是不同的几何估计问题。

尺度是一个不能跳过的细节

如果同时估计 $s$ ，单向误差 $\sum\|sa'_i-R^Tb'_i\|^2$ 的最优尺度，等价写成 $s=\sum b_i'^TRa'_i/\sum\|a'_i\|^2$ 。反向独立求解时，两个估计一般不互为倒数。

原文第 2-D、2-E 节强调正反变换的对称性，并讨论相应尺度准则。其对称形式给出两组中心化均方根长度之比。不能把这个对称尺度直接标成任意单向平方误差的最小解；目标定义改变，最佳参数也会改变。

对机器人的两个米制坐标系，常常应固定尺度为 1。随意允许尺度漂移，可能把标定错误或非刚性变形吸收进尺度，得到好看的拟合残差却失去正确物理含义。

中心化与旋转求解尚未结束配准，恢复平移还依赖选定尺度目标。下面把这些步骤放回同一流程，并用共线点检查旋转究竟是否可辨识。

三、实现尺度与平移恢复，用共线点检查歧义

输入：已知对应的三维点 a[1:N]、b[1:N]；尺度固定为 1
检查对应关系、点数及是否接近共线
计算两组质心并中心化
H ← 对所有中心化点对累加 a_i*b_i转置
由 H 构造对称 4×4 矩阵 N
q ← N 的最大特征值对应单位特征向量
R ← 将 q 转成与公式约定一致的旋转矩阵
t ← 目标质心-R*源质心
输出 R、t、全部点残差与退化诊断

自构例子：同一条直线藏着未被测到的旋转

令源点都在 $x$ 轴上，目标点由一个刚体变换产生。绕源 $x$ 轴再旋转任意角度，所有源点的位置仍然不变。因此测量无法区分这类旋转，任何算法都不应从这些点推断出唯一完整姿态。

加入一个明显偏离直线的点，就能提供额外方向约束。但如果它只偏离极小距离，微小噪声就可能大幅改变估计姿态。这说明退化通常有连续的邻域，而不仅是一个精确的特殊点。

再把某个对应点误配到远处。平方残差会给这个点很大权重，最优解可能偏向错误对应。若使用 RANSAC 包裹 Horn 解，应明确：Horn 负责给定样本拟合，RANSAC 负责寻找一致数据，稳健性来自组合而非闭式解本身。

四、对照闭式推导、原文例子与教学图

Fig. 2：三点确定局部坐标系，但所选三点会决定哪些测量直接进入坐标构造 — 原论文方法图摘引：Berthold K. P. Horn（1987），Fig. 2，PDF 第 2 页；核对原文。原版权保留。

读图：三点确定局部坐标系，但所选三点会决定哪些测量直接进入坐标构造。它解释了为何使用全部对应点的对称最小二乘问题，比任意挑三点搭坐标系更有意义。

刚体配准先固定尺度，再讨论原文的尺度对称性；改变损失函数或开放尺度会改变最优解，不能只替换一个公式。

第 1-B 节，图 1。 点数与不共线条件；说明问题约束和基本退化。

图1：同一组空间点在左右两个坐标系中的观测向量 — Berthold K. P. Horn（1987），图 1，PDF 第 2 页；原文与图注；放大查看。原版权保留。

读图：虚线连接同一点在两个坐标系中的表示；问题是求共同的刚体变换，而不是为每一对点独立拟合。

第 2-A 节，图 2。 三点构造坐标轴；指出选择性使用测量信息的局限。

本页定位：图 2内嵌图。

第 2-D、2-E 节。 尺度与正反对称性；不同准则的尺度不能混淆。

第 4-A 至 4-C 节。 交叉矩阵、四元数矩阵与流程；从点统计量得到最优旋转。

附录 A3。 单位向量二次型最大化；最大特征向量解的理论证明。

这是以解析推导为核心的论文，不是大型机器人成功率基准。原文没有提供可以据此声称“导航精度提高若干百分比”的实验。理论最优性针对已定义的点对残差，不能直接推广到有离群点、各向异性噪声或未知对应的情况。

一个合适的复现实验应同时报告旋转误差、平移误差、点残差和点集几何条件。只报告残差可能掩盖近共线条件下很大的姿态不确定性。

五、检查对应、误差模型和尺度目标

贡献是让一个带旋转约束的几何优化问题，通过适当表示化为小型线性代数问题，并仔细讨论正反对称性。中心化还使平移和旋转的职责清楚分离。

成本主要是建立对应关系和检查几何条件。对称 $4\times4$ 特征值问题本身很小，输入点统计量的累加随点数增长；但这不包含特征匹配或点云搜索的成本。

原文关于唯一性的分析需结合最大特征值是否单独以及点集几何。本文进一步提醒：单位、左右手坐标系和四元数乘法约定不一致，会产生看似正常但方向错误的结果。最简单的验证是用已知旋转生成点对，再检查算法恢复的是该变换还是它的逆。

六、追问：去掉均值后配准很好，为什么仍要检查恢复平移时采用的尺度？

去掉均值后配准很好，为什么仍要检查恢复平移时采用的尺度？

参考答案：中心化只分离了平移，不会决定尺度估计属于哪一种目标。恢复平移时必须使用同一个尺度、旋转与两组质心，否则中心化残差与最终坐标映射不是同一问题。应先声明是刚体还是相似配准，并核对正文的尺度目标差别。

七、从已知对应走向ICP与鲁棒认证

模块组合：ICP：不知道谁对应谁，也可以交替求解形状对齐。交替生成最近点对应并调用刚体配准，新增未知对应的处理。
平行路线：TEASER：错误匹配占多数时，怎样配准点云并检查答案。在对应含大量离群点时改用鲁棒目标与可认证求解。

八、自测与参考答案

1．四元数解出现正负两个向量，是否表示两个不同姿态？

不是。 $q$ 和 $-q$ 对应相同旋转矩阵。真正的姿态不唯一来自约束不足或最大特征值退化，不是四元数符号本身。

2．为什么不能任意放开尺度以获得更低残差？

两个物理米制坐标系应保持长度。自由尺度可能吸收其他误差，降低残差却破坏刚体假设。参数自由度必须服从实际问题。

3．闭式最优解为什么仍可能配准错误？

因为它只保证给定对应和目标函数下的最优。如果对应错误或噪声模型不合适，数学最优并不等于真实变换。应先核查输入，再判断求解器。