第六篇视觉slam中的优化问题梳理及雅克比推导

优化问题定义以及求解

通用定义

解决问题的开始一定是定义清楚问题。这里引用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}
\]
\[
\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定义及指数映射

\[
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{s0}(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 & v\\ 0^\top & 0\end{bmatrix}\end{matrix}\right|\omega\in\mathbb{R}^3,v\in\mathbb{R}^3,\epsilon=[v,\omega]^\top
\end{Bmatrix}
\]

\[
\begin{aligned}
\text{exp}(\epsilon^\wedge) &= \underbrace{\text{exp}{\begin{bmatrix}\omega^\wedge & v\\ 0^\top & 0\end{bmatrix}}}_{泰勒级数展开} \\
&= \mathbf{I} + \begin{bmatrix}\omega^\wedge & v\\ 0^\top & 0\end{bmatrix} + \frac{1}{2!}\begin{bmatrix}\omega^{\wedge2} & \omega^\wedge v\\ 0^\top & 0\end{bmatrix} + \frac{1}{3!}\begin{bmatrix}\omega^{\wedge3} & \omega^{\wedge2} v\\ 0^\top & 0\end{bmatrix} + \dots \\
&= \begin{bmatrix}\text{exp}(\omega^\wedge) & \mathbf{V}v\\ 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
\]

首先从最简单的位姿优化开始。

位姿优化

已知图像特征点在图像中的坐标集合\(\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)=\mathbf{p}_{I_k} - \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}
\]

参考

Giorgio Grisetti, Rainer Kummerle. g2o: A general Framework for (Hyper) Graph Optimization. 2017
高翔. 视觉SLAM十四讲. 2017
Strasdat H. Local accuracy and global consistency for efficient visual SLAM[D]. Department of Computing, Imperial College London, 2012.
Lie Groups for 2D and 3D Transformations

巴特西

第六篇视觉slam中的优化问题梳理及雅克比推导

优化问题定义以及求解

通用定义

求解最优问题

优化库

视觉SLAM中的优化问题

相机投影模型

立体视觉观测函数

SO3、SE3定义及指数映射

位姿优化

单目误差

双目误差

参考

最新文章

热门文章

巴特西

第六篇 视觉slam中的优化问题梳理及雅克比推导

优化问题定义以及求解

通用定义

求解最优问题

优化库

视觉SLAM中的优化问题

相机投影模型

立体视觉观测函数

SO3、SE3定义及指数映射

位姿优化

单目误差

双目误差

参考

最新文章

热门文章

第六篇视觉slam中的优化问题梳理及雅克比推导