李群和李代数（三）：优化基础

首先，我们还是从熟悉的BCH近似公式开始。

1. BCH近似公式

前面已经介绍过BCH公式了，BCH公式表达的是两个矩阵的指数相乘

$\begin{aligned} \ln ( \exp (\mathbf{A}) \exp (\mathbf{B}) ) & = \sum^\infty_{n=1} \frac{-1^{n-1}}{n} \sum_{r_i + s_r > 0, 1 \le i \le n} \frac{ ( \sum^n_{i=1} (r_i + s_i) )^{-1} }{ \prod^n_{i=1} r_i ! s_i! } [\mathbf{A}^{r_1} \mathbf{B}^{s_1} ... \mathbf{A}^{r_n}\mathbf{B}^{r_n}] \\ & = \mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\ldots \end{aligned}$

其中，[] 为李括号。

对应的有李乘积公式

$\exp( \mathbf{A} + \mathbf{B} ) = \lim_{\alpha \to \infty} ( \exp( \mathbf{A} / \alpha ) \exp( \mathbf{B} / \alpha ) )^\alpha$

1.1 旋转

考虑在优化中，更新量都为小量，可以用BCH公式近似

$\begin{aligned} \ln ( \mathbf{C}_1 \mathbf{C}_2 )^\vee & = \ln ( \exp( \phi_1^\wedge ) \exp ( \phi_2^\wedge ) )^\vee \\ & \approx \ \begin{cases} \mathbf{J}_l (\phi_2)^{-1} \phi_1 + \phi_2 \quad \text{若}\phi_1\text{很小} \\ \phi_1+ \mathbf{J}_r (\phi_1)^{-1} \phi_2 \quad \text{若}\phi_2\text{很小} \end{cases} \end{aligned}$

其中，Jr和Jl分别为SO3的右雅可比和左雅可比。

$\begin{aligned} \mathbf{J}r(\phi) & = \sum^\infty{n=0}\frac{1}{(n+1)!} (-\phi^\wedge)^n = \int^1_0 \mathbf{C}^{-\alpha} d\alpha \\ & = \frac{\sin\phi}{\phi} \mathbf{1} + (1 - \frac{\sin\alpha}{\phi})\mathbf{a}\mathbf{a}^T - \frac{1 - \cos\phi}{\phi} \mathbf{a}^\wedge \\ \mathbf{J}_l(\phi) & = \sum^\infty{n=0}\frac{1}{(n+1)!} (\phi^\wedge)^n = \int^1_0 \mathbf{C}^{\alpha} d\alpha \\ & = \frac{\sin\phi}{\phi} \mathbf{1} + (1 - \frac{\sin\alpha}{\phi})\mathbf{a}\mathbf{a}^T - \frac{1 - \cos\phi}{\phi} \mathbf{a}^\wedge \\ \mathbf{J}_r(\phi)^{-1} & = \sum^\infty{n=0} \frac{B_n}{n!} (-\phi^\wedge)^n = \frac{\phi}{2} \cot \frac{\phi}{2} \mathbf{1} + ( 1 - \frac{\phi}{2} \cot \frac{\phi}{2}) \mathbf{a}\mathbf{a}^T + \frac{\phi}{2} \mathbf{a}^\wedge \\ \mathbf{J}_l(\phi)^{-1} & = \sum^\infty{n=0} \frac{B_n}{n!} (\phi^\wedge)^n = \frac{\phi}{2} \cot \frac{\phi}{2} \mathbf{1} + ( 1 - \frac{\phi}{2} \cot \frac{\phi}{2}) \mathbf{a}\mathbf{a}^T - \frac{\phi}{2} \mathbf{a}^\wedge \end{aligned}$

对于左右雅可比有

$\begin{aligned} \mathbf{J}_l (\phi) & = \mathbf{C} \mathbf{J}_r (\phi) \\ \mathbf{J}_l (-\phi) & = \mathbf{J}_r (\phi) \end{aligned}$

1.2 姿态

与SO3类似，有

对应的雅可比矩阵及其逆矩阵为

如你所见，姿态的雅可比矩阵真的不好求解，不好求解到我都懒得打了...

利用公式

和伴随 $\tau$ 的直接级数表示方法，也可以得到左雅可比的直接级数表示法

和SO3相对应，SE3的左右雅可比有如下性质

$\mathbf{J}_l ( \xi ) = \tau \mathbf{J}_r ( \xi ), \quad \mathbf{J}_l( -\xi) = \mathbf{J}_r (\xi)$

最终，姿态T及其伴随 $\tau$ 可以表示为

2. 微积分和优化

首先定义两个操作符来操作4 * 1的向量：

上两式的结果分别对应4 * 6和6 * 4的矩阵。

这样，就可以有如下恒等式

$\xi^\wedge \mathbf{p} = \mathbf{p}^\odot \xi \quad \mathbf{p}^T \xi^\wedge = \xi^\wedge \mathbf{p}^\circledcirc$

这些操作符和等式会在后面对姿态的操作中见到。

2.1 微积分和优化

我们先讨论下被旋转后的点关于该旋转量该旋转量（李代数的向量空间）的雅可比（求导）

$\frac{\partial \mathbf{C} \mathbf{v} }{ \partial \phi }$

先对 $\phi=(\phi_1, \phi_2, \phi_3)$ 中的某一个量进行微分（方向导数）

$\frac{\partial \mathbf{C} \mathbf{v} }{ \partial \phi_i } = \lim_{h \to 0} \frac{ \exp((\phi + h \mathbf{1}_i)^\wedge)\mathbf{v} - \exp(\phi^\wedge) \mathbf{v} }{h}$

利用近似BCH公式和一阶泰勒展开，有

$\exp ( (\phi + h \mathbf{1}_i)^\wedge ) \approx \exp ( (\mathbf{J}_l h \mathbf{1}_i)^\wedge ) \exp( \phi^\wedge ) \approx (\mathbf{1} + h (\mathbf{J}_l \mathbf{1}_i)^\wedge ) \exp(\phi^\wedge)$

由此，我们自然可以得到

$\frac{\partial \mathbf{C} \mathbf{v} }{ \partial \phi_i } = (\mathbf{J}_l \mathbf{1}_i)^\wedge \mathbf{C} \mathbf{v} = -(\mathbf{C} \mathbf{v})^\wedge \mathbf{J}_l \mathbf{1}_i$

将三个方向的微分结果堆叠在一起，就有

$\frac{\partial \mathbf{C} \mathbf{v} }{ \partial \phi } = -(\mathbf{C} \mathbf{v})^\wedge \mathbf{J}_l$

如果 $\mathbf{C}\mathbf{v}$ 出现在某个标量函数的内部，如u(Cv)，通过链式法则就有

$\frac{\partial u}{\partial \phi} = \frac{\partial u}{\partial x} \frac{\partial x}{\partial \phi} = -\frac{\partial u}{\partial x}(\mathbf{C} \mathbf{v})^\wedge \mathbf{J}_l$

如果要实现梯度下降，直接选择负梯度方向即可：

$\phi = \phi_{op} - \alpha \mathbf{J}^T \underbrace{ (\mathbf{C}_{op} \mathbf{v})^\wedge \frac{\partial u}{\partial x} |^T_{ x = C_{op} v } }_\delta$

其中， $\alpha > 0$ 定义了步长大小。

而该式也可以保证函数u的值是减小的：

$u( \exp(\phi^\wedge) \mathbf{v} ) - u( \exp(\phi_{op}^\wedge) \mathbf{v}) \approx \frac{ \partial u}{\partial \phi} \cdot \alpha \mathbf{J}_l^T \delta = -\alpha \delta^T (\mathbf{J}_l \mathbf{J}^T_l) \delta$

但这种方式有点复杂。一个比较简洁的办法是找到一个关于C的优化步长，该步长再表达为（左乘）微小旋转的形式，直接应用在李群上（而不是在李代数上进行操作）。

$\mathbf{C} = \exp( \psi^\wedge ) \mathbf{C}_{op}$

考虑前面介绍的基于李代数求导的方法和公式，可以有

$\mathbf{C} = \exp ( \phi ^\wedge ) = \exp ( (\phi_{op} - \alpha \mathbf{J}^T_l \delta)^\wedge ) \approx \exp(-\alpha (\mathbf{J}_l \mathbf{J}^T_l \delta)^\wedge ) \mathbf{C}{op}$

对比一下就可以发现，可以让 $\psi = -\alpha \mathbf{J}_l \mathbf{J}^T_l \delta$ 来完成和刚刚相同的事情。

可是该过程仍然要计算雅可比Jl，我们也可以完全丢掉关于雅可比的项，只使用

$\psi = -\alpha \delta$

该过程仍然会使目标函数减小，但方向将变为

$u( \exp(\phi^\wedge) \mathbf{v} ) - u( \exp(\phi_{op}^\wedge) \mathbf{v}) = -\alpha \delta^T \delta$

我们可以进一步考虑一个更加简洁的模型：只计算扰动量施加在左边的时候，关于扰动 $\psi$ 的雅可比矩阵。

仍然是只考虑某个方向的雅可比，有：

$\frac{\partial \mathbf{C} \mathbf{v} }{ \partial \psi_i } = \lim_{h \to 0} \frac{ \exp(h \mathbf{1}i^\wedge) \mathbf{C} \mathbf{v} - \mathbf{C} \mathbf{v} }{h} \approx \lim_{h \to 0} \frac{ (\mathbf{1} + h \mathbf{1}_i^\wedge) \mathbf{C} \mathbf{v} - \mathbf{C} \mathbf{v} }{h} = -(\mathbf{C} \mathbf{v} )^\wedge \mathbf{1}_i$

同样把3个方向的微分堆叠到一起就有

$\frac{\partial \mathbf{C} \mathbf{v} }{ \partial \psi } = -(\mathbf{C} \mathbf{v})^\wedge$

可以发现，这里完全就没有雅可比矩阵了。

所以，对于优化问题，最简单的方式就是跳过所有推导，直接用扰动的方式来思考。

$\mathbf{C} = \exp( \psi^\wedge ) \mathbf{C}_{op}$

当我们将旋转和任意点相乘时，可以表达为如下近似形式：

$\mathbf{C} \mathbf{v} = \exp( \psi^\wedge ) \mathbf{C}_{op} \mathbf{v} \approx \mathbf{C}_{op} \mathbf{v} - ( \mathbf{C}_{op} \mathbf{v} )^\wedge \psi$