实战 | 如何制作一个SLAM轨迹真值获取装置？
点击下方卡片，关注“新机器视觉”公众号
重磅干货，第一时间送达
本文知乎作者杨小东授权转载，未经授权禁止二次转载
原文：
SLAM性能的一个很重要的指标是定位精度，公开的室内数据集一般会采用动捕设备获取设备轨迹真值。但是动捕设备价格昂贵，OptiTrack百万以上，国产的Nokov >20W。没有动捕设备，我们就很难用自己采集的数据评估SLAM的定位性能。
为此，我们搞了一个廉价的轨迹真值捕获装置。用一个固定的单目相机获取平面运动的轨迹真值。我们主要是用这种方法来测室内移动机器人的运动轨迹。
本文给出了原理和代码。
注意：只能测2D平面内的运动，不是3D。
1. 原理
如下图，支架上固定一个下视的单目相机，用来捕捉机器人顶端平贴的一张ArUco码。ArUco码相对的机器人的位姿是已知的（这个应该很容以做到吧）。我们要解决的问题就是利用单目相机测量ArUco码在平面上的位姿。实际上，OpenCV提供的ArUco的库，可以实现ArUco码的识别，并提供码四个角点的像素坐标。如果我们能计算出到四个角点在平面上的坐标，就可以计算出机器人在平面上的位姿（位置和朝向）。下面就来说一下如何计算一个点在平面上的位置。
世界坐标系下，一个3D的点
，投影到图像坐标下，可表示为
T 是外参。我们想要测量的是2D平面内的坐标，为了方便，可以把世界坐标系的原点设在平面之上，Z 轴垂直于被这个平面（就是ArUco那个平面）。那么
的 z =0 。式（1）就可以变为
平面上的点
和图像上的像素
之间就建立了联系。它们之间就是一个单应矩阵H 。
我们只要知道这个单应矩阵H ，就可以通过图像像素
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
本文仅做学术分享，如有侵权，请联系删文。