前言

  一个二维的 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.   这时就有了两个思路：   假设T c ′c＝exp( δξ^)是微小增量,T c ′w=exp( δξ^)T cw=exp( δξ^​ )exp( ξ^)。注意小增量是直接放在李代数的。在推导前，先熟悉一个性质,下面公式中粗体P是P 的齐次坐标形式。  
  有了这个性质，可以开始推导了，推导过程省略齐次坐标最后一行  
  这里 [⋅] ×和(cdot)^都是表示把向量转换成反对称矩阵,采用两种形式纯粹是为了书写清楚，有的时候式子太长了，用hat括不下。 另一个思路是T c ′w=exp([δξ+ξ] ×)。这里是直接在李代数上叠加一个微小变量。
  注意分子上面减去的那一部分和δξ没关系，可以直接忽视，问题是  
  这是因为矩阵的幂指数可不能随便展开，需要引入专门解决这个问题的BCH公式:  
  具体参见barfoot书公式(7.80)。上面等式中，不妨假设ξ 1=δξ,则有:  
  或者  
  回到求导的公式：  
  上述两种方式都没问题，相差一个J ℓ,但是思路１更简单明了，也是gtsam作者采用的推导方式。  

Bundle Adjustment中的雅克比

  和上节一样，相机位姿为T cw,世界坐标系一点P w，投影方程为  
  其中P c=(x,y,z) T  

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

  世界坐标系中３d点到像素坐标的转换为：  
  联立上文的两个偏导得  
  图像坐标误差对坐标P w求导  
  注意，上面的导数有的程序里面在前面都多了一个负号，这是由误差向量的定义是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边长为s i z e = 2 s size=2ssize=2s，四个角点P mi在marker坐标系中的表示如下图所示。  
  已知图像某个角点坐标为
为了和orbslam代码统一，这里我们采用e=p img−p。  
  所以偏导数是两部分，一个是对相机的 T cw，一个是对marker的 T mw  

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

  和之前单点ba中的雅克比推导一样，这里可以直接将角点坐标P mi转换到相机坐标系下，P ci＝T cmP mi，雅克比的计算只需要把坐标P ci​代入公式１即可，同时由于误差向量多了个负号，所以公式１前面要加一个负号。  

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

  先把求雅克比时最难部分的表达式写出来：  
  有了表达式就可以依葫芦画瓢，跟思路１一样，推导过程如下：不过前面推导的时候，我们省略了齐次坐标的最后一行，这里我们先不省略。  
  去掉最后一行对应的齐次坐标，可以得到没有其次坐标时的偏导数如下：  
  到这一步就知道为啥不省略齐次坐标最后一行了  
  注意，写到程序里的时候，上面公式(2)也要再取个负号，因为误差向量为e=p img−p