基于平面 marker 的 Bundle Adjustmet

前言

一个二维的 marker 通常有四个角点,如果把四个角点当做独立的三维特征点去参与BA优化,那么需要十二个参数,并且四个角点之间的约束(边长以及正交)还不好加入优化。这篇博客是将一个marker用6自由度的坐标系进行建模,推导了整个Marker的重投影误差函数和雅克比矩阵,并在g2o中进行了实现和集成。代码会开放在我的github,博客的pdf版本也会在代码对应的note文件夹里(貌似csdn对部分latex支持不友好)。这个工作是17年初的时候做的,笔记在草稿箱中存了差不多两年了,现在开放出来希望能帮到有需要的小伙伴。
另外,基于 marker 的 structure from motion 或者 slam 的工作都有对应论文,google一下很容易找到,在这里推荐比较经典的 Mapping and Localization from Planar Markers.

marker BA 公式推导

李代数求导基础

有四种方式进行李代数的求导: gtsam作者笔记里的推导方式,strasdat博士论文里的推导方式,TUM kerl硕士论文里的推导方式,最后就是barfoot的state esitamtion for robotics一书中的推导了。最直观简介的是gtsam和kerl的推导,最完备最可扩展的推导是barfoot的方式,也就是高翔书上的推导。

目的:空间中一点 P w P_w Pw,通过 T c w T_{cw} Tcw转换到相机坐标系下 P c P_c Pc,高斯牛顿的时候需要不断调整优化 T c w T_{cw} Tcw.

这时就有了两个思路:

  1. 假设 T c ′ c = e x p ( δ ξ ^ ) T_{c'c}=exp(\hat{\delta\xi}) Tccexp(δξ^)是微小增量, T c ′ w = e x p ( δ ξ ^ ) T c w = e x p ( δ ξ ^ ) e x p ( ξ ^ ) T_{c'w}=exp(\hat{\delta\xi})T_{cw}=exp(\hat{\delta\xi})exp({\hat\xi}) Tcw=exp(δξ^)Tcw=exp(δξ^)exp(ξ^)。注意小增量是直接放在李代数的。在推导前,先熟悉一个性质,下面公式中粗体 P \mathbf{P} P P P P的齐次坐标形式。
    ξ ^ P = [ ω ^ v 0 0 ] [ P 1 ] = [ ω × P + v 0 ] = [ − P ^ I 3 0 0 ] [ ω v ] \hat\xi \mathbf{P}=\begin{bmatrix} \hat{\mathbf{\omega}}& \mathbf{v}\\ \mathbf{0} & \mathbf{0}\end{bmatrix}\begin{bmatrix}P \\ 1 \end{bmatrix}=\begin{bmatrix} \mathbf{\omega}\times P+\mathbf{v}\\ \mathbf{0}\end{bmatrix}=\begin{bmatrix} -\hat P&&\mathbf{I}_3 \\ \mathbf{0} && \mathbf{0}\end{bmatrix}\begin{bmatrix}\mathbf{\omega}\\\mathbf{v}\end{bmatrix} ξ^P=[ω^0v0][P1][ω×P+v0]=[P^0I30][ωv]
    有了这个性质,可以开始推导了,推导过程省略齐次坐标最后一行
    ∂ ( P c ′ ) ∂ ( δ ξ ) = ∂ ( e x p ( δ ξ ^ ) T c w P w ) ∂ ( δ ξ ) ≈ ∂ ( ( I + δ ξ ^ ) P c ) ∂ ( δ ξ ) = ∂ ( δ ξ ^ P c ) ∂ ( δ ξ ) = [ [ − P c ] × , I 3 ] 3 × 6 = ( 0 z − y 1 0 0 − z 0 x 0 1 0 y − x 0 0 0 1 ) 3 × 6 \begin{aligned} \frac{\partial(P_{c'})}{\partial(\delta\xi)} = & \frac{\partial(exp(\hat{\delta\xi})T_{cw}P_w)}{\partial(\delta\xi)} \\\approx & \frac{\partial((I+\hat{\delta\xi})P_c)}{\partial(\delta\xi)} = \frac{\partial(\hat{\delta\xi}P_c)}{\partial(\delta\xi)}=[[-P_c]_{\times},\mathbf{I}_{3}]_{3\times6} \\=& \begin{pmatrix} 0 & z & -y & 1 & 0 &0 \\ -z & 0 & x & 0 & 1 & 0 \\ y & -x & 0 & 0 & 0 & 1 \end{pmatrix}_{3\times 6} \end{aligned} (δξ)(Pc)==(δξ)(exp(δξ^)TcwPw)(δξ)((I+δξ^)Pc)=(δξ)(δξ^Pc)=[[Pc]×,I3]3×60zyz0xyx01000100013×6
    这里 [ ⋅ ] × [\cdot]_{\times} []× ( c d o t ) ^ \hat{(cdot)} (cdot)^ 都是表示把向量转换成反对称矩阵,采用两种形式纯粹是为了书写清楚,有的时候式子太长了,用hat括不下。
  2. 另一个思路是 T c ′ w = e x p ( [ δ ξ + ξ ] × ) T_{c'w}=exp([\delta\xi+\xi]_{\times}) Tcw=exp([δξ+ξ]×)。这里是直接在李代数上叠加一个微小变量。
    ∂ ( e x p ( ξ ^ ) P w ) ∂ ( δ ξ ) = lim ⁡ δ ξ → 0 e x p ( [ δ ξ + ξ ] × ) P w − e x p ( ξ ^ ) P w δ ξ \frac{\partial(exp(\hat\xi)P_w)}{\partial(\delta\xi)}=\lim_{\delta\xi\rightarrow0}\frac{exp([\delta\xi+\xi]_{\times})P_w-exp(\hat\xi)P_w}{\delta\xi} (δξ)(exp(ξ^)Pw)=δξ0limδξexp([δξ+ξ]×)Pwexp(ξ^)Pw
    注意分子上面减去的那一部分和 δ ξ \delta\xi δξ没关系,可以直接忽视,问题是
    e x p ( [ δ ξ + ξ ] × ) = e x p ( [ δ ξ ] × + [ ξ ] × ) ≠ e x p ( [ δ ξ ] × ) e x p ( [ ξ ] × ) exp([\delta\xi+\xi]_{\times})= exp([\delta\xi]_{\times}+[\xi]_{\times})\neq exp([\delta\xi]_{\times})exp([\xi]_{\times}) exp([δξ+ξ]×)=exp([δξ]×+[ξ]×)=exp([δξ]×)exp([ξ]×)
    这是因为矩阵的幂指数可不能随便展开,需要引入专门解决这个问题的BCH公式:
    l n ( e x p ( ξ 1 ^ ) e x p ( ξ ^ 2 ) ) ≈ { J ℓ ( ξ 2 ) − 1 ξ 1 + ξ 2 i f ξ 1 i s s m a l l ξ 1 + J r ( ξ 1 ) − 1 ξ 2 i f ξ 2 i s s m a l l ln(exp(\hat{\xi_1})exp(\hat\xi_2))\approx \{ \begin{matrix} \mathbf{J_\ell}(\xi_2)^{-1}\xi_1 + \xi_2 & if\quad \xi_1 \quad is \quad small \\ \xi_1 + \mathbf{J_r}(\xi_1)^{-1}\xi_2 & if\quad \xi_2 \quad is \quad small \end{matrix} ln(exp(ξ1^)exp(ξ^2)){J(ξ2)1ξ1+ξ2ξ1+Jr(ξ1)1ξ2ifξ1issmallifξ2issmall具体参见barfoot书公式(7.80)。上面等式中,不妨假设 ξ 1 = δ ξ \xi_1=\delta\xi ξ1=δξ,则有:
    e x p ( δ ξ ^ ) e x p ( ξ ^ ) = e x p ( [ ξ + J ℓ ( ξ ) − 1 δ ξ ] × ) exp(\hat{\delta\xi})exp(\hat\xi)=exp([\xi+\mathbf{J_\ell(\xi)^{-1}\delta\xi}]_{\times}) exp(δξ^)exp(ξ^)=exp([ξ+J(ξ)1δξ]×)或者
    e x p ( [ δ ξ + ξ ] × ) = e x p ( [ J ℓ ( ξ ) δ ξ ] × ) e x p ( ξ ^ ) exp([\delta\xi+\xi]_{\times})=exp([\mathbf{J_\ell(\xi)\delta\xi}]_{\times})exp(\hat\xi) exp([δξ+ξ]×)=exp([J(ξ)δξ]×)exp(ξ^)
    回到求导的公式:
    lim ⁡ δ ξ → 0 e x p ( [ δ ξ + ξ ] × ) P w − e x p ( ξ ^ ) P w δ ξ = lim ⁡ δ ξ → 0 e x p ( [ J ℓ ( ξ ) δ ξ ] × ) e x p ( ξ ^ ) P w δ ξ = lim ⁡ δ ξ → 0 ( I + [ J ℓ ( ξ ) δ ξ ] × ) e x p ( ξ ^ ) P w δ ξ = lim ⁡ δ ξ → 0 [ J ℓ ( ξ ) δ ξ ] × P c δ ξ = [ [ − P c ] × , I 3 ] 3 × 6 J ℓ \begin{aligned} \lim_{\delta\xi\rightarrow0}\frac{exp([\delta\xi+\xi]_{\times})P_w-exp(\hat\xi)P_w}{\delta\xi}=& \lim_{\delta\xi\rightarrow0}\frac{exp([\mathbf{J_\ell}(\xi)\delta\xi]_{\times})exp(\hat\xi)P_w}{\delta\xi} \\= & \lim_{\delta\xi\rightarrow0}\frac{(\mathbf{I}+[\mathbf{J_\ell}(\xi)\delta\xi]_{\times})exp(\hat\xi)P_w}{\delta\xi} \\= & \lim_{\delta\xi\rightarrow0}\frac{[\mathbf{J_\ell}(\xi)\delta\xi]_{\times}P_c}{\delta\xi}=[[-P_c]_{\times},\mathbf{I}_{3}]_{3\times6}\mathbf{J_\ell} \end{aligned} δξ0limδξexp([δξ+ξ]×)Pwexp(ξ^)Pw===δξ0limδξexp([J(ξ)δξ]×)exp(ξ^)Pwδξ0limδξ(I+[J(ξ)δξ]×)exp(ξ^)Pwδξ0limδξ[J(ξ)δξ]×Pc=[[Pc]×,I3]3×6J

上述两种方式都没问题,相差一个 J ℓ \mathbf{J_\ell} J,但是思路1更简单明了,也是gtsam作者采用的推导方式。

Bundle Adjustment中的雅克比

和上节一样,相机位姿为 T c w T_{cw} Tcw,世界坐标系一点 P w P_w Pw,投影方程为
p = ( u v ) = π ( P c ) = ( x f x z + c x y f y z + c y ) p=\binom{u}{v}=\pi(P_c)=\binom{\frac{xf_x}{z}+c_x}{\frac{yf_y}{z}+c_y} p=(vu)=π(Pc)=(zyfy+cyzxfx+cx)容易得到
∂ p ∂ P c = ( ∂ u ∂ x ∂ u ∂ y ∂ u ∂ z ∂ v ∂ x ∂ v ∂ y ∂ v ∂ z ) 2 × 3 = ( f x z 0 − x f x z 2 0 f y z − y f y z 2 ) 2 × 3 \frac{\partial p}{\partial P_c}=\begin{pmatrix} \frac{\partial u}{\partial x} & \frac{\partial u}{\partial y} & \frac{\partial u}{\partial z}\\ \frac{\partial v}{\partial x} & \frac{\partial v}{\partial y} & \frac{\partial v}{\partial z} \end{pmatrix}_{2\times 3}=\begin{pmatrix} \frac{f_x}{ z} & 0 & \frac{-xf_x}{z^2}\\ 0 & \frac{f_y}{z} & \frac{-yf_y}{z^2} \end{pmatrix}_{2\times 3} Pcp=(xuxvyuyvzuzv)2×3=(zfx00zfyz2xfxz2yfy)2×3
其中 P c = ( x , y , z ) T \color{red}{P_c=(x,y,z)^T} Pc=(x,y,z)T.

图像坐标误差对相机位姿增量求导

世界坐标系中3d点到像素坐标的转换为:
p = ( u v ) = π ( T c w P w ) p=\binom{u}{v}=\pi(T_{cw}P_w) p=(vu)=π(TcwPw)联立上文的两个偏导得
∂ p ∂ ( δ ξ ) = ∂ p ∂ P c ⋅ ∂ P c ∂ ( δ ξ ) = ( − x y z 2 f x ( 1 + x 2 z 2 ) f x − y z f x 1 z f x 0 − x z 2 f x − ( 1 + y 2 z 2 ) f y x y z 2 f y x z f y 0 1 z f y − y z 2 f y ) 2 × 6 ( 1 ) \begin{aligned} \frac{\partial p}{\partial(\delta\xi)}=&\frac{\partial p}{\partial P_c}\cdot\frac{\partial P_c}{\partial(\delta\xi)} \\ =& \begin{pmatrix} -\frac{xy}{ z^2}f_x & (1+\frac{x^2}{z^2})f_x & -\frac{y}{z}f_x & \frac{1}{z}f_x & 0 & -\frac{x}{z^2}f_x\\ -(1+\frac{y^2}{z^2})f_y & \frac{xy}{z^2}f_y & \frac{x}{z}f_y & 0 & \frac{1}{z}f_y & -\frac{y}{z^2}f_y\end{pmatrix}_{2\times 6} \end{aligned}\qquad(1) (δξ)p==Pcp(δξ)Pc(z2xyfx(1+z2y2)fy(1+z2x2)fxz2xyfyzyfxzxfyz1fx00z1fyz2xfxz2yfy)2×6(1)

图像坐标误差对坐标 P w P_w Pw求导

P c = ( x y z ) = T c w P w = ( x w r 1 + y w r 2 + z w r 3 + t 1 x w r 4 + y w r 5 + z w r 6 + t 2 x w r 7 + y w r 8 + z w r 9 + t 3 ) P_c =\begin{pmatrix} x\\y \\z \end{pmatrix}= T_{cw}P_w = \begin{pmatrix} x_wr_1+y_wr_2+z_wr_3+t_1 \\x_wr_4+y_wr_5+z_wr_6+t_2\\x_wr_7+y_wr_8+z_wr_9+t_3\end{pmatrix} Pc=xyz=TcwPw=xwr1+ywr2+zwr3+t1xwr4+ywr5+zwr6+t2xwr7+ywr8+zwr9+t3
∂ P c ∂ P w = ( r 1 r 2 r 3 r 4 r 5 r 6 r 7 r 8 r 9 ) = R \frac{\partial P_c}{\partial P_w} = \begin{pmatrix} r_1 & r_2 & r_3 \\r_4 & r_5 & r_6\\ r_7 & r_8 & r_9\end{pmatrix}=\mathbf{R} PwPc=r1r4r7r2r5r8r3r6r9=R
∂ p ∂ ( P w ) = ∂ p ∂ P c ⋅ ∂ P c ∂ ( P w ) = ( f x z 0 − x f x z 2 0 f y z − y f y z 2 ) 2 × 3 ⋅ R \begin{aligned} \frac{\partial p}{\partial(P_w)}=&\frac{\partial p}{\partial P_c}\cdot\frac{\partial P_c}{\partial(P_w)} \\ =& \begin{pmatrix} \frac{f_x}{ z} & 0 & \frac{-xf_x}{z^2}\\ 0 & \frac{f_y}{z} & \frac{-yf_y}{z^2} \end{pmatrix}_{2\times 3}\cdot\mathbf{R} \end{aligned} (Pw)p==Pcp(Pw)Pc(zfx00zfyz2xfxz2yfy)2×3R

注意,上面的导数有的程序里面在前面都多了一个负号,这是由误差向量的定义是 e = p − p i m g e= p - p_{img} e=ppimg还是 e = p i m g − p e= p_{img} - p e=pimgp造成的,其中 p i m g p_{img} pimg是检测到的图像坐标, p p p是理论计算得到的坐标,上面我们采用的误差向量是 e = p − p i m g e= p - p_{img} e=ppimg,orbslam中采用的是 e = p i m g − p e= p_{img} - p e=pimgp

marker bundle adjustment中的雅克比

marker作为一个平面,四个角点之间有空间位置的约束,因此不能简单把marker的ba问题当成四个点的ba问题。在这里,ba的时候,我们采用优化调整marker坐标系姿态来调整marker各角点的空间位置,优化变量从角点的位置变成了marker坐标系的6个变量。

marker以marker中心为原点,垂直于纸面向上为z,正对marker水平向右为x轴。marker坐标系到世界坐标系的转换 T m w T_{mw} Tmw
marker边长为 s i z e = 2 s size=2s size=2s,四个角点 P m i P_{m}^{i} Pmi在marker坐标系中的表示如下图所示。

这里写图片描述

已知图像某个角点坐标为 p i m g = ( u , v ) T p_{img}=(u,v)^T pimg=(u,v)T,为了和orbslam代码统一,这里我们采用 e = p i m g − p e= p_{img} - p e=pimgp
p = π ( T c w T m w − 1 P m ) p=\pi(T_{cw}T^{-1}_{mw}P_m) p=π(TcwTmw1Pm)
所以偏导数是两部分,一个是对相机的 T c w T_{cw} Tcw,一个是对marker的 T m w T_{mw} Tmw

角点图像坐标误差对相机位姿增量求导

和之前单点ba中的雅克比推导一样,这里可以直接将角点坐标 P m i P_m^{i} Pmi转换到相机坐标系下, P c i = T c m P m i \mathbf{P}_c^i=T_{cm}\mathbf{P}_m^i PciTcmPmi,雅克比的计算只需要把坐标 P c i P_c^i Pci代入公式1即可,同时由于误差向量多了个负号,所以公式1前面要加一个负号

角点图像坐标对marker位姿增量的求导

先把求雅克比时最难部分的表达式写出来:
P c = T c w T m w − 1 P m \mathbf{P}_c=T_{cw}T^{-1}_{mw}\mathbf{P}_m Pc=TcwTmw1Pm
δ ξ = ξ m ′ m → ξ m ′ w = ξ m w + δ ξ \delta\xi=\xi_{m'm}\rightarrow \xi_{m'w}=\xi_{mw}+\delta\xi δξ=ξmmξmw=ξmw+δξ
有了表达式就可以依葫芦画瓢,跟思路1一样,推导过程如下:不过前面推导的时候,我们省略了齐次坐标的最后一行,这里我们先不省略。
lim ⁡ δ ξ → 0 ∂ P c ∂ ( δ ξ ) = lim ⁡ δ ξ → 0 e x p ( ξ ^ c w ) [ e x p ( δ ξ ^ ) e x p ( ξ ^ m w ) ] − 1 P m − e x p ( ξ ^ c w ) [ e x p ( ξ ^ m w ) ] − 1 P m δ ξ = lim ⁡ δ ξ → 0 e x p ( ξ ^ c w ) e x p ( ξ ^ m w ) − 1 e x p ( δ ξ ^ ) − 1 P m δ ξ = lim ⁡ δ ξ → 0 T c w T w m e x p ( − δ ξ ^ ) P m δ ξ = lim ⁡ δ ξ → 0 T c m ( I − δ ξ ^ ) P m δ ξ = lim ⁡ δ ξ → 0 T c m ( − δ ξ ^ ) P m δ ξ = T c m ∗ [ [ P m ] × − I 3 0 0 ] = [ R c m t c m 0 1 ] [ [ P m ] × − I 3 0 0 ] \begin{aligned} \lim_{\delta\xi\rightarrow0}\frac{\partial \mathbf{P_c}}{\partial(\delta\xi)} =& \lim_{\delta\xi\rightarrow0}\frac{exp(\hat{\xi}_{cw})[exp(\hat{\delta\xi})exp(\hat{\xi}_{mw})]^{-1}\mathbf{P_m}-exp(\hat{\xi}_{cw})[exp(\hat{\xi}_{mw})]^{-1}\mathbf{P_m}}{\delta\xi} \\ =& \lim_{\delta\xi\rightarrow0}\frac{exp(\hat{\xi}_{cw})exp(\hat{\xi}_{mw})^{-1}exp(\hat{\delta\xi})^{-1}\mathbf{P_m}}{\delta\xi} \\ =& \lim_{\delta\xi\rightarrow0}\frac{T_{cw}T_{wm}exp(-\hat{\delta\xi})\mathbf{P_m}}{\delta\xi} \\ =& \lim_{\delta\xi\rightarrow0}\frac{T_{cm}(I-\hat{\delta\xi})\mathbf{P_m}}{\delta\xi} \\ =& \lim_{\delta\xi\rightarrow0}\frac{T_{cm}(-\hat{\delta\xi})\mathbf{P_m}}{\delta\xi} \\ =& {T_{cm}*\begin{bmatrix} [P_m]_{\times}&& -\mathbf{I}_{3}\\ \mathbf{0}&& \mathbf{0}\end{bmatrix}} \\=& \begin{bmatrix} \mathbf{R}_{cm} && \mathbf{t}_{cm}\\ \mathbf{0}&& 1\end{bmatrix}\begin{bmatrix} [P_m]_{\times}&& -\mathbf{I}_{3}\\ \mathbf{0}&& \mathbf{0}\end{bmatrix} \end{aligned} δξ0lim(δξ)Pc=======δξ0limδξexp(ξ^cw)[exp(δξ^)exp(ξ^mw)]1Pmexp(ξ^cw)[exp(ξ^mw)]1Pmδξ0limδξexp(ξ^cw)exp(ξ^mw)1exp(δξ^)1Pmδξ0limδξTcwTwmexp(δξ^)Pmδξ0limδξTcm(Iδξ^)Pmδξ0limδξTcm(δξ^)PmTcm[[Pm]×0I30][Rcm0tcm1][[Pm]×0I30]
去掉最后一行对应的齐次坐标,可以得到没有其次坐标时的偏导数如下:
∂ P c ∂ ( δ ξ ) = R c m [ [ P m ] × − I 3 ] \frac{\partial P_c}{\partial(\delta\xi)} = \mathbf{R}_{cm}\begin{bmatrix} [P_m]_{\times}&& -\mathbf{I}_{3}\end{bmatrix} (δξ)Pc=Rcm[[Pm]×I3]
到这一步就知道为啥不省略齐次坐标最后一行了
∂ p ∂ ( δ ξ ) = ∂ p ∂ P c ⋅ ∂ P c ∂ ( δ ξ ) = ( f x z 0 − x f x z 2 0 f y z − y f y z 2 ) 2 × 3 ⋅ R c m ⋅ ( 0 − z m y m − 1 0 0 z m 0 − x m 0 − 1 0 − y m x m 0 0 0 − 1 ) 3 × 6 ( 2 ) \begin{aligned} \frac{\partial p}{\partial(\delta\xi)}=&\frac{\partial p}{\partial P_c}\cdot\frac{\partial P_c}{\partial(\delta\xi)} \\ =& \begin{pmatrix} \frac{f_x}{ z} & 0 & \frac{-xf_x}{z^2}\\ 0 & \frac{f_y}{z} & \frac{-yf_y}{z^2} \end{pmatrix}_{2\times 3}\cdot\mathbf{R}_{cm}\cdot\begin{pmatrix} 0 & -z_m & y_m & -1 & 0 &0 \\ z_m & 0 & -x_m & 0 & -1 & 0 \\ -y_m & x_m & 0 & 0 & 0 & -1 \end{pmatrix}_{3\times 6} \end{aligned}\qquad(2) (δξ)p==Pcp(δξ)Pc(zfx00zfyz2xfxz2yfy)2×3Rcm0zmymzm0xmymxm01000100013×6(2)
注意,写到程序里的时候,上面公式(2)也要再取个负号,因为误差向量为 e = p i m g − p e=p_{img}-p e=pimgp

©️2020 CSDN 皮肤主题: 游动-白 设计师:上身试试 返回首页