Apriltag使用

Apriltag使用

开源项目：https://github.com/AprilRobotics/apriltag

apriltag库是一个纯c的库，基本不依赖任何其他库。

Apriltag目前有版本2和版本3，版本3可兼容版本2.版本3的tag如下图：

tag2如下：

tag3是检测里面四个点，tag2是检测外面四个点。所以同样的大小的tag，tag2的位姿精度会更高。但tag3的检测速度更快(是tag2的两倍以上)。考虑位姿精度，目前我们使用tag2。

我们实际使用4个tag，id分别是tag36h11的1,2,3,4。

代码实现步骤

1. 构建apriltag检测器并配置参数

    apriltag_family_t* tf =tag36h11_create();
    tag_det = apriltag_detector_create();
    apriltag_detector_add_family(tag_det, tf);
    tag_det->quad_decimate = 2;　//降低图片分辨率，经实验该参数可大幅提升检测时间且不影响检测结果
    tag_det->refine_edges = 0;　
    tag_det->quad_sigma = 0;　//高斯降噪，如果噪声较大的图片，可使用该参数
    tag_det->nthreads = 4;　//使用几个线程来运行
    tag_det->debug = 0;
    
   
   

   这几个参数可以改，可以看看这几个参数的具体含义根据实际需求进行调试。

2. 图片去畸变　apriltag默认输入图片为无畸变图片，我们必须输入去畸变图片。
   
   

实验结果显示，去畸变和不去畸变检测结果相差较大，这是由于marker的检测原理导致(检测直线段，未去畸变的是曲线)。故不可采用先检测角点，再将角点去畸变的方式。

图片去畸变较快的方法：

cv::initUndistortRectifyMap(intrinsics,distCoeffs,Mat(),intrinsics, cv::Size(COL,ROW), CV_32FC1, map1,map2); //只执行一次
 cv::remap(grayImage,un_gray_Image,map1,map2,CV_INTER_LINEAR);

3. 检测角点（apriltag是第三方库，网上有很多资料，比我们写的文档更好。）

   image_u8_t image = { .width = m_image.cols,
                            .height = m_image.rows,
                            .stride = m_image.cols,
                            .buf = m_image.data
                          }; //转为apriltag库的格式
   
    zarray_t *detections = apriltag_detector_detect(tag_det, &image);//检测tag
   //利用zarray_size(detections)可知检测到几个tag。
   for (int i = 0; i < zarray_size(detections); i++)
       {
           apriltag_detection_t *det; 
           zarray_get(detections, i, &det);　//将检测结果放入det里面
      }
   

   我们看看apriltag_detection_t结构体

   typedef struct apriltag_detection apriltag_detection_t;
   struct apriltag_detection
   {
       apriltag_family_t *family;
       int id;//tag的id
   
       int hamming;
       float decision_margin;
   
       matd_t *H;　//代表从(-1,1), (1,1), (1,-1),(-1,-1）转到像素坐标
       double c[2];　//tag中心点在像素坐标下的坐标
       double p[4][2]; //像素坐标下的坐标，４个点从左下角开始逆时针旋转
   };
   

4. 计算marker转到相机坐标的位姿(注意是从marker转到相机)

   首先谈谈坐标系，有三个坐标系，像素坐标系(二维)、tag坐标系(三维)、相机坐标系(三维)。我们以tag坐标系为基准，计算出相机的位姿。

   四个角点是从左下角逆时针旋转。四个角点在tag坐标系(遵循右手原则)下的坐标如下图。(s是tag的长度的一半),在
   

   

   计算tag转到相机坐标系，目前有两种方法，一种是直接使用april里面的位姿估计，一种是我们用3d-2d　pnp求解。

   a. 构建pnp求解(不需要管库的坐标系，按照我们定义的坐标系即可)

   opencv的pnp求解

   $ p = K(RP + t) $

   P是第0帧图像下的坐标(即世界坐标系)，p是当前帧的像素坐标，求解的R t是从第0帧图像（世界坐标系）转到当前帧的位姿。opencv用的JPL坐标系，从global 到 local.

   我们现在已知1.四个角点在tag坐标系下的坐标((-s,-s,0), (s,-s,0), (s,s,0), (-s,s,0))，2.四个角点在相机的像素坐标系下的坐标。我们需要求解tag坐标系转到camera坐标系的R,t。

   将第0帧图像下的坐标轴换成tag坐标系，认为tag坐标系就是世界坐标系。恰好就可以套用opencv的pnp。

   代码：

   objectPoints.push_back(cv::Point3d(-tag_half_size,-tag_half_size, 0));
   objectPoints.push_back(cv::Point3d( tag_half_size,-tag_half_size, 0));
   objectPoints.push_back(cv::Point3d( tag_half_size, tag_half_size, 0));
   objectPoints.push_back(cv::Point3d(-tag_half_size, tag_half_size, 0));
   imagePoints.push_back(cv::Point2d(det->p[0][0], det->p[0][1]));
   imagePoints.push_back(cv::Point2d(det->p[1][0], det->p[1][1]));
   imagePoints.push_back(cv::Point2d(det->p[2][0], det->p[2][1]));
   imagePoints.push_back(cv::Point2d(det->p[3][0], det->p[3][1]));
   cv::solvePnP(objectPoints, imagePoints, intrinsics, tmp_distCoeffs, rvec, tvec);
   //objectPoints就是上面4个点在tag坐标系下的3d坐标　imagePoints就是像素坐标系下的坐标
   

   b.使用apriltag自带的库求解。这就会涉及库的坐标系和我们的坐标系转换。

apriltag_detection_info_t tag_info;
tag_info.cx=cameraParam.m_cx;
tag_info.cy=cameraParam.m_cy;
tag_info.fx=cameraParam.m_fx;
tag_info.fy=cameraParam.m_fy;
tag_info.tagsize=find_mark.length;
tag_info.det=det;
apriltag_pose_t pose;

estimate_tag_pose(&tag_info, &pose);
Vector3d ori_relative_P;
Matrix3d ori_rotation_matrix3d;
memcpy(&ori_relative_P, pose.t->data, sizeof(Vector3d));
memcpy(&ori_rotation_matrix3d, pose.R->data, sizeof(Matrix3d));

目前我们使用a的做法。

5. 调试经验：

   1.相机位姿的精度主要取决于tag四个角点的检测像素精度。根据以前测试经验，存在角点误检，导致位姿误差大。主要有两种情况：
   一、运动(特别是转弯)过程中照片有点模糊
   二、相机与tag存在较大的倾角(30度以外误差比较大)
   三、距离越远，误差越大，2m以外谨慎使用。
   可能还要其他情况导致误检。
   另外相机的内参也会影响计算的位姿，一定要标定好相机内参(重投影误差<0.15)，做好畸变校正。
   故需要加入严格的判断，最好让相机是正对mark。