二维空间和三维空间刚体变换中的雅克比矩阵的推导

1 预备的知识

补充一些李群 $\mathrm{SE}(2)$ 和李代数 $\mathrm{se}(2)$ 的知识, 因为视觉 SLAM十四讲¹中只给出了 $\mathrm{SE}(3)$ 和 $\mathrm{se}(3)$ 的对应关系, 但是没有给出二维空间的对应关系。其中二维空间的李群李代数参考Lie Groups for 2D and 3D Transformations。²

1.1 李群 S E ( 2 ) \mathrm{SE}(2) SE(2)

二维刚体变换公式为 $T,T\in \mathrm{SE}(2)$, 如下所示:
R = [ cos ⁡ θ − sin ⁡ θ sin ⁡ θ cos ⁡ θ ] ∈ SO ⁡ ( 2 ) , t = [ t x t y ] ∈ R 2 T = ( R t 0 1 ) = [ cos ⁡ θ − sin ⁡ θ t x sin ⁡ θ cos ⁡ θ t y 0 0 1 ] ∈ SE ⁡ ( 2 ) \begin{aligned} & \mathbf{R}=\left[\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right] \in \operatorname{SO}(2), \mathbf{t}=\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \in \mathbb{R}^2 \\ & T=\left(\begin{array}{ll} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{array}\right)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & t_x \\ \sin \theta & \cos \theta & t_y \\ 0 & 0 & 1 \end{array}\right] \in \operatorname{SE}(2) \end{aligned} ​R=[cosθsinθ​−sinθcosθ​]∈SO(2),t=[tx​ty​​]∈R2T=(R0​t1​)= ​cosθsinθ0​−sinθcosθ0​tx​ty​1​ ​∈SE(2)​

1.2 李代数 s e ( 2 ) {{s e}(2)} se(2)

与$\mathrm{SE}(2)$对应的李代数 ${\mathbf{\varphi }}^{\wedge }\in \mathrm{se}(2)$, 如下
G 1 = ( 0 0 1 0 0 0 0 0 0 ) , G 2 = ( 0 0 0 0 0 1 0 0 0 ) , G 3 = ( 0 − 1 0 1 0 0 0 0 0 ) ϕ = ( u 1 ⋅ u 2 θ ) T ∈ R 3 ϕ ∧ = u 1 G 1 + u 2 G 2 + θ G 3 = [ 0 − θ u 1 θ 0 u 2 0 0 0 ] ∈  se (2)  \begin{aligned} & G_1=\left(\begin{array}{lll} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right), G_2=\left(\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{array}\right), G_3=\left(\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right) \\ & \boldsymbol{\phi}=\left(\begin{array}{lll} \mathrm{u}_1 \cdot \mathrm{u}_2 & \theta \end{array}\right)^T \quad \in \mathbb{R}^3 \\ & \boldsymbol{\phi}^{\wedge}=\mathbf{u}_1 G_1+\mathbf{u}_2 G_2+\theta G_3 \quad=\left[\begin{array}{ccc} 0 & -\theta & \mathrm{u}_1 \\ \theta & 0 & \mathrm{u}_2 \\ 0 & 0 & 0 \end{array}\right] \in \text { se (2) } & \end{aligned} ​G1​= ​000​000​100​ ​,G2​= ​000​000​010​ ​,G3​= ​010​−100​000​ ​ϕ=(u1​⋅u2​​θ​)T∈R3ϕ∧=u1​G1​+u2​G2​+θG3​= ​0θ0​−θ00​u1​u2​0​ ​∈ se (2) ​​ 上式中的 $\mathbf{\varphi }$ 即为我们理解的旋转向量, 或者称为角轴。注意 ${\mathbf{\varphi }}^{\wedge }$ 是反对称阵, 但是和 $\mathbf{so}(3)$ 中的反对称阵写法不同。在视觉 SLAM 十四讲中, 主要介绍的是三维空间, 李群 $\mathrm{SE}(3$ ) 和李代数 $\mathrm{se}(3)$ 是一一对应 (旋转角度在 $\pm \pi$ 之间, 李群和李代数元素是一一对应的) 的, $\mathrm{SE}(3)$ 为 $4\times$ 4 的旋转矩阵, $\mathrm{se}(3)$ 为 $6\times 1$ 的旋转向量。李群 $\mathrm{SO}(3)$ 和李代数 $\mathrm{so}(3)$ 是一一对应的, $\mathrm{SO}(3)$ 为 $3\times 3$ 的旋转矩阵, $\mathrm{so}(3)$ 为 $3\times 1$ 的旋转向量。

1.3 指数映射（将李代数 se ⁡ ( 2 ) \operatorname{se}(2) se(2) 转换为李群 S E ( 2 ) \mathrm{SE}(2) SE(2) )

已知李代数 $\mathrm{se}(2)$ 如下:
ϕ = [ u 1 u 1 θ ] ∈ R 3 ϕ ∧ = [ 0 − θ u 1 θ 0 u 2 0 0 0 ] ∈ se ⁡ ( 2 ) \begin{gathered} \boldsymbol{\phi}=\left[\begin{array}{c} \mathrm{u}_1 \\ \mathrm{u}_1 \\ \theta \end{array}\right] \in \mathbb{R}^3 \\ \boldsymbol{\phi}^{\wedge}=\left[\begin{array}{ccc} 0 & -\theta & \mathrm{u}_1 \\ \theta & 0 & \mathrm{u}_2 \\ 0 & 0 & 0 \end{array}\right] \in \operatorname{se}(2) \end{gathered} ϕ= ​u1​u1​θ​ ​∈R3ϕ∧= ​0θ0​−θ00​u1​u2​0​ ​∈se(2)​那么对应的李群 $\mathrm{SE}(2)$ 为:
$$\mathrm{T}=\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\left(\begin{array}{cc}\exp \left({\theta }^{\wedge }\right)& \mathbf{Vu}\\ 0& 0\end{array}\right)\dots \dots \dots \dots \text{ (1) }$$其中 $\exp \left({\theta }^{\wedge }\right)=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right],\mathbf{V}=\frac{1}{\theta }\cdot \left(\begin{array}{cc}\sin \theta & -(1-\cos \theta )\\ 1-\cos \theta & \sin \theta \end{array}\right)$, $\mathbf{u}=\left[\begin{array}{l}{\mathrm{u}}_1\\ {\mathrm{u}}_2\end{array}\right]$ 。那么
T = exp ⁡ ( ϕ ∧ ) = [ cos ⁡ θ − sin ⁡ θ sin ⁡ θ θ u 1 − 1 − cos ⁡ θ θ u 2 sin ⁡ θ cos ⁡ θ 1 − cos ⁡ θ θ u 1 + sin ⁡ δ θ δ θ u 2 0 0 1 ] … … … …  (2)  \mathrm{T}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & \frac{\sin \theta}{\theta} \mathrm{u}_1-\frac{1-\cos \theta}{\theta} \mathrm{u}_2 \\ \sin \theta & \cos \theta & \frac{1-\cos \theta}{\theta} \mathrm{u}_1+\frac{\sin \delta_\theta}{\delta_\theta} \mathrm{u}_2 \\ 0 & 0 & 1 \end{array}\right] \ldots \ldots \ldots \ldots \text { (2) } T=exp(ϕ∧)= ​cosθsinθ0​−sinθcosθ0​θsinθ​u1​−θ1−cosθ​u2​θ1−cosθ​u1​+δθ​sinδθ​​u2​1​ ​………… (2) 从公式 (2) 可以知道, 李代数的 $\mathbf{\varphi }$ 这个向量中的三个量 ${\mathrm{u}}_1$ 、 ${\mathrm{u}}_2、\theta$, 刚好能对应到 $\mathbf{T}$ 中的 $\theta$, 因此李代数中的 $\theta$ 确实真实反映了旋转,可是 ${\mathrm{u}}_1、{\mathrm{u}}_2$ 只是部分反映了平移, 需要乘以 $\mathbf{V}$, 才是变换成 $\mathbf{T}$ 后真正的平移量。

1.4 求极限

当 $x\to 0$ 时, $\sin x\sim x,1-\cos x\sim \frac{1}{2}{x}^2,\cos x\sim 1,x^2\sim 0$
当 $x,y\to 0$ 时, $x\cdot y\sim 0$

2 二维和三维刚体变换求雅可比矩阵

2.1 问题描述

在 gtsam 的官方文档gtsam.pdf 中的 3.2 节 Defining Custom Factors,构造一个类似于 GPS 的因子, 它是二维的坐标。在自定义因子时，需要提供雅可比矩阵。
因此如何求导雅可比矩阵是关键问题，而雅可比矩阵是误差方程对状态量或者状态增量的求导。问题也就转换为在旋转矩阵 $\mathrm{SE}(2)$ 上做扰动,然后对扰动量（状态增量）求导，即为我们需要的雅可比矩阵。但是由于李群只对乘法封闭, 不对加分封闭, 因此我们没法对这个扰动量求导，不过李群和李代数是一一对应的, 而李代数是对加法封闭的。我们在求导时, 只能对线性空间求导, 李群不是线性空间, 李代数是线性空间, 因此我们将问题转换为对扰动后的李代数求导。误差方程为:
$$E(q)\triangleq h(q)-m$$其中, $E(q)$ 为误差函数, $m$ 为二维的观测量, 观测量是不准确的, $h(q)$ 认为是一个广义的测量函数, 它能求出入参送进去之后的二维坐标, 我们希望优化的就是 $h(q)$ 函数, 最终得到 $E(q)$ 最小的目的, 也就是最小二乘, 如下所示:
$$h(q)=\left[\begin{array}{l}{q}_x\\ q_y\end{array}\right]$$

2.2 方法 1: 对扰动的量 Δ T \Delta T ΔT 对应的李代数 ξ \xi ξ 进行求导

在 gtsam 中, 定义雅可比矩阵如下所示, 注意我们最终要的是 $H$ :
$$\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)\approx \mathbf{h}(\mathbf{T})+\mathbf{H}\xi \dots \dots \dots \dots \text{ (3) }$$ 换言之，
$$\mathbf{E}(\mathbf{q})=\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)-\mathbf{h}(\mathbf{T})=\mathbf{H}\xi \dots \dots \dots \dots \text{ (4) }$$其中 T = [ cos ⁡ q θ − sin ⁡ q θ q x sin ⁡ q θ cos ⁡ q θ q x 0 0 1 ] T=\left[\begin{array}{ccc}\cos q_\theta & -\sin q_\theta & q_x \\ \sin q_\theta & \cos q_\theta & q_x \\ 0 & 0 & 1\end{array}\right] T= ​cosqθ​sinqθ​0​−sinqθ​cosqθ​0​qx​qx​1​ ​, 对 $T$ 做扰动, 扰动的量为 $\Delta T,\xi$ 为 $\Delta T$对应的李代数。

公式 (4) 中的 $\mathrm{H}$ 即为我们要的雅可比矩阵, 直观上看 $\mathbf{H}=\mathbf{E}(\mathbf{q}){\xi }^{-1}$，可惜的是 $\xi$ 向量是没有逆的, 因此只能求导，则
$$\mathbf{H}=\frac{\partial \mathbf{E}(\mathbf{q})}{\partial \mathbf{\xi }}\dots \dots \dots \dots \dots \dots \text{ (5) }$$为了更好的理解公式 (5), 我们做一个类比, 比如线性空间 $y=$ $kx$, 如果已知一组 $y_i$ 和对应的 $x_i$, 那么导数 $y^{\prime }=k=\frac{y_i}{x_i}$, 另一种方式是已知了表达式 $y=kx$, 那么 $y^{\prime }=\frac{\partial y}{\partial x}$, 两种方式都可以求出来 $k$ 。可是对于公式 (4), 我们是没法直接对 $\xi$ 这个向量求逆, 因此只能通过求导的方式来求出 $H$ 。
由于我们无法对非线性空间求导, 只能对线性空间求导, 定义 $\xi$ 李代数如下所示。注意这里面的三个量需要转换到李群 $\Delta T$, 才能找到对 $q_x$ 和$q_y$ 的扰动, 也就是说我们后面在做优化的过程中, 求的是 $\xi$ 变化量, 然后根据指数映射求出 $\Delta T$, 最终 $T_{\text{result }}=T\Delta T$, 最终的位置和姿态从 $T_{\text{result }}$ 中取出来即可。
ξ = [ δ x δ y δ θ ] \xi=\left[\begin{array}{l} \delta x \\ \delta y \\ \delta \theta \end{array}\right] ξ= ​δxδyδθ​ ​利用李代数 $\xi$ 求出李群 $\Delta T$, 即指数映射公式 (2)。注意，我也参考学习了GTSAM曲线拟合与自定义因子的部分内容，指数映射公式写的是错误的，其他地方都写的很好。可得:
Δ T = [ cos ⁡ δ θ − sin ⁡ δ θ sin ⁡ δ θ δ θ δ x − 1 − cos ⁡ δ θ δ θ δ y sin ⁡ δ θ cos ⁡ δ θ 1 − cos ⁡ δ θ δ θ δ x + sin ⁡ δ θ δ θ δ y 0 0 1 ] \Delta T=\left[\begin{array}{ccc} \cos \delta \theta & -\sin \delta \theta & \frac{\sin \delta \theta}{\delta \theta} \delta x-\frac{1-\cos \delta \theta}{\delta \theta} \delta y \\ \sin \delta \theta & \cos \delta \theta & \frac{1-\cos \delta \theta}{\delta \theta} \delta x+\frac{\sin \delta_\theta}{\delta_\theta} \delta y \\ 0 & 0 & 1 \end{array}\right] ΔT= ​cosδθsinδθ0​−sinδθcosδθ0​δθsinδθ​δx−δθ1−cosδθ​δyδθ1−cosδθ​δx+δθ​sinδθ​​δy1​ ​利用目录 1.4 中的极限法则, $\xi$ 的三个量为小量, 也就是趋近于0，那么 $\cos \delta \theta \sim 1,\sin \delta \theta \sim$ $\delta \theta$ 。对 $\Delta T$ 右上角两个式子展开求, 那么:
$$\begin{array}{rl}& \underset{\xi \to 0}{\lim }\frac{\sin {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_x-\frac{1-\cos {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_y\\ =& \underset{\xi \to 0}{\lim }1\cdot {\delta }_x-\frac{\frac{1}{2}{\delta }_{\theta }^2}{{\delta }_{\theta }}\cdot {\delta }_y\\ =& \underset{\xi \to 0}{\lim }{\delta }_x\\ & \underset{\xi \to 0}{\lim }\frac{1-\cos {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_x+\frac{\sin {\delta }_{\theta }}{{\delta }_{\theta }}{\delta }_y\\ =& \underset{\xi \to 0}{\lim }\frac{\frac{1}{2}{\delta }_{\theta }^2}{{\delta }_{\theta }}{\delta }_x+{\delta }_y.\\ =& \underset{\xi \to 0}{\lim }{\delta }_y.\end{array}$$因此, Δ T ≈ [ 1 − δ θ δ x δ θ 1 δ y 0 0 1 ] \Delta T \approx\left[\begin{array}{ccc}1 & -\delta \theta & \delta x \\ \delta \theta & 1 & \delta y \\ 0 & 0 & 1\end{array}\right] ΔT≈ ​1δθ0​−δθ10​δxδy1​ ​。
那么 h ( T ⋅ exp ⁡ ξ ∧ ) ≈ h ( [ cos ⁡ ( q θ ) − sin ⁡ ( q θ ) q x sin ⁡ ( q θ ) cos ⁡ ( q θ ) q y 0 0 1 ] [ 1 − δ θ δ x δ θ 1 δ y 0 0 1 ] ) \boldsymbol{h}\left(\boldsymbol{T} \cdot \exp \xi^{\wedge}\right) \approx h\left(\left[\begin{array}{ccc}\cos \left(q_\theta\right) & -\sin \left(q_\theta\right) & q_x \\ \sin \left(q_\theta\right) & \cos \left(q_\theta\right) & q_y \\ 0 & 0 & 1\end{array}\right]\left[\begin{array}{ccc}1& -\delta \theta & \delta x \\ \delta \theta & 1 & \delta y \\ 0 & 0 & 1\end{array}\right]\right) h(T⋅expξ∧)≈h ​ ​cos(qθ​)sin(qθ​)0​−sin(qθ​)cos(qθ​)0​qx​qy​1​ ​ ​1δθ0​−δθ10​δxδy1​ ​ ​
= h ( [ cos ⁡ q θ − sin ⁡ ( q θ ) ⋅ δ θ − cos ⁡ ( q θ ) δ θ − sin ⁡ ( q θ ) cos ⁡ ( q θ ) δ x − sin ⁡ ( q θ ) δ y + q x sin ⁡ q θ + cos ⁡ ( q θ ) ⋅ δ θ − sin ⁡ ( q θ ) δ θ + cos ⁡ ( q θ ) sin ⁡ ( q θ ) δ x + cos ⁡ ( q θ ) δ y + q y 0 0 1 ] ) =h\left(\left[\begin{array}{ccc} \cos q_\theta-\sin \left(q_\theta\right) \cdot \delta_\theta & -\cos \left(q_\theta\right) \delta_\theta-\sin \left(q_\theta\right) & \cos \left(q_\theta\right) \delta_x-\sin \left(q_\theta\right) \delta_y+q_x \\ \sin q_\theta+\cos \left(q_\theta\right) \cdot \delta_\theta & -\sin \left(q_\theta\right) \delta_\theta+\cos \left(q_\theta\right) & \sin \left(q_\theta\right) \delta_x+\cos \left(q_\theta\right) \delta_y+q_y \\ 0 & 0 & 1 \end{array}\right]\right) =h ​ ​cosqθ​−sin(qθ​)⋅δθ​sinqθ​+cos(qθ​)⋅δθ​0​−cos(qθ​)δθ​−sin(qθ​)−sin(qθ​)δθ​+cos(qθ​)0​cos(qθ​)δx​−sin(qθ​)δy​+qx​sin(qθ​)δx​+cos(qθ​)δy​+qy​1​ ​ ​$$=\left[\begin{array}{l}{q}_x+\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ q_y+\sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]$$那么, 由 $\mathbf{E}(\mathbf{q})=\mathbf{h}\left(\mathbf{T}\cdot \exp {\xi }^{\wedge }\right)-\mathbf{h}(\mathbf{T})$, 可得

$\mathbf{E}(\mathbf{q})=\left[\begin{array}{l}{q}_x+\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ q_y+\sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]-\left[\begin{array}{l}{q}_x\\ q_y\end{array}\right]={\left[\begin{array}{c}\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ \sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]}_{2\times 1}$, 则 $H$ 是函数 $E(q)$ 对 $\xi$ 求导数，那么
H = ∂ E ( q ) ∂ ξ = ∂ [ cos ⁡ ( q θ ) δ x − sin ⁡ ( q θ ) δ y sin ⁡ ( q θ ) δ x + cos ⁡ ( q θ ) δ y ] 2 × 1 ∂ [ δ x δ y δ θ ] 3 × 1 = [ cos ⁡ q θ − sin ⁡ q θ 0 sin ⁡ q θ cos ⁡ q θ 0 ] 2 × 3 = [ R 2 × 2 0 2 × 1 ] \begin{aligned} & H= \frac{\partial E(q)}{\partial \xi} \\ &= \frac{\partial\left[\begin{array}{l} \cos \left(q_\theta\right) \delta x-\sin \left(q_\theta\right) \delta y \\ \sin \left(q_\theta\right) \delta x+\cos \left(q_\theta\right) \delta y \end{array}\right]_{2 \times 1}}{\partial\left[\begin{array}{l} \delta x \\ \delta y \\ \delta \theta \end{array}\right]_{3 \times 1}}\\ &=\left[\begin{array}{ccc} \cos q_\theta & -\sin q_\theta & 0 \\ \sin q_\theta & \cos q_\theta & 0 \end{array}\right]_{2 \times 3} \\ &=\left[\begin{array}{cc} R_{2 \times 2} & 0_{2 \times 1} \end{array}\right] \end{aligned} ​H=∂ξ∂E(q)​=∂ ​δxδyδθ​ ​3×1​∂[cos(qθ​)δx−sin(qθ​)δysin(qθ​)δx+cos(qθ​)δy​]2×1​​=[cosqθ​sinqθ​​−sinqθ​cosqθ​​00​]2×3​=[R2×2​​02×1​​]​ $H$即为我们要的三维空间中误差方程的雅可比矩阵, 其中$R_{2\times 2}\in \mathrm{SO}(2)$。至此, 我们求出来了雅可比矩阵，这个矩阵和gtsam.pdf 中的 3.2 节 Defining Custom Factors中的雅可比矩阵一样，我们推导成功了。

2.2 方法 2: 直接用公式推导 3

用误差扰动公式求导，不过这个公式是怎么来的，我还是很纳闷:
$$\begin{array}{rl}& {\hat{q}}_A=q_A+\delta q\\ & \cdots \cdots =q_A+\left[\begin{array}{cc}\cos \left(q_{\theta }\right)& -\sin \left(q_{\theta }\right)\\ \sin \left(q_{\theta }\right)& \cos \left(q_{\theta }\right)\end{array}\right]\left[\begin{array}{l}\delta x\\ \delta y\end{array}\right]\end{array}$$ ${\hat{q}}_A$ 为A点的坐标估计量, $q_A$ 为A点的坐标真值, $\delta q=E(q)=\left[\begin{array}{cc}\cos \left(q_{\theta }\right)& -\sin \left(q_{\theta }\right)\\ \sin \left(q_{\theta }\right)& \cos \left(q_{\theta }\right)\end{array}\right]\left[\begin{array}{l}\delta x\\ \delta y\end{array}\right]=$ ${\left[\begin{array}{l}\cos \left(q_{\theta }\right)\delta x-\sin \left(q_{\theta }\right)\delta y\\ \sin \left(q_{\theta }\right)\delta x+\cos \left(q_{\theta }\right)\delta y\end{array}\right]}_{2\times 1}$ 为误差项, 而 $H$ 是函数 $\mathbf{E}(\mathbf{q})$ 对 $\xi$ 求导数,那么
H = ∂ E ( q ) ∂ ξ = ∂ [ cos ⁡ ( q θ ) δ x − sin ⁡ ( q θ ) δ y sin ⁡ ( q θ ) δ x + cos ⁡ ( q θ ) δ y ] 2 × 1 ∂ [ δ x δ y δ θ ] 3 × 1 = [ cos ⁡ q θ − sin ⁡ q θ 0 sin ⁡ q θ cos ⁡ q θ 0 ] 2 × 3 = [ R 2 × 2 0 2 × 1 ] \begin{aligned} & H= \frac{\partial E(q)}{\partial \xi} \\ &= \frac{\partial\left[\begin{array}{l} \cos \left(q_\theta\right) \delta x-\sin \left(q_\theta\right) \delta y \\ \sin \left(q_\theta\right) \delta x+\cos \left(q_\theta\right) \delta y \end{array}\right]_{2 \times 1}}{\partial\left[\begin{array}{l} \delta x \\ \delta y \\ \delta \theta \end{array}\right]_{3 \times 1}}\\ &=\left[\begin{array}{ccc} \cos q_\theta & -\sin q_\theta & 0 \\ \sin q_\theta & \cos q_\theta & 0 \end{array}\right]_{2 \times 3} \\ &=\left[\begin{array}{cc} R_{2 \times 2} & 0_{2 \times 1} \end{array}\right] \end{aligned} ​H=∂ξ∂E(q)​=∂ ​δxδyδθ​ ​3×1​∂[cos(qθ​)δx−sin(qθ​)δysin(qθ​)δx+cos(qθ​)δy​]2×1​​=[cosqθ​sinqθ​​−sinqθ​cosqθ​​00​]2×3​=[R2×2​​02×1​​]​ $H$即为我们要的三维空间中误差方程的雅可比矩阵, 其中$R_{2\times 2}\in \mathrm{SO}(2)$。

2.3 将方法 2 类比推导到三维空间

用方法 2 进行方法类比, 扩展到三维空间, 利用方法 2 , q = [ q x q y q z q α q β q γ ] 2, q=\left[\begin{array}{l}q_x \\ q_y \\ q_z \\ q_\alpha \\ q_\beta \\ q_\gamma\end{array}\right] 2,q= ​qx​qy​qz​qα​qβ​qγ​​ ​， 此时 ξ 6 × 1 = [ δ x δ y δ z δ α δ β δ γ ] \xi_{6 \times 1}=\left[\begin{array}{l}\delta x \\ \delta y \\ \delta z \\ \delta \alpha \\ \delta \beta \\ \delta \gamma\end{array}\right] ξ6×1​= ​δxδyδzδαδβδγ​ ​ 为六维向量。那么 $\delta q$ 如下:
E = δ q = [ cos ⁡ γ cos ⁡ β cos ⁡ γ sin ⁡ β sin ⁡ α − sin ⁡ γ cos ⁡ α cos ⁡ γ sin ⁡ β cos ⁡ α + sin ⁡ γ sin ⁡ α sin ⁡ γ cos ⁡ β sin ⁡ γ sin ⁡ β sin ⁡ α + cos ⁡ γ cos ⁡ α sin ⁡ γ sin ⁡ β cos ⁡ α − cos ⁡ γ sin ⁡ α − sin ⁡ β cos ⁡ β sin ⁡ α cos ⁡ β sin ⁡ α ] 3 × 3 [ δ x δ y δ z ] 3 × 1 = [ cos ⁡ γ cos ⁡ β ⋅ δ x + ( cos ⁡ γ sin ⁡ β sin ⁡ α − sin ⁡ γ cos ⁡ α ) ⋅ δ y + ( cos ⁡ γ sin ⁡ β cos ⁡ α + sin ⁡ γ sin ⁡ α ) ⋅ δ z sin ⁡ γ cos ⁡ β ⋅ δ x + ( sin ⁡ γ sin ⁡ β sin ⁡ α + cos ⁡ γ cos ⁡ α ) ⋅ δ y + ( sin ⁡ γ sin ⁡ β cos ⁡ α − cos ⁡ γ sin ⁡ α ) ⋅ δ z − sin ⁡ β ⋅ δ x + cos ⁡ β sin ⁡ α ⋅ δ y + cos ⁡ β cos ⁡ α ⋅ δ z ] 3 × 1 \begin{gathered} E=\delta q=\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right]_{3 \times 3}\left[\begin{array}{l} \delta x \\ \delta y \\ \delta z \end{array}\right]_{3 \times 1} \\ =\left[\begin{array}{rr} \cos \gamma \cos \beta \cdot \delta_x+(\cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha) \cdot \delta y+(\cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha) \cdot \delta z \\ \sin \gamma \cos \beta \cdot \delta_x+(\sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha) \cdot \delta y+(\sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha) \cdot \delta z \\ -\sin \beta \cdot \delta_x+\cos \beta \sin \alpha \cdot \delta_y+\cos \beta \cos \alpha \cdot \delta_z \end{array}\right]_{3 \times 1} \end{gathered} E=δq= ​cosγcosβsinγcosβ−sinβ​cosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinα​cosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβsinα​ ​3×3​ ​δxδyδz​ ​3×1​= ​cosγcosβ⋅δx​+(cosγsinβsinα−sinγcosα)⋅δy+(cosγsinβcosα+sinγsinα)⋅δzsinγcosβ⋅δx​+(sinγsinβsinα+cosγcosα)⋅δy+(sinγsinβcosα−cosγsinα)⋅δz−sinβ⋅δx​+cosβsinα⋅δy​+cosβcosα⋅δz​​ ​3×1​​那么雅可比矩阵 $H$ 如下：
H = ∂ E ( q ) ∂ ξ = [ ∂ E 1 ∂ δ x ∂ E 1 ∂ δ y ∂ E 1 ∂ δ z ∂ E 1 ∂ δ α ∂ E 1 ∂ δ β ∂ E 1 ∂ δ γ ∂ E 2 ∂ δ x ∂ E 2 ∂ δ y ∂ E 2 ∂ δ z ∂ E 2 ∂ δ α ∂ E 2 ∂ δ β ∂ E 2 ∂ δ γ ∂ E 3 ∂ δ x ∂ E 3 ∂ δ y ∂ E 3 ∂ δ z ∂ E 3 ∂ δ α ∂ E 3 ∂ δ β ∂ E 3 ∂ δ γ ] 3 × 6 = [ cos ⁡ γ cos ⁡ β cos ⁡ γ sin ⁡ β sin ⁡ α − sin ⁡ γ cos ⁡ α cos ⁡ γ sin ⁡ β cos ⁡ α + sin ⁡ γ sin ⁡ α 0 0 0 sin ⁡ γ cos ⁡ β sin ⁡ γ sin ⁡ β sin ⁡ α + cos ⁡ γ cos ⁡ α sin ⁡ γ sin ⁡ β cos ⁡ α − cos ⁡ γ sin ⁡ α 0 0 0 − sin ⁡ β cos ⁡ β sin ⁡ α cos ⁡ β sin ⁡ α 0 0 0 ] 3 × 6 = [ R 3 × 3 0 3 × 3 ] \begin{aligned} & H=\frac{\partial E(q)}{\partial \xi}=\left[\begin{array}{llllll} \frac{\partial E_1}{\partial \delta_x} & \frac{\partial E_1}{\partial \delta_y} & \frac{\partial E_1}{\partial \delta_z} & \frac{\partial E_1}{\partial \delta_\alpha} & \frac{\partial E_1}{\partial \delta_\beta} & \frac{\partial E_1}{\partial \delta_\gamma} \\ \frac{\partial E_2}{\partial \delta_x} & \frac{\partial E_2}{\partial \delta_y} & \frac{\partial E_2}{\partial \delta_z} & \frac{\partial E_2}{\partial \delta_\alpha} & \frac{\partial E_2}{\partial \delta_\beta} & \frac{\partial E_2}{\partial \delta_\gamma} \\ \frac{\partial E_3}{\partial \delta_x} & \frac{\partial E_3}{\partial \delta_y} & \frac{\partial E_3}{\partial \delta_z} & \frac{\partial E_3}{\partial \delta_\alpha} & \frac{\partial E_3}{\partial \delta_\beta} & \frac{\partial E_3}{\partial \delta_\gamma} \end{array}\right]_{3 \times 6} \\ & =\left[\begin{array}{llllll} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha & 0 & 0 & 0 \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha & 0 & 0 & 0 \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha & 0 & 0 & 0 \end{array}\right]_{3 \times 6} \\ & =\left[R_{3 \times 3} \quad 0_{3 \times 3}\right]&& \end{aligned} ​H=∂ξ∂E(q)​= ​∂δx​∂E1​​∂δx​∂E2​​∂δx​∂E3​​​∂δy​∂E1​​∂δy​∂E2​​∂δy​∂E3​​​∂δz​∂E1​​∂δz​∂E2​​∂δz​∂E3​​​∂δα​∂E1​​∂δα​∂E2​​∂δα​∂E3​​​∂δβ​∂E1​​∂δβ​∂E2​​∂δβ​∂E3​​​∂δγ​∂E1​​∂δγ​∂E2​​∂δγ​∂E3​​​ ​3×6​= ​cosγcosβsinγcosβ−sinβ​cosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinα​cosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβsinα​000​000​000​ ​3×6​=[R3×3​03×3​]​​​ $H$即为我们要的三维空间中误差方程的雅可比矩阵, 其中$R_{3\times 3}\in \mathrm{SO}(3)$。这个矩阵对应于gtsam中的gtsam::GPSFactor中的雅可比矩阵，因为gtsam::GPSFactor的因子是在三维空间中的优化，上面这个雅可比矩阵和gtsam代码中gtsam::GPSFactor的雅可比矩阵是对应的。
接下来为了更好的说明，我们来展示下gtsam中的两个函数，gtsam::GPSFactor的雅可比矩阵是从Pose3::translation函数中来的，是*Hself << Z_3x3, rotation().matrix()，也就是由一个3x3的零矩阵和R阵组成，和我们上面推导的 $H$ 阵对应，只不过它把3x3的零矩阵写在前面了，这是因为代码中${\xi }_{6\times 1}$先写角度的扰动，后写位置的扰动，参考a question about GPSFactor。