四元数 (Quaternion)在位姿（SE(3)）表示下的各类导数（雅可比）知识（2）

一、符号与约定

• 四元数记作 q=w, v=w,x,y,zq = w,\\, \\mathbf{v} = w, x, y, zq=w,v=w,x,y,z，其中 v=(x,y,z)T\mathbf{v}=(x,y,z)^Tv=(x,y,z)T。单位四元数表示旋转。

• 旋转矩阵由四元数表示：

  R(q)=(w2−v⊤v)I+2 vv⊤+2wv×, R(q) = (w^2 - \mathbf v^\top \mathbf v) I + 2\,\mathbf v \mathbf v^\top + 2w\\mathbf v_\times, R(q)=(w2−v⊤v)I+2vv⊤+2wv×,

  其中 v×\\mathbf v_\timesv× 为反对称矩阵（叉乘矩阵）。

• 将三维向量 ppp 作为"纯四元数"写作 pq=0,pp_q=0, ppq=0,p，旋转的四元数运算为 p′=q⊗pq⊗q∗p' = q \otimes p_q \otimes q^*p′=q⊗pq⊗q∗，等价于 p′=R(q) pp' = R(q)\, pp′=R(q)p。

两种常用扰动（local update）约定：

1. 左扰动（left-multiplicative） ： qnew=δq⊗qq_{\text{new}} = \delta q \otimes qqnew=δq⊗q，通常 δq=exp⁡ ⁣(12δθ)\delta q=\exp\!\big(\tfrac{1}{2}\delta\theta\big)δq=exp(21δθ)。常用于 pose 左乘扰动（例如：在优化时把扰动乘到当前估计之前）。
2. 右扰动（right-multiplicative） ： qnew=q⊗δqq_{\text{new}} = q \otimes \delta qqnew=q⊗δq。两者结果不同，导数形式也不同。使用时务必统一约定（代码/论文里也要写明）。

---

二、基本推导工具（常用恒等式）

• 对任意向量 a,ba,ba,b： a×b=a×ba\times b = a \times ba×b=a×b；并且 a×T=−a×a\times^T = -a_\timesa×T=−a×。

• 若 R=R(q)R = R(q)R=R(q)，则小角扰动 δθ∈R3\delta\theta \in \mathbb R^3δθ∈R3 的指数映射近似：

  exp⁡(δθ×)≈I+δθ×(当 ∥δθ∥→0). \exp(\\delta\\theta\times) \approx I + \\delta\\theta\times \quad (\text{当 }\|\delta\theta\|\to 0). exp(δθ×)≈I+δθ×(当 ∥δθ∥→0).

---

三、对"旋转后的点" p′=R(q) pp' = R(q)\,pp′=R(q)p 的常用雅可比

1. 相对于四元数分量 q=(w,v)q=(w,\mathbf v)q=(w,v) 的解析雅可比（3×4）

先写一个便于实现的形式（把导数按四元数分量拼成 3×4 矩阵）：

令 p′ ⁣= ⁣R(q)pp'\!=\!R(q)pp′=R(q)p。定义

• 标量分量导数（对 www）：

  ∂p′∂w=2w p+2 v×p \frac{\partial p'}{\partial w} = 2w\,p + 2\,\mathbf v\times p ∂w∂p′=2wp+2v×p

  （这是一个 3×1 向量）

• 向量分量导数（对 v\mathbf vv）为 3×3 矩阵：

  ∂p′∂v=  2((v⊤p) I+vp⊤−pv⊤)  −  2w p× \frac{\partial p'}{\partial \mathbf v} =\; 2\Big( (\mathbf v^\top p)\,I + \mathbf v p^\top - p \mathbf v^\top \Big) \;-\; 2w\,p_\times ∂v∂p′=2((v⊤p)I+vp⊤−pv⊤)−2wp×

于是整体的 3×4 Jacobian（按列为对 w,x,y,zw,x,y,zw,x,y,z）为

Jq  =    ∂p′∂w  ,  ∂p′∂v   J_q \;=\; \big \\; \\tfrac{\\partial p'}{\\partial w} \\;,\\; \\tfrac{\\partial p'}{\\partial \\mathbf v} \\; \\big Jq=∂w∂p′,∂v∂p′

说明：上述公式直接来自 R(q)p=(w2−v⊤v)p+2v(v⊤p)+2w(v×p)R(q)p = (w^2 - v^\top v) p + 2 v (v^\top p) + 2 w (v\times p)R(q)p=(w2−v⊤v)p+2v(v⊤p)+2w(v×p) 对 w,vw,vw,v 求导得到的结果（把 v=(x,y,z)v=(x,y,z)v=(x,y,z) 看成变量）。

---

2. 相对于"最小参数化"旋转量 δθ∈R3\delta\theta\in\mathbb R^3δθ∈R3（左扰动）的雅可比（3×3）

左扰动 ： qnew=δq⊗qq_{\text{new}} = \delta q \otimes qqnew=δq⊗q，当 δq≈1,12δθ\delta q\approx1,\\tfrac{1}{2}\\delta\\thetaδq≈1,21δθ（小角近似）时，有经典且非常实用的恒等式：

  ∂(R(q)p)∂δθ∣δθ=0=−  R(q) p ×   \boxed{\;\frac{\partial (R(q)p)}{\partial \delta\theta}\Big|{\delta\theta=0} = -\,\\,R(q)\\,p\\,\times\;} ∂δθ∂(R(q)p) δθ=0=−R(q)p×

即：对于左扰动，微小扰动 δθ\delta\thetaδθ 对旋转后点的线性影响是 −Rp× δθ-R p_\times \, \delta\theta−Rp×δθ。

---

3. 对于 右扰动 qnew=q⊗δqq_{\text{new}} = q \otimes \delta qqnew=q⊗δq 的雅可比

同样小角近似下：

  ∂(R(q)p)∂δθ∣δθ=0=− R(q) p×   \boxed{\;\frac{\partial (R(q)p)}{\partial \delta\theta}\Big|{\delta\theta=0} = -\,R(q)\,p\times\;} ∂δθ∂(R(q)p) δθ=0=−R(q)p×

（注意与左扰动的区别 ------ 左扰动得出是 −Rp×-R p\times−Rp×，右扰动是 −Rp×-Rp\times−Rp×。二者只在左乘与右乘的位置上不同，使用时必须一致。）

---

四、把"4 维四元数雅可比"换成"3 维局部变量雅可比"（在优化中经常需要）

很多优化框架（金字塔式）会用 4 维四元数作为存储，但 用 3 维扰动更新（LocalParameterization）。我们需要两段链式雅可比：

1. ∂p′∂q\dfrac{\partial p'}{\partial q}∂q∂p′ （3×4，见上面的 JqJ_qJq）
2. ∂q∂δθ\dfrac{\partial q}{\partial \delta\theta}∂δθ∂q （4×3，把最小扰动映射到四元数增量）

下面给出 ∂qnew∂δθ\dfrac{\partial q_{\text{new}}}{\partial \delta\theta}∂δθ∂qnew 在左扰动下的线性表达（评估点 δθ=0\delta\theta=0δθ=0）：

令 q=w;vq=w;\\mathbf vq=w;v，定义 4×3 矩阵

KaTeX parse error: Expected 'EOF', got '}' at position 128: ...}-v^T,\ \ 下三行为 }̲wI+v_\times\t...

则（小角近似）

  ∂qnew∂δθ  =  12 S(q)   \boxed{\; \frac{\partial q_{\text{new}}}{\partial \delta\theta} \;=\; \tfrac{1}{2}\, S(q)\;} ∂δθ∂qnew=21S(q)

（这个矩阵来自： 0,δθ⊗q=−v⊤δθ;  wδθ+δθ×v0,\\delta\\theta\otimes q = -v\^\\top \\delta\\theta;\\; w\\delta\\theta + \\delta\\theta\\times v0,δθ⊗q=−v⊤δθ;wδθ+δθ×v）

于是组合链式：

∂(R(q)p)∂δθ=∂(R(q)p)∂q  ∂q∂δθ=Jq⋅12S(q) \frac{\partial (R(q)p)}{\partial \delta\theta} = \frac{\partial (R(q)p)}{\partial q}\;\frac{\partial q}{\partial \delta\theta} = J_q \cdot \frac12 S(q) ∂δθ∂(R(q)p)=∂q∂(R(q)p)∂δθ∂q=Jq⋅21S(q)

代数化简后可验证上面的恒等式（最终等于 −Rp×-R p_\times−Rp×），这是一个很重要的一致性检验。

---

五、其他常见导数

1. 四元数乘法的线性矩阵（方便求导）

定义左乘矩阵 L(q)L(q)L(q) 和右乘矩阵 R(q)R(q)R(q)，使得对任意四元数 ppp:

q⊗p=L(q) p,p⊗q=R(q) p, q\otimes p = L(q)\, p,\qquad p\otimes q = R(q)\, p, q⊗p=L(q)p,p⊗q=R(q)p,

其中（按 q=w,x,y,zq=w,x,y,zq=w,x,y,z）：

L(q)=w−x−y−zxw−zyyzw−xz−yxw,R(q)=w−x−y−zxwz−yy−zwxzy−xw. L(q)= \begin{bmatrix} w & -x & -y & -z\\ x & w & -z & y\\ y & z & w & -x\\ z & -y & x & w \end{bmatrix},\quad R(q)= \begin{bmatrix} w & -x & -y & -z\\ x & w & z & -y\\ y & -z & w & x\\ z & y & -x & w \end{bmatrix}. L(q)= wxyz−xwz−y−y−zwx−zy−xw ,R(q)= wxyz−xw−zy−yzw−x−z−yxw .

因此 ：若 f(q)=q⊗pf(q)=q\otimes pf(q)=q⊗p，则 ∂f∂q=R(p)\dfrac{\partial f}{\partial q} = R(p)∂q∂f=R(p)（因为 q⊗p=R(p)qq\otimes p = R(p) qq⊗p=R(p)q）；类似地，对第二个乘子求导使用 L(⋅)L(\cdot)L(⋅)。

2. 共轭 / 逆的导数

• 四元数共轭（单位四元数的逆） q−1=q∗=w,−vq^{-1} = q^* = w, -\\mathbf vq−1=q∗=w,−v。这是线性的：导数矩阵是 diag⁡(1,−1,−1,−1)\operatorname{diag}(1,-1,-1,-1)diag(1,−1,−1,−1)。

---

六、位姿（SE(3)）作用点 ------ 全部雅可比

位姿用 T=(q,t)T=(q,t)T=(q,t)，点变换为 y=R(q)p+ty = R(q)p + ty=R(q)p+t。

常用雅可比（以残差 r=y−ymeasr = y - y_{\text{meas}}r=y−ymeas 为例）：

• ∂y∂t=I3\dfrac{\partial y}{\partial t} = I_{3}∂t∂y=I3；
• ∂y∂δθ\dfrac{\partial y}{\partial \delta\theta}∂δθ∂y（左扰动） =− R(q)p ×= -\\,R(q)p\\,_\times=−R(q)p×；
• 若需要 ∂y∂q\dfrac{\partial y}{\partial q}∂q∂y（3×4），用上面的 JqJ_qJq。

这些就是构造后端雅可比时的基本构件（pose-to-point residual、reprojection 等都可基于这些块做链式乘积）。

---

七、数值实现（Eigen / C++ 代码片段）

下面给出实用函数：计算 JqJ_qJq（3×4）与 S(q)S(q)S(q)（4×3），并用它们得到以局部扰动 δθ\delta\thetaδθ 的雅可比（应与 −Rp×-R p_\times−Rp× 一致）。

// 假设使用 Eigen，q: Eigen::Quaterniond, p: Eigen::Vector3d
Eigen::Matrix<double,3,4> jacobian_p_wrt_q(const Eigen::Quaterniond &q, const Eigen::Vector3d &p) {
  double w = q.w();
  Eigen::Vector3d v(q.x(), q.y(), q.z());
  Eigen::Vector3d vp = v.cross(p); // v x p
  Eigen::Matrix<double,3,4> J;
  // column 0 : d/dw
  J.col(0) = 2.0 * ( w * p + vp );
  // columns 1..3 : d/dv
  Eigen::Matrix3d term1 = 2.0 * ( (v.dot(p)) * Eigen::Matrix3d::Identity()
                                  + v * p.transpose()
                                  - p * v.transpose() );
  Eigen::Matrix3d term2 = -2.0 * w * (Eigen::Matrix3d() << 0,-p.z(), p.y(),
                                                   p.z(), 0, -p.x(),
                                                  -p.y(), p.x(), 0).finished();
  J.block<3,3>(0,1) = term1 + term2;
  return J;
}

Eigen::Matrix<double,4,3> S_of_q(const Eigen::Quaterniond &q) {
  Eigen::Matrix<double,4,3> S;
  Eigen::Vector3d v(q.x(), q.y(), q.z());
  S.row(0) = -v.transpose();
  Eigen::Matrix3d v_x;
  v_x <<   0, -v.z(),  v.y(),
         v.z(),    0, -v.x(),
        -v.y(), v.x(),    0;
  S.block<3,3>(1,0) = q.w() * Eigen::Matrix3d::Identity() + v_x;
  return S;
}

// 验证 J_delta = 0.5 * J_q * S(q) 与 -[R p]_x 是否一致 (3x3)
Eigen::Matrix3d jacobian_p_wrt_delta_theta(const Eigen::Quaterniond &q, const Eigen::Vector3d &p) {
  Eigen::Matrix<double,3,4> Jq = jacobian_p_wrt_q(q, p);
  Eigen::Matrix<double,4,3> S = S_of_q(q);
  Eigen::Matrix3d Jdelta = 0.5 * Jq * S;
  return Jdelta; // 应等于 - skew(R(q)*p)
}

（注：上面 term2 通过构造 -2 w [p]_x；实际实现中建议用 Eigen::Matrix3d::Zero() 然后赋值为 -2.0 * w * skew(p)，为可读性考虑）

---

八、在 Ceres 中的建议

• 在 Ceres 里 不要直接把四元数当成自由的 4 维变量 --- 使用 ceres::EigenQuaternionParameterization 或自定义 LocalParameterization 把更新映射到 3 维扰动上（LocalSize=3）。

• 在 cost function 的 Evaluate 中，若 Ceres 给出 jacobians（相对于局部扰动 δθ\delta\thetaδθ），你应当返回对局部扰动的雅可比（即 3×3 的块，使用上面的 −Rp×-Rp_\times−Rp× 等）。

• 若你算出的雅可比是对四元数 qqq 的（3×4），需要用链式法则与 12S(q)\tfrac12 S(q)21S(q) 相乘转换为对局部扰动（3×3）：

  Jwrt δθ=Jq⋅12S(q). J_{\text{wrt}\,\delta\theta} = J_q \cdot \tfrac12 S(q). Jwrtδθ=Jq⋅21S(q).

• 对数/指数映射与左/右 Jacobian（Jl(ω)J_l(\omega)Jl(ω)）在大角度扰动下使用更精确的表达式（不是小角近似）以提高数值稳定性。

---

九、补充：SO(3) 的左雅可比（Left Jacobian）用于更高精度的映射

若使用指数映射 exp⁡(ω)\exp(\omega)exp(ω) 把 R3\mathbb R^3R3 映到 SO(3)，则常用的左雅可比（用于把空间微分从李代数映射到群）是：

令 θ=∥ω∥\theta=\|\omega\|θ=∥ω∥，则

Jl(ω)=I−1−cos⁡θθ2ω×+θ−sin⁡θθ3ω×2. J_l(\omega) = I - \frac{1-\cos\theta}{\theta^2}\\omega\times + \frac{\theta-\sin\theta}{\theta^3}\\omega\times^2. Jl(ω)=I−θ21−cosθω×+θ3θ−sinθω×2.

其逆 Jl(ω)−1J_l(\omega)^{-1}Jl(ω)−1 也有解析形式（常在精确线性化时使用，例如 IMU 预积分或精确的 pose graph 雅可比推导）。这在大角度时比小角近似更靠谱。

---

十、总结（实用提醒）

• 优先使用局部 3D 扰动并用 LocalParameterization（例如 Ceres），避免直接优化 4D 四元数。

• 区分左扰动 / 右扰动 ------ 导数表达式会不同（但都可以互相转换），只要在整个系统里保持一致即可。

• 实战中最常用的块：

  • ∂(Rp)/∂δθ=−Rp×\partial (R p)/\partial \delta\theta = -R p_\times∂(Rp)/∂δθ=−Rp×（左扰动），
  • ∂(Rp)/∂t=I\partial (R p)/\partial t = I∂(Rp)/∂t=I（平移），
  • ∂(Rp)/∂q\partial (R p)/\partial q∂(Rp)/∂q （如需对 4 分量求导，见上面的 JqJ_qJq）。

• 当你实现复杂残差（pose-pose、pose-point、reprojection）时，用这些基本块按链式法则拼接即可。

---