1. 从“旋转”到“扰动”为什么我们需要李代数求导在机器人、计算机视觉或者SLAM即时定位与地图构建领域摸爬滚打过的朋友对“李群李代数”这个词组一定不陌生。它常常和“姿态估计”、“优化”、“后端”这些词捆绑出现听起来既高深又晦涩。很多教程会直接甩给你一堆公式SO(3)、so(3)、SE(3)、se(3)然后告诉你李群是旋转矩阵的集合李代数是反对称矩阵的集合它们通过指数映射和对数映射联系起来。公式背下来了代码也许也能照着敲但一到要自己推导一个雅可比矩阵或者理解优化框架里那个“扰动模型”时脑袋就一片空白了。问题的核心往往就出在“求导”这一步。我们最熟悉的求导是对一个标量函数f(x)关于变量x的求导。但当我们的变量是一个“旋转”或者“刚体变换”时事情就变得棘手了。旋转矩阵R本身有9个元素但它们并不是独立的需要满足R^T R I且det(R)1你无法直接对这9个数进行“自由”的加减和求导因为随便加一个小量得到的矩阵很可能就不再是旋转矩阵了。这就好比在一个球面上移动你不能直接在三维坐标上加减那样会跑到球面外面去。李代数求导就是为了解决在像旋转矩阵李群这样的“约束空间”上进行微积分运算而诞生的工具。它的核心思想是我们不直接对群元素如旋转矩阵R求导而是对与之对应的李代数元素如旋转向量φ求导。李代数空间是一个向量空间在这里我们可以自由地进行加法、数乘从而可以定义我们熟悉的导数。而连接群和代数的正是那个关键的“扰动”模型。简单来说当我们需要计算一个函数f(R)关于旋转R的导数时我们不是计算f(RΔR)因为ΔR没有定义。而是计算f(R ⊕ δφ)关于小扰动δφ的导数这里的⊕表示李群上的左乘或右乘扰动δφ是李代数空间中的一个微小向量。这个导数就是所谓的“李代数求导”它最终会给出一个雅可比矩阵这个矩阵是优化算法如高斯-牛顿法、列文伯格-马夸尔特法能够直接使用的。所以如果你曾被VINS-Mono、ORB-SLAM等开源项目里那些关于旋转的雅可比推导搞得头晕眼花或者想自己手写一个基于李代数的BA光束法平差而不知从何下手那么彻底搞懂李代数求导就是你从“会用轮子”到“能造轮子”的关键一步。接下来我将抛开复杂的数学外壳用尽可能直观的方式带你走过从概念理解到公式推导再到代码实现的完整路径。2. 重温基石李群与李代数的“前世今生”在直接切入求导之前我们必须对操作对象有一个清晰的认识。李群和李代数不是两个孤立的概念它们是一体两面的关系理解这种关系是理解后续所有推导的基础。2.1 李群我们操作的“舞台”李群首先是一个群满足封闭性、结合律、单位元和逆元这四个基本性质。其次它还是一个光滑流形这意味着在它上面可以进行微积分。对于我们工程师来说最关心的就是那几个具体的李群SO(3) - 特殊正交群三维旋转群所有合法的三维旋转矩阵R的集合。R ∈ R^{3×3}, 满足R^T R I且det(R) 1。它描述了物体在三维空间中的所有可能朝向。SE(3) - 特殊欧氏群三维刚体运动群所有合法的三维刚体变换T的集合。T [R, t; 0, 1]其中R ∈ SO(3),t ∈ R^3。它同时描述了物体的朝向和位置。关键障碍这些群元素R,T本身对加法不封闭。R1 R2通常不是一个旋转矩阵。因此f(R)对R的导数∂f/∂R在通常意义下是病态的因为我们无法定义R的一个“微小变化”ΔR使得R ΔR仍然在 SO(3) 中。2.2 李代数群元素“家门口”的切线空间可以把李群想象成一个弯曲的空间比如球面而李代数就是这个弯曲空间在单位元Identity处的切空间。这个切空间是一个向量空间对加法和数乘封闭完美适合做微积分。so(3) - SO(3)对应的李代数其元素是三维反对称矩阵φ^^表示将向量映射为反对称矩阵。实际上我们更常用与之同构的三维向量φ [φ1, φ2, φ3]^T。φ的方向代表旋转轴模长代表旋转角度。这就是旋转向量或角轴表示。se(3) - SE(3)对应的李代数其元素是一个六维向量ξ [ρ, φ]^T其中φ同上代表旋转ρ与平移相关但并非直接的平移向量。通常也写作ξ^为一个4x4的矩阵。指数映射与对数映射这是连接群和代数的桥梁。指数映射 exp: 将李代数φ(so(3)) 映射到李群R(SO(3))。物理意义是绕着轴φ/|φ|旋转|φ|弧度。公式为罗德里格斯公式R exp(φ^) I sinθ/θ * φ^ (1-cosθ)/θ^2 * (φ^)^2其中θ|φ|。对数映射 log: 将李群R映射回李代数φ。是指数映射的逆运算。最重要的一点指数映射在单位元附近是局部微分同胚的。这意味着在φ0对应RI附近李代数空间中的一个小向量δφ可以通过指数映射唯一地对应到李群元素R附近的一个小扰动。这正是我们能够进行李代数求导的理论根基。个人心得刚开始学的时候我总是试图去记忆 so(3) 和 se(3) 的具体矩阵形式后来发现这反而增加了负担。对于工程应用最重要的是抓住两个核心1) so(3) 对应三维向量φ2) se(3) 对应六维向量ξ。具体的指数/对数映射计算都有成熟且高效的库函数如 Sophus, Eigen来实现我们更需要理解的是它们所建立的“群-代数对应关系”以及如何利用这种关系来定义导数。3. 两种核心扰动模型左乘与右乘既然不能直接对R求导我们就采用“曲线救国”的策略在李群上施加一个微小扰动这个扰动由李代数描述。根据扰动相乘的方向主要分为两种模型这两种模型在公式上略有不同但核心思想一致。假设我们有一个函数f(R)其中R ∈ SO(3)。我们想求f关于R的导数。3.1 左乘扰动模型我们考虑在R的左边乘以一个由李代数δφ描述的微小扰动f(R)相对于R的导数定义为f相对于这个左扰动δφ的导数在δφ0处取值。数学上我们构造f(exp(δφ^) R)然后计算∂f(exp(δφ^) R) / ∂δφ在δφ 0处求值。为什么叫“左乘”因为扰动exp(δφ^)乘在了原始旋转R的左边。这个模型非常自然可以理解为在世界坐标系下对姿态R施加了一个微小旋转δφ。3.2 右乘扰动模型我们考虑在R的右边乘以一个由李代数δφ描述的微小扰动f(R exp(δφ^))然后计算∂f(R exp(δφ^)) / ∂δφ在δφ 0处求值。为什么叫“右乘”因为扰动exp(δφ^)乘在了原始旋转R的右边。这个模型可以理解为在物体自身坐标系下对姿态R施加了一个微小旋转δφ。3.3 左扰动与右扰动的关系与选择两种模型求出的雅可比矩阵是不同的但它们之间存在一个简单的关系。通常在视觉SLAM的BA问题中我们更常用左乘扰动模型因为它导出的雅可比形式更简洁并且与优化变量通常是世界坐标系下的相机姿态的更新方向一致。一个非常重要的结论是∂(R p)/∂δφ左扰动 -(R p)^一个3x3的反对称矩阵∂(R p)/∂δφ右扰动 -(R p)^ R或等价的-R (p)^取决于定义其中p是一个三维点坐标(·)^表示将向量转换为反对称矩阵。踩坑实录早期我自己推导雅可比时常常混淆左扰动和右扰动导致优化时梯度方向错误算法不收敛。一个很实用的检查方法是假设R是相机姿态p是世界点。用左扰动模型exp(δφ^) R意味着扰动施加在世界系那么R p对δφ的雅可比应该只和旋转后的点R p有关即-(R p)^。用右扰动模型R exp(δφ^)意味着扰动施加在相机系雅可比则会多出一个R。记住一种模型推荐左扰动并贯穿始终可以避免很多混乱。4. SO(3)上的李代数求导实战以重投影误差为例理论说了这么多我们来点实际的。在视觉SLAM中一个最经典的例子就是**重投影误差Reprojection Error**关于相机旋转R的雅可比计算。这是Bundle Adjustment的核心组成部分。问题设定相机内参矩阵K已知。一个世界点P_w [X, Y, Z]^T。相机姿态由旋转R和平移t描述即T [R, t]。该点在相机坐标系下的坐标为P_c R P_w t。该点的归一化相机坐标为p_n [u_n, v_n, 1]^T P_c / Z_c其中Z_c是P_c的第三个分量深度。该点的像素观测坐标为z [u_s, v_s]^T K p_n只取前两维。我们有一个实际的像素测量值z_meas。重投影误差定义为e z - z_meas。我们的目标是计算误差e关于旋转R的导数∂e/∂R。由于R是群元素我们采用左乘扰动模型即计算∂e/∂δφ其中扰动作用于RR exp(δφ^) R。4.1 链式法则分解这是一个典型的复合函数求导问题我们使用链式法则一步步拆解∂e/∂δφ (∂e/∂z) * (∂z/∂p_n) * (∂p_n/∂P_c) * (∂P_c/∂δφ)∂e/∂z很简单e z - z_meas所以∂e/∂z I_{2×2}单位矩阵。∂z/∂p_nz K p_n这里p_n是三维齐次坐标[u_n, v_n, 1]^Tz是二维像素坐标[u_s, v_s]^T。假设内参矩阵K [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]那么z [fx * u_n cx, fy * v_n cy]^T所以∂z/∂p_n是一个 2x3 的矩阵∂z/∂p_n [[fx, 0, 0], [0, fy, 0]]因为z只与p_n的前两个分量有关且是线性关系。∂p_n/∂P_cp_n P_c / Z_c其中P_c [X_c, Y_c, Z_c]^T。这是一个三维向量对三维向量的求导结果是 3x3 的雅可比矩阵。p_n [X_c/Z_c, Y_c/Z_c, 1]^T我们分别对X_c, Y_c, Z_c求偏导∂p_n/∂P_c [[1/Z_c, 0, -X_c/Z_c^2], [ 0, 1/Z_c, -Y_c/Z_c^2], [ 0, 0, 0 ]]注意第三行都是0因为p_n的第三个分量恒为1与P_c无关。∂P_c/∂δφ这是最关键的一步用到李代数求导的核心结论。在左乘扰动模型下P_c (exp(δφ^) R) P_w t。我们求P_c对δφ在δφ0处的导数。 根据SO(3)的求导公式可以通过泰勒展开或直接微分推导得出∂(exp(δφ^) R P_w) / ∂δφ |_{δφ0} ∂( (I δφ^ ...) R P_w) / ∂δφ |_{δφ0} ∂(δφ^ (R P_w)) / ∂δφ -(R P_w)^其中(·)^是向量到反对称矩阵的算子。 因为t与δφ无关所以∂P_c/∂δφ -(R P_w)^。 注意R P_w正是P_c减去平移t之前的部分即P_c - t。但在求导这一刻δφ0所以R就是原来的R。因此更直接地∂P_c/∂δφ -(P_c - t)^ -(R P_w)^4.2 雅可比矩阵的最终形式将以上四部分用链式法则乘起来我们得到重投影误差e关于旋转左扰动δφ的雅可比矩阵J_RJ_R ∂e/∂δφ I_{2×2} * (∂z/∂p_n) * (∂p_n/∂P_c) * (-(R P_w)^)由于I_{2×2}是单位阵可以忽略。代入具体矩阵J_R [[fx, 0, 0], * [[1/Z_c, 0, -X_c/Z_c^2], * ( - (R P_w)^ ) [0, fy, 0]] [0, 1/Z_c, -Y_c/Z_c^2], [0, 0, 0 ]]先计算前两个矩阵的乘积J_tempJ_temp [[fx/Z_c, 0, -fx*X_c/Z_c^2], [0, fy/Z_c, -fy*Y_c/Z_c^2]]这是一个 2x3 的矩阵。然后让它与 3x3 的反对称矩阵-(R P_w)^相乘。注意我们只需要前两行误差关于三维扰动δφ的导数所以J_temp是 2x3-(R P_w)^是 3x3结果J_R是一个 2x3 的矩阵。J_R J_temp * ( - (R P_w)^ )设P_w旋转后的部分为P_r R P_w [X_r, Y_r, Z_r]^T则(P_r)^为[0, -Z_r, Y_r] [Z_r, 0, -X_r] [-Y_r, X_r, 0]所以-(P_r)^就是上面矩阵的每个元素取负。最终通过矩阵乘法我们可以得到J_R每个元素的具体表达式。这个 2x3 的雅可比矩阵就是我们在优化算法中要使用的它描述了像素误差如何随着相机旋转的微小变化用李代数δφ表示而变化。实操技巧在实际代码实现中我们很少会像上面那样展开成标量形式然后相乘因为既不高效也不利于阅读。通常的做法是使用线性代数库按照链式法则的顺序进行矩阵乘法运算。例如在C中使用Eigen库你可以清晰地写出每一步Eigen::Vector3d Pc R * Pw t; // 相机坐标系点 double Zc Pc.z(); Eigen::Vector3d Pr R * Pw; // 用于反对称矩阵 Eigen::Matrixdouble, 2, 3 J_temp; J_temp fx/Zc, 0, -fx*Pc.x()/(Zc*Zc), 0, fy/Zc, -fy*Pc.y()/(Zc*Zc); Eigen::Matrix3d skew_Pr -Sophus::SO3d::hat(Pr); // 计算 -(R*Pw)^ Eigen::Matrixdouble, 2, 3 J_R J_temp * skew_Pr;这样的代码既高效又清晰地反映了数学推导的过程。5. SE(3)上的李代数求导统一处理旋转与平移在SLAM中我们更常同时优化相机的旋转和平移即姿态T ∈ SE(3)。对应的李代数是ξ ∈ se(3)是一个六维向量ξ [ρ, φ]^T其中φ对应旋转ρ对应平移但注意ρ并不直接等于平移向量t它们通过指数映射关联。对于重投影误差e我们现在需要求它关于完整姿态T的李代数ξ的导数即∂e/∂ξ。同样采用左乘扰动模型扰动为exp(δξ^)其中δξ [δρ, δφ]^T。5.1 扰动对相机坐标系点的影响在左扰动下变换后的点为P_c exp(δξ^) T P_w这里P_w是齐次坐标。我们需要求P_c关于δξ在δξ0处的导数。根据SE(3)的指数映射性质和李代数求导公式可以推导出∂P_c / ∂δξ [I, -(P_c)^]这是一个 3x6 的矩阵。其中I是 3x3 的单位矩阵对应扰动平移部分δρ的导数。这很直观平移扰动直接加到坐标上。-(P_c)^是 3x3 的反对称矩阵对应扰动旋转部分δφ的导数。这和我们在SO(3)中推导的完全一致。注意这里的P_c是三维坐标不是齐次坐标。这个公式是SE(3)李代数求导的核心结论之一。5.2 完整的雅可比矩阵现在我们可以类似SO(3)的情况用链式法则计算∂e/∂ξ∂e/∂ξ (∂e/∂z) * (∂z/∂p_n) * (∂p_n/∂P_c) * (∂P_c/∂δξ)前三项(∂e/∂z),(∂z/∂p_n),(∂p_n/∂P_c)与第4节中完全一样。 第四项(∂P_c/∂δξ)就是我们刚得到的 3x6 矩阵[I, -(P_c)^]。因此重投影误差关于SE(3)左扰动δξ的雅可比矩阵J_T为J_T J_temp * [I, -(P_c)^]其中J_temp是第4.2节中定义的 2x3 矩阵[[fx/Z_c, 0, -fx*X_c/Z_c^2], [0, fy/Z_c, -fy*Y_c/Z_c^2]]。进行矩阵乘法后J_T是一个 2x6 的矩阵。我们可以把它分成左右两块左边 2x3 块J_temp * I J_temp这就是关于平移扰动δρ的雅可比。实际上它就是关于相机坐标系点P_c的平移的导数。右边 2x3 块J_temp * (-(P_c)^)这就是关于旋转扰动δφ的雅可比与第4节中求得的J_R完全一致。所以J_T [J_temp, J_temp * (-(P_c)^)] [J_t, J_R]。这个结果非常优美且直观重投影误差关于SE(3)姿态的雅可比就是关于平移的雅可比和关于旋转的雅可比的并置。这为我们分别理解和计算两者提供了便利。5.3 与反函数求导法则的微妙联系在微积分中我们学过反函数的求导法则如果y f(x)可逆且x f^{-1}(y)那么dx/dy (dy/dx)^{-1}。在李群求导中虽然不直接是反函数关系但指数映射和对数映射作为一对互逆的映射它们的微分即雅可比也存在紧密联系。具体来说exp(φ^)在φ0处的雅可比即从李代数到李群的映射微分是一个单位矩阵。而log(R)在RI处的雅可比即从李群到李群的映射微分也是单位矩阵。但在非原点处它们的雅可比矩阵互为逆矩阵在相应的意义下。这一点在需要进行BCHBaker-Campbell-Hausdorff公式近似或者当扰动不是加在群元素旁边而是加在指数映射内部时即f(exp(φδφ)^)模型会变得非常重要。深度思考我们常用的左乘扰动模型f(exp(δφ^) R)本质上是在对“乘法”中的增量进行求导。而另一种模型f(exp(φ δφ)^)是直接对李代数参数φ进行求导。后者求导时需要额外乘上一个J_r(φ)或J_l(φ)即exp(φ^)的右雅可比或左雅可比这个雅可比来自于exp函数的微分。在φ很小即R接近单位阵时这个雅可比接近单位阵两种模型近似等价。但在优化迭代中当R离初始值较远时两种模型得到的更新步长会有差异。大多数SLAM库为了简洁和效率默认使用左乘扰动模型因为它避免了计算J_r(φ)。6. 从理论到代码雅可比计算的实现与验证理解了数学推导最终还是要落地到代码。一个正确且高效的雅可比实现是优化算法快速收敛的保障。6.1 代码实现示例C with Eigen Sophus以下是一个计算重投影误差关于SE(3)姿态雅可比的函数示例。我们假设使用左乘扰动模型。#include Eigen/Dense #include sophus/so3.hpp #include sophus/se3.hpp /** * brief 计算重投影误差关于相机姿态SE3的雅可比矩阵左乘扰动模型 * param[in] P_w 世界坐标系下的3D点坐标 * param[in] T_cw 相机姿态将世界点变换到相机坐标系P_c T_cw * P_w * param[in] K 相机内参矩阵 (3x3) * param[out] J_T 重投影误差关于SE3扰动ξ的雅可比矩阵 (2x6) * param[out] P_c_proj 计算过程中的相机坐标系点和投影点可选用于调试 * return 如果点在前方Z0返回true否则返回false雅可比无意义 */ bool computeReprojectionJacobianSE3( const Eigen::Vector3d P_w, const Sophus::SE3d T_cw, const Eigen::Matrix3d K, Eigen::Matrixdouble, 2, 6 J_T, Eigen::Vector3d* P_c_proj nullptr) { // 1. 将世界点变换到相机坐标系 Eigen::Vector3d P_c T_cw * P_w; // Sophus::SE3d 重载了 * 运算符作用于三维点 // 检查深度是否为正点在相机前方 if (P_c.z() 1e-6) { return false; } double Z P_c.z(); double Z_inv 1.0 / Z; double Z2_inv Z_inv * Z_inv; // 2. 计算归一化平面坐标 (x, y) double x_n P_c.x() * Z_inv; double y_n P_c.y() * Z_inv; // 3. 计算像素坐标 (u, v) double fx K(0, 0), fy K(1, 1), cx K(0, 2), cy K(1, 2); // 这里我们只计算雅可比不实际计算误差值 // Eigen::Vector2d z_proj(fx * x_n cx, fy * y_n cy); if (P_c_proj) { *P_c_proj P_c; } // 4. 计算中间雅可比 J_temp ∂e/∂P_c (实际上∂e/∂z * ∂z/∂p_n * ∂p_n/∂P_c) // e z_proj - z_meas, 所以 ∂e/∂z_proj I因此 J_temp ∂z/∂p_n * ∂p_n/∂P_c // ∂z/∂p_n [[fx, 0, 0], [0, fy, 0]] // ∂p_n/∂P_c [[1/Z, 0, -X/Z^2], [0, 1/Z, -Y/Z^2], [0,0,0]] // 相乘得到 J_temp (2x3): // J_temp [[fx/Z, 0, -fx*X/Z^2], // [0, fy/Z, -fy*Y/Z^2]] Eigen::Matrixdouble, 2, 3 J_temp; J_temp fx * Z_inv, 0, -fx * P_c.x() * Z2_inv, 0, fy * Z_inv, -fy * P_c.y() * Z2_inv; // 5. 计算 ∂P_c/∂δξ (3x6) // 根据公式 ∂P_c/∂δξ [I, - (P_c)^] Eigen::Matrixdouble, 3, 6 J_Pc_se3; J_Pc_se3.setIdentity(); // 设置左边3x3块为单位阵 I J_Pc_se3.block3, 3(0, 3) -Sophus::SO3d::hat(P_c); // 设置右边3x3块为 -(P_c)^ // 6. 链式法则J_T J_temp * J_Pc_se3 J_T J_temp * J_Pc_se3; return true; }6.2 数值微分验证确保你的推导万无一失在复杂的系统中手动推导的雅可比很容易出错。一个黄金法则是用数值微分Numerical Differentiation来验证你解析推导Analytic Differentiation的雅可比矩阵。数值微分的原理很简单就是利用导数的定义∂f/∂x ≈ (f(x Δ) - f(x)) / Δ其中Δ是一个很小的数例如1e-6或1e-7。对于我们的问题我们要验证J_T的每一列。J_T是2x6的共有12个元素。第j列对应第j个李代数扰动分量δξ_j。验证步骤如下计算当前姿态T下的投影点z0和解析雅可比J_analytic。对δξ的第j个分量加上一个小扰动epsilon构造扰动向量delta_xi_j。用扰动后的李代数ξ delta_xi_j通过指数映射得到扰动后的姿态T_perturbed。计算扰动后的投影点z_perturbed。数值微分得到的雅可比第j列J_numeric_col_j (z_perturbed - z0) / epsilon。比较J_analytic的第j列和J_numeric_col_j。它们应该非常接近。如果每一列的数值微分结果都与解析解匹配相对误差在1e-6量级或以下那么恭喜你你的雅可比推导和代码实现很可能是正确的。避坑指南数值微分验证时epsilon的选择很关键。太小会受浮点数精度限制放大舍入误差太大则不满足导数的线性近似条件。通常尝试1e-6,1e-7,1e-8观察结果是否稳定。另外验证要在多个不同的随机姿态T和空间点P_w上进行以确保普适性。这是我调试任何优化问题雅可比时的必备步骤它能帮你快速定位是推导错误还是代码笔误。7. 超越重投影李代数求导在其他场景中的应用李代数求导的应用绝不限于重投影误差。在任何涉及旋转或刚体变换的优化问题中它都是基石。场景一点云配准ICP在迭代最近点算法中我们需要优化一个变换T使得两个点云对齐。误差函数通常是点对之间的距离e_i T * P_i - Q_i或点到平面的距离。这里T ∈ SE(3)。计算误差e_i关于T的导数就必须用到SE(3)的李代数求导。导数形式会比视觉重投影更简单因为误差直接定义在三维欧氏空间。场景二IMU预积分在融合视觉和IMU的VIO视觉惯性里程计中IMU测量值需要在两个关键帧之间进行预积分。预积分的结果相对旋转、速度、位置变化是定义在流形上的。对这些预积分量进行优化时关于关键帧姿态的雅可比计算本质上就是李代数求导并且由于BCH公式的存在形式会更加复杂。场景三机器人运动学在机械臂控制中我们需要计算末端执行器位姿关于关节角度的雅可比几何雅可比。当使用旋转矩阵表示末端姿态时姿态部分的雅可比计算就涉及到SO(3)的求导。更进阶的如果考虑动力学角速度、加速度与姿态导数的关系也通过李代数旋转向量及其时间导数来联系表述会非常简洁。场景四全局姿态优化Pose Graph Optimization在SLAM的后端位姿图优化中的误差项常定义为两个位姿之间相对变换的差e_{ij} log(T_{ij}^{-1} * T_i^{-1} * T_j)其中log是SE(3)的对数映射。这个误差本身就在李代数空间中。优化变量T_i,T_j是群元素我们需要计算误差e_{ij}关于T_i和T_j的导数这同样需要李代数求导的知识并且会涉及到伴随矩阵Adjoint的出现。经验之谈当你面对一个包含旋转的优化问题感觉无从下手时不妨问自己两个问题1) 我的优化变量是什么是否是SO(3)或SE(3)的元素2) 我的误差函数是如何定义的只要误差函数可以表示为优化变量的函数你就可以尝试用扰动模型左乘或右乘然后通过链式法则结合SO(3)/SE(3)的求导公式一步步推导出雅可比。第一次推导可能会很慢但这个过程对于深入理解问题本质至关重要。推导完成后务必用数值微分进行验证。