1. 项目概述从“反函数求导”到李代数求导的思维跃迁最近在机器人学、SLAM即时定位与地图构建和计算机视觉的圈子里一个老生常谈但又常谈常新的基础问题又被大家翻出来讨论李代数求导。这听起来像是个纯数学的抽象话题但如果你正在用Ceres、g2o或者自己手写后端优化处理三维空间中的旋转、位姿估计那你每天都在和它打交道。问题的核心往往源于一个更直观的困惑我们都知道对普通函数y f(x)求导但面对一个旋转矩阵R它是李群SO(3)的一个元素我们想优化一个目标函数J(R)这个“导数”dJ/dR该怎么定义又该怎么算直接对矩阵的9个元素求导行不行这里面的坑我踩过不止一次。网络上热议的“反函数的求导法则”给了我们一个绝佳的切入点。在单变量微积分里如果y f(x)且反函数x f^{-1}(y)存在那么dx/dy 1 / (dy/dx)。这个法则简洁有力但它隐含了一个关键前提函数定义在普通的向量空间欧氏空间上加减法和数乘是良定义的。然而旋转矩阵构成的李群SO(3)不是一个向量空间。两个旋转矩阵相加结果可能不再是旋转矩阵会破坏正交性因此“R ΔR”这种直接的扰动是无效的。这就是我们无法直接定义dJ/dR的根本原因——定义域不是一个线性空间。那么工程师们是怎么解决这个问题的答案就是李代数。李代数对于SO(3)来说就是那个由三维反对称矩阵组成的空间so(3)可以等同于三维向量φ是一个向量空间。我们不再直接对李群元素R求导而是将R用其对应的李代数φ参数化通过指数映射R exp(φ^)然后对李代数参数φ求导。这就把非线性空间上的优化问题转化到了其切空间一个线性空间上的优化问题。所以李代数求导的本质是为李群上的函数寻找一个在对应向量空间李代数上的、有效的导数定义和计算方法。它解决了在机器人姿态估计、相机标定、点云配准等任务中无法直接对旋转/变换进行微分的核心难题。接下来我将拆解这个过程的每一步从为什么需要李代数到两种核心的求导模型扰动模型和直接求导再到具体的公式推导、代码实现片段以及我在实际调参和优化中积累的避坑指南。无论你是刚接触视觉SLAM的学生还是正在调试姿态解算模块的工程师希望这篇深入“为什么”和“怎么做”的梳理能给你带来实实在在的帮助。2. 核心思路为何绕道李代数——从群论到优化要理解李代数求导不能一上来就硬记公式。我们必须先搞清楚为什么这条路非走不可。这背后是李群Lie Group的几何结构所决定的。2.1 李群SO(3)的非线性困境三维旋转矩阵R的集合构成了特殊正交群 SO(3)。它有两个关键性质封闭性两个旋转矩阵相乘结果仍是旋转矩阵。流形结构SO(3)是一个三维的光滑流形你可以想象成一个弯曲的三维空间。在它上面每个点一个旋转矩阵附近的一小块区域可以近似地看成一个平直的欧氏空间这个近似的平直空间就是切空间。问题就出在这里。在平直的欧氏空间比如R^3中我们可以定义导数f(x) lim_{Δx-0} (f(xΔx) - f(x)) / Δx。这个定义依赖于“加法”x Δx。但在SO(3)流形上R ΔR这个操作没有几何意义结果会跑出流形不再是旋转矩阵。因此lim_{ΔR-0} (J(RΔR) - J(R)) / ΔR这个表达式本身就无法成立。注意这里常有一个误解认为可以对矩阵的每个元素R_ij单独求偏导。这在数学上作为R^9上的函数是可行的但得到的9x1梯度向量在用于更新R时如R R - α * gradient会破坏R的正交约束导致迭代几次后R就不再是旋转矩阵优化会发散。这就是所谓的“约束优化”问题而李代数提供了一种优雅的无约束参数化方法。2.2 李代数 so(3)切空间的化身每个李群都有一个对应的李代数记为g。对于SO(3)其李代数是so(3)由所有三维反对称矩阵组成φ^ [ [0, -φ_z, φ_y], [φ_z, 0, -φ_x], [-φ_y, φ_x, 0] ]其中φ [φ_x, φ_y, φ_z]^T是一个三维向量。李代数so(3)是一个向量空间对加法和数乘封闭。李群和李代数之间通过指数映射和对数映射相联系指数映射exp: so(3) - SO(3)R exp(φ^)。它将李代数中的一个向量切空间中的一个方向映射为李群上的一个元素。对数映射log: SO(3) - so(3)φ^ log(R)。这是指数映射的逆在局部。关键洞察来了当我们在李群R处施加一个微小的扰动这个扰动应该发生在它的切空间里。切空间正好就是李代数so(3)。因此我们考虑对R左乘一个由微小李代数δφ生成的微小旋转R exp(δφ^) * R由于δφ很小根据指数映射在单位元处的泰勒展开有exp(δφ^) ≈ I δφ^一阶近似。这样我们就把李群上的扰动转换到了李代数这个向量空间上。求导的对象就从R变成了δφ。2.3 “反函数求导法则”的启发与局限网络热词“反函数的求导法则”在这里提供了一个有趣的类比。在我们这个场景下指数映射exp可以看作是从李代数参数空间到李群变换空间的“函数”。我们关心的是目标函数J(R) J(exp(φ^))对φ的导数。这可以看作是一个复合函数求导dJ/dφ (∂J/∂R) * (∂R/∂φ)。这里的∂R/∂φ就是指数映射在φ处的导数它有一个具体的雅可比矩阵形式。而“反函数”对数映射log的导数在优化理论的某些环节比如计算残差关于参数的雅可比时也会出现。但必须清醒认识到这个类比是形式上的其底层逻辑与实数域上的反函数求导法则有本质不同。实数域的反函数求导依赖于导数的乘法逆元而在李群李代数中我们处理的是矩阵指数、李括号和流形上的微分几何结构。直接套用dφ/dR 1/(dR/dφ)是行不通的因为dR/dφ是一个雅可比矩阵并非标量。这个热词的价值在于它提醒我们关注“映射”与“逆映射”的微分关系这是理解李代数求导中各种雅可比矩阵来源的一个心理锚点。3. 两种求导模型扰动模型与直接求导在实际应用中主要有两种处理李代数求导的思路扰动模型和直接求导或称解析求导。它们各有优劣适用于不同场景。3.1 扰动模型直观的几何意义扰动模型的核心思想与我们上一节的描述完全一致在李群元素上施加一个左扰动或右扰动然后利用指数映射的一阶近似推导出目标函数相对于这个扰动量的导数。以SO(3)上的左扰动模型为例 假设有一个目标函数J(R)其中R是待优化的旋转矩阵。我们考虑一个左扰动ΔR exp(δφ^)扰动后的旋转为R ΔR * R。 目标函数的变化为ΔJ J(exp(δφ^) * R) - J(R)当δφ为小量时利用exp(δφ^) ≈ I δφ^我们可以将ΔJ表达为δφ的线性函数ΔJ ≈ (∂J/∂(δφ)) * δφ这里的∂J/∂(δφ)就是一个1x3的行向量如果J是标量它就是目标函数关于李代数扰动δφ的导数。通过链式法则展开我们最终可以得到∂J/∂(δφ) lim_{δφ-0} (J(exp(δφ^) * R) - J(R)) / δφ在形式上的定义并通过计算得到具体表达式。扰动模型的优点直观几何意义明确就是在当前估计值旁边“轻轻推一下”。公式相对简洁很多情况下最终导数的形式比较干净。适用于复杂函数当J(R)是多个变换复合的结果时扰动模型可以通过在每个变换处分别施加扰动来推导逻辑清晰。扰动模型的缺点需要手动推导对于每一个新的目标函数都需要重新进行扰动和泰勒展开的推导过程可能繁琐。可能存在近似误差依赖于一阶泰勒近似虽然对于优化算法如高斯-牛顿法的迭代步长足够但在理论上是近似的。3.2 直接求导解析求导通用的数学框架直接求导不依赖于几何扰动而是直接利用李代数的指数映射将J(R)重写为J(exp(φ^))然后直接对李代数参数φ求导。这需要用到指数映射的导数公式。对于R exp(φ^)其关于φ的导数是一个3x3的雅可比矩阵J_r(φ)满足R exp(φ^) I φ^ * J_r(φ)当φ不大时有更精确的罗德里格斯公式及其微分形式。 更一般地对于函数f(R) f(exp(φ^))根据链式法则∂f/∂φ (∂f/∂R) * (∂R/∂φ)其中∂R/∂φ就是指数映射的导数雅可比J_r(φ)。这个雅可比矩阵有闭式解可以通过罗德里格斯公式推导出来。直接求导的优点理论严谨是精确的导数不依赖于一阶近似除了指数映射本身的泰勒展开式。通用性强一旦推导出指数映射、对数映射及其雅可比的通用公式如SO(3)的J_r SE(3)的J_l等就可以像搭积木一样计算复杂函数的导数无需为每个特定函数重新推导。便于自动微分现代优化库如Ceres Solver的自动求导功能在底层处理旋转参数时本质上就是基于这种李代数的参数化和对应的导数规则。直接求导的缺点公式复杂指数映射的雅可比J_r(φ)本身的表达式就比较复杂涉及正弦余弦函数和系数。理解门槛稍高需要更扎实的李群李代数基础才能理解各个雅可比矩阵的物理意义和相互关系。实操心得在工程实践中我强烈推荐掌握并优先使用直接求导解析求导的框架。原因有三第一其通用性可以节省大量重复推导的时间尤其是在项目涉及多种不同残差模型时。第二主流优化库如Ceres, g2o, GTSAM内部都采用了这种范式理解它有助于你正确使用和调试这些库。第三当你需要自己实现一个自定义的残差块时基于解析导数公式编写的代码通常比基于扰动模型数值差分验证的代码更高效、更精确。扰动模型更适合用于快速验证想法或者理解某个特定导数项的几何含义。4. SO(3)与SE(3)上的求导公式详解理论说再多不如看公式和代码。这里我们分别给出SO(3)旋转和SE(3)变换上最核心的求导公式。这些是你在手写优化器或阅读开源SLAM代码时会反复遇到的。4.1 SO(3)上的导数场景有一个三维点p世界坐标系经过旋转R后得到相机坐标系下的点p R * p。现在有一个关于p的函数比如重投影误差的范数e ||u - π(K * p)||^2其中π是投影函数K是内参。我们需要求误差e关于旋转R的导数。我们通过李代数φ来参数化R exp(φ^)。核心公式指数映射的右雅可比J_r(φ)对于R exp(φ^)当φ的模长θ ||φ||不太大时有罗德里格斯公式R I sinθ/θ * φ^ (1-cosθ)/θ^2 * (φ^)^2其对φ的导数右雅可比为J_r(φ) ∂(exp(φ^))/∂φ I - (1-cosθ)/θ^2 * φ^ (θ - sinθ)/θ^3 * (φ^)^2当θ接近于0时需要使用极限形式以避免除零错误lim_{θ-0} J_r(φ) I。这个J_r(φ)是一个3x3矩阵。它的作用是当我们对李代数φ有一个微小增量δφ时对应的旋转矩阵变化的一阶近似为δR ≈ (J_r(φ) * δφ)^。注意这里(J_r(φ) * δφ)^表示将向量J_r(φ) * δφ转换为反对称矩阵。求导链式法则 假设误差函数e f(p) f(R * p)那么∂e/∂φ (∂e/∂p) * (∂p/∂φ)其中∂p/∂φ ∂(R*p)/∂φ ∂(exp(φ^)*p)/∂φ。利用李代数求导的公式或扰动模型推导可以得出∂(exp(φ^)*p)/∂φ lim_{δφ-0} (exp((φδφ)^)*p - exp(φ^)*p) / δφ -(R*p)^ * J_r(φ)这里(R*p)^是向量(R*p)的反对称矩阵。因此∂e/∂φ (∂e/∂p) * ( -(R*p)^ * J_r(φ) )这个3x1的向量就是误差e关于旋转李代数参数φ的梯度可以用于梯度下降法。对于高斯-牛顿法我们需要的是雅可比矩阵J ∂e/∂φ。注意事项公式中的负号-非常关键它来源于李代数的伴随性质。不同的文献或代码库可能因扰动方式左乘/右乘和坐标系约定的不同导致符号差异。在实现时务必与你所使用的框架如Ceres中的Eigen::Quaternion参数化方式保持一致最可靠的方法是用数值微分如中心差分法对小例子进行验证。4.2 SE(3)上的导数SE(3)是包含旋转和平移的刚体变换群其元素T [R, t; 0, 1]。对应的李代数se(3)是一个6维向量ξ [ρ, φ]^T其中φ是旋转部分与so(3)对应ρ是与平移相关的部分。指数映射为T exp(ξ^)其中ξ^是一个4x4的矩阵。核心公式SE(3)指数映射的雅可比J_l(ξ)对于T exp(ξ^)其关于李代数ξ的“左雅可比”J_l(ξ)是一个6x6的矩阵。它将李代数se(3)上的微小增量δξ映射到变换矩阵T的微小变化以李代数形式表示。具体公式比SO(3)更复杂通常分块表示为J_l(ξ) [ J_11, J_12; 0, J_r(φ) ]其中J_r(φ)就是SO(3)的右雅可比J_11和J_12是与φ和ρ有关的3x3矩阵有闭式解涉及φ的幂级数。更常用的形式扰动模型下的雅可比在视觉SLAM中我们经常直接求一个点p经过变换T后的坐标p T * p R * p t关于李代数ξ的导数。 令p exp(ξ^) * p齐次坐标下的变换。利用SE(3)的扰动模型左扰动exp(δξ^)可以推导出∂(exp(ξ^)*p)/∂δξ [ I, -(R*p t)^; 0, 0 ]这是一个4x6的矩阵但通常我们只取前三维得到3x6的矩阵 更常见的是将其拆分为关于平移ρ和旋转φ的两部分关于平移部分ρ的导数∂p/∂ρ R简单直观因为平移直接加在旋转后的点上关于旋转部分φ的导数∂p/∂φ -(R*p)^注意这里没有J_r(φ)这是因为我们采用了扰动模型且扰动施加在变换矩阵上。如果采用直接对ξ求导则会包含J_l(ξ)的相关块。一个经典的SLAM例子重投影误差的雅可比假设世界点P_w相机位姿T_cw将世界点变换到相机坐标系相机内参K观测像素坐标为z [u, v]^T。重投影误差为e z - π(K * (T_cw * P_w))其中π是去畸变和投影到像素平面的函数π([x, y, z]^T) [x/z, y/z]^T。 我们需要求e关于位姿李代数ξ的雅可比J ∂e/∂ξ。 计算步骤计算相机坐标系下的点P_c T_cw * P_w [X, Y, Z]^T。计算投影点p π(P_c) [X/Z, Y/Z]^T。误差e z - p。计算e关于P_c的导数∂e/∂P_c这是一个2x3的矩阵投影函数的雅可比。计算P_c关于李代数扰动δξ的导数∂P_c/∂δξ这是一个3x6的矩阵即上面提到的[I, -(R*p_w)^]其中p_w是P_w的前三维。链式法则J ∂e/∂ξ (∂e/∂P_c) * (∂P_c/∂δξ)得到一个2x6的雅可比矩阵。这个2x6的雅可比矩阵就是捆绑调整Bundle Adjustment中对一个观测边位姿-路标点关于位姿节点的导数。它是整个优化问题中最核心的计算单元之一。5. 代码实现与数值验证理论推导是基础但最终要落地到代码。这里我给出一些关键部分的C代码片段使用Eigen库并强调实现中的细节和验证方法。5.1 SO(3)指数映射、对数映射及其雅可比#include Eigen/Core #include Eigen/Geometry #include cmath // 将三维向量 phi 转换为反对称矩阵 Eigen::Matrix3d skew(const Eigen::Vector3d v) { Eigen::Matrix3d m; m 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return m; } // SO(3) 指数映射李代数 phi - 旋转矩阵 R Eigen::Matrix3d ExpSO3(const Eigen::Vector3d phi) { double theta phi.norm(); if (theta 1e-10) { return Eigen::Matrix3d::Identity(); // 单位旋转 } Eigen::Matrix3d phi_hat skew(phi); Eigen::Matrix3d R Eigen::Matrix3d::Identity() sin(theta) / theta * phi_hat (1 - cos(theta)) / (theta * theta) * phi_hat * phi_hat; return R; } // SO(3) 对数映射旋转矩阵 R - 李代数 phi Eigen::Vector3d LogSO3(const Eigen::Matrix3d R) { double theta acos((R.trace() - 1) / 2.0); if (theta 1e-10) { return Eigen::Vector3d::Zero(); } Eigen::Matrix3d phi_hat (R - R.transpose()) / (2 * sin(theta)) * theta; Eigen::Vector3d phi(phi_hat(2,1), phi_hat(0,2), phi_hat(1,0)); return phi; } // SO(3) 指数映射的右雅可比 J_r(phi) Eigen::Matrix3d JrSO3(const Eigen::Vector3d phi) { double theta phi.norm(); if (theta 1e-10) { return Eigen::Matrix3d::Identity(); } Eigen::Vector3d a phi / theta; // 旋转轴单位向量 Eigen::Matrix3d a_hat skew(a); Eigen::Matrix3d J Eigen::Matrix3d::Identity() - (1 - cos(theta)) / (theta * theta) * a_hat (theta - sin(theta)) / (theta * theta * theta) * a_hat * a_hat; return J; }5.2 一个完整的求导示例旋转点云的匹配误差假设我们有两个点云P {p_i}和Q {q_i}我们想找到一个旋转R使得Σ_i || R * p_i - q_i ||^2最小。这是一个经典的普氏分析Procrustes Analysis问题。目标函数为J(R) 0.5 * Σ_i || R * p_i - q_i ||^2。我们来计算其关于李代数φ的梯度。解析梯度推导使用扰动模型 考虑左扰动ΔR exp(δφ^)扰动后误差e_i(δφ) (exp(δφ^) * R * p_i - q_i)。 目标函数J(δφ) 0.5 * Σ_i e_i^T e_i。 对δφ求导∂J/∂δφ Σ_i e_i^T * (∂e_i/∂δφ)其中∂e_i/∂δφ ∂(exp(δφ^) * R * p_i)/∂δφ |_{δφ0}。利用公式∂(exp(δφ^) * p)/∂δφ |_{δφ0} -p^注意符号这里取决于扰动定义可得∂e_i/∂δφ -(R * p_i)^。 因此梯度为g ∂J/∂δφ -Σ_i (R * p_i - q_i)^T * (R * p_i)^由于a^T * b^ -b^T * a对于反对称矩阵和向量点积的性质可以化简为向量形式g Σ_i (R * p_i) × q_i这里×是叉积。这个结果非常优美梯度等于所有对应点叉积的和。代码实现Eigen::Vector3d computeGradientForPointCloudAlign( const Eigen::Matrix3d R, const std::vectorEigen::Vector3d points_p, const std::vectorEigen::Vector3d points_q) { Eigen::Vector3d gradient Eigen::Vector3d::Zero(); for (size_t i 0; i points_p.size(); i) { Eigen::Vector3d rotated_p R * points_p[i]; // 梯度 g (R * p_i) × q_i gradient rotated_p.cross(points_q[i]); } return gradient; // 这是目标函数 J 关于李代数扰动 δφ 的梯度 } // 使用梯度下降法迭代优化简化版 Eigen::Matrix3d alignPointClouds( const std::vectorEigen::Vector3d P, const std::vectorEigen::Vector3d Q, int max_iterations 100) { Eigen::Matrix3d R Eigen::Matrix3d::Identity(); // 初始化为单位阵 double learning_rate 1e-4; for (int iter 0; iter max_iterations; iter) { Eigen::Vector3d grad computeGradientForPointCloudAlign(R, P, Q); // 更新李代数参数φ_{k1} φ_k - α * gradient // 注意我们的梯度是关于扰动δφ的所以这里用负梯度更新 Eigen::Vector3d delta_phi -learning_rate * grad; // 将李代数增量转换为旋转矩阵并左乘到当前估计上对应左扰动模型 Eigen::Matrix3d delta_R ExpSO3(delta_phi); R delta_R * R; // 更新旋转 if (grad.norm() 1e-6) break; // 收敛判断 } return R; }5.3 数值验证确保你的推导和代码正确在实现任何李代数求导代码后数值验证是必不可少的一步。使用中心差分法来验证解析梯度的正确性。bool testGradientNumerically( const Eigen::Matrix3d R, const std::vectorEigen::Vector3d P, const std::vectorEigen::Vector3d Q) { double eps 1e-7; Eigen::Vector3d analytic_grad computeGradientForPointCloudAlign(R, P, Q); Eigen::Vector3d numeric_grad; for (int i 0; i 3; i) { Eigen::Vector3d delta Eigen::Vector3d::Zero(); delta(i) eps; // 计算 J(R * exp(δφ^)) 左扰动模型 Eigen::Matrix3d R_plus ExpSO3(delta) * R; Eigen::Matrix3d R_minus ExpSO3(-delta) * R; double J_plus 0, J_minus 0; for (size_t j 0; j P.size(); j) { J_plus (R_plus * P[j] - Q[j]).squaredNorm(); J_minus (R_minus * P[j] - Q[j]).squaredNorm(); } J_plus * 0.5; J_minus * 0.5; // 中心差分 numeric_grad(i) (J_plus - J_minus) / (2 * eps); } double diff (analytic_grad - numeric_grad).norm(); std::cout Analytic grad: analytic_grad.transpose() std::endl; std::cout Numeric grad: numeric_grad.transpose() std::endl; std::cout Difference norm: diff std::endl; return diff 1e-6; }运行这个测试函数如果解析梯度与数值梯度的差异在1e-6量级以下通常就可以认为你的推导和代码是正确的。这是调试自定义优化问题中最有效的方法。6. 常见陷阱与工程实践要点在实际项目中应用李代数求导除了理解公式更需要避开一些常见的坑。下面是我从多个SLAM和三维重建项目中总结出的经验。6.1 参数化的一致性左乘、右乘与坐标系这是最混乱的地方。李代数的扰动可以施加在旋转矩阵的左边或右边exp(δφ^)*R或R*exp(δφ^)这分别对应着**体坐标系左乘和惯性世界坐标系右乘**下的扰动。不同的扰动方式会导致求导公式差一个负号或伴随矩阵。左扰动体坐标系R exp(δφ^) * R。这意味着扰动是施加在当前旋转矩阵所代表的物体坐标系上。在机器人学中这通常更自然因为我们在本地机体坐标系下施加控制量。右扰动惯性坐标系R R * exp(δφ^)。这意味着扰动是施加在固定的世界坐标系上。如何选择这取决于你的问题定义和参数更新方式。关键是在整个系统中保持一致性。大多数现代SLAM库如OKVIS, VINS-Mono和优化库如Ceres的EigenQuaternionParameterization默认或推荐使用右扰动因为它在更新全局位姿时更直观T_{world}_new T_{world}_old * exp(ξ^)。但也有一些库使用左扰动。实操心得在阅读论文或代码时第一件事就是搞清楚它使用的是左扰动还是右扰动。自己实现时明确选择一种并贯穿始终。最稳妥的方法是为你选择的参数化方式用数值微分编写一个测试用例验证你的解析导数公式。永远不要假设。6.2 奇异性问题与处理李群李代数映射存在奇异性。对于SO(3)当旋转角度θ π时对数映射log(R)不唯一存在两个相反的旋转轴对应同一个旋转矩阵。虽然在实际优化中迭代步长通常很小不太可能直接跳到π但在初始化或优化剧烈震荡时可能遇到。影响奇异性会导致雅可比矩阵J_r(φ)在θ0附近出现数值不稳定虽然公式中有极限定义或者在θ接近π时对数映射计算不稳定。应对策略使用四元数在优化内部可以使用四元数作为旋转的参数化。四元数没有奇异性除了双覆盖问题可通过强制w非负解决且更新时可以通过在李代数空间计算增量再转换为四元数更新q_{new} q_old * δq(δφ)其中δq(δφ)是由小量李代数δφ转换来的增量四元数。Ceres库就提供了EigenQuaternionParameterization来实现这种操作。鲁棒的计算在实现ExpSO3和LogSO3函数时必须处理θ接近于0的情况使用泰勒展开或直接返回单位矩阵/零向量避免除以零。合理的初始化优化问题需要一个好的初始值避免初始位姿离真值太远导致李代数增量过大。6.3 与优化库的集成以Ceres Solver为例在实际项目中我们很少从头编写优化器而是使用Ceres、g2o等库。理解李代数求导能帮助你正确使用这些库。在Ceres中如果你要优化一个旋转矩阵或四元数你需要定义一个局部参数化。以四元数为例// 自定义一个使用李代数更新的四元数参数化对应右扰动 class QuaternionLocalParameterization : public ceres::LocalParameterization { public: virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { // x 是当前四元数 [w, x, y, z] // delta 是李代数增量 δφ (3维) Eigen::Mapconst Eigen::Quaterniond q(x); Eigen::Mapconst Eigen::Vector3d delta_vec(delta); // 将李代数增量转换为增量四元数 (使用指数映射的一阶近似) Eigen::Quaterniond delta_q(1, delta_vec[0]/2, delta_vec[1]/2, delta_vec[2]/2); delta_q.normalize(); // 可选的保证是单位四元数 // 右乘更新 (对应右扰动) Eigen::MapEigen::Quaterniond q_plus(x_plus_delta); q_plus q * delta_q; return true; } virtual bool ComputeJacobian(const double* x, double* jacobian) const override { // 雅可比矩阵是 4x3表示四元数更新关于李代数增量的导数 // 对于右扰动这个雅可比是 [ -q.x/2, -q.y/2, -q.z/2; // q.w/2, -q.z/2, q.y/2; // q.z/2, q.w/2, -q.x/2; // -q.y/2, q.x/2, q.w/2 ] // 具体公式可参考相关文献 // 这里为简洁省略实现Ceres的EigenQuaternionParameterization已提供 return true; } virtual int GlobalSize() const override { return 4; } virtual int LocalSize() const override { return 3; } };然后在添加参数块时指定这个局部参数化ceres::Problem problem; Eigen::Quaterniond q; // 待优化的旋转 problem.AddParameterBlock(q.coeffs().data(), 4, new QuaternionLocalParameterization);对于残差计算你需要在自定义的代价函数中正确计算残差关于旋转用四元数表示的导数。这个导数通常是关于李代数扰动δφ的导数即我们前面推导的2x3或2x6雅可比。Ceres的自动求导AutoDiffCostFunction可以帮你计算这个导数只要你正确实现了残差函数并且参数块使用了正确的局部参数化。关键点优化库在内部进行迭代时它是在你的局部参数空间即李代数空间中计算增量delta然后通过Plus函数更新全局参数四元数或旋转矩阵。因此你的残差函数关于参数的导数必须被解释为关于这个局部李代数参数的导数。如果你自己提供解析导数AnalyticCostFunction就必须提供这个导数。6.4 性能考量雅可比矩阵的计算效率在大型BA问题中雅可比矩阵的计算是性能瓶颈。李代数求导涉及三角函数sin,cos和除法比简单的欧氏空间求导昂贵。优化技巧预计算与共享对于同一个位姿参数它在多个残差项观测中共享。计算该位姿对应的旋转矩阵R及其相关的J_r(φ)或反对称矩阵(R*p)^时应只计算一次并缓存避免在每个残差中重复计算。利用稀疏性BA问题的雅可比矩阵是高度稀疏的。一个观测只依赖于一个位姿和一个路标点。在实现时应构建稀疏的雅可比矩阵或直接使用优化库的稀疏求解器。近似雅可比在某些对精度要求不极致、但速度要求高的场景如视觉里程计可以使用常数雅可比或更简单的近似。例如当旋转增量很小时J_r(φ) ≈ Iexp(φ^) ≈ I φ^。这可以大幅减少计算量但可能影响收敛速度或精度需要实验权衡。7. 总结与延伸思考李代数求导不是一项孤立的技术它是连接三维几何与非线性优化的桥梁。掌握它你就能更自信地处理SLAM、三维重建、机器人运动学中任何涉及旋转和变换的优化问题。回顾一下核心脉络因为旋转矩阵李群本身没有普通的加法我们无法直接定义导数。于是我们转向它的李代数——一个向量空间。通过指数映射我们将李群上的扰动对应到李代数上的增量从而在向量空间上定义并计算导数。这具体表现为两种模型直观的扰动模型和通用的直接求导解析求导模型。最终我们得到了像∂(R*p)/∂φ -(R*p)^这样既简洁又深刻的公式。在实际操作中我最大的体会是理解的一致性比记忆公式更重要。你必须清楚你使用的优化库Ceres/g2o默认的参数更新方式是左乘还是右乘你的雅可比计算是相对于哪种扰动。一旦混淆优化要么不收敛要么收敛到错误的值。建立一个简单的数值验证流程如第5.3节是保证代码正确的“安全带”。最后李代数求导的思想可以推广到更复杂的李群如SIM(3)相似变换群、SL(3)单应变换群等。其核心模式是一致的找到李群对应的李代数切空间在切空间上进行微积分和优化。当你下次看到“李代数求导”这个词时希望你的第一反应不再是复杂的公式而是一个清晰的几何图景在弯曲的流形表面沿着切平面的方向寻找使目标函数下降的最速路径。这正是非线性优化在几何世界中的优雅体现。