AprilTag3求取位姿

参考：

1）https://github.com/AprilRobotics/apriltag

2）April Tag定位和识别 - 知乎

3) AprilTag标记跟踪 · OpenMV中文入门教程​​​​​​

4)  如何快速生成不同系列的Apriltag码？-CSDN博客

5）https://www.cnblogs.com/code-fun/p/17787552.html

二、生成AprilTag二维码方式

1.通过OpenMV生成

  下载openmv软件：http://openmv.cc

• 

2.官网下载

。。。。。。。。。。。。。。。。

三、相机标定

3.1 标定板制作

标定板：https://github.com/MegviiRobot/CamLaserCalibraTool/tree/master/doc​​​​​​

下的april_6x6_80x80cm_A0.pdf，然后打印出来，固定好相机采集待标定图像。

3.2 标定

标定代码：GitHub - ethz-asl/kalibr: The Kalibr visual-inertial calibration toolbox

标定主要获取相机的内参矩阵和畸变矩阵。

四、位姿估计

4.1 源码：

位姿估计代码:https://github.com/AprilRobotics/apriltag

4.2 环境配置

安装opencv4、Eigen3

4.3 更改源码

#include <iostream>

#include "opencv2/opencv.hpp"

#include <eigen3/Eigen/Dense>  //necessary before including the opencv eigen lib

#include <opencv2/core/eigen.hpp> //include linear calculation lib

#include<opencv2/imgcodecs.hpp>

extern "C" {

#include "apriltag.h"

#include "apriltag_pose.h" //pose estimation lib

#include "tag36h11.h"

#include "tag25h9.h"

#include "tag16h5.h"

#include "tagCircle21h7.h"

#include "tagCircle49h12.h"

#include "tagCustom48h12.h"

#include "tagStandard41h12.h"

#include "tagStandard52h13.h"

#include "common/getopt.h"

}

using namespace std;

using namespace cv;

 

int main(int argc, char *argv[])

{

    getopt_t *getopt = getopt_create();

    getopt_add_bool(getopt, 'h', "help", 0, "Show this help");

    getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");

    getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");

    getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");

    getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads");

    getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor");

    getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");

    getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");

    getopt_add_string(getopt, 'a', "axis", "world", "Choose axis to use (world/camera)");

    if (!getopt_parse(getopt, argc, argv, 1) ||

            getopt_get_bool(getopt, "help")) {

        printf("Usage: %s [options]\n", argv[0]);

        getopt_do_usage(getopt);

        exit(0);

    }

    // Initialize camera

    VideoCapture cap("/home/zc/apriltag/apriltag-with-pose-estimation-master/TP.mp4");

    if (!cap.isOpened()) {

        cerr << "Couldn't open video capture device" << endl;

        return -1;

    }

    // Initialize tag detector with options

    apriltag_family_t *tf = NULL;

    const char *famname = getopt_get_string(getopt, "family");

    if (!strcmp(famname, "tag36h11")) {

        tf = tag36h11_create();

    } else if (!strcmp(famname, "tag25h9")) {

        tf = tag25h9_create();

    } else if (!strcmp(famname, "tag16h5")) {

        tf = tag16h5_create();

    } else if (!strcmp(famname, "tagCircle21h7")) {

        tf = tagCircle21h7_create();

    } else if (!strcmp(famname, "tagCircle49h12")) {

        tf = tagCircle49h12_create();

    } else if (!strcmp(famname, "tagStandard41h12")) {

        tf = tagStandard41h12_create();

    } else if (!strcmp(famname, "tagStandard52h13")) {

        tf = tagStandard52h13_create();

    } else if (!strcmp(famname, "tagCustom48h12")) {

        tf = tagCustom48h12_create();

    } else {

        printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");

        exit(-1);

    }

    const char *axisname = getopt_get_string(getopt, "axis");

    unsigned int coordinate;

    if(!strcmp(axisname,"world"))

        coordinate = 1;

    else

        coordinate = 0;

//  input camera intrinsic matrix

    apriltag_detection_info_t info;

    info.tagsize = 0.0335;    //0.146-0.012*4;   //The size of Tag

    info.det = NULL;

    info.fx = 553.762;   //848.469;   //focal length of x

    info.fy = 577.223; //847.390;   //focal length of y

    // info.tagsize = 0.146-0.012*4;   //The size of Tag

    // info.det = NULL;

    // info.fx = 848.469;   //focal length of x

    // info.fy = 847.390;   //focal length of y

    // info.cx = cap.get(CAP_PROP_FRAME_WIDTH)/2;

    // info.cy = cap.get(CAP_PROP_FRAME_HEIGHT)/2;

    info.cx = 761.361;

    info.cy = 525.239;

    // /**自己添加畸变参数*/

    // // 相机内参矩阵

    Mat camera_matrix = (Mat_<double>(3, 3) << 5.53762634e+02, 0., 7.61361084e+02, 0., 5.77223877e+02,

       5.25239990e+02, 0., 0., 1.);

    // 相机畸变系数

    Mat dist_coeffs = (Mat_<double>(5, 1) << 2.04713028e-02, -1.52593590e-02, 5.94881538e-04,

       -1.23523809e-02, 3.54151428e-03);

   

    apriltag_detector_t *td = apriltag_detector_create();

    apriltag_detector_add_family(td, tf);

    td->quad_decimate = getopt_get_double(getopt, "decimate");

    td->quad_sigma = getopt_get_double(getopt, "blur");

    td->nthreads = getopt_get_int(getopt, "threads");

    td->debug = getopt_get_bool(getopt, "debug");

    td->refine_edges = getopt_get_bool(getopt, "refine-edges");

    Mat frame, gray;

    while (true) {

        cap >> frame;

        Mat frame_new,frameP;

        frame_new=frame.clone();

        int rows = frame_new.rows, clos = frame_new.cols;

        cv::Size imageSize(clos, rows);

        Mat map1, map2;

        const double alpha = 0;  //

        Mat NewCameraMatrix = getOptimalNewCameraMatrix(camera_matrix,dist_coeffs, imageSize, alpha, imageSize, 0);

        initUndistortRectifyMap(camera_matrix,dist_coeffs, cv::Mat(), NewCameraMatrix, imageSize, CV_16SC2, map1, map2);

        remap(frame_new,frameP,map1, map2, INTER_LINEAR);

        cvtColor(frameP, gray, COLOR_BGR2GRAY);

       

        // Make an image_u8_t header for the Mat data

        image_u8_t im = { .width = gray.cols,

            .height = gray.rows,

            .stride = gray.cols,

            .buf = gray.data

        };

        zarray_t *detections = apriltag_detector_detect(td, &im);

        // Draw detection outlines

        for (int i = 0; i < zarray_size(detections); i++) {

            apriltag_detection_t *det;

            zarray_get(detections, i, &det);

            info.det = det;

            apriltag_pose_t pose;

            estimate_tag_pose(&info, &pose);

            //calculate cam position in world coordinate

            if (coordinate)

            {

                Mat rvec(3, 3, CV_64FC1, pose.R->data); //rotation matrix

                Mat tvec(3, 1, CV_64FC1, pose.t->data); //translation matrix

               

                for(int i=0;i<3;i++)

                {

                    for(int j=0;j<3;j++)

                    {

                        cout<<" "<<rvec.ptr<float>(i)[j];

                    }

                    cout<<endl;

                }

                cout<<"-------------------"<<endl;

                for(int i=0;i<3;i++)

                {

                    for(int j=0;j<3;j++)

                    {

                        cout<<" "<<tvec.ptr<float>(i)[j];

                    }

                    cout<<endl;

                }

                Mat Pos(3, 3, CV_64FC1);

                //可解算出世界坐标系下相机的位置。如果需要计算相机坐标系下标签的位置，则不需要加上“-”号。

                Pos = -rvec.inv() * tvec;

                cout << "x: " << Pos.ptr<double>(0)[0] << endl;

                cout << "y: " << Pos.ptr<double>(1)[0] << endl;

                cout << "z: " << Pos.ptr<double>(2)[0] << endl;

                cout << "-----------world--------------" << endl;

            }

            else

            {

                cout << "x: " << pose.t ->data[0] << endl;

                cout << "y: " << pose.t ->data[1] << endl;

                cout << "z: " << pose.t ->data[2] << endl;

                cout << "-----------camera-------------" << endl;

            }

            //draw the line and show tag ID

            line(frameP, Point(det->p[0][0], det->p[0][1]),

                     Point(det->p[1][0], det->p[1][1]),

                     Scalar(0, 0xff, 0), 2);

            line(frameP, Point(det->p[0][0], det->p[0][1]),

                     Point(det->p[3][0], det->p[3][1]),

                     Scalar(0, 0, 0xff), 2);

            line(frameP, Point(det->p[1][0], det->p[1][1]),

                     Point(det->p[2][0], det->p[2][1]),

                     Scalar(0xff, 0, 0), 2);

            line(frameP, Point(det->p[2][0], det->p[2][1]),

                     Point(det->p[3][0], det->p[3][1]),

                     Scalar(0xff, 0, 0), 2);

            stringstream ss;

            ss << det->id;

            String text = ss.str();

            int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;

            double fontscale = 1.0;

            int baseline;

            Size textsize = getTextSize(text, fontface, fontscale, 2,

                                            &baseline);

            putText(frameP, text, Point(det->c[0]-textsize.width/2,

                                       det->c[1]+textsize.height/2),

                    fontface, fontscale, Scalar(0xff, 0x99, 0), 2);

        }

        apriltag_detections_destroy(detections);

        imshow("Tag Detections", frameP);

        if (waitKey(30) >= 0)

            break;

    }

    apriltag_detector_destroy(td);

    if (!strcmp(famname, "tag36h11")) {

        tag36h11_destroy(tf);

    } else if (!strcmp(famname, "tag25h9")) {

        tag25h9_destroy(tf);

    } else if (!strcmp(famname, "tag16h5")) {

        tag16h5_destroy(tf);

    } else if (!strcmp(famname, "tagCircle21h7")) {

        tagCircle21h7_destroy(tf);

    } else if (!strcmp(famname, "tagCircle49h12")) {

        tagCircle49h12_destroy(tf);

    } else if (!strcmp(famname, "tagStandard41h12")) {

        tagStandard41h12_destroy(tf);

    } else if (!strcmp(famname, "tagStandard52h13")) {

        tagStandard52h13_destroy(tf);

    } else if (!strcmp(famname, "tagCustom48h12")) {

        tagCustom48h12_destroy(tf);

    }

    getopt_destroy(getopt);

    return 0;

}

4.4 编译

mkdir build && cd build

cmake ..

make

sudo make install

4.5 执行

./opencv_demo f tag36h11 a world