基于平面 marker 的 Bundle Adjustmet

Posted 2023-02-27 白巧克力亦唯心

前言

一个二维的 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 }$和 ( c d o t ) ^ \\hat(cdot) (cdotROS实验笔记之——基于EKF融合Visual-marker localization与IMU

   Unity Container 中基于 Marker 接口注册接口到不同的 Lifetime

   一个仿3D的平面游戏页面

   week63 PnP位姿估计 基于ArUco Marker来估算camera的位姿

   翻译OpenCV文档:ArUco Marker的检测 （Detection of ArUco Markers）

   在Opencv中校准后查找点世界坐标