優化問題定義以及求解
通用定義
解決問題的開始一定是定義清楚問題。這里引用g2o的定義。
\[\begin{aligned} \mathbf{F}(\mathbf{x})&=\sum_{k\in \mathcal{C}} \underbrace{\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k)^\top \Omega_k\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k)}_{\mathbf{F}_k} \\ \mathbf{x}^* &= \underset{\mathbf{x}}{\operatorname{argmin}}\mathbf{F}(\mathbf{x}) \end{aligned} \tag{1} \]
- \(\mathbf{x}=(\mathbf{x}_1^\top,\dots,\mathbf{x}_n^\top)^\top\)，\(\mathbf{x}_i\in \mathbf{x}\)為向量，表示一組參數；
- \(\mathbf{x}_k=(\mathbf{x}_{k_1}^\top,\dots,\mathbf{x}_{k_q}^\top)^\top\subset \mathbf{x}\)，第k次約束參數子集；
- \(\mathbf{z}_k\)可以當做觀測向量，\(\Omega_k\)可以認為是觀測協方差矩陣，是個對稱矩陣；
- \(\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k)\)是誤差函數；
\(\mathbf{F}(\mathbf{x})\)其實就是總測量誤差的平方和，這里簡單起見假設\(\Omega_k=\begin{bmatrix}\sigma_1^2&0 \\ 0 & \sigma_2^2\end{bmatrix}\)，
可以把\(\mathbf{F}_k(\mathbf{x})\)當做單次測量誤差平方和，假設\(\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k)=(e_1,e_2)^\top\)，展開看
\[\begin{aligned} \mathbf{F}_k(\mathbf{x})&=\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k)^\top \Omega_k\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k) \\ &=\sigma_1^2e_1^2+\sigma_2^2e_2^2 \end{aligned} \]
問題就是求使得測量誤差平方和最小的參數的值。
求解最優問題
簡化誤差方程定義:\(\mathbf{e}_k(\mathbf{x}_k,\mathbf{z}_k) \overset{def.}{=} \mathbf{e}_k(\mathbf{x}_k) \overset{def.}{=} \mathbf{e}_k(\mathbf{x})\)。誤差方程在值\(\breve{\mathbf{x}}\)處進行一階泰勒級數近似展開：
\[\begin{aligned} \mathbf{e}_k(\breve{\mathbf{x}}_k+\Delta\mathbf{x}_k) &=\mathbf{e}_k(\breve{\mathbf{x}}+\Delta\mathbf{x}) \\ &\simeq \mathbf{e}_k(\breve{\mathbf{x}})+\mathbf{J}_k\Delta\mathbf{x} \end{aligned} \tag{2} \]
其中\(\mathbf{J}_k\)是\(\mathbf{e}_k(\mathbf{x})\)在\(\breve{\mathbf{x}}\)處的雅克比矩陣，代入(1)中得：
\[\begin{aligned} \mathbf{F}_k(\breve{\mathbf{x}}+\Delta\mathbf{x}) &= \mathbf{e}_k(\breve{\mathbf{x}}+\Delta\mathbf{x})^\top\Omega_k\mathbf{e}_k(\breve{\mathbf{x}}+\Delta\mathbf{x}) \\ &\simeq (\mathbf{e}_k(\breve{\mathbf{x}})+\mathbf{J}_k\Delta\mathbf{x})^\top\Omega_k(\mathbf{e}_k(\breve{\mathbf{x}})+\mathbf{J}_k\Delta\mathbf{x}) \\ &=\underbrace{(\mathbf{e}_k(\breve{\mathbf{x}})^\top+(\mathbf{J}_k\Delta\mathbf{x})^\top)}_{A^\top+B^\top = (A+B)^\top}\Omega_k(\mathbf{e}_k(\breve{\mathbf{x}})+\mathbf{J}_k\Delta\mathbf{x}) \\ &= \mathbf{e}_k(\breve{\mathbf{x}})^\top\Omega_k\mathbf{e}_k(\breve{\mathbf{x}})+\underbrace{\mathbf{e}_k(\breve{\mathbf{x}})^\top\Omega_k\mathbf{J}_k\Delta\mathbf{x}+(\mathbf{J}_k\Delta\mathbf{x})^\top\Omega_k\mathbf{e}_k(\breve{\mathbf{x}})}_{當A^TB為標量時，A^TB=B^TA}+\Delta\mathbf{x}^\top\mathbf{J}_k^\top\Omega_k\mathbf{J}_k\Delta\mathbf{x} \\ &=\underbrace{\mathbf{e}_k(\breve{\mathbf{x}})^\top\Omega_k\mathbf{e}_k(\breve{\mathbf{x}})}_{標量c_k}+2\underbrace{\mathbf{e}_k(\breve{\mathbf{x}})^\top\Omega_k\mathbf{J}_k}_{向量\mathbf{b}_k^\top}\Delta\mathbf{x}+\Delta\mathbf{x}^\top\underbrace{\mathbf{J}_k^\top\Omega_k\mathbf{J}_k}_{矩陣\mathbf{H}_k}\Delta\mathbf{x} \\ &=c_k+2\mathbf{b}_k^\top\Delta\mathbf{x}+\Delta\mathbf{x}^\top\mathbf{H}_k\Delta\mathbf{x} \end{aligned} \tag{3} \]
因此
\[\begin{aligned} \mathbf{F}(\breve{\mathbf{x}}+\Delta\mathbf{x}) &=\sum_{k\in \mathcal{C}} \mathbf{F}_k(\breve{\mathbf{x}}+\Delta\mathbf{x}) \\ &\simeq \sum_{k\in \mathit{C}} c_k+2\mathbf{b}_k\Delta\mathbf{x}+\Delta\mathbf{x}^\top\mathbf{H}_k\Delta\mathbf{x} \\ &= c+2\mathbf{b}^\top\Delta\mathbf{x}+\Delta\mathbf{x}^\top\mathbf{H}\Delta\mathbf{x} \end{aligned} \tag{4} \]
問題轉化為求(4)的最小值，求標量\(\mathbf{F}(\breve{\mathbf{x}}+\Delta\mathbf{x})\)的微分
\[\begin{aligned} d\mathbf{F}(\breve{\mathbf{x}}+\Delta\mathbf{x}) &= 2\mathbf{b}^\top d(\Delta\mathbf{x}) + \underbrace{d(\Delta\mathbf{x}^\top)\mathbf{H}\Delta\mathbf{x}}_{d(X^T) = (dX)^T}+\Delta\mathbf{x}^\top\mathbf{H}d(\Delta\mathbf{x}) \\ &= 2\mathbf{b}^\top d(\Delta\mathbf{x}) + \underbrace{(d(\Delta\mathbf{x}))^\top\mathbf{H}\Delta\mathbf{x}}_{當A^TB為標量時，A^TB=B^TA} + \Delta\mathbf{x}^\top\mathbf{H}d(\Delta\mathbf{x}) \\ &= 2\mathbf{b}^\top d(\Delta\mathbf{x}) + \underbrace{\Delta\mathbf{x}^\top\mathbf{H}^\top d(\Delta\mathbf{x}) + \Delta\mathbf{x}^\top\mathbf{H}d(\Delta\mathbf{x})}_{\Omega_k為對稱陣，因此H為對稱陣} \\ &= 2(\mathbf{b}^\top + \Delta\mathbf{x}^\top\mathbf{H}^\top)d(\Delta\mathbf{x}) \\ &= 2(\mathbf{b} + \mathbf{H}\Delta\mathbf{x})^\top d(\Delta\mathbf{x}) \end{aligned} \]
對照\(d\mathbf{F}=\frac{\partial \mathbf{F}}{\partial \Delta\mathbf{x}}^Td(\Delta\mathbf{x})\)，得\(\frac{\partial \mathbf{F}}{\partial \Delta\mathbf{x}}=\mathbf{b} + \mathbf{H}\Delta\mathbf{x}\)
求\(\frac{\partial \mathbf{F}}{\partial \Delta\mathbf{x}}=0\)，注意因為\(\mathbf{F}\)非負，所以極值處為極小值。
問題又轉為求解線性方程 \(\mathbf{H}\Delta\mathbf{x} = -\mathbf{b}\)，所得到的解為\(\Delta\mathbf{x}^*\)，增量更新\(\mathbf{x}^*=\breve{\mathbf{x}}+\Delta\mathbf{x}^*\)。以次方式不斷迭代求最優問題。
優化庫
在實際的工程中，我們會使用優化庫求解這些優化問題。在使用這些優化庫的時候，我們只需要定義好誤差函數\(\mathbf{e}_k\)計算誤差，誤差函數在某值處的雅克比矩陣\(\mathbf{J}_k\)，定義好觀測的協方差矩陣\(\Omega_k\)，優化庫便可以幫我們求解最優問題。優化庫有很多種，Ceres，g2o，gtsam等，Ceres自身有自動求導甚至不需要我們計算雅克比矩陣，但是搞清楚他們的優化原理還是很有必要的。
視覺SLAM中的優化問題
相機投影模型
已知相機內參\(\mathbf{K}=\begin{bmatrix}f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{bmatrix}\)，相機坐標系下空間點\(\mathbf{p}_{c}=[x_c,y_c,z_c]^\top\in \mathbb{R}^3\)投影到像平面點\(\mathbf{p}_{I}=[u,v]^\top\in \mathbb{R}^2\)的函數為：
\[\begin{aligned} \text{proj}(\mathbf{p}_{c})&=[\frac{1}{z_c}\mathbf{K}\mathbf{p}_{c}]_{1:2} \\ &= \begin{bmatrix}f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix}x_c/z_c \\ y_c/z_c \\ 1 \end{bmatrix}_{1:2} \\ &= \begin{bmatrix}f_x*x_c/z_c+c_x \\ f_y*y_c/z_c+c_y \end{bmatrix} \end{aligned} \]
\[\begin{aligned} \frac{\partial \text{proj}(\mathbf{p}_{c})}{\partial \mathbf{p}_{c}}&= \begin{bmatrix}\frac{\partial u}{\partial x_c} & \frac{\partial u}{\partial y_c} & \frac{\partial u}{\partial z_c} \\ \frac{\partial v}{\partial x_c} & \frac{\partial v}{\partial y_c} & \frac{\partial v}{\partial z_c} \end{bmatrix}\\ &= \begin{bmatrix}f_x/z_c & 0 & -f_x*x_c/z_c^2 \\ 0 & f_y/z_c & -f_y*y_c/z_c^2 \end{bmatrix} \end{aligned} \tag{5} \]
立體視覺觀測函數
假設雙目相機的基線為\(b\)，相機坐標系下空間點\(\mathbf{p}_{c}=[x_c,y_c,z_c]^\top\in \mathbb{R}^3\)投影到左右相機平面的坐標為\([u_l,v_l]^\top,[u_r,v_r]^\top\)，假設是水平雙目，則有\(u_l-u_r=\frac{bf_x}{z_c}\)，那么
\[u_r=u_l-\frac{bf_x}{z_c}=f_x*x_c/z_c+c_x - \frac{bf_x}{z_c} \]
對\(u_r\)(為\(\mathbf{p}_c\)的函數)求導：
\[\begin{aligned} \frac{\partial u_r}{\partial \mathbf{p}_{c}} &= \begin{bmatrix}\frac{\partial u_r}{\partial x_c} & \frac{\partial u_r}{\partial y_c} & \frac{\partial u_r}{\partial z_c} \end{bmatrix} \\ &= \begin{bmatrix}f_x/z_c & 0 & -f_x*(x_c-b)/z_c^2\end{bmatrix} \end{aligned} \]
與相機投影模型整合起來有
\[\begin{aligned} \mathbf{z}_{stereo}&=\binom{\text{proj}(\mathbf{p}_{c})}{u_r} \\ &= \begin{bmatrix}f_x*x_c/z_c+c_x \\ f_y*y_c/z_c+c_y \\ f_x*x_c/z_c+c_x - \frac{bf_x}{z_c} \end{bmatrix} \end{aligned} \]
\[\frac{\partial \mathbf{z}_{stereo}}{\partial \mathbf{p}_{c}} = \begin{bmatrix}f_x/z_c & 0 & -f_x*x_c/z_c^2 \\ 0 & f_y/z_c & -f_y*y_c/z_c^2 \\ f_x/z_c & 0 & -f_x*(x_c-b)/z_c^2\end{bmatrix} \tag{6} \]
SO3、SE3、SIM3定義及指數映射
\[SO(3) = \begin{Bmatrix} \mathbf{R}\in\mathbb{R}^{3\times 3}|\mathbf{R}\mathbf{R}^\top=\mathbf{I},\text{det}(\mathbf{R})=1 \end{Bmatrix} \]
\[\mathfrak{so}(3) = \begin{Bmatrix} \omega^\wedge=\left.\begin{matrix}\begin{bmatrix}0 & -\omega_3 & \omega_2\\\omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0\end{bmatrix}\end{matrix}\right|\omega=[\omega_1,\omega_2,\omega_3]^\top\in\mathbb{R}^3 \end{Bmatrix} \]
\(\text{exp}(\omega^\wedge)\in SO(3)\)，證明見羅德里格斯公式。
\[SE(3) = \begin{Bmatrix} \mathbf{T}=\begin{bmatrix}\mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1\end{bmatrix}\in\mathbb{R}^{4\times 4}|\mathbf{R}\in SO(3),\mathbf{t}\in\mathbb{R}^3 \end{Bmatrix} \]
\[\mathfrak{se}(3) = \begin{Bmatrix} \epsilon^\wedge=\left.\begin{matrix}\begin{bmatrix}\omega^\wedge & \nu\\ 0^\top & 0\end{bmatrix}\end{matrix}\right|\omega\in\mathbb{R}^3,\nu\in\mathbb{R}^3,\epsilon=[\nu,\omega]^\top \end{Bmatrix} \]
\[\begin{aligned} \text{exp}(\epsilon^\wedge) &= \underbrace{\text{exp}{\begin{bmatrix}\omega^\wedge & \nu\\ 0^\top & 0\end{bmatrix}}}_{泰勒級數展開} \\ &= \mathbf{I} + \begin{bmatrix}\omega^\wedge & \nu\\ 0^\top & 0\end{bmatrix} + \frac{1}{2!}\begin{bmatrix}\omega^{\wedge2} & \omega^\wedge \nu\\ 0^\top & 0\end{bmatrix} + \frac{1}{3!}\begin{bmatrix}\omega^{\wedge3} & \omega^{\wedge2} \nu\\ 0^\top & 0\end{bmatrix} + \dots \\ &= \begin{bmatrix}\text{exp}(\omega^\wedge) & \mathbf{V}\nu\\ 0^\top & 0\end{bmatrix} \in SE(3) ,\mathbf{V}=\mathbf{I}+\frac{1}{2!}\omega^{\wedge} + \frac{1}{3!}\omega^{\wedge2} + \dots \end{aligned} \]
實際上
\[\mathbf{V} = \left\{\begin{matrix} \mathbf{I}+\frac{1}{2}\omega^{\wedge}+\frac{1}{6}\omega^{\wedge2} = \mathbf{I}, & \theta \rightarrow 0 \\ \mathbf{I}+\frac{1-cos(\theta)}{\theta^2}\omega^{\wedge}+\frac{\theta-sin(\theta)}{\theta^3}\omega^{\wedge2}, & else \end{matrix}\right. \: \: \: with \:\:\theta=\left\|\omega\right\|_2 \]
\[Sim(3) = \begin{Bmatrix} \mathbf{S}=\begin{bmatrix}s\mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1\end{bmatrix}\in\mathbb{R}^{4\times 4}|s\mathbf{R}\in \mathbb{R}^+(3)\times SO(3),\mathbf{t}\in\mathbb{R}^3 \end{Bmatrix} \]
\[\mathfrak{sim}(3) = \begin{Bmatrix} \psi^\wedge=\left.\begin{matrix}\begin{bmatrix}\omega^\wedge+\rho\mathbf{I} & \nu\\ 0^\top & 0\end{bmatrix}\end{matrix}\right|\omega\in\mathbb{R}^3,\nu\in\mathbb{R}^3,\rho \in \mathbb{R}, \psi=[\nu,\omega,\rho]^\top \end{Bmatrix} \]
\[\begin{aligned} \text{exp}(\psi^\wedge) &= \text{exp}(\begin{bmatrix}\omega^\wedge+\rho\mathbf{I} & \nu\\ 0^\top & 0\end{bmatrix}) \\ &= \begin{bmatrix}e^\rho\text{exp}(\omega^\wedge) & W\nu\\ 0^\top & 0\end{bmatrix}\in Sim(3) \end{aligned} \]
具體的證明可以參考文獻[3]。
首先從最簡單的位姿優化開始。
位姿優化
已知圖像特征點在圖像中的坐標集合\(\mathcal{P}_I=\left\{\mathbf{p}_{I_1}, \mathbf{p}_{I_2}, \ldots, \mathbf{p}_{I_n}\right\},\mathbf{p}_{I_i}\in \mathbb{R}^2\)， 以及對應的空間坐標\(\mathcal{P}_w=\left\{\mathbf{p}_{w_1}, \mathbf{p}_{w_2}, \ldots, \mathbf{p}_{w_n}\right\},\mathbf{p}_{w_i}\in \mathbb{R}^3\)，求解世界坐標系到相機的變換矩陣\(\mathbf{T}_{cw}^*=\begin{bmatrix} \mathbf{R}_{cw}^* & \mathbf{t}_{cw}^* \\ 0^\top & 1 \end{bmatrix}\)的最優值。
誤差函數
假設變換矩陣的初始值為\(\mathbf{T}_{cw}=\begin{bmatrix} \mathbf{R}_{cw} & \mathbf{t}_{cw} \\ 0^\top & 1 \end{bmatrix}=\text{exp}(\xi_0^\wedge ),\xi^\wedge_0\in{\mathfrak{se}(3)}\)，加在該初值的左擾動為\(\text{exp}(\epsilon^\wedge )\)。
單目誤差
\[\mathbf{e}_k(\xi)=\mathbf{p}_{I_k} - \text{proj}(\text{exp}(\xi^\wedge )\cdot\mathbf{p}_{w_k}) \]
\[\begin{aligned} \mathbf{J}_k=\frac{\partial \mathbf{e}_k}{\partial \epsilon} = -\frac{\partial \text{proj}(\mathbf{p}_{c})}{\partial \mathbf{p}_{c}}\cdot \left.\begin{matrix} \frac{\partial \text{exp}(\epsilon^\wedge )\text{exp}(\xi^\wedge )\cdot\mathbf{p}_{w_k}}{\partial \epsilon}\end{matrix}\right|_{\epsilon=0} \end{aligned} \]
\[\begin{aligned} \left.\begin{matrix} \frac{\partial \text{exp}(\epsilon^\wedge )\text{exp}(\xi^\wedge )\cdot\mathbf{p}_{w_k}}{\partial \epsilon} \end{matrix}\right|_{\xi=\xi_0, \epsilon=0} &\approx \left.\begin{matrix}\frac{\partial\underbrace{(I+\epsilon^\wedge )}_{泰勒展開近似}\text{exp}(\xi_0^\wedge )\cdot\mathbf{p}_{w_k}}{\partial \epsilon}\end{matrix}\right|_{\xi=\xi_0, \epsilon=0} \\ &=\left.\begin{matrix}\frac{\partial\epsilon^\wedge \text{exp}(\xi_0^\wedge )\cdot\mathbf{p}_{w_k}}{\partial \epsilon}\end{matrix}\right|_{\xi=\xi_0, \epsilon=0} \\ &=\left.\begin{matrix}\frac{\partial \begin{bmatrix}\omega^\wedge & v \\ 0^\top & 0 \end{bmatrix}\begin{bmatrix}\underbrace{\mathbf{R}_{cw}*\mathbf{p}_{w_k}+\mathbf{t}_{cw}}_{\mathbf{p}_c} \\ 1\end{bmatrix}}{\partial \epsilon}\end{matrix}\right|_{\xi=\xi_0, \epsilon=0} \\ &=\left.\begin{matrix}\frac{\partial \begin{bmatrix}\omega^\wedge\mathbf{p}_c+v \end{bmatrix}_{3\times 1}}{\partial \epsilon}\end{matrix}\right|_{\epsilon=0} \\ &=\left.\begin{matrix}\frac{\partial \begin{bmatrix}-\mathbf{p}_c^\wedge\omega+v \end{bmatrix}_{3\times 1}}{\partial \epsilon}\end{matrix}\right|_{\epsilon=0} \\ &=\left.\begin{matrix}\frac{\partial -\begin{bmatrix}0 & -z_c & y_c \\ z_c & 0 & -x_c \\ -y_c & x_c & 0 \end{bmatrix}\begin{bmatrix}\omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix}+\begin{bmatrix}v_1 \\ v_2 \\ v_3 \end{bmatrix}}{\partial \epsilon}\end{matrix}\right|_{\epsilon=0} \\ &=\left.\begin{matrix}\frac{\partial \begin{bmatrix}z_c*\omega_2-y_c*\omega_3+v_1 \\ -z_c*\omega_1+x_c*\omega_3+v_2 \\ y_c*\omega_1-x_c*\omega_2+v_3 \end{bmatrix}}{\partial \epsilon}\end{matrix}\right|_{\epsilon=0} \\ &= \begin{bmatrix}\mathbf{I}_{3\times 3} & -\mathbf{p}_c^\wedge\end{bmatrix} \end{aligned} \]
結合(5)有
\[\begin{aligned} \mathbf{J}_k=-\begin{bmatrix}f_x/z_c & 0 & -f_x*x_c/z_c^2 \\ 0 & f_y/z_c & -f_y*y_c/z_c^2 \end{bmatrix} \cdot \begin{bmatrix}\mathbf{I}_{3\times 3} & -\mathbf{p}_c^\wedge\end{bmatrix} \end{aligned} \]
雙目誤差
\[\mathbf{e}_k(\xi)=\begin{bmatrix}\mathbf{p}_{I_k} \\ u_r\end{bmatrix} - \mathbf{z}_{stereo}(\text{exp}(\xi^\wedge )\cdot\mathbf{p}_{w_k}) \]
\[\begin{aligned} \mathbf{J}_k=\frac{\partial \mathbf{e}_k}{\partial \epsilon} &= -\frac{\partial \mathbf{z}_{stereo}(\mathbf{p}_{c})}{\partial \mathbf{p}_{c}}\cdot \left.\begin{matrix} \frac{\partial \text{exp}(\epsilon^\wedge )\text{exp}(\xi^\wedge )\cdot\mathbf{p}_{w_k}}{\partial \epsilon}\end{matrix}\right|_{\epsilon=0} \\ &= -\begin{bmatrix}f_x/z_c & 0 & -f_x*x_c/z_c^2 \\ 0 & f_y/z_c & -f_y*y_c/z_c^2 \\ f_x/z_c & 0 & -f_x*(x_c-b)/z_c^2\end{bmatrix}\cdot \begin{bmatrix}\mathbf{I}_{3\times 3} & -\mathbf{p}_c^\wedge\end{bmatrix} \end{aligned} \]
BA
BA問題除了需要優化位姿還需要優化空間點坐標。位姿優化和上節內容一樣，現在來看下空間點坐標優化相關的內容。
誤差函數同樣是計算重投影誤差：
\[\mathbf{e}_k(\xi)=\mathbf{p}_{I_k} - \text{proj}(\mathbf{T}_{cw}\cdot\mathbf{p}_{w_k}) \]
計算關於空間坐標點的導數
\[\begin{aligned} \mathbf{J}_k=\frac{\partial \mathbf{e}_k}{\partial \mathbf{p}_w} &= -\frac{\partial \mathbf{z}_{stereo}(\mathbf{p}_{c})}{\partial \mathbf{p}_c}\cdot \left.\begin{matrix} \frac{\partial (\mathbf{R}_{cw}\cdot\mathbf{p}_{w}+\mathbf{t}_{cw})}{\partial \mathbf{p}_{w}}\end{matrix}\right|_{\mathbf{p}_{w}=\mathbf{p}_{w_k}} \\ &=-\begin{bmatrix}f_x/z_c & 0 & -f_x*x_c/z_c^2 \\ 0 & f_y/z_c & -f_y*y_c/z_c^2 \end{bmatrix}\cdot \mathbf{R}_{cw} \end{aligned} \]
與位姿優化一樣，雙目的時候：
\[\begin{aligned} \mathbf{J}_k=\frac{\partial \mathbf{e}_k}{\partial \mathbf{p}_w} &= -\frac{\partial \text{proj}(\mathbf{p}_{c})}{\partial \mathbf{p}_c}\cdot \left.\begin{matrix} \frac{\partial (\mathbf{R}_{cw}\cdot\mathbf{p}_{w}+\mathbf{t}_{cw})}{\partial \mathbf{p}_{w}}\end{matrix}\right|_{\mathbf{p}_{w}=\mathbf{p}_{w_k}} \\ &=-\begin{bmatrix}f_x/z_c & 0 & -f_x*x_c/z_c^2 \\ 0 & f_y/z_c & -f_y*y_c/z_c^2 \\ f_x/z_c & 0 & -f_x*(x_c-b)/z_c^2\end{bmatrix}\cdot \mathbf{R}_{cw} \end{aligned} \]
回環優化
當我們檢測到回環，假設這兩個幀分別為\(kf_i,kf_j\)，顯然兩幀各自的變換矩陣已知\(T_{iw},T_{jw}\)，利用兩幀中共有的特征點我們可以初步評估出這兩幀的相對變換矩陣\(T_{ji}\)，將這兩幀的相對變換矩陣作為待優化變量，\(T_{ji}\)作為待優化的變量的觀測值（初始值）。假設系統的位姿十分精確，在檢測到回環后，既定的事實是\(T_{ji}*T_{iw}*T_{jw}^{-1}=\mathbf{I}\)。顯然現實中SLAM系統在經過長時間的運行后，一定會出現誤差，那么我們優化的目標就是通過調整絕對位姿\(T_{iw},T_{jw}\)使得\(T_{ji}*T_{iw}*T_{jw}^{-1}=\mathbf{I}\)成立。誤差函數定義為兩個位姿在其切空間的殘差\(\mathbf{e}=\text{log}(T_{ji}*T_{iw}*T_{jw}^{-1})^\vee\)，如果沒有尺度漂移，比如說雙目SLAM系統，\(\mathbf{e}\in \mathfrak{se}(3)\)。假設是單目SLAM系統，存在尺度漂移，則\(\mathbf{e}=\text{log}(S_{ji}*S_{iw}*S_{jw}^{-1})^\vee\in \mathfrak{sim}(3)\)。通過求解優化問題得到了優化后的絕對位姿\(T^*_{iw},T^*_{jw}\)（或者\(S^*_{iw},S^*_{jw}\)），還需要調整空間點。假設\(p_{i}\in \mathbb{R}^3\)是變換矩陣為\(T_{iw}\)的幀中的點，對應的世界坐標點的點\(p_{w_i}=T^{-1}_{iw}p_i\)，\(p^*_i=S^*_{iw}p_{w_i}\)是校正后的幀中的點，其對應的世界坐標系值為\(p^*_{w_i}=S^{-1}_{iw}p^*_i\)。
重點來了，如何求\(\mathbf{e}\)的雅克比矩陣呢？
\[\begin{aligned} \mathbf{J}_k &= \frac{\partial }{\partial \epsilon}\text{log}(T_{ji}\left.\begin{matrix} \text{exp}(\epsilon^\wedge)T_{iw}T_{jw}^{-1})^\vee\end{matrix}\right|_{\epsilon=0} \end{aligned} \]
具體的結果及推導過程請看文獻[3]。
至此SLAM相關的優化理論都已經梳理清楚，下回我們來分析openvslam中具體實現過程。
參考
[1] Giorgio Grisetti, Rainer Kummerle. g2o: A general Framework for (Hyper) Graph Optimization. 2017.
[2] 高翔. 視覺SLAM十四講. 2017.
[3] Strasdat H. Local accuracy and global consistency for efficient visual SLAM[D]. Department of Computing, Imperial College London, 2012.
[4] Ethan Eade. Lie Groups for 2D and 3D Transformations.
