4-基于ArUco相机姿态评估

1-简介

基于ArUco评估相机姿态，可以使用OPENCV的外部库（opencv_contrib）中的aruco模块，可以参考安装目录（库目录）：
…\opencv_contrib-4.5.4\modules\aruco\src\

使用时需要添加头文件：

#include<opencv2/aruco.hpp>

using namespace cv::aruco;

2-创建Aruco

void createAruco() 
{

    Ptr<Dictionary> d = getPredefinedDictionary(DICT_6X6_250);

    RNG rng(12345);

    for (size_t i = 0; i < 9; i++)
    {
        char fileName[1024];
        Mat makerImage;
        drawMarker(d, static_cast<int>(rng.uniform(0, 250)), 200, makerImage, 1);
        sprintf_s(fileName, "D:/C++Working/image/maker/%d.png", i);
        imwrite(fileName, makerImage);
    }
}

应当注意到，我们需要检测到一个Marker在空间中发生了旋转，但是，检测的过程需要确定它的初始角度，所以每个角落需要是明确的，不能有歧义，保证上述这点也是靠二进制编码完成的。
markers的字典是在一个特殊应用中使用到的marker的集合。这仅仅是每个marker的二进制编码的链表。
字典的主要性质是字典的大小和marker的大小：
字典的大小是组成字典的marker的数量
marker的大小是这些marker的尺寸（位的个数）
aruco模块包含了一些预定义的字典，这些字典涵盖了一系列的字典大小和Marker尺寸。
有些人可能会认为Marker的id是从十进制转成二进制的。但是，考虑到较大的marker会有较多的位数，管理如此多的数据不那么现实，这并不可能。反之，一个marker的id仅需是marker在它所在的字典的下标。例如，一个字典里的五个marker的id是：0,1,2,3和4。
更多有关字典的信息在“选择字典”部分提及。

Ptr<Dictionary> d = getPredefinedDictionary(DICT_6X6_250);
...
drawMarker(d, static_cast<int>(rng.uniform(0, 250)), 200, makerImage, 1);

首先，我们通过选择aruco模块中一个预定义的字典来创建一个字典对象，具体而言，这个字典是由250个marker组成的，每个marker的大小为6x6bits(DICT_6X6_250)

     drawMarker的参数如下：

第一个参数是之前创建的字典对象。
第二个参数是marker的id，在这个例子中选择的是字典DICT_6X6_250第23个marker。注意到每个字典是由不同数目的Marker组成的，在这个例子中，有效的Id数字范围是0到249。不在有效区间的特定id将会产生异常。
第三个参数，200，是输出Marker图像的大小。在这个例子中，输出的图像将是200x200像素大小。注意到这一参数需要满足能够存储特定字典 的所有位。所以，举例而言，你不能为6x6大小的marker生成一个5x5图像（这还没有考虑到Marker的边界）。除此之外，为了避免变形，这一参数最好和位数+边界的大小成正比，至少要比marker的大小大得多（如这个例子中的200)，这样变形就不显著了。
第四个参数是输出的图像。
最终，最后一个参数是一个可选的参数，它指定了Marer黑色边界的大小。这一大小与位数数目成正比。例如，值为2意味着边界的宽度将会是2的倍数。默认的值为1。

3-检测Aruco

给定一个可以看见ArUco marker的图像，检测程序应当返回检测到的marker的列表。每个检测到的marker包括：

图像四个角的位置（按照原始的顺序）
marker的Id
marker检测过程由以下两个主要步骤构成：

检测有哪些marker。在这一阶段我们分析图像，以找到哪些形状可以被识别为Markers。首先要做的是利用自适应性阈值来分割marker，然后从阈值化的图像中提取外形轮廓，并且舍弃那些非凸多边形的，以及那些不是方形的。我们还使用了一些额外的滤波（来剔除那些过小或者过大的轮廓，过于相近的凸多边形，等）
检测完marker之后，我们有必要分析它的内部编码来确定它们是否确实是marker。此步骤首先提取每个标记的标记位。为了达到这个目的，首先，我们需要对图像进行透视变换，来得到它规范的形态（正视图）。然后，对规范的图像用Ossu阈值化以分离白色和黑色位。这一图像根据marker大小和边界大小被分为不同格子，我们统计落在每个格子中的黑白像素数目来决定这是黑色还是白色的位。最终，我们分析这些位数来决定这个marker是属于哪个特定字典的，如果有必要的话，需要对错误进行检测。
一、读取参数文件

static bool readDetectorParameters(string filename, Ptr<aruco::DetectorParameters>& params) {
    FileStorage fs(filename, FileStorage::READ);
    if (!fs.isOpened())
        return false;
    fs["adaptiveThreshWinSizeMin"] >> params->adaptiveThreshWinSizeMin;
    fs["adaptiveThreshWinSizeMax"] >> params->adaptiveThreshWinSizeMax;
    fs["adaptiveThreshWinSizeStep"] >> params->adaptiveThreshWinSizeStep;
    fs["adaptiveThreshConstant"] >> params->adaptiveThreshConstant;
    fs["minMarkerPerimeterRate"] >> params->minMarkerPerimeterRate;
    fs["maxMarkerPerimeterRate"] >> params->maxMarkerPerimeterRate;
    fs["polygonalApproxAccuracyRate"] >> params->polygonalApproxAccuracyRate;
    fs["minCornerDistanceRate"] >> params->minCornerDistanceRate;
    fs["minDistanceToBorder"] >> params->minDistanceToBorder;
    fs["minMarkerDistanceRate"] >> params->minMarkerDistanceRate;
    fs["cornerRefinementMethod"] >> params->cornerRefinementMethod;
    fs["cornerRefinementWinSize"] >> params->cornerRefinementWinSize;
    fs["cornerRefinementMaxIterations"] >> params->cornerRefinementMaxIterations;
    fs["cornerRefinementMinAccuracy"] >> params->cornerRefinementMinAccuracy;
    fs["markerBorderBits"] >> params->markerBorderBits;
    fs["perspectiveRemovePixelPerCell"] >> params->perspectiveRemovePixelPerCell;
    fs["perspectiveRemoveIgnoredMarginPerCell"] >> params->perspectiveRemoveIgnoredMarginPerCell;
    fs["maxErroneousBitsInBorderRate"] >> params->maxErroneousBitsInBorderRate;
    fs["minOtsuStdDev"] >> params->minOtsuStdDev;
    fs["errorCorrectionRate"] >> params->errorCorrectionRate;
    return true;
}

二、参数文件

%YAML:1.0
nmarkers: 1024
adaptiveThreshWinSizeMin: 3
adaptiveThreshWinSizeMax: 23
adaptiveThreshWinSizeStep: 10
adaptiveThreshWinSize: 21
adaptiveThreshConstant: 7
minMarkerPerimeterRate: 0.03
maxMarkerPerimeterRate: 4.0
polygonalApproxAccuracyRate: 0.05
minCornerDistanceRate: 0.05
minDistanceToBorder: 3
minMarkerDistance: 10.0
minMarkerDistanceRate: 0.05
cornerRefinementMethod: 0
cornerRefinementWinSize: 5
cornerRefinementMaxIterations: 30
cornerRefinementMinAccuracy: 0.1
markerBorderBits: 1
perspectiveRemovePixelPerCell: 8
perspectiveRemoveIgnoredMarginPerCell: 0.13
maxErroneousBitsInBorderRate: 0.04
minOtsuStdDev: 5.0
errorCorrectionRate: 0.6

三、检测

void detectAruco() {

    Ptr<cv::aruco::DetectorParameters> parameters = aruco::DetectorParameters::create();
    bool readOk = readDetectorParameters("detector_params.yml", parameters);
    if (!readOk) {
        cerr << "Invalid detector parameters file" << endl;
        return ;
    }

    VideoCapture captrue(1);
    if (!captrue.isOpened()) {
        return;
    }
    Mat img;
    while (captrue.read(img))
    {
       // flip(img, img, 1);
        vector<int> markerIds;
        vector< vector<Point2f> > markerCorners, rejectedCandidates;
        Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
        cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
        drawDetectedMarkers(img, markerCorners, markerIds);
        imshow("ss", img);

        char c = waitKey(300);
        if (c == 27) {
            break;
        }
    }
}

detectMarkers 的参数为：

第一个参数是待检测marker的图像。
第二个参数是字典对象，在这一例子中是之前定义的字典 (DICT_6X6_250).
检测出的markers存储在markerCorners 和 markerIds结构中：
markerCorners 是检测出的图像的角的列表。对于每个marker，将返回按照原始顺序排列的四个角（从左上角顺时针开始）。因此，第一个点是左上角的角，紧接着右上角、右下角和左下角。
markerIds 是在markerCorners检测出的所有maker的id列表.注意返回的markerCorners和markerIds 向量具有相同的大小。
第四个参数是类型的对象 DetectionParameters. 这一对象包含了检测阶段的所有参数。这一参数将在 下一章节详细介绍。
最后的参数, rejectedCandidates, 返回了所有的marker候选, 例如， 那些被检测出来的不是有效编码的方形。每个候选同样由四个角定义， 它的 形式和markerCorners的参数一样。这一参数可以省略，它仅仅用于debug阶段，或是用于“再次寻找”策略（见refineDetectedMarkers()）

detectMarkers()之后，接下来你想要做的事情可能是检查你的marker是否被正确地检测出来了。幸运的是，aruco模块提供了一个函数，它能在输入图像中来绘制检测出来的markers，这个函数就是drawDetectedMarkers() ,例子如下：

 cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);

img 是输入/输出图像，程序将在这张图上绘制marker。（它通常就是检测出marker的那张图像）
markerCorners 和 markerIds 是检测出marker的结构，它们的格式和 detectMarkers()函数提供的一样。

4-姿态评估

接下来你想要做的应当是通过Marker检测来获取相机pose。
为了展现相机的Pose检测，你需要知道你的相机的校准（Calibration）参数。这是一个相机矩阵和畸变系数。如果你不知道如何校准你的相机，你可以看一看calibrateCamera() 函数，以及OpenCV的校准教程。你同样可以使用aruco模块来校准你的相机，这在使用aruco进行校准的教程中将会介绍。注意这个过程只需要做一次，除非你的相机的光学性质发生了改变（例如调焦）

    最后，在校准之后我们得到的是相机矩阵：这是一个3x3的矩阵，包含了焦距和相机中心坐标（相机的内参），以及畸变系数：一个包含五个以上元素的向量，它描述的是相机产生的畸变。

    当你用ArUco marker来检测相机Pose时，你可以单独地检测每个Marker的pose。如果你想要从一堆Marker里检测出一个pose，你需要的是aruco板。

    涉及到marker的相机pose是一个从marker坐标系统到相机坐标系统的三维变换。这是由一个旋转和一个平移向量确定的（参见 solvePnP() 函数）

    aruco模块提供了一个函数，用来检测所有探测到的Marker的pose。

void estimatePose() {

    Ptr<cv::aruco::DetectorParameters> parameters = aruco::DetectorParameters::create();
    bool readOk = readDetectorParameters("detector_params.yml", parameters);
    if (!readOk) {
        cerr << "Invalid detector parameters file" << endl;
        return;
    }

    VideoCapture captrue(1);
    if (!captrue.isOpened()) {
        return;
    }
    Mat img;
    Mat cameraMatrix = (Mat_<float>(3, 3) << 6.0097346876749680e+02, 0., 3.2031703441387049e+02, 0.,
        6.0225171288478975e+02, 2.3947860507800820e+02, 0., 0., 1.);
    cout << cameraMatrix << endl;
    Mat distCoeffs = (Mat_<float>(5, 1) << -1.3711862233491057e-01, 2.7024113413198220e-01,
        9.1669430855309032e-04, -3.3899628828653657e-03, 0.);
    cout << distCoeffs << endl;
    while (captrue.read(img))
    {
        // flip(img, img, 1);
        vector<int> markerIds;
        vector< vector<Point2f> > markerCorners, rejectedCandidates;
        Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
        cv::aruco::detectMarkers(img, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
        if (markerIds.size() > 0) {
            drawDetectedMarkers(img, markerCorners, markerIds);
            vector<cv::Vec3d> rvecs, tvecs;
            cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs); // draw axis for each marker 
            for (int i = 0; i < markerIds.size(); i++) {
                cv::aruco::drawAxis(img, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
                cout << tvecs[i] << endl;
            }
                
                
        }
       
        imshow("ss", img);

        char c = waitKey(300);
        if (c == 27) {
            break;
        }
    }
}

一、相机参数标定

 Mat cameraMatrix = (Mat_<float>(3, 3) << 6.0097346876749680e+02, 0., 3.2031703441387049e+02, 0.,
        6.0225171288478975e+02, 2.3947860507800820e+02, 0., 0., 1.);
    cout << cameraMatrix << endl;
    Mat distCoeffs = (Mat_<float>(5, 1) << -1.3711862233491057e-01, 2.7024113413198220e-01,
        9.1669430855309032e-04, -3.3899628828653657e-03, 0.);
    cout << distCoeffs << endl;

相机的参数标定输入时注意单位，一般都选择以***米***做单位。

二、姿态评估

 vector<cv::Vec3d> rvecs, tvecs;
 cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);

corners 参数是marker的角向量，是由detectMarkers() 函数返回的。
第二个参数是marker的大小（单位是米或者其它）。注意Pose检测的平移矩阵单位都是相同的。
cameraMatrix 和 distCoeffs 是需要求解的相机校准参数。
rvecs（姿态角饶Z轴、Y轴、X轴的旋转角） 和 tvecs 分别是每个markers角的旋转和平移向量。

这一函数获取的marker坐标系统处在marker重心，Z坐标指向纸面外部，如下图所示。坐标的颜色为,X:红色，Y：绿色，Z：蓝色。