se(3)与so(3)左乘、右乘扰动模型雅克比推导

本文分别详细推导了$\mathfrak{se}(3)$ 与 $\mathfrak{so}(3)$ 的左乘、右乘扰动模型的雅克比矩阵，特别提供了$\mathfrak{se}(3)$左乘扰动模型优化特征点的重投影误差的例子。
手动码公式，如有谬误请包涵指正。

一、$\mathfrak{se}(3)$ 左乘扰动模型求解点重投影误差优化问题

使用李代数对位姿进行优化的原因：
旋转矩阵自身是带有约束的（正交且行列式为1），它们作为优化变量时，会引入额外的约束，使得优化变得困难。通过李群和李代数之间的转换关系，可以把位姿估计变为无约束的优化问题，简化求解方式。

使用高斯牛顿法或者LM法对重投影误差进行优化，需要求得重投影误差的雅克比矩阵。
因为会用到链式法则，所以先求两个有用的导数。

1. 投影函数：
   $$h(\mathbf{p})=h\left(\left[\begin{array}{l}{p}_x\\ p_y\\ p_z\end{array}\right]\right)=\left[\begin{array}{c}{c}_x+f_x\frac{p_x}{p_z}\\ c_y+f_y\frac{p_y}{p_z}\end{array}\right]$$
   投影函数对3D点坐标求导：
   $$\frac{\partial h(\mathbf{p})}{\partial \mathbf{p}}=\left[\begin{array}{ccc}{f}_x/p_z& 0& -f_x{p}_x/p_z^2\\ 0& f_y/p_z& -f_y{p}_y/p_z^2\end{array}\right]$$
   观测到的投影坐标是2维的，对3维点进行求导，所以结果是 2x3 的矩阵。
2. 变换矩阵作用于3D点（使用齐次坐标表示，3D点最后一维是1），使用左乘扰动模型对李代数进行求导：
   $$\begin{array}{rl}\frac{\partial (\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}{\mathbf{\varphi }}^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\mathbf{\rho }\\ 0\end{array}\right]}{\delta \mathbf{\xi }}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (\mathbf{T}\mathbf{p}{)}^{\odot }.\end{array}$$

注：${\lim }_{\mathbf{\xi }\to 0}\exp \left({\mathbf{\xi }}^{\wedge }\right)=\left[\begin{array}{cc}\mathbf{I}+{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 1\end{array}\right]={\mathbf{I}}_4+\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]=\mathbf{I}+{\mathbf{\xi }}^{\wedge }$，注意这里使用的是前面提到的伪指数映射 $$\exp \left({\mathbf{\xi }}^{\wedge }\right)=\left[\begin{array}{cc}\exp \left({\mathbf{\varphi }}^{\wedge }\right)& \mathbf{\rho }\\ 0^T& 1\end{array}\right]$$
不然根据原来的指数映射求极限后得到的不是这个结果。

3. 重投影误差求雅克比矩阵（左乘扰动模型）：
   $$\frac{\partial h\left(\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}\right)-\mathbf{u}}{\partial \mathbf{p}/\partial \delta \mathbf{\xi }}=\frac{\partial h\left(\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}\right)}{\partial \mathbf{p}/\partial \delta \mathbf{\xi }}$$
   这里 $\mathbf{u}$ 是3D点 $\mathbf{p}$ 的2D观测坐标。
   3.1 根据链式法则，对3D点坐标求导：
   $$\begin{array}{rl}{\mathbf{J}}_{\mathbf{p}}=\frac{\partial h\left(\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}\right)}{\partial \mathbf{p}}& ={\frac{\partial h\left({\mathbf{p}}^{\prime }\right)}{\partial {\mathbf{p}}^{\prime }}|}_{{\mathbf{p}}^{\prime }=\mathbf{Tp}=\mathbf{g}}{\frac{\partial \exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}}{\partial \mathbf{p}}|}_{\delta \mathbf{\xi }=0}\\ & ={\frac{\partial h\left({\mathbf{p}}^{\prime }\right)}{\partial {\mathbf{p}}^{\prime }}|}_{{\mathbf{p}}^{\prime }=\mathbf{Tp}=\mathbf{g}}\frac{\partial \mathbf{Tp}}{\partial \mathbf{p}}\\ & =\left(\begin{array}{ccc}{f}_x/g_z& 0& -f_x{g}_x/g_z^2\\ 0& f_y/g_z& -f_y{g}_y/g_z^2\end{array}\right)\mathbf{R}\end{array}$$
   3.2 对左乘扰动量求导：
   $$\begin{array}{rl}{\mathbf{J}}_{\delta \mathbf{\xi }}=\frac{\partial h\left(\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}\right)}{\partial \delta \mathbf{\xi }}& ={\frac{\partial h\left({\mathbf{p}}^{\prime }\right)}{\partial {\mathbf{p}}^{\prime }}|}_{{\mathbf{p}}^{\prime }=\mathbf{Tp}=\mathbf{g}}\frac{\partial \exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}}{\partial \delta \mathbf{\xi }}\\ & =\left(\begin{array}{ccc}{f}_x/g_z& 0& -f_x{g}_x/g_z^2\\ 0& f_y/g_z& -f_y{g}_y/g_z^2\end{array}\right)\left(\begin{array}{ll}{\mathbf{I}}_3& -[\mathbf{g}{]}^{\wedge }\end{array}\right)\\ & =\left(\begin{array}{cccccc}\frac{f_x}{g_z}& 0& -f_x\frac{g_x}{g_z^2}& -f_x\frac{g_x{g}_y}{g_z^2}& f_x\left(1+\frac{g_x^2}{g_z^2}\right)& -f_x\frac{g_y}{g_z}\\ 0& \frac{f_y}{g_z}& -f_y\frac{g_y}{g_z^2}& -f_y\left(1+\frac{g_y^2}{g_z^2}\right)& f_y\frac{g_x{g}_y}{g_z^2}& f_y\frac{g_x}{g_z}\end{array}\right)\end{array}$$
   这里 $\frac{\partial \exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{Tp}}{\partial \delta \mathbf{\xi }}$ 实际上应该是一个 4x6 的矩阵，因为变换得到的齐次坐标是4维的，但是投影函数只用到了前3维，所以前面求得的该式的结果相应的取前3行就好。
4. 求得雅克比矩阵后就可以使用高斯牛顿法、LM法对3D点坐标和位姿进行迭代优化。求得 $s0(3)$ 左扰动增量后，使用左乘更新：
   $${\mathbf{T}}_{n+1}=\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right){\mathbf{T}}_n$$

除了利用左乘扰动模型优化位姿，还可以使用右乘扰动模型，又或者是对旋转（使用 $s0(3)$ 扰动模型）和平移量分别进行优化更新。

二、$\mathfrak{se}(3)$ 右乘扰动模型雅克比

$$\begin{array}{rl}\frac{\partial (\mathbf{T}\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left({\mathbf{\xi }}^{\wedge }\right)\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left({\mathbf{\xi }}^{\wedge }\right)\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left({\mathbf{\xi }}^{\wedge }\right)\delta {\mathbf{\xi }}^{\wedge }\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\mathbf{R}({\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{\rho })+\mathbf{t}\\ 0\end{array}\right]}{\delta \mathbf{\xi }}=\left[\begin{array}{cc}\mathbf{R}& -\mathbf{R}{\mathbf{p}}^{\wedge }\\ 0^T& 0^T\end{array}\right]\end{array}$$

三、3 维平移量与 $\mathfrak{so}(3)$ 左乘扰动模型雅克比

$$\begin{array}{rl}\frac{\partial (\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{R}\mathbf{p}+\mathbf{t})}{\partial \mathbf{t}}& =\mathbf{I}\end{array}$$
$$\begin{array}{rl}\frac{\partial (\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{R}\mathbf{p}+\mathbf{t})}{\partial \mathbf{\varphi }}& =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{(\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{R}\mathbf{p}+\mathbf{t})-(\mathbf{R}\mathbf{p}+\mathbf{t})}{\mathbf{\varphi }}\\ & \approx \underset{\mathbf{\varphi }\to 0}{\lim }\frac{((\mathbf{I}+{\mathbf{\varphi }}^{\wedge })\mathbf{R}\mathbf{p}+\mathbf{t})-(\mathbf{R}\mathbf{p}+\mathbf{t})}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{{\mathbf{\varphi }}^{\wedge }\mathbf{R}\mathbf{p}}{\mathbf{\varphi }}\\ & =-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$

四、$\mathfrak{so}(3)$ 右乘扰动模型雅克比

$$\begin{array}{rl}\frac{\partial (\mathbf{R}\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}+\mathbf{t})}{\partial \mathbf{\varphi }}& =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{(\mathbf{R}\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}+\mathbf{t})-(\mathbf{R}\mathbf{p}+\mathbf{t})}{\mathbf{\varphi }}\\ & \approx \underset{\mathbf{\varphi }\to 0}{\lim }\frac{(\mathbf{R}(\mathbf{I}+{\mathbf{\varphi }}^{\wedge })\mathbf{p}+\mathbf{t})-(\mathbf{R}\mathbf{p}+\mathbf{t})}{\mathbf{\varphi }}\\ & =\underset{\mathbf{\varphi }\to 0}{\lim }\frac{\mathbf{R}{\mathbf{\varphi }}^{\wedge }\mathbf{p}}{\mathbf{\varphi }}\\ & =-\mathbf{R}{\mathbf{p}}^{\wedge }\end{array}$$

附：李群与李代数的定义

1. 群是一种集合加上一种运算的代数结构，群要求这个运算满足：封闭性，结合律，幺元，逆（封结幺逆）：

   扫描二维码关注公众号，回复： 15494328 查看本文章
   • 封闭性：$\forall a_1,a_2\in A,a_1\cdot a_2\in A$.
   • 结合律：$\forall a_1,a_2,a_3\in A,\left(a_1\cdot a_2\right)\cdot a_3=a_1\cdot \left(a_2\cdot a_3\right)$.
   • 幺元：$\exists a_0\in A,$ s.t. $\forall a\in A,a_0\cdot a=a\cdot a_0=a$.
   • 逆：$\forall a\in A,\exists a^{-1}\in A,$ s.t. $a\cdot a^{-1}=a_0$.

   李群是指具有连续（光滑）性质的群。
   特殊正交群SO(3)：
   S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{\boldsymbol{R} \in \mathbb{R}^{3 \times 3}| \boldsymbol{RR}^T = \boldsymbol{I}, det(\boldsymbol{R}) = 1\} SO(3)={ R∈R3×3∣RRT=I,det(R)=1}
   特殊欧式群SE(3)：
   $$SE(3)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\right\}$$

2. 李代数由一个集合V，一个数域F和一个二元运算符（李括号）[,]组成，满足以下性质：封闭性，双线性，自反性，雅可比等价。李括号表达了两个元素的差异。

   • 封闭性：$\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}$.
   • 双线性：$\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F}$, 有：
     $[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]$
   • 自反性：$\forall \mathbf{X}\in \mathbb{V},[\mathbf{X},\mathbf{X}]=0$.
   • 雅可比等价：$\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{Y},\mathbf{X}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0$.

   SO(3)对应的李代数：
   $$\mathfrak{so}(3)=\left\{\mathbf{\varphi }\in {\mathbb{R}}^3,\mathbf{\Phi }={\mathbf{\varphi }}^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$
   SE(3)对应的李代数：
   $$\mathfrak{se}(3)=\left\{\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\mathbf{\varphi }\in \mathfrak{so}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
   其中 hat(wedge) operator $(\cdot {)}^{\wedge }$：
   $$\begin{gathered} \mathbf{\omega }=\left[\begin{array}{l}x\\ y\\ z\end{array}\right],{\mathbf{\omega }}^{\wedge }=\left(\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right) \\ ({\mathbf{\omega }}^{\wedge }{)}^{\vee }={\left(\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right)}^{\vee }=\left[\begin{array}{l}x\\ y\\ z\end{array}\right]=\mathbf{\omega } \end{gathered}$$

附：李群与李代数的互相转换

1. so(3) -> SO(3)，指数映射，即罗德里格斯公式：
   $$\begin{gathered} \exp \left(\mathbf{\varphi }\right)=\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge } \\ \exp \left(\mathbf{\varphi }\right)=\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\mathbf{I}+\frac{\sin \theta }{\theta }{\mathbf{\varphi }}^{\wedge }+\frac{1-\cos \theta }{{\theta }^2}({\mathbf{\varphi }}^{\wedge }{)}^2 \end{gathered}$$

2. SO(3) -> so(3)，对数映射：
   $$\cos \theta =\frac{tr(\mathbf{R})-1}{2},\mathbf{Ra}=\mathbf{a}$$

3. se(3) -> SE(3):
   $$\begin{gathered} \exp \left({\mathbf{\xi }}^{\wedge }\right)=\left[\begin{array}{cc}\exp \left({\mathbf{\varphi }}^{\wedge }\right)& \mathbf{t}\\ 0^T& 1\end{array}\right],\mathbf{t}=\mathbf{J}\mathbf{\rho } \\ \mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge } \\ \mathbf{J}=\mathbf{I}+\frac{1-\cos \theta }{{\theta }^2}{\mathbf{\varphi }}^{\wedge }+\frac{\theta -\sin \theta }{{\theta }^3}{\left({\mathbf{\varphi }}^{\wedge }\right)}^2 \end{gathered}$$
   其中
   $${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{ll}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]$$

4. SE(3)->se(3)
   $$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}\mathbf{R}\mathbf{a}=\mathbf{a}\mathbf{t}=\mathbf{J}\mathbf{\rho }$$

定义伪指数（pseudo-exponential）：
$$\exp \left({\mathbf{\xi }}^{\wedge }\right)=\left[\begin{array}{cc}\exp \left({\mathbf{\varphi }}^{\wedge }\right)& \mathbf{\rho }\\ 0^T& 1\end{array}\right]$$
伪指数的定义是为了简化求解雅克比矩阵。

参考

[1] 视觉SLAM14讲
[2] J.-l. Blanco, “A tutorial on SE (3) transformation parameterizations and on-manifold optimization,” no. 3, 2013.