《自动驾驶中的SLAM技术》第2章: 基础数学知识回顾习题

自动驾驶中的SLAM技术

第2章: 基础数学知识回顾

习题 1

分别使用左右扰动模型，计算 $\frac{\partial \mathbf{R}^{-1}\mathbf{p}}{\partial \mathbf{R}}$ 。

左扰动模型
$\begin{align*} \frac{\partial \mathbf{R}^{-1}\mathbf{p}}{\partial \mathbf{R}} &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathrm{Exp}\left( \delta \boldsymbol{\phi} \right) \mathbf{R} \right) ^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathbf{R}^{-1}\mathrm{Exp}\left( -\delta \boldsymbol{\phi} \right) \mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathbf{R}^{-1}\left( \mathbf{I}-\delta \boldsymbol{\phi} ^{\land} \right) \mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\mathbf{R}^{-1}\delta \boldsymbol{\phi} ^{\land}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathbf{R}^{-1}\mathbf{p}^{\land}\delta \boldsymbol{\phi}}{\delta \boldsymbol{\phi}} \\ &= \mathbf{R}^{-1}\mathbf{p}^{\land} \end{align*}$

右扰动模型
$\begin{align*} \frac{\partial \mathbf{R}^{-1}\mathbf{p}}{\partial \mathbf{R}} &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathbf{R}\mathrm{Exp}\left( \delta \boldsymbol{\phi} \right) \right) ^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Exp}\left( -\delta \boldsymbol{\phi} \right) \mathbf{R}^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathbf{I}-\delta \boldsymbol{\phi} ^{\land} \right) \mathbf{R}^{-1}\mathbf{p}-\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\delta \boldsymbol{\phi} ^{\land}\mathbf{R}^{-1}\mathbf{p}}{\delta \boldsymbol{\phi}} \\ &= \underset{\delta \boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\left( \mathbf{R}^{-1}\mathbf{p} \right) ^{\land}\delta \boldsymbol{\phi}}{\delta \boldsymbol{\phi}} \\ &= \left( \mathbf{R}^{-1}\mathbf{p} \right) ^{\land} \end{align*}$

习题 2

分别使用左右扰动模型，计算 $\frac{\partial \mathbf{R}1\mathbf{R}{2}^{-1}}{\partial \mathbf{R}_2}$ 。

注: 不能直接说 $\mathbf{R}_1\mathbf{R}_2$ 对 $\mathbf{R}_1$ 或 $\mathbf{R}_2$ 的导数，那样就变成矩阵对向量求导，无法使用矩阵来描述。在不引入张量的前提下，我们最多只能求向量到向量的导数。因此分子部分必须加上 Log 符号，取为矢量。

左扰动模型
$\begin{align*} \frac{\partial \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\partial \mathbf{R}_2} &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\left( \mathrm{Exp}\left( \boldsymbol{\phi} \right) \mathbf{R}_2 \right) ^{-1} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1}\mathrm{Exp}\left( -\boldsymbol{\phi} \right) \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) +\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \left( -\boldsymbol{\phi} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{^{-1}} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \boldsymbol{\phi}}{\boldsymbol{\phi}} \\ &= -\mathbf{J}_{r}^{-1}\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \end{align*}$

右扰动模型
$\begin{align*} \frac{\partial \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\partial \mathbf{R}_2} &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\left( \mathbf{R}_2\mathrm{Exp}\left( \boldsymbol{\phi} \right) \right) ^{-1} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathrm{Exp}\left( -\boldsymbol{\phi} \right) \mathbf{R}_{2}^{-1} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1}\mathrm{Exp}\left( -\mathbf{R}_2\boldsymbol{\phi} \right) \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) +\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \left( -\mathbf{R}_2\boldsymbol{\phi} \right) -\mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right)}{\boldsymbol{\phi}} \\ &= \underset{\boldsymbol{\phi} \rightarrow 0}{\lim}\frac{-\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \mathbf{R}_2\boldsymbol{\phi}}{\boldsymbol{\phi}} \\ &= -\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{R}_1\mathbf{R}_{2}^{-1} \right) \right) \mathbf{R}_2 \end{align*}$

注:
$\mathrm{Log}\left( \mathbf{A}\mathbf{B} \right) \approx \begin{cases} \mathrm{Log}\left( \mathbf{B} \right) +\mathbf{J}_{l}^{-1}\left( \mathrm{Log}\left( \mathbf{B} \right) \right) \mathrm{Log}\left( \mathbf{A} \right) ,\text{当}\mathbf{A}\approx \mathbf{I} \\ \mathrm{Log}\left( \mathbf{A} \right) +\mathbf{J}_{r}^{-1}\left( \mathrm{Log}\left( \mathbf{A} \right) \right) \mathrm{Log}\left( \mathbf{B} \right) ,\text{当}\mathbf{B}\approx \mathbf{I} \end{cases}$
$\mathrm{Exp}\left( \boldsymbol{\phi } \right) \boldsymbol{R}=\boldsymbol{R}\mathrm{Exp}\left( \boldsymbol{R}^{\top}\boldsymbol{\phi } \right)$