为什么在SLAM中使用链式求导？为什么对微小扰动进行求导？

为什么在SLAM中使用链式求导？为什么对微小扰动进行求导？

链式求导是一个很常用、很简单的模型，但是为什么用这个模型呢？在SLAM十四讲中并没有对这个问题进行阐述。

这与非线性优化的方法有关，首先对于一个最小二乘问题：
$$\underset{x}{\min }\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2$$
高斯牛顿法是处理这个问题最简单的方法，它的做法是将f(x)进行泰勒展开：
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}$$
然后开始求J最小时，delta_x的值。首先进行展开：
$$\begin{array}{rl}\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}{\Vert }^2& =\frac{1}{2}(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}{)}^T(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x})\\ & =\frac{1}{2}\left(\Vert f(\mathbf{x}){\Vert }_2^2+2f(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x})\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\mathbf{J}(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\right)\end{array}$$
然后对delta_x进行求偏导，并令其为0：
$$\mathbf{J}(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x})\Delta \mathbf{x}=-\mathbf{J}(\mathbf{x}{)}^{T}f(\mathbf{x})$$
这样得到的delta_x可以让惩罚函数达到最小。这就让函数从一个寻找最优x的问题转换成为寻找最优delta_x问题，而让x成为当前迭代的初始条件，这就是非线性优化、最小二乘方法的核心最底层的逻辑。

在这个过程中对x求导的过程变成了对delta_x求导数，这种方法对我们理解SLAM中的求导是十分有用的。

因此在PNP中，同样对微小变量delta_zeta进行求导。
$$\frac{\partial e}{\partial \delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{e(\delta \mathbf{\xi }\oplus \mathbf{\xi })}{\delta \mathbf{\xi }}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}$$
这里的 ⊕ 指李代数上的左乘扰动。第二项为变换后的点关于李代数的导数，
$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}=\left[\mathbf{I},-{\mathbf{P}}^{\prime \wedge }\right]$$

推导参考。

$$\begin{array}{rl}& \frac{\partial {\tilde{\mathbf{P}}}^{\prime }}{\partial \mathbf{\xi }}=\frac{\partial (\mathbf{T}\cdot \tilde{\mathbf{P}})}{\partial \mathbf{\xi }}\\ & =\frac{\partial \left(\exp \left({\mathbf{\xi }}^{\wedge }\right)\tilde{\mathbf{P}}\right)}{\partial \delta \mathbf{\xi }}(\text{ 左扰动模型 })\end{array}$$

$$\begin{array}{l}={\lim }_{\delta \xi \to 0}\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\tilde{\mathbf{P}}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\tilde{\mathbf{P}}}{\delta \mathbf{\xi }}\\ \approx {\lim }_{\delta \xi \to 0}\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\tilde{\mathbf{P}}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\tilde{\mathbf{P}}}{\delta \mathbf{\xi }}\end{array}$$

$$\begin{array}{l}={\lim }_{\delta \xi \to 0}\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\tilde{\mathbf{P}}}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \xi \to 0}\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\cdot \mathbf{P}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ ={\lim }_{\delta \xi \to 0}\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}\cdot \mathbf{P}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{\delta \mathbf{\xi }}\end{array}$$

$$\begin{array}{l}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\cdot \mathbf{P}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\\ =\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]\end{array}$$

这里面用到了左乘扰动模型去寻找流形空间中的优化，实际上这个可以理解成为旋转矩阵与旋转矢量的关系。

因此，通过链式求导把这个问题给解决了，与非线性优化的处理是同一个思路，只不过用P’对delta_zeta求导正好有现成的数学工具可以用。