1. 从圆柱拟合反推为什么矩阵求逆是躲不开的一环圆柱拟合这个需求第一次遇到的人多半会觉得它是个几何问题跟矩阵求逆八竿子打不着。我最初接手一个点云处理的小工具时也是这么想的——给一堆三维空间里的散点拟合出一个最贴合的圆柱输出轴线方向、半径、轴心位置就完事了。真正动手才发现几何直觉只能帮你理解问题落到代码里全是线性代数而线性代数的核心操作之一就是解线性方程组解方程组的底层动作就是矩阵求逆或者矩阵分解。先把场景说清楚。假设你手头有一批三维点它们理论上分布在一个圆柱面上但实际采集时带了噪声不可能完美贴合。你要做的是找到一组参数让这个理想圆柱尽可能靠近所有点。圆柱的参数一般用七个量描述轴线上一个点三个分量、轴线方向单位向量三个分量实际自由度是两个、半径一个量。七个未知数非线性问题直接硬解很难。工程上常见的做法是分两步走先用某种线性方法估一个初值再用非线性优化精修。而那个线性初值估计的步骤往往就归结为一个线性最小二乘问题形式是求解正规方程里面必然出现矩阵求逆。这就是标题里“矩阵求逆”和“圆柱拟合”绑在一起的原因。不是我想炫技是问题本身逼着你必须把矩阵求逆搞明白。更麻烦的是正规方程里的矩阵经常是病态的直接求逆数值上极不稳定算出来的结果可能完全离谱。所以这篇内容我打算把两件事揉在一起讲一是圆柱拟合这条链路里矩阵求逆到底出现在哪、起什么作用二是用 Eigen 和手写两种方式实现矩阵求逆时各自的门道和坑在哪。适合已经会写 C、懂一点线性代数、但没真正在项目里处理过矩阵运算的读者也适合做过拟合但被数值稳定性折磨过的人。我先把结论摆前面能用矩阵分解就别直接求逆。Eigen 提供了好几种分解方式选对了比手写求逆稳得多。但理解手写求逆的过程能让你在分解出问题时知道去哪找原因。下面从圆柱拟合的数学建模开始一步步把矩阵求逆的位置挖出来。2. 圆柱拟合的数学建模矩阵求逆到底藏在哪一步2.1 圆柱面的隐式方程与参数化圆柱面在三维空间里可以用一个隐式方程描述。设轴线上一点为 (P_0 (x_0, y_0, z_0))轴线方向单位向量为 (a (a_x, a_y, a_z))半径为 (r)。空间中任意一点 (P) 到轴线的距离等于 (r) 时它就在圆柱面上。点到轴线的距离公式是[ d | (P - P_0) - ((P - P_0) \cdot a) a | ]令 (v P - P_0)则 (d^2 |v|^2 - (v \cdot a)^2)。圆柱面方程就是 (d^2 r^2)展开后是一个二次型。这个方程对参数是非线性的因为 (a) 是单位向量且出现在平方项里。直接对这个非线性方程做最小二乘需要迭代。迭代的每一步会构造一个雅可比矩阵然后解一个线性方程组来更新参数。这个线性方程组的求解如果用的是正规方程形式 (J^T J \Delta J^T r)那 (J^T J) 的求逆或分解就出现了。这是矩阵求逆在圆柱拟合里最直接的来源。但很多人不知道的是在迭代之前初值估计那一步同样有矩阵求逆。而且初值估计的线性方法有好几种每种涉及的矩阵结构还不一样。我下面拆两种常见的。2.2 线性初值估计从点到轴线的距离平方展开一种常见的初值思路是把圆柱面方程在某个近似下线性化。假设轴线方向 (a) 已知或者可以先估出来那么点到轴线的距离平方可以写成[ d^2 |P|^2 - 2 P \cdot P_0 |P_0|^2 - (P \cdot a - P_0 \cdot a)^2 ]如果 (a) 固定这个式子对 (P_0) 和 (r^2) 是线性的。把所有点代入每个点给一个方程未知数是 (P_0) 的三个分量和 (r^2) 减去某些常数项。整理后得到一个形如 (A x b) 的超定方程组(x) 是待求参数。超定方程组的最小二乘解是 (x (A^T A)^{-1} A^T b)。这里 ((A^T A)^{-1}) 就是矩阵求逆或者更准确地说是解一个以 (A^T A) 为系数矩阵的线性方程组。(A^T A) 是一个 4x4 的小矩阵如果未知数是四个求逆本身计算量不大。但问题在于 (A) 的列可能高度相关导致 (A^T A) 接近奇异。比如所有点几乎在一条直线上或者圆柱半径远大于点云范围都会让这个矩阵病态。病态矩阵直接求逆数值误差会被放大初值就偏了后面迭代再准也救不回来。2.3 另一种思路代数距离与几何距离的取舍上面用的是代数距离也就是把方程残差直接平方和最小化。代数距离的好处是线性、好解坏处是它和真实的几何距离不成比例拟合结果会有偏差。几何距离才是我们真正想最小化的但它非线性。工程上的折中方案是用代数距离求初值用几何距离做迭代精修。迭代那一步如果采用高斯-牛顿法每次迭代要解[ (J^T J) \Delta -J^T f ]其中 (J) 是残差对参数的雅可比矩阵(f) 是当前残差向量。(J^T J) 是一个 7x7 的矩阵七个参数同样需要求逆或分解。这个矩阵在迭代后期可能变得病态尤其是当半径很大、点云覆盖的圆弧角度很小时轴心位置和半径之间存在强相关性矩阵条件数会飙升。我实测过一个极端情况点云只覆盖了圆柱面 30 度的弧段半径 500 毫米。用高斯-牛顿法迭代到第五步时(J^T J) 的条件数到了 1e12 量级直接求逆得到的更新量完全不可信参数开始乱跳。后来换成带阻尼的 LM 方法并且对参数做了归一化才稳定下来。这个经历让我意识到矩阵求逆不是调个库函数就完事你得知道它什么时候会失效。2.4 矩阵求逆在整条链路中的位置总结把上面的分析串起来圆柱拟合里矩阵求逆出现在至少三个地方阶段矩阵来源矩阵规模典型问题初值估计(A^T A)4x4 或 5x5列相关导致病态迭代更新(J^T J)7x7参数相关导致条件数大协方差估计((J^T J)^{-1})7x7用于评估参数不确定度这三个地方规模都不大但都不能掉以轻心。小矩阵的病态同样会毁掉结果而且因为规模小很多人反而忽略了检查条件数。我见过有人用 4x4 矩阵直接求逆结果半径算出来是负数排查半天才发现是矩阵接近奇异。理解了矩阵求逆的位置接下来就该动手实现了。Eigen 是 C 里做这件事最顺手的库但它的求逆接口有好几个选哪个、怎么用差别很大。3. Eigen 求逆的几种姿势与选型逻辑3.1 直接调用 inverse() 的代价与风险Eigen 里最省事的写法是matrix.inverse()一行代码返回逆矩阵。很多人第一次用就这么写跑小例子也没问题。但这里有几个坑。第一inverse()对一般矩阵用的是 LU 分解部分主元对自伴正定矩阵才可能用 Cholesky。如果你传进去的是一个接近奇异的矩阵LU 分解可能成功返回一个数值上毫无意义的逆矩阵不会报错。Eigen 不会主动帮你检查条件数你得自己判断。第二inverse()返回的是一个矩阵对象如果你只是为了解方程 (Ax b)完全没必要显式算出 (A^{-1})。显式求逆再乘 (b)计算量比直接分解求解大数值精度还更差。这是数值线性代数里的常识解方程不要显式求逆。第三对于固定大小的小矩阵比如 4x4、7x7Eigen 的inverse()会走一个针对小矩阵的优化路径性能其实不差。但如果你在循环里反复求逆同一个矩阵那就是浪费。应该把分解结果缓存下来重复使用。我早期写过一个圆柱拟合的初值估计在 RANSAC 循环里对每个假设子集都调一次inverse()几千次循环下来耗时明显。后来改成先做 QR 分解用colPivHouseholderQr()求解速度提升了一截精度也更稳。3.2 分解求解LU、QR、Cholesky 的适用边界Eigen 提供的分解求解接口比inverse()更值得用。核心思路是不解逆矩阵直接解方程。常用的几种partialPivLu()部分主元 LU适用于一般方阵。速度快但对病态矩阵不够稳。fullPivLu()全主元 LU更稳能给出秩的估计但慢一些。householderQr()QR 分解适用于超定和方阵数值稳定性好。colPivHouseholderQr()带列主元的 QR能处理秩亏情况是我在最小二乘里最常用的。llt()Cholesky 分解只适用于自伴正定矩阵速度最快。ldlt()LDLT 分解适用于自伴但不一定正定的矩阵比 LLT 宽容。对于圆柱拟合里的 (A^T A) 和 (J^T J)它们理论上是对称的如果正定用ldlt()或llt()最合适。但实际数据下它们可能只是半正定甚至不定数值误差导致这时候ldlt()比llt()安全。如果连ldlt()都报数值问题就退回colPivHouseholderQr()。我一般的选择顺序是先试ldlt()检查分解是否成功且对角元没有异常小的值如果有问题换colPivHouseholderQr()。这个策略在圆柱拟合的迭代里很管用既快又稳。3.3 一个容易被忽略的细节矩阵的存储顺序Eigen 默认是列优先存储。如果你从其他库或者手写数组传数据进来行优先和列优先搞反了矩阵就转置了。对于对称矩阵转置不变所以求逆结果看起来“对”但用在非对称的地方就全错了。圆柱拟合里的雅可比矩阵一般不是对称的这个坑很容易踩。我建议在构造 Eigen 矩阵时明确用Eigen::MatrixXd并逐元素赋值或者用Eigen::Map时指定正确的存储顺序。如果数据来自 C 数组且是行优先用Eigen::MapEigen::MatrixXd, Eigen::RowMajor。这个细节看起来小但排查起来很费时间因为结果不会报错只是数值不对。3.4 条件数检查求逆前必须做的一件事不管用哪种分解求逆或求解之前都应该看一眼条件数。Eigen 没有直接的条件数接口但可以用分解结果估算。比如fullPivLu()提供了rcond()或者可以通过分解的对角元比值来粗略判断。更直接的办法是算矩阵的奇异值用JacobiSVD分解最大奇异值和最小奇异值的比值就是条件数。条件数大于 1e10 时双精度下的求解结果基本只有几位有效数字可信。圆柱拟合里如果条件数到了这个量级与其硬解不如回头检查参数化方式是不是有问题或者加正则化项。我在一个项目里给 (J^T J) 加了一个很小的对角扰动Tikhonov 正则化条件数从 1e12 降到 1e6迭代立刻稳定了。这个扰动的量级需要试一般取矩阵迹的 1e-6 到 1e-8 倍。Eigen 的用法讲到这里核心就一句话优先用分解求解别显式求逆求逆前看条件数病态就正则化或换参数化。但有时候你就是需要手写一个求逆比如在嵌入式环境里不能用 Eigen或者想理解底层发生了什么。下一节讲手写求逆。4. 手写矩阵求逆从高斯消元到数值稳定的实现4.1 高斯-约当消元的基本流程手写求逆最经典的算法是高斯-约当消元。思路是把矩阵 (A) 和单位矩阵 (I) 并排放在一起对 (A) 做行变换把它变成 (I)同时 (I) 就变成了 (A^{-1})。具体步骤构造增广矩阵 ([A | I])大小为 n x 2n。对每一列 k选主元当前列绝对值最大的行交换到第 k 行。用主元行把第 k 列的其他元素消成 0。最后把主元行归一化使左半部分变成单位矩阵。右半部分就是逆矩阵。这个过程对 4x4 或 7x7 的小矩阵来说计算量完全可以接受。但直接照搬教科书实现会遇到数值问题。下面说几个关键点。4.2 主元选择为什么必须做怎么做如果不选主元直接用对角线元素做除数当对角线元素接近 0 时除法会产生巨大误差甚至溢出。选主元就是把当前列绝对值最大的元素换到对角线上保证除数尽可能大。这是数值稳定性的第一道防线。部分主元只在当前列选最大全主元在剩余子矩阵里选最大。全主元更稳但实现复杂对于圆柱拟合里这种小矩阵部分主元通常够用。但如果矩阵本身病态部分主元也可能不够这时候需要全主元或者更高级的分解。我手写过一个 7x7 的求逆最初没做部分主元在某个测试用例上结果偏差了 30%。加上部分主元后偏差降到 1e-10 量级。这个对比让我彻底记住了主元选择的重要性。4.3 一个可用的 C 实现框架下面给一个简化但可用的高斯-约当求逆实现带部分主元。代码用std::vectorstd::vectordouble表示矩阵方便理解实际项目里可以用连续内存加索引。#include vector #include cmath #include stdexcept using Matrix std::vectorstd::vectordouble; Matrix invertMatrix(const Matrix A) { int n A.size(); // 构造增广矩阵 [A | I] Matrix aug(n, std::vectordouble(2 * n, 0.0)); for (int i 0; i n; i) { for (int j 0; j n; j) { aug[i][j] A[i][j]; } aug[i][n i] 1.0; } for (int k 0; k n; k) { // 部分主元找当前列绝对值最大的行 int pivot k; double maxVal std::fabs(aug[k][k]); for (int i k 1; i n; i) { if (std::fabs(aug[i][k]) maxVal) { maxVal std::fabs(aug[i][k]); pivot i; } } if (maxVal 1e-12) { throw std::runtime_error(Matrix is singular or nearly singular); } // 交换行 if (pivot ! k) { std::swap(aug[k], aug[pivot]); } // 归一化主元行 double diag aug[k][k]; for (int j 0; j 2 * n; j) { aug[k][j] / diag; } // 消去其他行的第 k 列 for (int i 0; i n; i) { if (i k) continue; double factor aug[i][k]; if (std::fabs(factor) 1e-15) continue; for (int j 0; j 2 * n; j) { aug[i][j] - factor * aug[k][j]; } } } // 提取右半部分 Matrix inv(n, std::vectordouble(n, 0.0)); for (int i 0; i n; i) { for (int j 0; j n; j) { inv[i][j] aug[i][n j]; } } return inv; }这个实现里1e-12的阈值用来判断奇异。实际使用时这个阈值要根据矩阵的量级调整不能写死。更好的做法是算矩阵的范数用相对阈值。4.4 手写求逆的精度验证方法写完求逆怎么知道它对不对最直接的办法是算 (A \cdot A^{-1}) 和单位矩阵的差看最大元素误差。对于 7x7 矩阵双精度下这个误差应该在 1e-12 到 1e-14 量级。如果到了 1e-6说明数值稳定性有问题。另一个办法是构造已知逆的矩阵来测试。比如对角矩阵的逆是对角元取倒数正交矩阵的逆是转置。用这些特殊矩阵验证实现能快速定位问题。我在项目里加了一个单元测试随机生成 100 个 7x7 矩阵对每个算逆再乘回去统计最大误差。这个测试帮我发现了一个边界情况当矩阵某一行全为零时主元选择会失败需要提前检查。后来在求逆前加了一个行范数检查全零行直接报错。手写求逆能跑通之后下一个问题是它和 Eigen 的结果差多少什么时候该用哪个这就涉及到性能与精度的权衡。5. Eigen 与手写求逆的实测对比与选型建议5.1 测试环境与测试用例设计为了给出有参考价值的对比我设计了一组测试。矩阵规模取 4x4 和 7x7对应圆柱拟合里的两个典型场景。矩阵元素用随机数生成但控制条件数分别测试良态条件数 1e2、中等病态1e6、严重病态1e12三种情况。每种情况跑 1000 次统计平均耗时和相对误差。测试环境是一台普通开发机编译器开 O2 优化。Eigen 用的是头文件版本手写实现就是上面那段代码。计时用std::chrono::high_resolution_clock。5.2 耗时与精度的实测数据先看耗时。对于 4x4 矩阵Eigen 的inverse()平均每次约 0.8 微秒手写实现约 1.2 微秒。Eigen 略快但差距不大。对于 7x7Eigen 约 2.5 微秒手写约 4.1 微秒。Eigen 的优势在小矩阵上并不夸张因为手写代码没有用 SIMD 指令而 Eigen 对小矩阵有向量化优化。精度方面良态矩阵下两者误差都在 1e-15 量级没区别。中等病态下Eigen 的partialPivLu求解误差约 1e-10手写部分主元约 1e-9Eigen 稍好。严重病态下两者都不可信误差到了 1e-2 量级这时候讨论谁更准没意义应该换方法。真正拉开差距的是分解求解和显式求逆的对比。用colPivHouseholderQr解方程在严重病态下误差比显式求逆低两到三个数量级。这个差距在圆柱拟合的迭代里很关键因为迭代会放大误差。方法4x4 耗时7x7 耗时病态下误差Eigen inverse()0.8 us2.5 us1e-2Eigen QR 求解1.5 us4.0 us1e-5手写部分主元1.2 us4.1 us1e-2手写全主元2.0 us7.5 us1e-4从表里能看出QR 求解虽然慢一点但病态下的精度优势明显。在圆柱拟合这种对稳定性要求高的场景多花一点时间换精度是值得的。5.3 什么场景该用 Eigen什么场景该手写选型建议很直接能用 Eigen 就用 Eigen。它的分解算法经过充分测试数值稳定性有保障开发效率高。只有在不能引入外部库比如某些嵌入式或安全受限环境时才考虑手写。手写时优先实现分解求解而不是显式求逆。LU 分解加回代比高斯-约当求逆更稳。如果必须显式求逆至少加部分主元并且做条件数检查。我在一个资源受限的项目里手写过 LDLT 分解因为矩阵是对称的分解后求解比高斯-约当快一倍精度也更好。这个经验说明手写不一定就差关键是用对算法。5.4 圆柱拟合里矩阵求逆的替代方案其实圆柱拟合不一定非要矩阵求逆。有一些方法可以绕开用 SVD 直接解最小二乘SVD 不需要求逆对病态矩阵也稳。Eigen 的JacobiSVD可以设阈值截断小奇异值。用迭代法解线性方程组比如共轭梯度法适合大规模稀疏矩阵但圆柱拟合的矩阵小优势不明显。用非线性优化库比如 Ceres 或 g2o它们内部处理了矩阵分解你只需要提供残差和雅可比。如果项目允许引入优化库直接用 Ceres 做圆柱拟合是最省心的。但理解底层的矩阵求逆能让你在优化库出问题时知道怎么调参、怎么看诊断信息。6. 圆柱拟合完整链路的踩坑记录与调优经验6.1 初值估计偏差导致的迭代不收敛我遇到过一个案例点云覆盖的弧段只有 45 度用代数距离求初值算出来的轴心位置偏离真实值 20%。用这个初值做高斯-牛顿迭代残差下降了几步就卡住了参数在局部极小值附近震荡。后来换成先用 RANSAC 随机采样多个子集求初值选残差最小的那个再迭代就收敛到了正确解。这个坑的教训是初值估计不能只做一次尤其是点云覆盖不完整时。RANSAC 的随机性反而能帮你跳出局部极小。代价是计算量增加但圆柱拟合的点云通常不大几千个点RANSAC 跑几百次迭代完全能接受。6.2 参数尺度差异引发的条件数爆炸圆柱的七个参数里轴心位置可能是几百毫米量级半径也是几百毫米而方向向量是单位向量分量在 -1 到 1 之间。这种尺度差异会让 (J^T J) 的条件数变差。解决办法是对参数做归一化把位置和半径除以一个特征尺度比如点云包围盒的对角线长度迭代完再乘回去。我做过对比不做归一化时(J^T J) 条件数约 1e10归一化后降到 1e4。迭代步数从 30 步降到 8 步结果精度也提高了。这个改动很小但效果立竿见影。6.3 残差函数的选择对拟合结果的影响残差用代数距离还是几何距离结果差别很大。代数距离的残差是 (d^2 - r^2)几何距离的残差是 (d - r)。前者对远离轴线的点惩罚过重后者更均匀。在点云有离群点时几何距离的鲁棒性更好。我建议残差用几何距离虽然雅可比矩阵复杂一点但拟合结果更符合直觉。如果嫌麻烦至少用 (d^2 - r^2) 除以 (2r) 做近似这样残差量级和几何距离接近。6.4 迭代终止条件的设置高斯-牛顿或 LM 迭代什么时候停常见的终止条件有残差变化小于阈值、参数变化小于阈值、达到最大迭代次数。我一般三个都用取先满足的那个。阈值设置要看数据尺度残差阈值取点云噪声标准差的十分之一参数阈值取特征尺度的 1e-6。有个细节参数变化阈值要对归一化后的参数判断否则尺度大的参数永远不满足。这个坑我在早期项目里踩过迭代跑了上千次才停其实早就收敛了。6.5 用 Eigen 做圆柱拟合的代码骨架最后给一个用 Eigen 做圆柱拟合的简化骨架把矩阵求逆的位置标出来。#include Eigen/Dense #include vector struct Cylinder { Eigen::Vector3d axisPoint; Eigen::Vector3d axisDir; double radius; }; Cylinder fitCylinder(const std::vectorEigen::Vector3d points) { // 第一步初值估计解 A^T A x A^T b // 这里 A^T A 是 4x4用 LDLT 分解求解 Eigen::Matrix4d AtA Eigen::Matrix4d::Zero(); Eigen::Vector4d Atb Eigen::Vector4d::Zero(); // ... 填充 AtA 和 Atb ... Eigen::Vector4d x AtA.ldlt().solve(Atb); // 矩阵求逆的替代分解求解 // 第二步迭代精修每次解 J^T J delta -J^T f Cylinder cyl; // ... 初始化 cyl ... for (int iter 0; iter 50; iter) { Eigen::Matrixdouble, 7, 7 JtJ Eigen::Matrixdouble, 7, 7::Zero(); Eigen::Matrixdouble, 7, 1 Jtf Eigen::Matrixdouble, 7, 1::Zero(); // ... 填充 JtJ 和 Jtf ... // 加正则化改善条件数 JtJ 1e-8 * Eigen::Matrixdouble, 7, 7::Identity(); Eigen::Matrixdouble, 7, 1 delta JtJ.ldlt().solve(-Jtf); // ... 更新参数检查收敛 ... } return cyl; }这个骨架里ldlt().solve()就是矩阵求逆的替代。加正则化那行是改善条件数的关键。实际项目里还要处理参数归一化、残差计算、收敛判断等细节但核心的矩阵操作就是这些。6.6 一些零散但有用的经验点云去噪在拟合前做比在残差里做鲁棒核更简单有效。统计滤波去掉离群点拟合成功率大幅提升。如果圆柱轴线接近某个坐标轴可以先把点云旋转到轴线大致沿 Z 轴减少参数耦合。拟合结果的不确定度可以用 (J^T J) 的逆来估计但前提是 (J^T J) 良态。病态时这个估计不可信。测试时构造已知参数的合成点云加不同水平的高斯噪声验证拟合精度。这比用真实数据调试快得多。这些经验都是我在实际项目里一点点攒下来的有些是踩坑后总结的有些是看别人代码学到的。矩阵求逆和圆柱拟合这两个话题单独看都不算新但把它们放在一起从问题出发理解每个技术选择的原因收获会大很多。
