問題描述
姿態估計是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} \]