在SLAM中，通常用BA（Bundle Adjustment）来实现多个三维点和不同相机位姿的优化
本文描述如何建立基于线特征的图优化，并推导相应的雅克比矩阵，并用g2o实现相应的类
1. 线特征误差及观测模型
假设相机位姿为 T c w T_{cw} Tcw表示世界坐标系到相机坐标系的变换，或者表示成 R c w , t c w R_{cw},t_{cw} Rcw,tcw。三维空间直线表示成
世界坐标下 L w = [ n w v w ] \mathcal{L}^{w}=\begin{bmatrix} n^{w} \\ v^{w}\end{bmatrix} Lw=[nwvw]
相机坐标下 L c = [ n c v c ] \mathcal{L}^{c}=\begin{bmatrix} n^{c} \\ v^{c}\end{bmatrix} Lc=[ncvc]
相机坐标转换成直线方程系数矩阵为
K = [ f y 0 0 0 f x 0 − f y c x − f x c y f x f y ] \mathcal{K}=\begin{bmatrix} f_{y} & 0& 0 \\ 0 & f_{x} & 0 \\ -f_{y}c_{x} & -f_{x}c_{y} & f_{x}f_{y} \end{bmatrix} K=⎣
⎡fy0−fycx0fx−fxcy00fxfy⎦
⎤
重投影误差为 e l = [ p s T l ′ l 1 2 + l 2 2 p e T l ′ l 1 2 + l 2 2 ] e_{l} = \begin{bmatrix} \frac {p_{s}^{T}l^{'}} {\sqrt{l_{1}^{2} + l_{2}^{2}}} \\ \frac {p_{e}^{T}l^{'}} {\sqrt{l_{1}^{2} + l_{2}^{2}}} \end{bmatrix} el=⎣
⎡l12+l22psTl′l12+l22peTl′⎦
⎤其中 l ′ = [ l 1 l 2 l 3 ] T l^{'}=\begin{bmatrix} l_{1} & l_{2} & l_{3} \end{bmatrix}^{T} l′=[l1l2l3]T，表示空间直线投影到二维图像上的直线方程系数
p s = [ u s v s 1 ] T ， p e = [ u e v e 1 ] T p_{s}=\begin{bmatrix} u_{s} & v_{s} & 1\end{bmatrix}^{T}，p_{e}=\begin{bmatrix} u_{e} & v_{e} & 1\end{bmatrix}^{T} ps=[usvs1]T，pe=[ueve1]T为与空间直线匹配的图像线特征起点和终点齐次坐标
坐标之间的变换为
[ n c v c ] = [ R c w [ t c w ] × R c w 0 R c w ] [ n w v w ] = H c w [ n w v w ] l ′ = [ f y 0 0 0 f x 0 − f y c x − f x c y f x f y ] n c = K n c \begin{aligned} \begin{bmatrix} n^{c} \\ v^{c} \end{bmatrix} &= \begin{bmatrix}R_{cw} & \left[t_{cw}\right]_\times R_{cw} \\ 0 & R_{cw}\end{bmatrix}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix} \\ &=\mathcal{H}_{cw}\begin{bmatrix}n^{w} \\v^{w}\end{bmatrix} \\l^{'} &=\begin{bmatrix} f_{y} & 0& 0 \\ 0 & f_{x} & 0 \\ -f_{y}c_{x} & -f_{x}c_{y} & f_{x}f_{y} \end{bmatrix}n^{c} \\ &= \mathcal{K}n^{c} \end{aligned} [ncvc]l′=[Rcw0[tcw]×RcwRcw][nwvw]=Hcw[nwvw]=⎣
⎡fy0−fycx0fx−fxcy00fxfy⎦
⎤nc=Knc
2. 线特征重投影误差关于位姿增量的雅克比矩阵
利用链式法则，可以求得
∂ e l ∂ δ ξ = ∂ e l ∂ l ′ ∂ l ′ ∂ L c ∂ L c ∂ δ ξ \frac{\partial e_{l}}{\partial \delta\xi}=\frac{\partial e_{l}}{\partial l^{'}} \frac{\partial l^{'}}{\partial \mathcal{L}^{c}} \frac{\partial \mathcal{L}^{c}}{\partial \delta\xi} ∂δξ∂el=∂l′∂el∂Lc∂l′∂δξ∂Lc
这里注意，这里需要涉及到矩阵求导，矩阵求导通常分为分子布局和分母布局，这两种情况只是表达形式的不同，本身并没有两样，下面统一都用分母布局，具体求导结果可以看这篇文章
第一部分计算
∂ e l ∂ l ′ = [ ∂ ( p s T l ′ l 1 2 + l 2 2 ) ∂ l 1 ∂ ( p s T l ′ l 1 2 + l 2 2 ) ∂ l 2 ∂ ( p s T l ′ l 1 2 + l 2 2 ) ∂ l 3 ∂ ( p e T l ′ l 1 2 + l 2 2 ) ∂ l 1 ∂ ( p e T l ′ l 1 2 + l 2 2 ) ∂ l 2 ∂ ( p e T l ′ l 1 2 + l 2 2 ) ∂ l 3 ] = [ u s l 2 2 − v s l 1 l 2 − l 1 l 3 ( l 1 2 + l 2 2 ) 3 2 v s l 2 2 − u s l 1 l 2 − l 1 l 3 ( l 1 2 + l 2 2 ) 3 2 1 l 1 2 + l 2 2 u e l 2 2 − v e l 1 l 2 − l 1 l 3 ( l 1 2 + l 2 2 ) 3 2 v e l 2 2 − u e l 1 l 2 − l 1 l 3 ( l 1 2 + l 2 2 ) 3 2 1 l 1 2 + l 2 2 ] \begin{aligned} \frac{\partial e_{l}}{\partial l^{'}} &= \begin{bmatrix} \frac{\partial(\frac{p_{s}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{1}} & \frac{\partial(\frac{p_{s}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{2}} & \frac{\partial(\frac{p_{s}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{3}} \\ \frac{\partial(\frac{p_{e}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{1}} & \frac{\partial(\frac{p_{e}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{2}} & \frac{\partial(\frac{p_{e}^{T}l^{'}}{\sqrt{l_{1}^{2}+l_{2}^{2}}})}{\partial l_{3}} \end{bmatrix} \\ &= \begin{bmatrix} \frac{u_{s}l_{2}^{2}-v_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{v_{s}l_{2}^{2}-u_{s}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{1}{\sqrt{l_{1}^{2}+l_{2}^{2}}} \\ \frac{u_{e}l_{2}^{2}-v_{e}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{v_{e}l_{2}^{2}-u_{e}l_{1}l_{2}-l_{1}l_{3}}{(l_{1}^{2}+l_{2}^{2})^{\frac{3}{2}}} & \frac{1}{\sqrt{l_{1}^{2}+l_{2}^{2}}} \end{bmatrix} \end{aligned} ∂l′∂el=⎣
⎡∂l1∂(l12+l22psTl′)∂l1∂(l12+l22peTl′)∂l2∂(l12+l22psTl′)∂l2∂(l12+l22peTl′)∂l3∂(l12+l22psTl′)∂l3∂(l12+l22peTl′)⎦
⎤=⎣
⎡(l12+l22)23usl22−vsl1l2−l1l3(l12+l22)23uel22−vel1l2−l1l3(l12+l22)23vsl22−usl1l2−l1l3(l12+l22)23vel22−uel1l2−l1l3
最低0.47元/天 解锁文章
475
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
