姿态估计中的雅可比求导

问题描述

姿态估计是SLAM中的一个基础问题。基于重投影误差的问题描述一般为求解下列的优化问题

\[\min_{\mathbf{T}}\mathbf{f},\quad \mathbf{f}=\mathbf{e}^T\mathbf{e}=\parallel p'-p \parallel^2 \]

其中\(p\)是观测到的（世界坐标系下的）三维点\(\mathbf{p}\)的图像投影，\(p'\)是计算得到的\(\mathbf{p}\)的图像投影，满足

\[p'=\lambda\mathbf{K}\mathbf{D}\mathbf{T}\mathbf{p} \]

其中，\(p=(u,v,1)^{T}\)，\(p'=(u',v',1)^{T}\)，\(\mathbf{p}=(x,y,z,1)^{T}\)， \(\mathbf{K}\)是相机内参矩阵，\(\mathbf{T}\)是两个不同位姿之间的相对变换矩阵。

\[\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{bmatrix} \]

\[\mathbf{D} = \begin{bmatrix} \mathbf{I_{3\times 3}} & \mathbf{0} \end{bmatrix} \]

\[\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \]

推导

最优化\(\mathbf{f}\)通常会近似为在每步迭代求解一个线性方程组（这块就不细讲了）

\[\mathbf{H}\mathbf{\Delta \mathbf{x}}=-\mathbf{b} \]

其中

\[\mathbf{H}=\mathbf{J}^T\mathbf{J} \]

\[\mathbf{J}=\frac{\partial \mathbf{e(\mathbf{x}\boxplus\Delta \mathbf{x})}}{\partial \Delta \mathbf{x}} \]

\[\mathbf{b} = \mathbf{J}^T\mathbf{e} \]

于是问题的关键转化为雅克比矩阵的计算。令

\[\hat{\mathbf{p}}=\mathbf{T}\mathbf{p}=(\hat{x},\hat{y},\hat{z},1)^T \]

\[\bar{\mathbf{p}}=\lambda\hat{\mathbf{p}}=(\frac{\hat{x}}{\hat{z}},\frac{\hat{y}}{\hat{z}},1,1)^T=(\bar{x},\bar{y},1,1)^T \]

\[p'=\mathbf{K}\mathbf{D}\bar{\mathbf{p}} =(f_x\bar{x}+c_x,f_y\bar{y}+c_y,1)^T \]

那么，

\[\frac{\partial \mathbf{e}}{\partial\delta} =\frac{\partial \mathbf{e}}{\partial p'} \frac{\partial p'}{\partial \bar{\mathbf{p}}} \frac{\partial \bar{\mathbf{p}}}{\partial \hat{\mathbf{p}}} \frac{\partial \hat{\mathbf{p}}}{\partial \delta} \]

我们容易推出以下等式

\[\frac{\partial \mathbf{e}}{\partial p'} = \begin{bmatrix} \mathbf{I}_{2\times2} & \mathbf{0}_{2\times1}\\ \mathbf{0}_{2\times1}^T & 0 \end{bmatrix} \]

\[\frac{\partial p'}{\partial \bar{\mathbf{p}}}= \begin{bmatrix} f_x & 0 & 0 & 0 \\ 0 & f_y & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix} \]

\[\frac{\partial \bar{\mathbf{p}}}{\partial \hat{\mathbf{p}}}= \frac{1}{\hat{z}^2}\begin{bmatrix} \hat{z} & 0 & -\hat{x} & 0\\ 0 & \hat{z} & -\hat{y} & 0 \\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 \end{bmatrix} \]

\[\frac{\partial \hat{\mathbf{p}}}{\partial\delta} =(\mathbf{T}\mathbf{p})^{\odot} =\begin{bmatrix} \mathbf{I}_{3\times 3} & -{[\hat{\mathbf{p}}]_{1:3}}^{\wedge}\\ \mathbf{0}_{3\times1}^T & \mathbf{0}_{3\times1}^T \end{bmatrix} \]

最后一个等式参见State Estimation of Robotics第7章公式(7.179)。
将上面的式子放在一起，便有

\[\frac{\partial \mathbf{e}}{\partial\delta} =\frac{1}{\hat{z}^2} \begin{bmatrix} f_x\hat{z} & 0 & -f_x\hat{x} & -f_x\hat{x}\hat{y} & f_x(\hat{x}^2+\hat{z}^2) & -f_x\hat{y}\hat{z}\\ 0 & f_y\hat{z} & -f_y\hat{y} & -f_y(\hat{y}^2+\hat{z}^2) & f_y\hat{x}\hat{y} & f_y\hat{x}\hat{z}\\ & \mathbf{0}_{3\times 1}^T & & & \mathbf{0}_{3\times 1}^T & \end{bmatrix} \]