上篇介绍了基于二维码识别方法，本章介绍将基于Apriltag的二维码位姿识别与基于自适应粒子滤波的定位方法相结合可以实现基于二维码的重定位效果。
首先通过二维码识别方法到二维码相对于相机的位姿。通过ROS的坐标转换工具将二维码相对于像素坐标系的位姿转换到机器人相对于相机坐标系下。至此便得到了二维码相对于相机的位姿信息，再通过坐标转换到机器人坐标系下，便得到了二维码相对于机器人坐标系的位姿信息。
接着在导航时同步打开二维码识别与保存功能包。通过将二维码相对于相机的坐标系进一步转换到相对于地图坐标系的位姿，这一步便得到了二维码在地图坐标系下的位姿。然后将二维码在地图坐标系下的位姿和二维码的ID信息（如图1所示）保存下来用于重定位。
图1 保存的二维码ID与位姿
最后在进行自适应粒子滤波时，同步打开二维码识别与位姿发布功能包。首先将保存到的二维码ID和位姿信息从文件中读取出来。当机器人位姿丢失时识别到二维码后，得到了当前时刻二维码相对于机器人的位姿信息。在导航时，已经保存了二维码在地图中的位姿。通过坐标系转换，可以用二维码在地图中的位姿与二维码在机器人坐标系中的位姿的逆相乘则得到了当前时刻机器人在地图中的位姿。将该位姿作为初始位姿（如下图2所示），以该位姿作为准确位姿在地图上按照一定协方差矩阵撒粒子。当机器人进一步运动时，机器人通过自适应粒子滤波定位方法进一步在地图上收敛到准确位姿。至此便完成了整个基于二维码的重定位过程。
图2 发布重定位位姿
机器人通过识别二维码进行重定位可以精确调整自身的位姿，以满足回桩的精度要求。
