点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达本文知乎作者杨小东授权转载，未经授权禁止二次转载
原文： 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 ，就可以通过图像像素
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
好消息！
小白学视觉知识星球
开始面向外开放啦👇👇👇
下载1：OpenCV-Contrib扩展模块中文版教程
在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。
下载2：Python视觉实战项目52讲
在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。
下载3：OpenCV实战项目20讲
在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。
交流群
欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~