Apriltag使用

Apriltag使用

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

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

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

Apriltag使用_第1张图片

tag2如下:

Apriltag使用_第2张图片
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默认输入图片为无畸变图片,我们必须输入去畸变图片。
    Apriltag使用_第3张图片
    Apriltag使用_第4张图片

实验结果显示,去畸变和不去畸变检测结果相差较大,这是由于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);
  1. 检测角点(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个点从左下角开始逆时针旋转
    };
    
  2. 计算marker转到相机坐标的位姿(注意是从marker转到相机)

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

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

    Apriltag使用_第6张图片

    计算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的做法。

  1. 调试经验:

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

你可能感兴趣的:(vlsam,自动驾驶,opencv,机器学习)