四元数 (Quaternion)中的扰动知识（6）

1. 左扰动定义

左乘扰动（left-multiplicative）的形式为：

$$q_{\text{new}}=\delta q\otimes q,$$

其中 $q=[w,\mathbf{v}]$（写作 $w$ 为标量部分，$\mathbf{v}\in {\mathbb{R}}^3$ 为向量部分），并把小扰动 $\delta \theta \in {\mathbb{R}}^3$ 用四元数近似表示为

$$\delta q\approx \left[1,\frac{1}{2}\delta \theta \right]$$

（这是指数映射在小角近似下的常用一阶展开： $\exp (\frac{1}{2}[\delta \theta ])\approx [1,\frac{1}{2}\delta \theta ]$）。

---

2. 四元数乘法公式约定

写成分块形式，若

$$p=[p_w,\mathbf{p}],r=[r_w,\mathbf{r}],$$

则

$$p\otimes r=(p_w{r}_w-{\mathbf{p}}^{\top }\mathbf{r},p_w\mathbf{r}+r_w\mathbf{p}+\mathbf{p}\times \mathbf{r}).$$

将上式用于 $\delta q\otimes q$，设 $\delta q=[{\delta }_w,\delta \mathbf{v}]$（后面代入 ${\delta }_w\approx 1,\delta \mathbf{v}\approx \frac{1}{2}\delta \theta$）：

• 标量部分：

  $$(\delta q\otimes q{)}_w={\delta }_{w}w-\delta {\mathbf{v}}^{\top }\mathbf{v}.$$

• 向量部分：

  $$(\delta q\otimes q{)}_{\mathbf{v}}={\delta }_w\mathbf{v}+w\delta \mathbf{v}+\delta \mathbf{v}\times \mathbf{v}.$$

---

3. 左扰动线性化（小扰动展开）

把 ${\delta }_w=1+O(\Vert \delta \theta {\Vert }^2)$ 和 $\delta \mathbf{v}=\frac{1}{2}\delta \theta +O(\Vert \delta \theta {\Vert }^3)$ 代入，并忽略二阶以上项：

• 标量部分：

  $$(\delta q\otimes q{)}_w\approx (1)\cdot w-{\left(\frac{1}{2}\delta \theta \right)}^{\top }\mathbf{v}=w-\frac{1}{2}{\mathbf{v}}^{\top }\delta \theta .$$

  所以标量部分的线性增量为 $-\frac{1}{2}{\mathbf{v}}^{\top }\delta \theta$。

• 向量部分：

  $$(\delta q\otimes q{)}_{\mathbf{v}}\approx 1\cdot \mathbf{v}+w\cdot \left(\frac{1}{2}\delta \theta \right)+\left(\frac{1}{2}\delta \theta \right)\times \mathbf{v}=\mathbf{v}+\frac{1}{2}(w\delta \theta +\delta \theta \times \mathbf{v}).$$

  向量部分的线性增量为 $\frac{1}{2}(w\delta \theta +\delta \theta \times \mathbf{v})$。

把标量与向量线性增量合并为矩阵形式：

$$q_{\text{new}}\approx \left[\begin{array}{c}w\\ \mathbf{v}\end{array}\right]\frac{1}{2}\left[\begin{array}{c}-{\mathbf{v}}^{\top }\\ w{I}_3+[\mathbf{v}{]}_{\times }\end{array}\right]\delta \theta .$$

这就是 $S(q)$ 的来源：第一行是 $-{\mathbf{v}}^{\top }$，下三行是 $w{I}_3+[\mathbf{v}{]}_{\times }$。

因此

$$\boxed{\frac{\partial q_{\text{new}}}{\partial \delta \theta }{|}_{\delta \theta =0}=\frac{1}{2}S(q)}$$

且

$$S(q)=\left[\begin{array}{ccc}{v}_x& -v_y& -v_z\\ w& -v_z& v_y\\ v_z& w& -v_x\\ v_y& v_x& w\end{array}\right]$$

（上式按列为对应 $\delta {\theta }_x,\delta {\theta }_y,\delta {\theta }_z$ 的系数；注意行顺序：第一行为标量分量，接下 3 行为向量分量）。

---

4. 左扰动显式矩阵（

令 $v=(v_x,v_y,v_z{)}^{\top }$，记 $[v{]}_{\times }$ 为

$$[v{]}_{\times }=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right].$$

则

$$S(q)=\left[\begin{array}{ccc}-v_x& -v_y& -v_z\\ w& -v_z& v_y\\ v_z& w& -v_x\\ -v_y& v_x& w\end{array}\right]\left[\begin{array}{c}-v^{\top }\\ w{I}_3+[v{]}_{\times }\end{array}\right].$$

---

5. 代数一致性验证

下面从代数角度用四元数乘法的左乘矩阵 $L(\delta q)$ 展开：

$$\delta q\otimes q=L(\delta q)q$$

在 $\delta q\approx [1,\frac{1}{2}\delta \theta ]$ 时，把 $L(\delta q)$ 线性化，对 $q$ 作用后得到同样的 $\frac{1}{2}S(q)\delta \theta$ 结果。两种方法等价。

---

6. 左扰动代码实现

下面的函数返回矩阵 $S(q)$（4×3），以及封装好的计算 $\frac{1}{2}S(q)\delta \theta$ 用法：

#include <Eigen/Core>
#include <Eigen/Geometry>// q assumed to be unit quaternion (Eigen::Quaterniond)
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());// first row: -v^TS.row(0) = -v.transpose();// lower 3x3: w * I + [v]_xEigen::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;
}// 使用示例： compute q_new ≈ q + 0.5 * S(q) * delta_theta
Eigen::Quaterniond left_update_quat(const Eigen::Quaterniond &q, const Eigen::Vector3d &delta_theta) {Eigen::Matrix<double,4,3> S = S_of_q(q);Eigen::Matrix<double,4,1> dq_lin = 0.5 * S * delta_theta; // linearized incrementEigen::Quaterniond q_new;q_new.w() = q.w() + dq_lin(0);q_new.x() = q.x() + dq_lin(1);q_new.y() = q.y() + dq_lin(2);q_new.z() = q.z() + dq_lin(3);q_new.normalize(); // optional: enforce unit length (error is O(|delta|^2))return q_new;
}

---

7. 右扰动定义

设：

$$q_{\text{new}}=q\otimes \delta q,\delta q\approx [1,\frac{1}{2}\delta \theta ],\delta \theta \in {\mathbb{R}}^3$$

四元数乘法公式（分块）：

$$q\otimes \delta q=\left[\begin{array}{c}w\\ \mathbf{v}\end{array}\right]\otimes \left[\begin{array}{c}{\delta }_w\\ \delta \mathbf{v}\end{array}\right]\left[\begin{array}{c}w{\delta }_w-{\mathbf{v}}^{\top }\delta \mathbf{v}\\ w\delta \mathbf{v}+{\delta }_w\mathbf{v}+\mathbf{v}\times \delta \mathbf{v}\end{array}\right].$$

---

8. 右扰动线性化（小角近似）

取 ${\delta }_w\approx 1,\delta \mathbf{v}\approx \frac{1}{2}\delta \theta$，忽略二阶项：

• 标量部分：

$$(q\otimes \delta q{)}_w\approx w-{\mathbf{v}}^{\top }\frac{1}{2}\delta \theta =w-\frac{1}{2}{\mathbf{v}}^{\top }\delta \theta$$

• 向量部分：

$$(q\otimes \delta q{)}_{\mathbf{v}}\approx \mathbf{v}+w\frac{1}{2}\delta \theta +\frac{1}{2}\delta \theta \times \mathbf{v}=\mathbf{v}+\frac{1}{2}(w{I}_3-[\mathbf{v}{]}_{\times })\delta \theta$$

注意与左扰动的区别：

• 左扰动向量增量是 $w{I}_3+[\mathbf{v}{]}_{\times }$
• 右扰动向量增量是 $w{I}_3-[\mathbf{v}{]}_{\times }$

---

9. 右扰动矩阵形式

$$q_{\text{new}}\approx q+\frac{1}{2}\underset{S_r(q)}{\underbrace{\left[\begin{array}{c}{\mathbf{v}}^{\top }\\ w{I}_3-[\mathbf{v}{]}_{\times }\end{array}\right]}}\delta \theta$$

其中：

$$S_r(q)=\left[\begin{array}{ccc}-v_x& -v_y& -v_z\\ w& v_z& -v_y\\ v_z& w& v_x\\ v_y& -v_x& w\end{array}\right].$$

这是右扰动对应的 4×3 雅可比矩阵。

---

10. 右扰动代码示例

Eigen::Matrix<double,4,3> S_right_of_q(const Eigen::Quaterniond &q) {Eigen::Matrix<double,4,3> S;Eigen::Vector3d v(q.x(), q.y(), q.z());// first row: -v^TS.row(0) = -v.transpose();// lower 3x3: w*I - [v]_xEigen::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;
}

---

11.对比

	左扰动 $\delta q\otimes q$	右扰动 $q\otimes \delta q$
矩阵形式	$S(q)=\left[\begin{array}{c}-v^{\top }\\ w{I}_3+[v{]}_{\times }\end{array}\right]$	$S_r(q)=\left[\begin{array}{c}-v^{\top }\\ w{I}_3-[v{]}_{\times }\end{array}\right]$
更新公式	$q_{\text{new}}\approx q+\frac{1}{2}S(q)\delta \theta$	$q_{\text{new}}\approx q+\frac{1}{2}{S}_r(q)\delta \theta$
对应于旋转方向	左扰动	右扰动

注意：Ceres 默认使用 右扰动 的 LocalParameterization，如果你在 Ceres 中定义四元数参数化，实际上它内部使用的就是 $S_r(q)$。

---