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

2019 年 11 月 10 日
笔记

优化问题定义以及求解

通用定义

解决问题的开始一定是定义清楚问题。这里引用g2o的定义。

[ begin{aligned} mathbf{F}(mathbf{x})&=sum_{kin mathcal{C}} underbrace{mathbf{e}_k(mathbf{x}_k,mathbf{z}_k)^top Omega_kmathbf{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}_iin mathbf{x})为向量，表示一组参数；
(mathbf{x}_k=(mathbf{x}_{k_1}^top,dots,mathbf{x}_{k_q}^top)^topsubset 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^2end{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_kmathbf{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+Deltamathbf{x}_k) &=mathbf{e}_k(breve{mathbf{x}}+Deltamathbf{x}) \ &simeq mathbf{e}_k(breve{mathbf{x}})+mathbf{J}_kDeltamathbf{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}}+Deltamathbf{x}) &= mathbf{e}_k(breve{mathbf{x}}+Deltamathbf{x})^topOmega_kmathbf{e}_k(breve{mathbf{x}}+Deltamathbf{x}) \ &simeq (mathbf{e}_k(breve{mathbf{x}})+mathbf{J}_kDeltamathbf{x})^topOmega_k(mathbf{e}_k(breve{mathbf{x}})+mathbf{J}_kDeltamathbf{x}) \ &=underbrace{(mathbf{e}_k(breve{mathbf{x}})^top+(mathbf{J}_kDeltamathbf{x})^top)}_{A^top+B^top = (A+B)^top}Omega_k(mathbf{e}_k(breve{mathbf{x}})+mathbf{J}_kDeltamathbf{x}) \ &= mathbf{e}_k(breve{mathbf{x}})^topOmega_kmathbf{e}_k(breve{mathbf{x}})+underbrace{mathbf{e}_k(breve{mathbf{x}})^topOmega_kmathbf{J}_kDeltamathbf{x}+(mathbf{J}_kDeltamathbf{x})^topOmega_kmathbf{e}_k(breve{mathbf{x}})}_{当A^TB为标量时，A^TB=B^TA}+Deltamathbf{x}^topmathbf{J}_k^topOmega_kmathbf{J}_kDeltamathbf{x} \ &=underbrace{mathbf{e}_k(breve{mathbf{x}})^topOmega_kmathbf{e}_k(breve{mathbf{x}})}_{标量c_k}+2underbrace{mathbf{e}_k(breve{mathbf{x}})^topOmega_kmathbf{J}_k}_{向量mathbf{b}_k^top}Deltamathbf{x}+Deltamathbf{x}^topunderbrace{mathbf{J}_k^topOmega_kmathbf{J}_k}_{矩阵mathbf{H}_k}Deltamathbf{x} \ &=c_k+2mathbf{b}_k^topDeltamathbf{x}+Deltamathbf{x}^topmathbf{H}_kDeltamathbf{x} end{aligned} tag{3} ]
因此

[ begin{aligned} mathbf{F}(breve{mathbf{x}}+Deltamathbf{x}) &=sum_{kin mathcal{C}} mathbf{F}_k(breve{mathbf{x}}+Deltamathbf{x}) \ &simeq sum_{kin mathit{C}} c_k+2mathbf{b}_kDeltamathbf{x}+Deltamathbf{x}^topmathbf{H}_kDeltamathbf{x} \ &= c+2mathbf{b}^topDeltamathbf{x}+Deltamathbf{x}^topmathbf{H}Deltamathbf{x} end{aligned} tag{4} ]
问题转化为求(4)的最小值，求标量(mathbf{F}(breve{mathbf{x}}+Deltamathbf{x}))的微分

[ begin{aligned} dmathbf{F}(breve{mathbf{x}}+Deltamathbf{x}) &= 2mathbf{b}^top d(Deltamathbf{x}) + underbrace{d(Deltamathbf{x}^top)mathbf{H}Deltamathbf{x}}_{d(X^T) = (dX)^T}+Deltamathbf{x}^topmathbf{H}d(Deltamathbf{x}) \ &= 2mathbf{b}^top d(Deltamathbf{x}) + underbrace{(d(Deltamathbf{x}))^topmathbf{H}Deltamathbf{x}}_{当A^TB为标量时，A^TB=B^TA} + Deltamathbf{x}^topmathbf{H}d(Deltamathbf{x}) \ &= 2mathbf{b}^top d(Deltamathbf{x}) + underbrace{Deltamathbf{x}^topmathbf{H}^top d(Deltamathbf{x}) + Deltamathbf{x}^topmathbf{H}d(Deltamathbf{x})}_{Omega_k为对称阵，因此H为对称阵} \ &= 2(mathbf{b}^top + Deltamathbf{x}^topmathbf{H}^top)d(Deltamathbf{x}) \ &= 2(mathbf{b} + mathbf{H}Deltamathbf{x})^top d(Deltamathbf{x}) end{aligned} ]
对照(dmathbf{F}=frac{partial mathbf{F}}{partial Deltamathbf{x}}^Td(Deltamathbf{x}))，得(frac{partial mathbf{F}}{partial Deltamathbf{x}}=mathbf{b} + mathbf{H}Deltamathbf{x})

求(frac{partial mathbf{F}}{partial Deltamathbf{x}}=0)，注意因为(mathbf{F})非负，所以极值处为极小值。

问题又转为求解线性方程 (mathbf{H}Deltamathbf{x} = -mathbf{b})，所得到的解为(Deltamathbf{x}^*)，增量更新(mathbf{x}^*=breve{mathbf{x}}+Deltamathbf{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 & 1end{bmatrix})，相机坐标系下空间点(mathbf{p}_{c}=[x_c,y_c,z_c]^topin mathbb{R}^3)投影到像平面点(mathbf{p}_{I}=[u,v]^topin 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 & 1end{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]^topin 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^2end{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^2end{bmatrix} tag{6} ]

SO3、SE3定义及指数映射

[ SO(3) = begin{Bmatrix} mathbf{R}inmathbb{R}^{3times 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 & 0end{bmatrix}end{matrix}right|omega=[omega_1,omega_2,omega_3]^topinmathbb{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 & 1end{bmatrix}inmathbb{R}^{4times 4}|mathbf{R}in SO(3),mathbf{t}inmathbb{R}^3 end{Bmatrix} ]

[ mathfrak{se}(3) = begin{Bmatrix} epsilon^wedge=left.begin{matrix}begin{bmatrix}omega^wedge & v\ 0^top & 0end{bmatrix}end{matrix}right|omegainmathbb{R}^3,vinmathbb{R}^3,epsilon=[v,omega]^top end{Bmatrix} ]

[ begin{aligned} text{exp}(epsilon^wedge) &= underbrace{text{exp}{begin{bmatrix}omega^wedge & v\ 0^top & 0end{bmatrix}}}_{泰勒级数展开} \ &= mathbf{I} + begin{bmatrix}omega^wedge & v\ 0^top & 0end{bmatrix} + frac{1}{2!}begin{bmatrix}omega^{wedge2} & omega^wedge v\ 0^top & 0end{bmatrix} + frac{1}{3!}begin{bmatrix}omega^{wedge3} & omega^{wedge2} v\ 0^top & 0end{bmatrix} + dots \ &= begin{bmatrix}text{exp}(omega^wedge) & mathbf{V}v\ 0^top & 0end{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|omegaright|_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_0in{mathfrak{se}(3)})，加在该初值的左扰动为(text{exp}(epsilon^wedge ))。

单目误差

[ mathbf{e}_k(xi)=mathbf{p}_{I_k} – text{proj}(text{exp}(xi^wedge )cdotmathbf{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 )cdotmathbf{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 )cdotmathbf{p}_{w_k}}{partial epsilon} end{matrix}right|_{xi=xi_0, epsilon=0} &approx left.begin{matrix}frac{partialunderbrace{(I+epsilon^wedge )}_{泰勒展开近似}text{exp}(xi_0^wedge )cdotmathbf{p}_{w_k}}{partial epsilon}end{matrix}right|_{xi=xi_0, epsilon=0} \ &=left.begin{matrix}frac{partialepsilon^wedge text{exp}(xi_0^wedge )cdotmathbf{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} \ 1end{bmatrix}}{partial epsilon}end{matrix}right|_{xi=xi_0, epsilon=0} \ &=left.begin{matrix}frac{partial begin{bmatrix}omega^wedgemathbf{p}_c+v end{bmatrix}_{3times 1}}{partial epsilon}end{matrix}right|_{epsilon=0} \ &=left.begin{matrix}frac{partial begin{bmatrix}-mathbf{p}_c^wedgeomega+v end{bmatrix}_{3times 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}_{3times 3} & mathbf{p}_c^wedgeend{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}_{3times 3} & mathbf{p}_c^wedgeend{bmatrix} end{aligned} ]

双目误差

[ mathbf{e}_k(xi)=mathbf{p}_{I_k} – mathbf{z}_{stereo}(text{exp}(xi^wedge )cdotmathbf{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 )cdotmathbf{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^2end{bmatrix}cdot -begin{bmatrix}mathbf{I}_{3times 3} & mathbf{p}_c^wedgeend{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