SE(3) 上的李代数求导（推导过程）

SE(3) 上的李代数求导（推导过程）

变量基本形式

$p$ 是一个三维向量 ，其齐次形式为
$$p=\left[\begin{array}{c}p\\ 1\end{array}\right]\text{\hspace{1em}(0)}$$

$T$ 是一个变换矩阵，其形式
$$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(1)}$$
$\xi$ 是一六维向量 ，其形式为
$$\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]$$
${\xi }^{\wedge }$ 是 向量 $\xi$ 的反对称矩阵，其形式
$${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\text{\hspace{1em}(3)}$$
$\exp ({\xi }^{\wedge })$ 是李代数 $\xi$ 的指数映射，其形式为
$$\exp ({\xi }^{\wedge })\left[\begin{array}{cc}\exp ({\varphi }^{\wedge })& J\rho \\ 0^T& 1\end{array}\right]\text{\hspace{1em}(4)}$$

前提假设

假设某空间点 $p$ 经过一次变换 $T$ (对应的李代数为 $\xi$ ) 得到 $Tp=\exp ({\xi }^{\wedge })p$ .

现在给 $T$ 左乘一个扰动$\Delta T=\exp (\delta {\xi }^{\wedge })$ ，设扰动项的李代数为 $\delta \xi =\left[\begin{array}{c}\delta \rho \\ \varphi \end{array}\right]$

公式推导

$$\frac{\partial (Tp)}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }\text{\hspace{1em}(5-1)}$$

$(5-1)$ ：根据导数的定义以及SE(3)的扰动定义(可以将这里的扰动理解为挪动一点点位置，等价于数学上的$+\Delta x$) ，具体形式参考《SLAM十四讲》公式，即下图

图

$$\begin{gathered} \underset{\delta \xi \to 0}{\lim }\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi } \\ =\underset{\delta \xi \to 0}{\lim }\frac{(I+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }\text{\hspace{1em}(5-2)} \end{gathered}$$
$(5-2)$ ：对 $\exp (\delta {\xi }^{\wedge })$ 应用一阶泰勒展开即可，参考$e^x\approx 1+x+o(x^2)$
$$\begin{gathered} \underset{\delta \xi \to 0}{\lim }\frac{(I+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }= \\ \underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })p}{\delta \xi }\text{\hspace{1em}(5-3)} \end{gathered}$$
$(5-3)$ ：略（不解释）
$$\begin{gathered} \underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })p}{\delta \xi } \\ =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]}{\delta \xi } \\ =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{\delta \xi }\text{\hspace{1em}(5-4)} \end{gathered}$$
$(5-4)$ :结合前提假设中的 $Tp=\exp ({\xi }^{\wedge })p$ ，应用矩阵乘法即可得到
$$\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]$$ ，

依据式$(3)$ 即可得到
$\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]$ ，将两矩阵相乘即可
$$\begin{gathered} \underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{\delta \xi } \\ =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta \rho -(Rp+t{)}^{\wedge }\delta \varphi \\ 0\end{array}\right]}{\delta \xi }\text{\hspace{1em}(5-5)} \end{gathered}$$
$(5-5)$ ：将 符号 $\wedge$ 看作 向量的 $\times$ 积符号 ，将$\times$ 积符号前后交换位置后需要进行变号，也即
$A\times B=-B\times A$

用定义法求解向量对向量求导

参考机器学习中的矩阵向量求导(二) 矩阵向量求导之定义法

先来一个简单的例子: $y=Ax$,其中$A$为$n\times m$的矩阵。$x,y$分别为$m,n$维向量。需要求导$\frac{\partial Ax}{\partial x}$ , 根据定义，结果应该是一个 $n\times m$ 的矩阵.

先求矩阵的第 $i$ 行和向量的内积对向量的第 $j$ 分量求导，用定义法求解过程如下：
$$\frac{\partial A_{i}x}{\partial x_j}=\frac{\partial A_{ij}{x}_j}{\partial x_j}=A_{ij}$$
可见矩阵 $A$ 的第 $i$ 行和向量的内积对向量的第 $j$ 分量求导的结果就是矩阵 $A$ 的$(i,j)$位置的值。
$$\begin{gathered} \underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta \rho -(Rp+t{)}^{\wedge }\delta \varphi \\ 0\end{array}\right]}{\delta \xi } \\ =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta \rho -(Rp+t{)}^{\wedge }\delta \varphi \\ 0\end{array}\right]}{\delta \left[\begin{array}{c}\rho \\ \varphi \end{array}\right]} \\ =\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0\end{array}\right]\text{\hspace{1em}(5-6)} \end{gathered}$$
$(5-6)$ 根据上面提到的用 定义法求解向量对向量求导 即可得到结果。

[参考]：

1. 机器学习中的矩阵向量求导(二) 矩阵向量求导之定义法
2. 《SLAM十四讲》

End…