使用Apriltags实现定位、评估定位精度的全流程记录

上周末做实验测试了Apriltags二维码的定位精度，现在将整体流程记录下来，便于后续参考与存档。
参考博客：
https://blog.csdn.net/qq_41839222/article/details/89606859
https://book.openmv.cc/image/apriltag.html

AprilTag是一个视觉基准系统，可用于AR，机器人定位和相机校准等视觉任务，能够计算相机在二维码坐标系下的精确位置与方向。简单来说，只要把Apriltag二维码贴到目标物体上，就可以在OpenMV上识别出这个标签的空间位置。

1、安装流程

官网链接：https://april.eecs.umich.edu/software/apriltag.html
源码地址：https://github.com/AprilRobotics/apriltag_ros

1）、安装环境：
当前我的电脑上已配置好的环境有ubuntu 16.04系统、Kinetic的ROS平台、Intel Realsense D435与Intel Realsense ZR300的相机驱动，包括相机的SDK库以及ROS包、同时已经编译了VINS-Monos的SLAM框架，并能够正常工作（承接本人的上一篇博客：搭建Vins-Mono环境与评估EuRoc定位精度、安装RealSense ZR300、D435驱动）。
Apriltags的安装非常简单，将源码放到工作空间路径下，如catkin_ws/src，然后编译即可。

2）、安装命令：

cd catkin_ws/src
git clone https://github.com/AprilRobotics/apriltag_ros.git
cd ..
catkin_make

2、生成Apriltags二维码

1）、安装OpenMV：
下载链接：https://singtown.com/openmv-download/ 下载合适自己系统的OpenMV版本。
安装命令如下：

#给openmv-ide-linux-x86_64-2.1.0.run权限
chmod +x ./openmv-ide-linux-*.run
#执行安装
./openmv-ide-linux-*.run

之后会弹出安装界面，一直点击Next，直到安装完成即可。

2）、生成Apriltags二维码：
单击 bin/openmvide 文件启动OpenMV IDE，在Tool—>Machine Vision—>AprilTag Generate中选择family，推荐使用TAG36H11。
然后填写需要生成的二维码个数，选择图片存放的文件夹。最后在该文件夹会生成二维码图片，并把图片打印出来即可。

3、运行Apriltags2_ros

1）、修改配置文件：
首先修改apriltags_ros/launch/continuous_detection.launch文件。
Apriltags2需要订阅相机图像数据与相机信息，比如对于Realsense ZR300，在开启相机时会发布两个相关的topic：/zr300_node/color/image_raw与/camera/camera_info，因此需将camera_name与camera_frame的内容修改为订阅的topic。

<launch>
    <arg name="launch_prefix" default="" /> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
    <arg name="node_namespace" default="apriltags2_ros_continuous_node" />
    <arg name="camera_name" default="/zr300_node/color" />
    <arg name="camera_frame" default="image_raw" />
    <arg name="image_topic" default="image_rect_color" />

之后修改apriltags_ros/config/tags.yaml文件，添加需要检测的二维码ID与自己测量得到的二维码尺寸大小（单位：米）。

standalone_tags:
  [
      {id: 0, size: 0.047},
      {id: 2, size: 0.061},
      {id: 1, size: 0.073}      
  ]

注意事项：
1、同一ID的二维码不能在该配置文件中以不同的大小出现两次，也不能出现在一张图片的两个地方。这些都将在检测中产生歧义。
2、确保打印的tag至少包含1位宽的白色边框，AprilTag2算法会对周围的白色边框进行采样。

2）、运行Apriltags2_ros：
分别打开四个终端，运行如下命令行：

#启动Realsense ZR300相机驱动
roslaunch maplab_realsense maplab_realsense.launch

#运行apriltags2_ros算法，持续检测
roslaunch apriltags2_ros continuous_detection.launch

# rviz可视化界面
rosrun rviz rviz

#输出最后的定位数据
rostopic echo /tf

最后正确运行的界面如下图所示：

4、坐标系变换

从最后apriltags2_ros算法的定位数据可知，该算法输出的是相机在二维码坐标系下的定位坐标，包含平移矢量position（也是位置坐标）与旋转四元素orientation。由于在实验过程中，我最终希望得到二维码在世界坐标系下的位置来求得定位误差，因此需要进行坐标系变换。
首先通过平移矢量与四元素计算二维码在相机坐标系下的位置，将旋转矩阵求逆即可，我的python代码如下：

#将四元数变换为4x4旋转矩阵
def quaternion_to_rotation_matrix(quat):
    q = quat.copy()
    n = np.dot(q, q)
    if n < np.finfo(q.dtype).eps:
        return np.identity(4)
    q = q * np.sqrt(2.0 / n)
    q = np.outer(q, q)
    rot_matrix = np.array(
        [[1.0 - q[2, 2] - q[3, 3], q[1, 2] + q[3, 0], q[1, 3] - q[2, 0], 0.0],
         [q[1, 2] - q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] + q[1, 0], 0.0],
         [q[1, 3] + q[2, 0], q[2, 3] - q[1, 0], 1.0 - q[1, 1] - q[2, 2], 0.0],
         [0.0, 0.0, 0.0, 1.0]],
        dtype=q.dtype)
    return rot_matrix

# 输入apriltags2_ros算法定位得到的四元数orientation和平移矢量position数据
orientation = {'x': 0.994908116006, 'y': 0.0222186926304, 'z': 0.0238889195828, 'w': 0.0953597919644}
position = {'x': 0.150438059249, 'y': 0.280975087468, 'z': 0.564888109148}

quaternion = np.asarray([orientation['w'], orientation['x'], orientation['y'], orientation['z']])
translation = np.asarray([position['x'], position['y'], position['z']])

qua2rota = quaternion_to_rotation_matrix(quaternion)  #四元素转4x4旋转矩阵
T_qua2rota = qua2rota
for i in range(3):
    T_qua2rota[i][3] = T_qua2rota[i][3] + translation[i] #在第四列加平移矢量
invT_qua2rota = np.linalg.inv(T_qua2rota) #矩阵求逆

camintag_position = np.asarray([0, 0, 0, 1])  #转为齐次坐标
tagincam_position = np.dot(invT_qua2rota, camintag_position) #两个矩阵相乘

之后只需获得相机坐标系到世界坐标系的转换关系，就可以求得二维码在世界坐标系下的位置。
这里参考博客https://blog.csdn.net/sinat_29886521/article/details/77506426 ，介绍了如何通过两个坐标系的对应点计算二者的转换关系。我之前已经在Vins-Mono框架下获得了相机在几个空间位置点处的定位数据，该数据在相机坐标系下，又在真实的空间环境中测量了该位置点在世界坐标系下的坐标，通过对应点就可以求得相机坐标系变换到世界坐标系时的旋转矩阵与平移矢量。我的python代码如下：

def align(model,data):
    """  
    Input:
    model -- first trajectory (3xn) 相机坐标系下的n个点坐标
    data -- second trajectory (3xn) 世界坐标系下的n个点坐标
    
    Output:
    rot -- rotation matrix (3x3) 旋转矩阵
    trans -- translation vector (3x1) 平移矢量
    trans_error -- translational error per point (1xn) 将相机坐标系下的点转换到世界坐标系后与真值的均方根误差（两点距离）    
    """
numpy.set_printoptions(precision=3,suppress=True)
model_zerocentered = model - model.mean(1)
data_zerocentered = data - data.mean(1)
   
W = numpy.zeros( (3,3) )
for column in range(model.shape[1]):
    #for column in range(3):
    W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
S = numpy.matrix(numpy.identity( 3 ))
if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
    S[2,2] = -1
rot = U*S*Vh
trans = data.mean(1) - rot * model.mean(1)   

model_aligned = rot * model + trans
alignment_error = model_aligned - data   
trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]    
return rot,trans,trans_error

到这里，使用Apriltags进行定位的全部流程就搞定啦～
我测量的定位精度大约在4-6cm，感觉误差挺大的，后续再深入学习一下看看具体哪里有问题。

冬日头秃的乔木同学
记于2019年11月20日