SLAM轨迹真值获取装置

SLAM性能的一个很重要的指标是 定位精度 ，公开的室内数据集一般会采用 动捕设备 获取设备轨迹真值。但是动捕设备 价格昂贵 ， OptiTrack 百万以上，国产的 Nokov >20W。没有动捕设备，我们就很难用 自己采集的数据 评估SLAM的定位性能。 为此，我们搞了一个廉价的轨迹真值捕获装置。用一个固定的 单目相机 获取 平面运动 的 轨迹真值 。我们主要是用这种方法来测室内移动机器人的运动轨迹。 本文给出了原理和 代码 。 注意：只能测 2D 平面内的运动，不是3D。

1. 原理

如下图，支架上固定一个下视的单目相机，用来捕捉机器人顶端平贴的一张ArUco码。ArUco码相对的机器人的位姿是已知的（这个应该很容以做到吧）。我们要解决的问题就是利用单目相机测量ArUco码在平面上的位姿。实际上，OpenCV提供的ArUco的库，可以实现ArUco码的识别，并提供码四个角点的像素坐标。如果我们能计算出到四个角点在平面上的坐标，就可以计算出机器人在平面上的位姿（位置和朝向）。下面就来说一下如何计算一个点在平面上的位置。

平面轨迹真值捕获原理

世界坐标系下，一个3D的点 ${}^w{\mathbf{p}} = {\left[ {x,y,z} \right]^T}$ ，投影到图像坐标下，可表示为

$$\lambda \left[ {\begin{array}{c} u\\ v\\ 1 \end{array}} \right] = \left[ {\begin{array}{l} {{f_x}}&0&{{c_x}}&0\\ 0&{{f_y}}&{{c_y}}&0\\ 0&0&1&0 \end{array}} \right]\underbrace {\left[ {\begin{array}{c} {{r_{11}}}&{{r_{12}}}&{{r_{13}}}&{{t_x}}\\ {{r_{21}}}&{{r_{22}}}&{{r_{23}}}&{{t_y}}\\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{t_z}}\\ 0&0&0&1 \end{array}} \right]}_{\bf{T}}\left[ {\begin{array}{c} x\\ y\\ z\\ 1 \end{array}} \right] \ (1) \\$$

$\mathbf T$ 是外参。我们想要测量的是2D平面内的坐标，为了方便，可以把世界坐标系的原点设在平面之上，$Z$ 轴垂直于被这个平面（就是ArUco那个平面）。那么 ${}^w{\bf{p}}$ 的 $z=0$ 。式（1）就可以变为

$$\lambda \left[ {\begin{array}{c} u\\ v\\ 1 \end{array}} \right] = \underbrace {{}\left[ {\begin{array}{c} {{f_x}}&0&{{c_x}}\\ 0&{{f_y}}&{{c_y}}\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{c} {{r_{11}}}&{{r_{12}}}&{{t_x}}\\ {{r_{21}}}&{{r_{22}}}&{{t_y}}\\ {{r_{31}}}&{{r_{32}}}&{{t_z}} \end{array}} \right]}_{\bf{H}_{3\times 3}}\left[ {\begin{array}{c} x\\ y\\ 1 \end{array}} \right] \ (2) \\$$

平面上的点 $[x,y]^T$ 和图像上的像素 $[u,v]^T$ 之间就建立了联系。它们之间就是一个单应矩阵 $\mathbf H$ 。

我们只要知道这个单应矩阵 ，就可以通过图像像素 获取其在平面上的2D坐标 。

$${\left[ {\begin{array}{c} x\\ y\\ 1 \end{array}} \right] = \lambda {H^{ - 1}}\left[ {\begin{array}{c} u\\ v\\ 1 \end{array}} \right]} \ (3)\\$$

2. 现在的问题是单应矩阵怎么来的呢？

从式（2）可以看出，只要知道摄像机的内参数 $\bf K$ 和外参数 $\bf T$ ， $\bf H$ 就有了。内参数 $\bf K$ 好说，大家应该都很熟悉。关键是 $\bf T$ ，我们这里用平面靶标来标定。具体的，如下图把一张棋盘格靶标放在ArUco码之上，通过PnP方法可以计算出靶标坐标系与摄像机坐标系之间的位姿变换 $\bf T ^{'}$ 。注意，棋盘格靶标这个平面并不是ArUco码的平面，因为棋盘格靶标有厚度 $t$ 。这个厚度 $t$ 我们很容易测量，认为是已知的。那么，相机坐标系和ArUco面上的世界坐标系之间的坐标变换就是

$${\bf{T}}{\rm{ = }}{\bf{T'}}\left[ {\begin{array}{c} 1&0&0&0\\ 0&1&0&0\\ 0&0&1&t\\ 0&0&0&1 \end{array}} \right] \ (4)\\$$

至此，我们就得到了外参数 $\bf T$ 。通过（2）式也就可以得到单应矩阵 $\bf H$ 。

标定单应矩阵H

3. 下面的问题是计算机器人的位姿

如上图，刚才我们提到，在图像上可以得到ArUco码四个角点的像素坐标，这里记为

$$\left[ {\begin{array}{c} {{u_i}} \\ {{v_i}} \end{array}} \right],i = 1\sim4 \ (5)\\$$

使用（3）式，可以分别计算出这四个点在世界坐标系下的坐标（就是平面内的坐标）

$$\left[ {\begin{array}{c} {{x_i}} \\ {{y_i}} \end{array}} \right],i = 1\sim4 \ (6) \\$$

关于机器人的位置，其实我们只需要一个点的坐标就好了，就是ArUco码中心点的坐标。因此我们把四个点合成为一个点：

$$\left[ {\begin{array}{c} {{x_c}} \\ {{y_c}} \end{array}} \right] = \frac{1}{4}\sum\limits_{i = 1}^n {\left[ {\begin{array}{c} {{x_i}} \\ {{y_i}} \end{array}} \right]} (7) \\$$

$[x_c,y_c]^T$就是机器人的位置。

下面还要计算一下机器人的姿态，在2D平面就是朝向了。用两个点就可以计算出朝向，现在我们手里有四个点，那就平均一下好了。

$$\begin{gathered} \theta = \frac{{\left\{ {\arctan2 ({{\mathbf{V}}_1}[2],{{\mathbf{V}}_1}[1]){\text{ + }}\arctan2 ({{\mathbf{V}}_2}[2],{{\mathbf{V}}_2}[1])} \right\}}}{2} , \\ {{\mathbf{V}}_1} = \left[ {\begin{array}{c} {{x_1}} \\ {{y_1}} \end{array}} \right] - \left[ {\begin{array}{c} {{x_4}} \\ {{y_4}} \end{array}} \right],{{\mathbf{V}}_2} = \left[ {\begin{array}{c} {{x_2}} \\ {{y_2}} \end{array}} \right] - \left[ {\begin{array}{c} {{x_3}} \\ {{y_3}} \end{array}} \right] \\ \end{gathered} \ (8) \\$$

$\theta$就是机器人的朝向。

4. 测量的精度问题

既然要作为真值去用，那就要求要有足够的测量精度。下面我们分析一下测量精度。我们把式（3）展开

$$\left[ {\begin{array}{c} x \\ y \\ 1 \end{array}} \right] = \lambda \underbrace {\left[ {\begin{array}{c} {{h_{11}}}&{{h_{12}}}&{{h_{13}}} \\ {{h_{21}}}&{{h_{22}}}&{{h_{23}}} \\ {{h_{31}}}&{{h_{32}}}&{{h_{33}}} \end{array}} \right]}_{{{\mathbf{H}}^{-1}}}\left[ {\begin{array}{c} u \\ v \\ 1 \end{array}} \right] \ (9) \\$$

消去 $\lambda$ 可以得到

$$\left[ {\begin{array}{c} x \\ y \end{array}} \right] = \left[ {\begin{array}{c} {\frac{{{h_{11}}u + {h_{12}}v + {h_{13}}}}{{{h_{31}}u + {h_{32}}v + {h_{33}}}}} \\ {\frac{{{h_{21}}u + {h_{22}}v + {h_{23}}}}{{{h_{31}}u + {h_{32}}v + {h_{33}}}}} \end{array}} \right] = F\left( {\left[ {\begin{array}{c} u \\ v \end{array}} \right]} \right)\ (10) \\$$

如果图像u方向上像素提取的精度-方差为 $\sigma_u^2$ ，v方向的精度-方差为$\sigma_v^2$ 。也就是像素点的协方差为

$${{\mathbf{\Sigma }}_{uv}} = \left[ {\begin{array}{c} {{\sigma _u}^2}&0 \\ 0&{{\sigma _v}^2} \end{array}} \right] \ (11)\\$$

那么测得的平面上点 $[x,y]^T$ 的协方差就是

$${{\mathbf{\Sigma }}_{xy}} = \frac{{\partial F}}{{\partial {{[u,v]}^T}}}{{\mathbf{\Sigma }}_{uv}}{\left( {\frac{{\partial F}}{{\partial {{[u,v]}^T}}}} \right)^T} \ (12) \\$$

至此就精度分析完了，也就是说标出来单应矩阵 $\bf H$ 之后，通过（12）式就可以计算出大致的测量精度。

实际上，也可以把摄像机的内外参带入到（3）式，再结合（12）式，可以获得一个解析的精度表达。这个比较复杂。如果相机安装的时候相对于测量平面仅是绕相机坐标系的x轴旋转一个角度的话，测量区域内的精度分布如下图。如果摄像机正对测量平面的话，测量区域的测量精度是相同的。当然，相机的分辨率越高精度越高。焦距越长精度越高，但是同时可测量的区域也会变小。

我自己使用的相机是大恒MER-302-56U3C相机，2048×1536像素；镜头是kowa LM3NC1M，3.5mm广角镜头；摄像机离测量平面的距离大概是2m。可测量范围大概是4m×3m。如果认为像素的提取精度为1个pixel的话，测量精度约为2mm。这个精度足够用于SLAM轨迹评估。

5. 关于测量范围

如果相机正对于测量平面，测量区域是一个矩形。如果相机倾斜放置的话，测量区域就是一般的四边形了。上图就是一种。测量区域的大小跟镜头的视场角、相机安装的高度、角度都有关系，比较复杂。总的来说，为了使测量平面内的测量精度一致，应该尽量让相机正对测量平面。那焦距越短，测量视场越大，但是测量精度随之降低。为了获取足够的测量精度，就需要一个高分辨率的相机。为了降低悬挂高度，就需要一个短焦镜头。

6. 代码

标定和测量的全部代码如下

ydsf16/ground_truth_estimation_2d

7. 参考资料

[1] Hartley R , Zisserman A . Multiple View Geometry in Computer Vision[M]. Cambridge University Press, 2003.

[2] 杨东升, 毕树生, 蔡月日,等. 基于平行面多靶标标定的单目大视场平面测量[J]. 光学学报, 2017(10):228-239.

----更多SLAM文章----

杨小东：[PnP]PnP问题之EPnP解法

杨小东：[PnP] PnP问题之DLT解法

杨小东：[ORB-SLAM2]卡方分布(Chi-squared)外点(outlier)剔除

杨小东：[ORB-SLAM2] ORB特征提取策略对ORB-SLAM2性能的影响

杨小东：[PR-3]ArUco EKF SLAM 扩展卡尔曼SLAM

杨小东：[PR-2] PF 粒子滤波/蒙特卡罗定位

----相关代码----

ydsf16 - Overview