C# 实现 3×3 SVD 与 Kabsch 刚体变换求解

去年给一套三坐标测量臂做配套软件，需要把测量得到的一组空间点拟合到理论模型坐标系里去，最后落到代码上，核心就是 C# 实现 SVD 奇异值分解求解旋转平移矩阵 这件事。听上去像教科书里的线性代数作业，真写起来坑不少：3×3 的 SVD 没有现成 API、奇异值排序一乱结果就飘、行列式为负那一步漏掉整组数据会被"镜像"过去、用 float 攒点还会让毫米级精度掉到零点几毫米。这类需求在工业测量、机器人视觉引导、点云配准、逆向工程里出现频率极高——凡是"我知道物体上一批点在它自己坐标系里的位置，也知道这批点在另一个坐标系里的位置，求这两个坐标系之间的位姿关系"的场景，都是同一道题。

这篇东西写给两类人：一类是刚接手这类需求、知道要用 SVD 但不确定 C# 里怎么落地的开发者；另一类是已经跑通了代码、发现结果时好时坏、想搞清楚哪里出的问题的人。我会从问题定义讲到手写 3×3 SVD 的完整实现，再到实测精度和踩过的坑，代码可以直接抄。

1. 先把问题说清楚：从点对到刚体变换

1.1 三对点、六个自由度与我们要解的到底是什么

刚体变换只有六个自由度：三个旋转、三个平移，没有缩放、没有剪切。所以只要有三个不共线的对应点对，理论上就能唯一确定这个变换——三个点各贡献三个方程，共九个方程，未知数六个，超定但有唯一最小二乘解。实际项目里我们通常给的点对远多于三对，这既是冗余也是保险：单点测量误差在只有三对点的时候会被完整地传递进位姿结果，点多一些就能靠最小二乘把噪声平均掉。

把这句话写成数学，就是找旋转矩阵 R 和平移向量 t，让下面这个残差平方和最小：

E(R, t) = Σ || R · p_i + t - q_i ||²

其中 p_i 是源坐标系下的点，q_i 是目标坐标系下的对应点，i 从 1 到 N。R 是 3×3 正交矩阵且行列式为 +1（这一点很关键，后面单独说），t 是 3×1 向量。

举几个我实际遇到过的具体形态：工件上三个基准孔的中心在工件坐标系下的坐标已知，用设备测出它们在设备坐标系下的坐标，求工件在设备上的安装位姿；扫描仪扫出来的点云和 CAD 数模做粗配准，人工点选四到六个特征点求初始位姿；机械臂末端相机看到标定板上的若干标记点，求相机相对末端法兰的位姿。这些任务表面差得很远，底层是同一道题，一行代码都不用改。

1.2 为什么最后选了 SVD，而不是欧拉角或者四元数

处理刚体变换求解，行业里常见的三条路：欧拉角加非线性优化、四元数特征值法（Horn 方法）、SVD 闭式解（Kabsch 算法）。我在不同项目里三种都写过，最后凡是新项目一律上 SVD，理由很实在。

欧拉角那条路的问题在于它把旋转拆成三个有顺序的角度，一旦中间那个角接近 ±90°，两个轴会重合，就是所谓的万向节死锁，优化器会在那附近剧烈震荡甚至不收敛。更麻烦的是它需要迭代，得给初值，得设步长，得判断收敛——在现场工控机上跑这套东西，调试成本比算法本身高得多。

四元数法（Horn 1987）很优雅，把问题转成构造一个 4×4 的对称矩阵然后求最大特征值对应的特征向量。这条路数值上非常稳，我至今保留着它的实现作为交叉验证工具。但它有个不讨喜的地方：那 4×4 矩阵的构造公式一长串，写错一个符号结果就是错的，而且很难直观判断错在哪。

SVD 这条路的好处是 全闭式、零迭代、代码短、副产品多 。同一个 SVD 分解出来的奇异值，顺手就能告诉你这组点是不是退化了——最小奇异值接近零说明点几乎共线，解不唯一，这时候就该给用户报"点位分布不合理，请重新选点"，而不是闷头给一个看起来很正常的错误答案。这个诊断能力是另外两条路给不了的，而在工程现场它比精度还值钱。

方案	是否需要迭代	数值稳定性	退化诊断	代码量	我的使用场景
欧拉角 + 非线性优化	需要，依赖初值	万向节附近差	无	中	早期项目，现已弃用
四元数特征值法	需要 4×4 特征分解	好	弱	中长	交叉验证、教学
SVD 闭式解	无需外层迭代	好	强（看奇异值）	短	现在的主力方案

1.3 C# 这边的技术选型：现成库还是自己写 150 行

C# 不像 Python 那样有随手可用的科学计算生态。可选的路大概三条：引一个成熟的 .NET 数值计算库、调原生数学库、自己写。

引库是最快的，几行代码就能出结果，但我在两个项目上被劝退过。一个是现场的工控机跑的是比较老的框架版本，库的依赖链拉不进去；另一个是交付要求里明确写了第三方组件清单要报备，为一个 3×3 的矩阵分解去走一遍流程，性价比太低。

调原生库的问题是要处理平台架构、打包分发、异常跨越边界时的行为，为一个纯数学问题引入这些复杂度不值。

自己写的门槛其实比想象的低得多。我们需要的不是通用 SVD，只是 3×3 实数矩阵的 SVD ，这个规模下用单边 Jacobi 方法，核心代码不到八十行，加上矩阵工具函数一共一百五十行左右，全部是纯托管代码，零依赖、可移植、可单步调试。我这套代码从 .NET Framework 4.5 一路跑到 .NET 8，一行没改过。

提示：如果你的场景是 N×M 的大矩阵分解，老老实实引库，别自己写。手写只适用于 3×3、4×4 这种固定小规模、需要闭式解的场景。

2. Kabsch 算法的推导骨架与每一步的物理含义

2.1 去质心：把旋转和平移解耦的关键一步

直接对 E(R,t) 求导也能做，但同时含 R 和 t 的时候式子会很脏。Kabsch 算法的第一个动作是 先去质心 ，把平移从问题里摘出去。

具体做法是分别算两组点的质心：

p̄ = (1/N) Σ p_i
q̄ = (1/N) Σ q_i

然后令 a_i = p_i - p̄，b_i = q_i - q̄，把问题改写成对去质心点求旋转，最后再把平移补回来：

t = q̄ - R · p̄

为什么这一步在数学上成立？因为对任意旋转 R，去质心后的残差和原残差只差一个常数项。展开 E(R,t) 对 t 求偏导并令其为零，解出来必然是 t = q̄ - R·p̄ 这个形式，也就是说对任何给定的 R，最优平移都是把源质心映射到目标质心。所以平移不是独立变量，它是 R 的函数，先摘掉它不影响最终结果的正确性。

工程上这一步还有一个额外的好处，很多人没意识到： 它对数值精度有实质影响 。假设坐标值是几百毫米量级，直接算叉积项，中间结果的量级是十万级，而真正有信息的偏差可能只有零点几，有效数字被大量浪费。先减掉质心，数值量级直接降下来，双精度的余量就宽裕很多。我在一次大尺寸结构件测量里，只加了这一行减质心，配准残差从 0.4 mm 降到 0.03 mm，就是因为之前拿 float 存的原始坐标在乘加过程中精度不够。

2.2 协方差矩阵 H：把最小二乘写成迹最大化

去质心之后问题变成：找 R 使 Σ ||R·a_i - b_i||² 最小。展开这个平方：

Σ ||R a_i - b_i||² = Σ (||a_i||² + ||b_i||² - 2 b_iᵀ R a_i)

前两项跟 R 无关（旋转不改变长度），所以最小化左边等价于最大化 Σ b_iᵀ R a_i。把这个和式改写成矩阵形式：

Σ b_iᵀ R a_i = trace(R · H)，其中 H = Σ a_i b_iᵀ

这里 H 是 3×3 矩阵，H 的第 r 行第 c 列元素等于 Σ a_i[r] · b_i[c]。它的物理含义是两组点的 互协方差 ：如果两组点分布方向完全一致，H 的迹会很大；如果某个方向上的分布被翻转了，对应项的符号就会变，这正是后面能检测出镜像的根源。

用 trace 有个好用的性质：trace(AB) = trace(BA)，对正交矩阵还成立 trace(RAB) = trace(ABR)。待会儿推导 R 的闭式解时全靠它。

2.3 SVD 与 R = V·Uᵀ 的由来

把 H 做奇异值分解：

H = U · S · Vᵀ

其中 U、V 都是 3×3 正交矩阵，S 是对角线上非负递减的奇异值矩阵。代进迹表达式里：

trace(R H) = trace(R U S Vᵀ) = trace(Vᵀ R U S)

令 M = Vᵀ R U，因为 U、V、R 都是正交阵，M 也是正交阵。于是 trace(M S) = Σ s_i · M_ii。正交矩阵的对角元绝对值不超过 1，所以这个和最大也超不过 Σ s_i，要取到最大就必须让 M 的对角元全部等于 1，也就是 M = I。

M = I 反解出来就是：

R = V · Uᵀ

这就是整个算法最核心的一行。注意顺序：是 V 乘 U 的转置，不是 U 乘 V 的转置。这两个结果互为转置，物理上方向正好相反，一个把源往目标转，一个把目标往源转，写反了会发现误差大得离谱但又不是完全没规律——这是最常见的错误，我在代码评审里至少见过五次。

2.4 行列式修正：那条绝对不能省掉的判断

上面推 R = V Uᵀ 的时候我默认了 M 可以是任意正交阵，但旋转矩阵的定义要求 det(R) = +1，而正交矩阵的行列式可以是 +1 也可以是 -1。det = -1 的正交矩阵对应的是"镜像 + 旋转"，它不是刚体运动，现实中物体不能靠旋转变成自己的镜像。

如果输入数据本身带着反射（比如某个轴的坐标符号搞反了、测量设备做了左右手坐标系混用），SVD 会给出一个 det(R) = -1 的解，这个解的残差可能非常小，看起来"拟合得很好"，但它是错的。

修正办法是标准的：算完 R 之后检查行列式，如果小于零，把 V 的 最后一列 （也就是对应最小奇异值的那一列）取反，重新算一次 R。

if (det(R) < 0) {
    V[:, 2] = -V[:, 2];   // 注意是最后一列
    R = V · Uᵀ;
}

为什么取最后一列而不是随便哪一列？因为取反某一列相当于在保持正交性的前提下翻转一个方向，代价是让这个方向上的贡献从 +s_i 变成 -s_i，迹的损失是 2s_i。要损失最小，当然选最小的那个 s_i，从而选对应的最后一列。这就是为什么 SVD 结果必须 按奇异值降序排好 ，否则你取反的可能不是最小那个，修正后的解就不是最优的了。

2.5 平移回代与残差评估：算完必须验

拿到 R 之后平移就是一行：

t = q̄ - R · p̄

到这里闭式解就完事了。但工程代码不能就这么交出去，必须带残差评估。我习惯至少算三个指标：

• RMSE ：sqrt(Σ ||R p_i + t - q_i||² / N)，直接反映配准好坏，单位跟输入一致，业务方能看懂。
• 最大单点残差 ：如果某一点的残差远超其他点，说明那一点的对应关系标错了，或者该点被遮挡/测量失败。
• 旋转角度误差 （有真值可对比时）：acos((trace(R_estᵀ R_true) - 1) / 2)，把旋转矩阵差折成一个直观的角度值。

矩阵差的迹这个公式值得记一下：两个旋转矩阵 R1、R2 之间的夹角 θ 满足 trace(R1ᵀR2) = 1 + 2cosθ。用这个公式判断误差比逐元素对比直观得多，我一般会把它做成日志里的一行输出。

3. C# 手写 3×3 SVD：单边 Jacobi 的完整实现

3.1 为什么用单边 Jacobi，而不是先求 HᵀH 的特征值

教科书写 SVD 有个常见套路：算 AᵀA，对它做特征分解得到特征值 λ 和特征向量 V，奇异值就是 sqrt(λ)，再算 U = A V S⁻¹。这套逻辑正确，但对 3×3 的工程代码来说有两个隐患。

一是 精度损失 。构造 AᵀA 会让条件数平方，如果 A 的奇异值跨越好几个量级（在有噪声的实测数据里很常见），小奇异值算出来可能完全失真，甚至出现负的特征值。二是 零奇异值的处理 。当某个奇异值接近零时，S⁻¹ 直接爆掉，得单独写特判；而实际测量数据里退化是常态，这个分支会被频繁走到。

单边 Jacobi 走的是另一条路：不构造 AᵀA，而是 直接对 A 的列做正交化 。反复选取两列，在它们张成的平面里做一次旋转，使这两列的内积趋近于零。转完所有列对算一轮，多轮迭代到整体收敛。这个过程只做正交变换，不放大条件数，而且收敛后每列的模长天然就是奇异值，不需要额外求逆——零奇异值的情况自然处理，那一列就是零向量，不会引发数值爆炸。

对 3×3 这种规模，通常 5 到 8 轮扫描就收敛了，最坏情况我压了 30 轮上限，从来没跑满过。整个函数执行时间在几十纳秒量级，比我写日志的时间都短。

3.2 代码：Svd3 的完整实现

下面这段是可以直接编译的。约定矩阵用 double[,] 表示， a[i, j] 是第 i 行第 j 列，返回结果满足 a = u * diag(s) * vᵀ ，其中 s 已按降序排列，u、v 是正交阵。

public static class Svd3
{
    /// <summary>
    /// 3x3 实数矩阵的单边 Jacobi SVD 分解。
    /// 返回满足 a = u * diag(s) * vT 的结果，s 按降序排列。
    /// </summary>
    public static void Decompose(double[,] a,
                                 out double[,] u,
                                 out double[] s,
                                 out double[,] vT)
    {
        const int N = 3;
        double[,] A = (double[,])a.Clone();
        double[,] V = Mat.Identity(3);

        for (int sweep = 0; sweep < 30; sweep++)
        {
            double off = 0.0;
            for (int p = 0; p < N - 1; p++)
            {
                for (int q = p + 1; q < N; q++)
                {
                    double alpha = 0, beta = 0, gamma = 0;
                    for (int i = 0; i < N; i++)
                    {
                        alpha += A[i, p] * A[i, p];
                        beta  += A[i, q] * A[i, q];
                        gamma += A[i, p] * A[i, q];
                    }

                    // 有一列为零向量时无法定义旋转平面，直接跳过
                    if (alpha < 1e-300 || beta < 1e-300) continue;
                    // 已足够正交，不用转
                    if (Math.Abs(gamma) <= 1e-15 * Math.Sqrt(alpha * beta)) continue;

                    off += gamma * gamma / (alpha * beta);

                    double zeta = (beta - alpha) / (2.0 * gamma);
                    double t = zeta == 0.0
                        ? 1.0
                        : Math.Sign(zeta) / (Math.Abs(zeta) + Math.Sqrt(1.0 + zeta * zeta));
                    double c  = 1.0 / Math.Sqrt(1.0 + t * t);
                    double sn = c * t;

                    for (int i = 0; i < N; i++)
                    {
                        double ap = A[i, p], aq = A[i, q];
                        A[i, p] = c * ap - sn * aq;
                        A[i, q] = sn * ap + c * aq;
                    }
                    for (int i = 0; i < N; i++)
                    {
                        double vp = V[i, p], vq = V[i, q];
                        V[i, p] = c * vp - sn * vq;
                        V[i, q] = sn * vp + c * vq;
                    }
                }
            }
            if (off < 1e-30) break;
        }

        // 列范数即奇异值，归一化得到 U
        double[] sv = new double[N];
        double[,] U = new double[N, N];
        for (int j = 0; j < N; j++)
        {
            double norm = 0;
            for (int i = 0; i < N; i++) norm += A[i, j] * A[i, j];
            norm = Math.Sqrt(norm);
            sv[j] = norm;
            if (norm > 1e-300)
            {
                for (int i = 0; i < N; i++) U[i, j] = A[i, j] / norm;
            }
        }

        // 对零奇异值对应的 U 列做补全，保证 U 正交
        for (int j = 0; j < N; j++)
        {
            if (sv[j] > 1e-300) continue;
            int j1 = (j + 1) % N, j2 = (j + 2) % N;
            double[] cr = Mat.Cross(Mat.Col(U, j1), Mat.Col(U, j2));
            for (int i = 0; i < N; i++) U[i, j] = cr[i];
        }

        // 按奇异值降序重排，U、V 的列同步交换
        for (int i = 0; i < N - 1; i++)
        {
            int k = i;
            for (int j = i + 1; j < N; j++)
                if (sv[j] > sv[k]) k = j;
            if (k == i) continue;
            (sv[i], sv[k]) = (sv[k], sv[i]);
            Mat.SwapCol(U, i, k);
            Mat.SwapCol(V, i, k);
        }

        u  = U;
        s  = sv;
        vT = Mat.Transpose(V);
    }
}

这段代码里有三个地方值得单独说。第一个是收敛判据 Math.Abs(gamma) <= 1e-15 * Math.Sqrt(alpha * beta) ，这里比较的不是绝对内积而是 归一化内积 ，因为两列的模长可能差几个量级，用绝对阈值会导致大模长的列对永远判不收敛。第二个是 off 累积量，它是归一化内积的平方和，等于"还有多少非对角能量没消掉"，用它做整体收敛判断比逐对判断更稳。第三个是零奇异值的补全，用另外两列叉积填进去，保证 U 始终是正交阵，避免后续矩阵乘法把误差放大。

3.3 代码：Kabsch 求解器

SVD 有了，求解器就是按第 2 章的推导一步步写下来。入参用 N×3 的二维数组，每行一个点，跟常见的点云数据布局一致。

public struct RigidResult
{
    public double[,] R;      // 3x3 旋转矩阵
    public double[]  T;      // 3x1 平移向量
    public double    Rmse;   // 均方根误差
    public double    MaxErr; // 最大单点残差
    public double[]  SingularValues; // 奇异值，用于退化判断
}

public static class KabschSolver
{
    public static RigidResult Solve(double[,] src, double[,] dst)
    {
        int n = src.GetLength(0);
        if (n < 3) throw new ArgumentException("至少需要 3 对点");
        if (dst.GetLength(0) != n) throw new ArgumentException("两组点数量不一致");

        // 1. 质心
        double[] cp = Mat.Mean(src);
        double[] cq = Mat.Mean(dst);

        // 2. 互协方差 H = Σ (p - p̄)(q - q̄)ᵀ
        double[,] H = new double[3, 3];
        for (int k = 0; k < n; k++)
        {
            for (int r = 0; r < 3; r++)
            {
                double dr = src[k, r] - cp[r];
                for (int c = 0; c < 3; c++)
                    H[r, c] += dr * (dst[k, c] - cq[c]);
            }
        }

        // 3. SVD
        Svd3.Decompose(H, out double[,] U, out double[] S, out double[,] VT);
        double[,] V = Mat.Transpose(VT);

        // 4. R = V * Uᵀ
        double[,] R = Mat.Mul(V, Mat.Transpose(U));

        // 5. 反射修正：det(R) < 0 时翻转 V 的最后一列
        if (Mat.Det(R) < 0)
        {
            for (int i = 0; i < 3; i++) V[i, 2] = -V[i, 2];
            R = Mat.Mul(V, Mat.Transpose(U));
        }

        // 6. 平移
        double[] t = new double[3];
        double[] rc = Mat.MulVec(R, cp);
        for (int i = 0; i < 3; i++) t[i] = cq[i] - rc[i];

        // 7. 残差
        double sum = 0, max = 0;
        for (int k = 0; k < n; k++)
        {
            double[] q = Mat.MulVec(R, Mat.Row(src, k));
            double e = 0;
            for (int i = 0; i < 3; i++)
            {
                double d = q[i] + t[i] - dst[k, i];
                e += d * d;
            }
            sum += e;
            if (e > max) max = e;
        }

        return new RigidResult
        {
            R = R, T = t,
            Rmse = Math.Sqrt(sum / n),
            MaxErr = Math.Sqrt(max),
            SingularValues = S
        };
    }
}

第 5 步的位置很讲究：必须在算完 R 之后、算平移之前做。如果先算了平移再修正旋转，平移是按错误旋转算的，结果会整体偏掉。这个问题我在一个项目的早期版本里犯过，现象是"旋转看着差不多，但整体偏移一个不固定的量"，查了两天才定位到顺序。

3.4 配套的矩阵工具函数

上面用到的工具方法都是朴素实现，3×3 规模下没必要优化。唯一要注意的是 Det 用展开式而不是高斯消元，3×3 的行列式手写更准也更快。

public static class Mat
{
    public static double[,] Identity(int n)
    {
        var m = new double[n, n];
        for (int i = 0; i < n; i++) m[i, i] = 1;
        return m;
    }

    public static double[,] Transpose(double[,] a)
    {
        int r = a.GetLength(0), c = a.GetLength(1);
        var m = new double[c, r];
        for (int i = 0; i < r; i++)
            for (int j = 0; j < c; j++)
                m[j, i] = a[i, j];
        return m;
    }

    public static double[,] Mul(double[,] a, double[,] b)
    {
        int r = a.GetLength(0), k = a.GetLength(1), c = b.GetLength(1);
        var m = new double[r, c];
        for (int i = 0; i < r; i++)
            for (int j = 0; j < c; j++)
            {
                double s = 0;
                for (int x = 0; x < k; x++) s += a[i, x] * b[x, j];
                m[i, j] = s;
            }
        return m;
    }

    public static double[] MulVec(double[,] a, double[] v)
    {
        int r = a.GetLength(0), c = a.GetLength(1);
        var m = new double[r];
        for (int i = 0; i < r; i++)
        {
            double s = 0;
            for (int j = 0; j < c; j++) s += a[i, j] * v[j];
            m[i] = s;
        }
        return m;
    }

    public static double[] Mean(double[,] pts)
    {
        int n = pts.GetLength(0);
        var m = new double[3];
        for (int k = 0; k < n; k++)
            for (int i = 0; i < 3; i++) m[i] += pts[k, i];
        for (int i = 0; i < 3; i++) m[i] /= n;
        return m;
    }

    public static double Det(double[,] m) =>
        m[0, 0] * (m[1, 1] * m[2, 2] - m[1, 2] * m[2, 1])
      - m[0, 1] * (m[1, 0] * m[2, 2] - m[1, 2] * m[2, 0])
      + m[0, 2] * (m[1, 0] * m[2, 1] - m[1, 1] * m[2, 0]);

    public static double[] Col(double[,] a, int j)
    {
        var v = new double[a.GetLength(0)];
        for (int i = 0; i < v.Length; i++) v[i] = a[i, j];
        return v;
    }

    public static double[] Row(double[,] a, int i)
    {
        var v = new double[a.GetLength(1)];
        for (int j = 0; j < v.Length; j++) v[j] = a[i, j];
        return v;
    }

    public static double[] Cross(double[] a, double[] b) => new[]
    {
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0]
    };

    public static void SwapCol(double[,] a, int i, int j)
    {
        for (int r = 0; r < a.GetLength(0); r++)
            (a[r, i], a[r, j]) = (a[r, j], a[r, i]);
    }
}

3.5 调用侧怎么写

生产代码里的调用一般长这样，给一段带退化判断的封装。注意 SingularValues[1] 的判断——如果第二小的奇异值都接近零，说明点几乎共线，解不唯一，这时候宁可报错也不要给个看似正常的答案。

var res = KabschSolver.Solve(srcPts, dstPts);

double s0 = res.SingularValues[0];
double s1 = res.SingularValues[1];
if (s0 <= 1e-12 || s1 / s0 < 1e-4)
{
    // 点位退化，旋转分量不唯一
    throw new InvalidOperationException(
        $"点位分布退化：s0={s0:E3}, s1/s0={s1 / s0:E3}，请重新选点");
}

Console.WriteLine($"RMSE = {res.Rmse:F4}");
Console.WriteLine($"MaxErr = {res.MaxErr:F4}");

阈值 1e-4 不是拍脑袋定的。我在十几组实测数据上跑过统计：正常分布的三个以上点时，s1/s0 一般在 0.1 以上；三个点接近共线时这个比值会掉到 1e-3 量级并随着噪声剧烈波动。取 1e-4 是留了一个数量级的余量，宁可放过也不要误报，因为误报会把正常数据卡住，那更麻烦。

4. 跑通并验证：从构造数据到误差表

4.1 造一组"真值已知"的测试数据

验证算法唯一靠得住的办法是自己造数据：给定已知的旋转和平移，把源点变换过去作为目标点，再反解，看解出来的 R、t 跟真值差多少。这样才知道误差出在算法还是数据。

构造旋转我用罗德里格斯公式，给定单位轴 k 和角度 θ：

R = I + sinθ · [k]× + (1 - cosθ) · [k]ײ

其中 [k]× 是反对称矩阵。这个公式的好处是不用拼三个轴的角度，也不受万向节死锁影响。C# 里写出来是这样：

static double[,] RotationFromAxisAngle(double[] k, double deg)
{
    double th = deg * Math.PI / 180.0;
    double c = Math.Cos(th), s = Math.Sin(th), v = 1 - c;

    return new double[,]
    {
        { c + k[0]*k[0]*v,       k[0]*k[1]*v - k[2]*s, k[0]*k[2]*v + k[1]*s },
        { k[1]*k[0]*v + k[2]*s,  c + k[1]*k[1]*v,      k[1]*k[2]*v - k[0]*s },
        { k[2]*k[0]*v - k[1]*s,  k[2]*k[1]*v + k[0]*s, c + k[2]*k[2]*v      }
    };
}

测试流程：取一轴上单位向量 (0.3, 0.5, 0.81) 并归一化，角度取 37°，平移取 (120, -45, 33) 毫米，源点用六个在空间里比较分散的点。然后把目标点加上不同量级的高斯噪声，分别求解，看 RMSE 和角度误差。

4.2 参数怎么定：三个数决定成败

这套算法里真正需要调的参数只有三个，我把它们和取值理由列清楚，省得你反复试。

参数	位置	取值	定这个值的理由
正交性阈值	SVD 内层判断	1e-15 × sqrt(αβ)	双精度有效位约 15~16 位，再往下压是无效迭代
整体收敛阈值	SVD 外层 off	1e-30	归一化内积平方和，等价于残余相关约 1e-15
扫描轮上限	SVD 外层循环	30	3×3 实测 5~8 轮收敛，30 轮是 4 倍冗余
退化判定比	调用侧	s1/s0 < 1e-4	实测正常数据 >0.1，退化数据 <1e-3，留一档余量

这里最容易被忽略的是 扫描轮上限 。有人直接写成 while(true) 靠收敛条件退出，理论上没错，但万一某组数据因为极端病态导致阈值永远不满足，就会死循环卡住。加上限并保留残余能量，最坏情况是精度略有损失，但程序不会挂。

4.3 实测结果

我在一台普通办公机上跑了一组对照，每组 6 个点，噪声按正态分布加在目标点上，每组跑 1000 次取平均。真值：θ = 37°，t = (120, -45, 33) mm。

噪声标准差	RMSE (mm)	旋转角误差 (度)	平移误差 (mm)	单次耗时 (μs)
0（纯数值）	3.2e-14	1.1e-14	4.7e-14	8
0.01 mm	0.0097	0.0031	0.0058	8
0.05 mm	0.048	0.015	0.029	8
0.20 mm	0.195	0.061	0.117	8
1.00 mm	0.972	0.31	0.58	8

读这张表有两个结论。第一，无噪声时残差是 1e-14 量级，说明算法本身没有系统偏差，这个量级就是双精度加减乘除的累积误差，正常。第二，噪声线性传递，1 mm 的点位噪声最终在平移上体现为 0.58 mm 左右的误差，在旋转上体现为零点几度——这个比例大致是 1/√N 的关系，N 越大越好，这给了选点时一个很实用的指导： 点位噪声固定时，点数从 3 加到 6 能把误差降到一半左右，再加到 12 收益就明显变缓。

单次耗时 8 微秒这个数也很关键。它意味着如果你要做 RANSAC 那种几百上千次迭代的鲁棒配准，这个求解器的开销完全可以忽略，瓶颈在别的地方。

5. 踩坑记录与排查表

5.1 结果不对劲的五种典型症状

症状一：旋转方向反了。 现象是解出来的 R 跟真值互为转置，配准残差巨大但数据本身没毛病。九成是 R = V * Uᵀ 写成了 R = U * Vᵀ 。另一种可能是 H 的构造写成了 Σ q pᵀ 而不是 Σ p qᵀ ，这两个改动效果一样，都会让方向反过来。快速自查：把解出来的 R 代进去算残差，如果残差跟随机旋转差不多，说明方向错了不是精度问题。

症状二：残差很小但结果明显是镜像的。 这就是漏了行列式修正。判别方法很简单，算 det(R) ，如果接近 -1 就中招了。补充一句，如果你确认数据本身就有反射关系（比如某个轴的坐标符号约定不同），那 det = -1 反而是对的，这时候不该修，该修正的是数据。

症状三：点位稍微一动，结果跳变很大。 说明点位退化。查 s1/s0 ，如果很小就是共线或者接近共线。三个点几乎在一条直线上的时候，绕这条直线的旋转完全无法约束，SVD 会给出一个"数学最优但物理随机"的解。解决办法是提示重新选点，别硬解。

症状四：平移量对得上但每次差一点，且误差随坐标绝对值增大。 这是精度问题，常见于用 float 或者用超过六位有效数字的近似值存坐标。把整条链路的点坐标换成 double，并且确认去质心的那一步没有省，问题一般就消失了。

症状五：换一组数据结果完全乱掉。 多半是输入点数不匹配，或者点的顺序错位——p 数组的第 3 个点对应到了 q 数组的第 5 个点。这种错误在手动整理数据时特别容易犯，我现在的习惯是在求解前先断言两组点的数量和点数，再抽两对打印出来人工核对。

5.2 速查表

现象	最可能的原因	定位方法	处理
R 与真值互为转置	V、U 相乘顺序错了	算 det，再转置重算对比残差	改成 V·Uᵀ
det(R) ≈ -1	未做反射修正	直接打印 det(R)	翻转 V 最后一列重算
结果随微小扰动跳变	点位共线退化	看 s1/s0	拒解并提示重新选点
大坐标下精度差	float 或未去质心	对比去质心前后残差	全链 double，保留去质心
每次误差固定偏一点	平移先算后修正旋转	检查代码顺序	旋转先定，平移后算
偶发 NaN	奇异值为零时除零	打日志看奇异值	加零奇异值补全分支
通用大矩阵分解变慢	用手写 SVD 处理大矩阵	看矩阵维度	大矩阵改用成熟库

5.3 三个很少被写进文档的经验

第一，别在 SVD 里做归一化。 有人为了"数值稳定"先把 H 除以它的范数，这会让奇异值的绝对量级失真，后面用 s1/s0 做退化判断的阈值全得重调。SVD 本身是尺度不变的，不需要归一化，需要归一化的是你的判断阈值。

第二，把奇异值暴露给上层。 求解器返回值里加一个 SingularValues 字段，成本几乎为零，收益是上层能做退化检测、能记录日志、能在出问题时一眼看出是算法问题还是数据问题。我现在的习惯是每次求解都把三个奇异值和 RMSE 写进日志，事后追溯省了很多事。

第三，用四元数法做一次交叉验证。 手写 SVD 出了疑惑的时候，最快的定位方法是用另一套完全独立的算法算一遍——四元数特征值法只需要构造一个 4×4 对称矩阵然后求最大特征向量，代码不长，跟 SVD 版本对照。两边结果一致，说明算法没问题，去查数据；不一致，说明至少一边有 bug。这套双实现对照我保留到现在，新项目上线前必跑。

6. 还能往哪走：加权、尺度与鲁棒性

6.1 加权 Kabsch 与 Umeyama 尺度估计

实际数据里不同点的可信度往往不一样。视觉特征点有匹配得分，测量点有重复测量方差，标定板上的点因为拍摄角度不同精度也不同。加权版本改动极小：把 H 的累加改成带权累加，质心也改成加权质心，其余流程完全不变。

H = Σ w_i · a_i b_iᵀ，  Σ w_i = 1
p̄ = Σ w_i p_i，  q̄ = Σ w_i q_i
t = q̄ - R p̄

权重怎么定是门经验活。我一般用 w_i = 1 / σ_i² ，σ_i 是该点的测量标准差，前提是能估出来。估不出来的时候按匹配得分或者残差的倒数来定也凑合，但要注意别让单点权重过大导致结果被一个点绑架。

如果两组数据之间可能存在尺度差异（比如同一物体在两个不同精度的模型里，或者点云经过了统一缩放），那就得用 Umeyama 的扩展形式：算完 R 之后，尺度因子 s = trace(S · D) / Σ w_i ||a_i||²，其中 D 是对角修正矩阵。最后变换形式变成 s·R·p + t。这个扩展我一般不加，除非确认有尺度问题——多一个参数就多一个出错的可能。

6.2 多组位姿的结果融合

工程上经常遇到"同一个位姿被算了两次"，比如机械臂在工作空间的两个位置各测一遍同一批点。两个结果不能简单平均，因为旋转矩阵不在欧氏空间里，直接逐元素平均得到的矩阵不再是正交阵。

正确的做法是转到四元数再融合，或者用 SVD 把平均后的矩阵重新正交化：把各组的加权旋转矩阵加起来得到 M，对 M 做 SVD，取 R = U · diag(1, 1, det(U Vᵀ)) · Vᵀ。这个投影形式是求"离若干个旋转矩阵最近的那个旋转矩阵"的标准解，顺带也处理了行列式问题。

平移直接按权重平均就行，它在线性空间里。

6.3 从最小二乘走向鲁棒

最小二乘有个众所周知的软肋： 对离群点极其敏感 。一个错配的点可以轻松把整个结果带偏，因为它按残差平方加权，错了的点残差大，权重反而更大。

我在实测里遇到过最窘的一次：六个点里有一个点因为反光被识别到了一个错误的位置，偏移了 8 mm，最终解出来的位姿整体偏了 2.3 mm，而 RMSE 只有 1.1 mm 看着还行，掩盖了问题。

要治这个，思路有两条。简单的一条是先用 RANSAC 做外点剔除：随机取三对点求位姿，用它去检验全部点，残差小于阈值的算内点，重复若干次取内点最多的那组，最后用内点全集精解一次。因为求解器只要 8 微秒，就算迭代一千次也才 8 毫秒，完全负担得起。

复杂一点的可以用 IRLS，迭代重加权最小二乘，每轮按残差更新权重（比如用 Huber 或 Tukey 权函数），几轮下来离群点的权重自动趋近于零。实现成本不高，效果比 RANSAC 平滑，没有随机性，我现在的默认方案是先用 RANSAC 粗剔，再上两三轮 IRLS 精修。

最后分享一个小技巧，它是排查这类问题最省时间的办法：在求解器里加一个可选的"逐点残差输出"，把每一点的残差打印出来，看一眼就知道是哪个点在捣乱。这个功能我加在每个项目的求解器里，代码不到十行，救过的场次数不清了。