基于平面 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$,通过$T_{cw}$转换到相机坐标系下$P_c$,高斯牛顿的时候需要不断调整优化$T_{cw}$.

这时就有了两个思路：

1. 假设$T_{c^{\prime }c}＝exp(\widehat{\delta \xi })$是微小增量, $T_{c^{\prime }w}=exp(\widehat{\delta \xi })T_{cw}=exp(\widehat{\delta \xi })exp(\hat{\xi })$。注意小增量是直接放在李代数的。在推导前，先熟悉一个性质,下面公式中粗体$\mathbf{P}$是$P$的齐次坐标形式。
   $$\hat{\xi }\mathbf{P}=\left[\begin{array}{cc}\hat{\omega }& \mathbf{v}\\ 0& 0\end{array}\right]\left[\begin{array}{c}P\\ 1\end{array}\right]＝\left[\begin{array}{c}\omega \times P+\mathbf{v}\\ 0\end{array}\right]=\left[\begin{array}{ccc}-\hat{P}& & {\mathbf{I}}_3\\ 0& & 0\end{array}\right]\left[\begin{array}{c}\omega \\ \mathbf{v}\end{array}\right]$$
   有了这个性质，可以开始推导了，推导过程省略齐次坐标最后一行
   $$\begin{array}{rl}\frac{\partial (P_{c^{\prime }})}{\partial (\delta \xi )}=& \frac{\partial (exp(\widehat{\delta \xi })T_{cw}{P}_w)}{\partial (\delta \xi )}\\ \approx & \frac{\partial ((I+\widehat{\delta \xi })P_c)}{\partial (\delta \xi )}=\frac{\partial (\widehat{\delta \xi }{P}_c)}{\partial (\delta \xi )}=[[-P_c{]}_{\times },{\mathbf{I}}_3{]}_{3\times 6}\\ =& {\left(\begin{array}{cccccc}0& z& -y& 1& 0& 0\\ -z& 0& x& 0& 1& 0\\ y& -x& 0& 0& 0& 1\end{array}\right)}_{3\times 6}\end{array}$$
   这里 $[\cdot {]}_{\times }$和$\widehat{(cdot)}$ 都是表示把向量转换成反对称矩阵,采用两种形式纯粹是为了书写清楚，有的时候式子太长了，用hat括不下。
2. 另一个思路是$T_{c^{\prime }w}=exp([\delta \xi +\xi {]}_{\times })$。这里是直接在李代数上叠加一个微小变量。
   $$\frac{\partial (exp(\hat{\xi })P_w)}{\partial (\delta \xi )}=\underset{\delta \xi \to 0}{\lim }\frac{exp([\delta \xi +\xi {]}_{\times })P_w-exp(\hat{\xi })P_w}{\delta \xi }$$
   注意分子上面减去的那一部分和$\delta \xi$没关系，可以直接忽视，问题是
   $$exp([\delta \xi +\xi {]}_{\times })=exp([\delta \xi {]}_{\times }+[\xi {]}_{\times })\ne exp([\delta \xi {]}_{\times })exp([\xi {]}_{\times })$$
   这是因为矩阵的幂指数可不能随便展开，需要引入专门解决这个问题的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ξ2​​ifξ1​issmallifξ2​issmall​具体参见barfoot书公式(7.80)。上面等式中，不妨假设${\xi }_1=\delta \xi$,则有:
   $$exp(\widehat{\delta \xi })exp(\hat{\xi })=exp([\xi +{\mathbf{J}}_{\mathbf{\ell }}(\xi {)}^{-1}\delta \xi {]}_{\times })$$或者
   $$exp([\delta \xi +\xi {]}_{\times })=exp([{\mathbf{J}}_{\mathbf{\ell }}(\xi )\delta \xi {]}_{\times })exp(\hat{\xi })$$
   回到求导的公式：
   $$\begin{array}{rl}\underset{\delta \xi \to 0}{\lim }\frac{exp([\delta \xi +\xi {]}_{\times })P_w-exp(\hat{\xi })P_w}{\delta \xi }=& \underset{\delta \xi \to 0}{\lim }\frac{exp([{\mathbf{J}}_{\mathbf{\ell }}(\xi )\delta \xi {]}_{\times })exp(\hat{\xi })P_w}{\delta \xi }\\ =& \underset{\delta \xi \to 0}{\lim }\frac{(\mathbf{I}+[{\mathbf{J}}_{\mathbf{\ell }}(\xi )\delta \xi {]}_{\times })exp(\hat{\xi })P_w}{\delta \xi }\\ =& \underset{\delta \xi \to 0}{\lim }\frac{[{\mathbf{J}}_{\mathbf{\ell }}(\xi )\delta \xi {]}_{\times }{P}_c}{\delta \xi }=[[-P_c{]}_{\times },{\mathbf{I}}_3{]}_{3\times 6}{\mathbf{J}}_{\mathbf{\ell }}\end{array}$$

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

Bundle Adjustment中的雅克比

和上节一样，相机位姿为$T_{cw}$,世界坐标系一点$P_w$，投影方程为
$$p=\left(\genfrac{}{}{0ex}{}{u}{v}\right)=\pi (P_c)=\left(\genfrac{}{}{0ex}{}{\frac{x{f}_x}{z}+c_x}{\frac{y{f}_y}{z}+c_y}\right)$$容易得到
$$\frac{\partial p}{\partial P_c}={\left(\begin{array}{ccc}\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{array}\right)}_{2\times 3}={\left(\begin{array}{ccc}\frac{f_x}{z}& 0& \frac{-x{f}_x}{z^2}\\ 0& \frac{f_y}{z}& \frac{-y{f}_y}{z^2}\end{array}\right)}_{2\times 3}$$
其中$P_c=(x,y,z{)}^T$.

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

世界坐标系中３d点到像素坐标的转换为：
$$p=\left(\genfrac{}{}{0ex}{}{u}{v}\right)=\pi (T_{cw}{P}_w)$$联立上文的两个偏导得
$$\begin{array}{rl}\frac{\partial p}{\partial (\delta \xi )}=& \frac{\partial p}{\partial P_c}\cdot \frac{\partial P_c}{\partial (\delta \xi )}\\ =& {\left(\begin{array}{cccccc}-\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{array}\right)}_{2\times 6}\end{array}(1)$$

图像坐标误差对坐标$P_w$求导

$$P_c=\left(\begin{array}{c}x\\ y\\ z\end{array}\right)=T_{cw}{P}_w=\left(\begin{array}{c}{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\end{array}\right)$$
$$\frac{\partial P_c}{\partial P_w}=\left(\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right)=\mathbf{R}$$
$$\begin{array}{rl}\frac{\partial p}{\partial (P_w)}=& \frac{\partial p}{\partial P_c}\cdot \frac{\partial P_c}{\partial (P_w)}\\ =& {\left(\begin{array}{ccc}\frac{f_x}{z}& 0& \frac{-x{f}_x}{z^2}\\ 0& \frac{f_y}{z}& \frac{-y{f}_y}{z^2}\end{array}\right)}_{2\times 3}\cdot \mathbf{R}\end{array}$$

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

marker bundle adjustment中的雅克比

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

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

已知图像某个角点坐标为 $p_{img}=(u,v{)}^T$,为了和orbslam代码统一，这里我们采用 $e=p_{img}-p$。
$$p=\pi (T_{cw}{T}_{mw}^{-1}{P}_m)$$
所以偏导数是两部分，一个是对相机的 $T_{cw}$，一个是对marker的 $T_{mw}$

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

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

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

先把求雅克比时最难部分的表达式写出来：
${\mathbf{P}}_c=T_{cw}{T}_{mw}^{-1}{\mathbf{P}}_m$
$$\delta \xi ={\xi }_{m^{\prime }m}\to {\xi }_{m^{\prime }w}={\xi }_{mw}+\delta \xi$$
有了表达式就可以依葫芦画瓢，跟思路１一样，推导过程如下：不过前面推导的时候，我们省略了齐次坐标的最后一行，这里我们先不省略。
$$\begin{array}{rl}\underset{\delta \xi \to 0}{\lim }\frac{\partial {\mathbf{P}}_{\mathbf{c}}}{\partial (\delta \xi )}=& \underset{\delta \xi \to 0}{\lim }\frac{exp({\hat{\xi }}_{cw})[exp(\widehat{\delta \xi })exp({\hat{\xi }}_{mw}){]}^{-1}{\mathbf{P}}_{\mathbf{m}}-exp({\hat{\xi }}_{cw})[exp({\hat{\xi }}_{mw}){]}^{-1}{\mathbf{P}}_{\mathbf{m}}}{\delta \xi }\\ =& \underset{\delta \xi \to 0}{\lim }\frac{exp({\hat{\xi }}_{cw})exp({\hat{\xi }}_{mw}{)}^{-1}exp(\widehat{\delta \xi }{)}^{-1}{\mathbf{P}}_{\mathbf{m}}}{\delta \xi }\\ =& \underset{\delta \xi \to 0}{\lim }\frac{T_{cw}{T}_{wm}exp(-\widehat{\delta \xi }){\mathbf{P}}_{\mathbf{m}}}{\delta \xi }\\ =& \underset{\delta \xi \to 0}{\lim }\frac{T_{cm}(I-\widehat{\delta \xi }){\mathbf{P}}_{\mathbf{m}}}{\delta \xi }\\ =& \underset{\delta \xi \to 0}{\lim }\frac{T_{cm}(-\widehat{\delta \xi }){\mathbf{P}}_{\mathbf{m}}}{\delta \xi }\\ =& T_{cm}\ast \left[\begin{array}{ccc}[P_m{]}_{\times }& & -{\mathbf{I}}_3\\ 0& & 0\end{array}\right]\\ =& \left[\begin{array}{ccc}{\mathbf{R}}_{cm}& & {\mathbf{t}}_{cm}\\ 0& & 1\end{array}\right]\left[\begin{array}{ccc}[P_m{]}_{\times }& & -{\mathbf{I}}_3\\ 0& & 0\end{array}\right]\end{array}$$
去掉最后一行对应的齐次坐标，可以得到没有其次坐标时的偏导数如下：
$$\frac{\partial P_c}{\partial (\delta \xi )}={\mathbf{R}}_{cm}\left[\begin{array}{ccc}[P_m{]}_{\times }& & -{\mathbf{I}}_3\end{array}\right]$$
到这一步就知道为啥不省略齐次坐标最后一行了
$$\begin{array}{rl}\frac{\partial p}{\partial (\delta \xi )}=& \frac{\partial p}{\partial P_c}\cdot \frac{\partial P_c}{\partial (\delta \xi )}\\ =& {\left(\begin{array}{ccc}\frac{f_x}{z}& 0& \frac{-x{f}_x}{z^2}\\ 0& \frac{f_y}{z}& \frac{-y{f}_y}{z^2}\end{array}\right)}_{2\times 3}\cdot {\mathbf{R}}_{cm}\cdot {\left(\begin{array}{cccccc}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{array}\right)}_{3\times 6}\end{array}(2)$$
注意，写到程序里的时候，上面公式(2)也要再取个负号，因为误差向量为$e=p_{img}-p$