实战 | 如何制作一个SLAM轨迹真值获取装置？

2019 年 10 月 16 日 计算机视觉life

点击上方“计算机视觉life”，选择“星标”

快速获得最新干货

本文知乎作者杨小东授权转载，未经授权禁止二次转载

原文： https://zhuanlan.zhihu.com/p/57235987

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

1. 原理

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

世界坐标系下，一个3D的点，投影到图像坐标下，可表示为

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

平面上的点和图像上的像素之间就建立了联系。它们之间就是一个单应矩阵H 。

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

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

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

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

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

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

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

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

就是机器人的位置。

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

4. 测量的精度问题

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

消去 λ可以得到

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

那么测得的平面上点的协方差就是

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

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

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

5. 关于测量范围

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

6. 代码

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

https://github.com/ydsf16

7. 参考资料

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

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

从零开始学习三维视觉核心技术SLAM，扫描查看介绍，3天内无条件退款

欢迎关注公众号：计算机视觉life，一起探索计算机视觉新世界~ 早就是优势，学习切忌单打独斗，这里有教程资料、练习作业、答疑解惑等，优质学习圈帮你少走弯路，快速入门！

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、算法竞赛、图像检测分割、自动驾驶、计算射影、医学影像、三维视觉等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~