如果你接触过机器人、SLAM、计算机视觉或者三维重建Eigen 这个名字大概率已经听得耳朵起茧了。它几乎是 C 生态里做矩阵运算、几何变换绕不开的那套基础库头文件搬过来就能用性能和易用性都相当能打。这个系列前面几篇聊过基础矩阵类型、运算规则、线性方程组求解这篇是第四篇我想把重心从算数挪到几何和分解上——也就是旋转、位姿、四元数、特征值分解和 SVD 这些在真实项目里最高频、也最容易写错的部分。1. 旋转的世界为什么只靠旋转矩阵还不够很多刚上手 Eigen 的同学处理三维旋转时习惯只用Matrix3d往里填九个数字觉得够用了。但真到了工程里尤其是涉及姿态估计、位姿插值、图优化、滤波更新的时候旋转矩阵就成了一个很尴尬的存在——九个元素描述三个自由度冗余不说还有正交约束和行列式为 1 的约束优化器稍微迭代几步矩阵就变成一个不伦不类的伪旋转矩阵。所以刚开始做位姿相关模块时我就吃过这个亏最后不得不老老实实把旋转的几种表达方式都摸清楚。1.1 旋转矩阵、旋转向量、欧拉角三兄弟Eigen 里表达旋转的常见方式有四类旋转矩阵、旋转向量轴角、欧拉角、四元数。前三个都在Eigen/Geometry模块里用法最直白的应该是旋转向量对应的AngleAxis。AngleAxis从名字就能看出来它说的是绕着某个轴转多少度。比如绕 Z 轴转 45 度#include Eigen/Core #include Eigen/Geometry #include iostream int main() { // 绕 Z 轴旋转 45 度单位向量表示转轴 Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d::UnitZ()); // 转成旋转矩阵 Eigen::Matrix3d rotation_matrix rotation_vector.toRotationMatrix(); std::cout Rotation matrix:\n rotation_matrix std::endl; // 从旋转矩阵再恢复出旋转向量 Eigen::AngleAxisd recovered(rotation_matrix); std::cout Axis: recovered.axis().transpose() std::endl; std::cout Angle: recovered.angle() std::endl; return 0; }这段代码没有什么难懂的地方但我想强调两个点。第一AngleAxisd的底层数据不是四元数也不是矩阵而是轴 角的存储方式但你可以把它当成一个旋转算子来用它和旋转矩阵相乘、和四元数相乘、直接作用于向量都可以。rotation_vector * point的效果就是把point旋转过去这一点和旋转矩阵的用法完全一致。第二旋转向量也不是万能的。最明显的问题就是它不便于插值。虽然有人尝试直接在轴角空间做线性插值但算出来的中间姿态往往不是最短路径而且当角度接近 0 或者 2π 的时候轴的估计会变得非常不稳定。所以做插值、平滑这些工作还是四元数更靠谱后面我会专门讲。再说欧拉角。欧拉角直观而且很多传感器陀螺仪、云台都直接用欧拉角输出姿态你不接触它也不行。Eigen 里通过eulerAngles这个函数把旋转矩阵转成欧拉角。// 按 Z、Y、X 的旋转顺序即先绕 Z 轴再绕 Y 轴最后绕 X 轴提取欧拉角 Eigen::Vector3d euler rotation_matrix.eulerAngles(2, 1, 0); std::cout Euler angles (roll, pitch, yaw): euler.transpose() std::endl;eulerAngles(2, 1, 0)里的参数用的是轴编号0 代表 X 轴1 代表 Y 轴2 代表 Z 轴。也就是说(2, 1, 0)对应的是 ZYX 内旋顺序这是航空、机器人领域非常常见的一种定义通常对应 yaw、pitch、roll。这里有一个特别容易踩的坑欧拉角的定义不唯一。同一个姿态你换一种旋转顺序解出来的三个角度完全不同就算旋转顺序一样也可能因为正值负值范围的约定不同而产生差异。Eigen 返回的角度范围、象限处理都和自己写的约定未必一致。我的建议是如果项目里只需要给人看、做简单的调试输出用欧拉角没问题但如果要拿这两个数字去和其他系统对接或者参与结算一定要先确认双方的旋转顺序和轴向定义否则差之毫厘谬以千里。1.2 欧拉角最经典的问题万向锁既然说到欧拉角就不得不提万向锁Gimbal Lock。用 ZYX 顺序的时候如果 pitch 等于正负 90 度roll 和 yaw 就变得不可分辨相当于丢失了一个旋转自由度。这不是 Eigen 的问题而是欧拉角这种表达方式本身在数学上就存在奇异点。所以很多做姿态估计的工程师到后面都会慢慢转向四元数——它没有奇异点而且插值特别方便。如果你刚接触这些概念我建议你先在本地把旋转矩阵、旋转向量、欧拉角三种表达方式互相转换的代码跑一遍对着一组已知角度验证输出把表达方式和旋转结果之间的关系彻底搞清楚再往下走。2. 四元数被很多 C 开发者低估的姿态描述利器四元数在不少初学者眼里是个玄学概念i 的平方等于负一看起来简单真到用的时候又不知道coeffs()的顺序、不知道两个四元数乘起来到底代表什么。但说实话一旦你用惯四元数就会觉得旋转矩阵和欧拉角都啰嗦。2.1 先理解四元数的双重覆盖现象简单来说一个单位四元数可以用来表示一个三维旋转。它由一个实部和三个虚部组成习惯上写成q w xi yj zk其中w是实部(x, y, z)是虚部对应的向量部分。单位四元数满足模长为 1。四元数有两个很反直觉的性质。第一个性质是q和-q表示同一个旋转。也就是说你把四个分量全部取相反数旋转效果完全一样。这在做四元数插值或者求平均的时候特别容易出问题——两个物理上几乎一致的姿态可能四元数表示却一个在正半区一个在负半区如果直接硬插值结果会绕一大圈。很多做骨骼动画的人都在这里吃过亏。第二个性质是四元数乘法不满足交换律但我们可以用它表示旋转的复合。q1 * q2表示先应用q2的旋转再应用q1的旋转这和矩阵乘法R1 * R2的语义是一致的。你在写代码的时候要特别小心这个顺序否则姿态会突然变得非常奇怪。2.2 Eigen 里四元数的几个高频操作Eigen 里的Quaterniond用法很直接。从旋转矩阵或者旋转向量构造四元数都只要一行Eigen::Quaterniond q1(rotation_matrix); // 从旋转矩阵构造 Eigen::Quaterniond q2(rotation_vector); // 从旋转向量AngleAxis构造反过来把四元数转成旋转矩阵用toRotationMatrix()Eigen::Matrix3d rot q1.toRotationMatrix();旋转一个点也很自然Eigen::Vector3d point(1.0, 0.0, 0.0); Eigen::Vector3d rotated q1 * point; // 相当于 rot * point但我用下来最想提醒大家的是关于coeffs()的输出顺序。Eigen 中q.coeffs()返回的是一个四维向量顺序是(x, y, z, w)注意实部w在最后一位。这个顺序和很多资料里写的(w, x, y, z)不一样也不符合我们平时手写四元数的习惯。我记得到现在为止团队里还有同事不小心写出q.coeffs()[3]当成 x 分量用的诡异代码。建议你第一次接触时就在代码里打印一下coeffs().transpose()强制用经验把顺序印在脑子里。2.3 四元数归一化与插值和旋转矩阵需要保持正交性类似四元数表示旋转的前提是单位化。Eigen 的Quaterniond在构造时并不会自动归一化你需要确认自己传入的数据是单位模长。如果你做的是从传感器数据读四元数那通常还好一般上游已经归一化了但如果是自己通过一些组合运算得到的四元数就得主动调normalize()或者normalized()。normalize()是就地归一化normalized()返回一个归一化副本。如果你后续打算用这个四元数去乘向量或者转旋转矩阵没有归一化会导致旋转之后向量模长变化最终反映到点云配准或者位姿估计上就是一个隐形的误差源。提到插值slerp是四元数最实用的操作之一。它做的是球面线性插值两个姿态之间能走出自然的最短弧路径double t 0.5; // 0 到 1 之间 Eigen::Quaterniond interpolated q1.slerp(t, q2);slerp的结果也可以用normalized()保证一下模长因为在某些特殊情况下数值误差会让模长略微偏离 1。做动画、机械臂轨迹规划、相机姿态平滑的时候四元数 slerp基本是标配。3. 变换矩阵与 Isometry把旋转和平移缝在一起旋转矩阵只能描述姿态但真实物体在三维空间里还有位置。一个刚体的完整位姿旋转加平移最简单的方式是用一个 4x4 的变换矩阵来表达。Eigen 里专门为这个场景提供了Isometry3d表示等距变换也就是刚体变换。3.1 为什么需要用齐次坐标我刚开始学图形学时也有疑问为什么不能直接维护一个Matrix3d和一个Vector3d非要搞 4x4 的齐次矩阵原因主要有两个。第一数学上统一。一个 4x4 矩阵既能表示旋转也能表示平移还能表示投影、缩放、剪切这些操作。把旋转和平移打包成一个矩阵组合变换的时候只需要做一次矩阵乘法逻辑上非常干净。第二避免把旋转和平移混在一起算时搞错顺序。如果你分开存旋转矩阵和平移向量那么在复合两个位姿的时候你必须记得新平移 旋转1 * 平移2 平移1这个公式写几次没问题写多了就容易出低级错误。用齐次变换矩阵一次 4x4 乘法就把复合关系算完了。但 4x4 矩阵也有开销。它比 3x3 矩阵多了一行一列如果只是大量做旋转向量乘法显然没那么划算。所以 Eigen 里有Transform类还针对常见情况提供了Isometry3d这种别名让用户可以按需选择。3.2 IsometryT 的 API 细节与组合顺序坑Isometry3d底层是Transformdouble, 3, IsometryEigen 在内部仍然存储 3x3 旋转和 3x1 平移但对外提供了统一的接口。构造一个刚体变换很简单Eigen::Isometry3d T Eigen::Isometry3d::Identity(); T.rotate(rotation_vector); // 设置旋转部分 T.pretranslate(Eigen::Vector3d(1, 2, 3)); // 设置平移部分 std::cout Transformation matrix:\n T.matrix() std::endl;rotate和pretranslate这两个名字很形象但我要提醒一句Eigen 这里的rotate默认右乘更新也就是说如果你连续调用两次rotate它做的是T T * R2 * R1这样的链式更新。对于从单位矩阵开始构造的场景T.rotate(rotation_vector)会直接把这个旋转变换设置进去这个是好用的。但如果你的T已经包含了一些信息想覆盖旋转部分最稳妥的手段还是直接访问底层矩阵T.linear() rotation_matrix; // 直接覆盖旋转部分 T.translation() Eigen::Vector3d(1, 2, 3);我个人更推荐后一种写法尤其在位姿更新、拼接、重算的场景下。因为rotate的语义是叠加旋转不是设置为这个旋转不少人在这个接口上栽过跟头。取变换矩阵的各个部分也有固定姿势Eigen::Matrix3d R T.linear(); // 旋转部分 Eigen::Vector3d t T.translation(); // 平移部分 Eigen::Matrix4d M T.matrix(); // 完整 4x4 矩阵变换一个点直接用T * point就行。这里的内部实现会先对point做旋转再加平移等价于 4x4 矩阵乘以齐次坐标后的结果但 Eigen 做了优化没有真的把点升维成四维再算。3.3 组合两个位姿时最容易犯的顺序错误位姿组合在 SLAM 和机械臂运动学里非常常见。假设有两个刚体变换T1和T2想把T1的坐标系套在T2的坐标系上得到新的变换T T1 * T2。关键问题是这个乘法顺序到底是谁先应用Eigen 的Transform遵循普通矩阵乘法的语义T T1 * T2表示先应用T2从右往左看是 T2 先作用再应用T1。这和旋转矩阵的复合顺序完全一致——右侧的变换先作用在点上。有些资料里会说左乘表示在世界坐标系下变换右乘表示在自身坐标系下变换这句话是对的但对新手来说容易在代码里看反。我的建议是永远用具体点来验证取一个点分别手动计算T1 * T2 * p和T2 * T1 * p看看哪个结果符合你预期的坐标系关系。调试位姿相关的代码这种拿一个给定点手算验证的习惯特别有价值。4. 特征值与 SVDEigen 从算工具变成思考工具很多人把 Eigen 当成一个高级计算器只会调A * B、A.inverse()。其实到了项目中期特征值分解和奇异值分解才是真正体现思考深度的部分。它们能用于主成分分析、点云匹配、矩阵伪逆、陀螺仪噪声分析等一系列场景。4.1 用 Eigen 求解特征值和特征向量Eigen 里最常用的特征值求解器是SelfAdjointEigenSolver它专门处理实对称矩阵的自伴随特征值分解。物理上很多矩阵天然是对称的比如协方差矩阵、惯性矩阵、应力张量等用这个求解器又稳又快。#include Eigen/Eigenvalues Eigen::Matrix3d A; A 4.0, 1.0, 0.0, 1.0, 3.0, 1.0, 0.0, 1.0, 2.0; Eigen::SelfAdjointEigenSolverEigen::Matrix3d solver(A); std::cout Eigenvalues:\n solver.eigenvalues() std::endl; std::cout Eigenvectors (each column):\n solver.eigenvectors() std::endl;eigenvalues()返回一个向量默认按升序排列小的在前大的在后。eigenvectors()返回一个矩阵每一列是对应特征值的特征向量。注意矩阵乘法验证A * v lambda * v的时候v必须是列向量也就是eigenvectors().col(i)。使用上有一个常见的坑SelfAdjointEigenSolver的模板参数必须是固定维度或者动态维度都可以但如果你的矩阵是动态尺寸构造时最好先明确大小避免反复分配内存。另外它只适用于对称矩阵如果你对非对称矩阵调用结果就是未定义的。面对一般矩阵应该用EigenSolver但它是复数域的输出的是一个复数矩阵判断是否理解到位的时候就容易看懵。4.2 SVD比特征值分解更通用、更稳的方法奇异值分解SVD不要求矩阵是方阵也不要求对称。一个任意矩阵A都能分解成U * S * V^T其中U和V是正交矩阵S是对角矩阵对角线上的元素是奇异值从大到小排列。Eigen 里用JacobiSVD求解非常直接#include Eigen/SVD Eigen::MatrixXd A(3, 2); A 1.0, 2.0, 2.0, 1.0, 3.0, 5.0; Eigen::JacobiSVDEigen::MatrixXd svd( A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::MatrixXd U svd.matrixU(); Eigen::VectorXd S svd.singularValues(); Eigen::MatrixXd V svd.matrixV();这里的ComputeFullU | ComputeFullV告诉求解器需要完整的U和V。如果矩阵尺寸特别大可以用ComputeThinU和ComputeThinV来节省存储跑起来也更快。后面做点云配准我建议用 thin 版本就够了。SVD 之所以重要一是它数值稳定性好当矩阵接近奇异或者说近似退化时特征值分解往往表现不佳而 SVD 依然会给出一个合理的分解结果二是奇异值能直接反映矩阵的秩和条件数比如我们判断一个求解问题是否病态就可以看最大奇异值和最小奇异值的比值。很多机器人领域的状态估计代码里求矩阵伪逆都是用 SVD 而不是直接调inverse()就是因为它对病态矩阵更友好。4.3 求伪逆SVD 带来的额外优势求非方阵或奇异矩阵的逆很多时候我们希望得到的是最小二乘意义下的伪逆。标准做法就是先把矩阵 SVD 分解取非零奇异值求倒数再重建矩阵。Eigen 里甚至不需要自己重建可以直接用svd.solve(b)来解最小二乘问题它内部就做了伪逆相关的事情。这一招在做超定方程组求解、拟合、标定时都非常常用。我在实际项目里比较少直接用手写 SVD 重建矩阵因为JacobiSVD的solve接口已经很够了。但如果你需要自己构造伪逆矩阵记得只对大于某个阈值的奇异值求倒数其他置零。阈值可以根据矩阵尺寸和数据精度来定我通常取最大奇异值乘以 1e-8 作为截断阈值。5. 综合实战用 SVD 完成一次点云配准基础概念讲了这么多如果只是会用 API还不够。这一节我们来做一个真正能用的东西给定两组具有对应关系的三维点用 SVD 求解它们之间的旋转矩阵和平移向量也就是经典的点云配准对应点已知的情况下。这个问题的数学背景是假设有一组点p_i经过某个刚体变换R, t之后得到q_i但我们只知道这些点对想反解出R和t。公式表达就是最小化sum_i || R * p_i t - q_i ||^25.1 简化问题先去质心这个优化问题有一个很经典的解叫做 Kabsch 算法或者 Umeyama 算法。核心思想是先把两组点的质心对齐这样平移量t可以先通过质心之差求出来剩下的问题就只是一个纯旋转问题。具体步骤是计算p的质心p_centroid和q的质心q_centroid。把两组点都减去各自的质心得到去中心化后的点集p_demean和q_demean。构造 3x3 的协方差矩阵H p_demean^T * q_demean。对H做 SVDH U * S * V^T。旋转矩阵R V * U^T。如果det(R) 0说明反射出现需要把V的最后一列取反再重新计算R。平移向量t q_centroid - R * p_centroid。这个算法在三维点云配准、结构光标定、双目相机外参估计这些场景里都非常常见。我第一次实现时没有做第 6 步的反射判断结果在高噪声数据上偶尔会出现一个镜像旋转当时排查了很久所以这里一定要写清楚。5.2 用 Eigen 写一个可运行的配准函数下面是一个完整的函数示例输入是两个Eigen::MatrixXd每行是一个三维点行数相同顺序一一对应输出是配准后的旋转矩阵和平移向量。#include Eigen/Core #include Eigen/SVD #include iostream struct Pose { Eigen::Matrix3d R; Eigen::Vector3d t; }; Pose SolveRigidTransform(const Eigen::MatrixXd p, const Eigen::MatrixXd q) { // 这里假设 p 和 q 都是 N x 3 的矩阵 assert(p.rows() q.rows()); assert(p.cols() 3 q.cols() 3); // 1. 计算质心 Eigen::RowVector3d p_centroid p.colwise().mean(); Eigen::RowVector3d q_centroid q.colwise().mean(); // 2. 去质心 Eigen::MatrixXd p_demean p.rowwise() - p_centroid; Eigen::MatrixXd q_demean q.rowwise() - q_centroid; // 3. 协方差矩阵 H sum_i p_i * q_i^T Eigen::Matrix3d H p_demean.transpose() * q_demean; // 4. SVD 分解 Eigen::JacobiSVDEigen::Matrix3d svd( H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d U svd.matrixU(); Eigen::Matrix3d V svd.matrixV(); // 5. 旋转矩阵 Eigen::Matrix3d R V * U.transpose(); // 6. 检查反射情况 if (R.determinant() 0) { V.col(2) * -1.0; R V * U.transpose(); } // 7. 平移 Eigen::Vector3d t q_centroid.transpose() - R * p_centroid.transpose(); Pose result; result.R R; result.t t; return result; }这里有几处细节我认为值得单独拎出来讲。colwise().mean()返回的是一个行向量表示每一列的平均值。对点云来说正好就是所有点的平均坐标。去质心用p.rowwise() - p_centroid这个表达式会把每一行都减去同一个行向量是 Eigen 里少有的广播机制写起来非常舒服。H p_demean.transpose() * q_demean这个操作的维度是3xN * Nx3结果是 3x3正好是协方差矩阵。很多教材里的写法是sum(p_i * q_i^T)等价于矩阵形式。用矩阵乘法一行代码搞定也比显式循环快得多。5.3 生成模拟数据验证结果光写函数不够还要证明它能跑。我用一个简单的模拟场景来验证先随机生成一批三维点构造一个已知的旋转和平移施加到这些点上再加一点噪声最后用上面的函数去反解对比误差。#include cmath #include cstdlib #include ctime int main() { std::srand(static_castunsigned(std::time(nullptr))); // 生成 50 个随机三维点 const int N 50; Eigen::MatrixXd p(N, 3); for (int i 0; i N; i) { p(i, 0) static_castdouble(std::rand()) / RAND_MAX * 10.0; p(i, 1) static_castdouble(std::rand()) / RAND_MAX * 10.0; p(i, 2) static_castdouble(std::rand()) / RAND_MAX * 10.0; } // 已知的旋转绕 Z 轴转 30 度和平移 Eigen::AngleAxisd rotation(30.0 * M_PI / 180.0, Eigen::Vector3d::UnitZ()); Eigen::Matrix3d R_gt rotation.toRotationMatrix(); Eigen::Vector3d t_gt(1.0, 2.0, 3.0); // 施加变换并加入小噪声 Eigen::MatrixXd q(N, 3); for (int i 0; i N; i) { Eigen::Vector3d pt p.row(i).transpose(); Eigen::Vector3d qt R_gt * pt t_gt; qt Eigen::Vector3d::Random() * 0.01; // 标准差约 0.01 的噪声 q.row(i) qt.transpose(); } // 用函数求解 Pose result SolveRigidTransform(p, q); std::cout Estimated R:\n result.R std::endl; std::cout Ground truth R:\n R_gt std::endl; std::cout Estimated t:\n result.t.transpose() std::endl; std::cout Ground truth t:\n t_gt.transpose() std::endl; // 计算旋转误差 Eigen::Matrix3d diff result.R - R_gt; double rot_error diff.norm(); double trans_error (result.t - t_gt).norm(); std::cout Rotation error norm: rot_error std::endl; std::cout Translation error norm: trans_error std::endl; return 0; }噪声比较小的时候旋转误差和平移误差应该都在 1e-2 量级甚至更低这足以说明算法和代码没有大问题。如果你加大噪声到 0.1 以上依然能保持大致正确只是误差会变大。这个实验特别适合用来验证自己对 SVD 分解、矩阵乘法顺序、坐标维度转换的理解。5.4 配准实战中的几个工程经验第一对应点匹配是另一个大问题。上面假设点对一一对应是已知的真实点云里通常不知道哪个点对应哪个点那就要走 ICP 或者特征匹配的路线。但不管前面的匹配怎么做最后一步用 SVD 求刚体变换的流程都是一样的所以这个函数可以直接复用。第二实际数据里经常混有离群点。一个离群点就可能把协方差矩阵拉偏导致结果明显变差。所以在做 SVD 求解之前不要天真地直接上全部点对最好先用某种方式过滤离群点或者做两步迭代先粗配准算出误差大的点对并剔除再精配准。第三注意数值范围。如果点云坐标数值特别大比如几十万量级减去质心之后点坐标仍然很大构造的H矩阵会出现数值差异太大的问题SVD 的稳定性会受到影响。这时候可以先对点云做缩放归一化或者使用更高精度的long double中间量。我用的做法是先把所有点绕质心做一个缩放到单位范围解出变换后再把缩放补偿回去。6. 几个我自己绕过的弯说给你听最后分享几个实操层面的小体会都是我在项目里切切实实踩过的。第一Eigen 的调试输出别只盯着cout。虽然std::cout matrix很方便但矩阵一大就看不出哪里不对。我后来习惯在可疑的地方打印矩阵维度、对角线元素、范数这些统计量再用matrix.isApprox(other, precision)做近似比较。这个函数比逐个元素比绝对差要科学得多因为它考虑了数值尺度。第二别频繁在代码里用auto接收 Eigen 表达式的返回值。Eigen 有延迟计算机制auto会把表达式本身存下来而不是立刻算完。如果后面这个表达式引用的矩阵被修改或者生命周期结束就可能得到神秘崩溃。我在很早以前就养成了Eigen 返回值尽量显式声明类型的习惯尤其是像transpose()、matrixU()这些要用具体类型接住。第三内存分配问题。Eigen 的固定尺寸对象比如Matrix3d、Vector3d默认不产生堆分配性能很好但一旦用MatrixXd很多操作会触发动态内存分配。在实时性要求高的循环里尽量把动态矩阵的尺寸提前固定下来或者复用预分配好的对象用noalias()防止乘法的临时变量。做点云配准这类运算时这部分优化很值得做。回到这篇的标题Eigen 的几何模块和分解工具说起来也就是一组头文件、几百行 API但真正的价值在于你遇到实际问题时知道该从口袋里掏出哪把钥匙。这篇文章把旋转表达、四元数、刚体变换、特征值分解和 SVD 从 API 到实战思路都过了一遍也在配准案例里演示了它们是怎么协同工作的。希望你在项目里遇到类似需求时能少走一点我当年走过的弯路。