使用视觉slam（simultaneous localization and mapping）技术时，经常需要获取机器人的更高精度的运动轨迹，又叫运动真值，从而判断视觉定位的精度，运动真值的精度通常比视觉高出一个数量级，也就是需要达到cm级别的精度。
获取运动真值的方法主要有运动相机、差分GPS、UWB技术和人工路标等，其中前三种方法需要购买较为昂贵的设备，从几千到几万不等，使用路标的方法价格低廉，只要操作合理也可以获得高精度的运动真值。本篇博客就是针对如何使用二维码当做路标，获取三维运动的机器人运动真值的问题进行阐述，需要的材料为打印出的二维码和一个200元左右的工业摄像头，本博客与下面的C++开源代码相对应。
QRCode-Localization
这一段是二维码原理的介绍，都是网络上能够找到的，可直接跳到2.2节。
常见的二维码上为啥三个角上有方块？
这其实是三个定位点，图形旋转也不影响识别。要了解二维码的原理，我们先要来好好认识一下它。现在最常见的二维码是QR二维码（QR是一种码制），我们便以它为例。
我们看一个二维码，最先看到的当然是几何图形。这些图形中，藏了不少重要的“部件”。
首先，QR二维码的三个“角”上有三个方块，可别小瞧这方块，它叫位置探测图形。有了这三个点，不管是从哪个方向读取二维码，信息都可以被识别。即使将二维码图形旋转，也可以识别。也许你会问，为什么不是四个角上都有方块呢？事实上，是可以设更多的点，但几何知识告诉我们，三点就可以确定一个平面，节省出的一个角可以嵌入更多信息。
另外，二维码上还有一些图形混杂在几何图形中，是肉眼看不出来的，比如定位图形和分隔符。定位图形就是图中连接三个位置探测图形之间的两根“线”，它的作用是决定二维码符号中模块的坐标，而分隔符的作用是将位置探测图形与符号的其余部分分开。也就是说，通过扫描能读取的数据信息在二维码中的位置是由定位图形和分隔符决定的。
还有两个图形肉眼也难以发现，位于左下角位置探测图形上面的是“版本信息”，每个二维码都有一个版本号，我们常说的V1.0、V2.0就是版本；包围在三个位置探测图形周边的则是“格式信息”，这指的是这个二维码采用的编码格式。
要实现对多二维码准确且稳定的解码，图像预处理十分重要。本算法在图像预处理阶段主要进行了三项工作：直方图均衡化、双边滤波和分块二值化。
其中直方图均衡化用于提高图像对比度，使得边缘信息更加地明显，双边滤波用于过滤掉一些噪声同时尽量不平滑边缘，使用分块滑窗二值化优点是可以比较好地解决图像光照不均匀的问题和避免栅格化。原图以及各个过程的结果展示在下图中。
二值化后的操作就是定位标轮廓提取了，首先使用边缘提取算法，得到二值化图像的边缘，然后依据定位标轮廓的特点从这些边缘中找到定位标轮廓，这部分原理参考了这位前辈的方法[1]。
每个点有两个轮廓。就是两个口，大“口”内部有一个小“口”，所以是两个轮廓，如果把这个“回”放到一个白色的背景下，从左到右，或从上到下画一条线。这条线经过的图案黑白比例大约为：黑白比例为1:1:3:1:1。
下图分别是轮廓提取和定位标识别两个过程的结果图。
这部分基于了一些实际操作的假设：在图像上，同一个二维码的三个定位标之间的距离要小于属于不同二维码的两个定位标的距离，这样就可以很方便地找到属于同一个二维码的定位标了。
找到同一个二维码的定位标后，使用投影几何保直线的方法确定二维码在像素坐标上的四个顶点的位置，比较的绕，先不展开了。得到二维码的四个顶点的像素坐标后，使用OpenCV4的QRCode类进行解码，解码得到的是与二维码实际位置相关的一系列数字，就能得到二维码像素坐标与世界坐标的多组对应关系。然后求解相机位姿，就是经典的PnP（Perspective n Points）问题了，我直接使用了OpenCV的接口进行求解。
按照源码的README，下载源码并进行编译后， 运行下面的指令
cd build
./Test ../5.jpg
一直按键，就能够一步步得到博客提到的所有过程的图片，还会有更详细的过程，比如找到的每个二维码四个顶点的区域，ROI等等
下载数据集，提取码18b3然后进行解压，数据集包含多张图片，以及一个txt文件，记录了每张图片采集时的ROS时间信息运行下面的命令，就能够在当前路径下得到文件trajectory.txt，获得估计出的position信息
./QRPositions path_to_dataset/data/
下载数据集,提取码7ev8 ，采集于相机在做圆周运动时的图像，类似地运行程序得到轨迹信息，得到轨迹如图中蓝色曲线，对蓝色曲线拟合得到红色圆周圆周轨迹
统计红色曲线偏离蓝色圆周的偏差值，得到下图，发现偏差的RMSE误差在3.1cm，还可以
[1]基于opencv 识别、定位二维码 （c++版）