Aruco-2-增强型aruco---aruco板

1. Aruco Board

ArUco板是一组marker，在计算相机位姿时，如同对单个marker计算相机位姿一样，但是更加鲁棒。

板不限于这种布置，并且可以表示任何2d或3d布局

ArUco板与一组独立的marker之间的区别在于，板中Marker之间的相对位置是先验的，这样可以将所有Marker的角点用于估计摄像机相对于整个ArUco板的姿态

使用ArUco板的好处是:

• 姿势估计更加通用，因为只需要一些Marker，所以即使存在遮挡或局部视图，也可以计算出姿势(相对于整个板的)
• 由于采用了大量的点对应关系（Marker角），因此获得的姿态通常更准确

1.1. 主要的类

class  Board {
public:
    std::vector<std::vector<cv::Point3f> > objPoints;
    cv::Ptr<cv::aruco::Dictionary> dictionary;
    std::vector<int> ids;
};

• 第一个参数objPoints: 这个板上面的角点在这个板的3D坐标系下的坐标的集合，对于每个marker，它的4个角以标准顺序存储，即顺时针顺序，从左上角开始
• 第二个参数: 指示棋盘Marker属于哪个marker字典
• 第三个参数ids: 表明了在objPoints里面每一个marker在字典中的索引

2. Aruco Board 标定相机

2.1. 使用函数calibrateCameraAruco()

无data，暂时没有进行

3. Aruco Board进行相机位姿估计

在进行相机位姿估计之前，同样需要先进行marker的检测detectMarkers()

3.1. estimatePoseBoard()函数

cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec);

• 第一、二个参数markerCorners，markerIds: marker检测得到的角点和id
• 第三个参数: 上面提到的Board类对象
• 第四、五个参数: 相机参数cameraMatrix和distCoeffs
• 最后两个参数: 平移和旋转(相对于整个Aruco板的)，如果作为数据传入，数据不为空的时候，作为初始位姿估计，然后计算，再作为输出

3.2. 代码

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include <opencv2/aruco/charuco.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

using namespace std;

int main()
{
    // 内参不知道哪个老哥整出来的
    double fx, fy, cx, cy, k1, k2, k3, p1, p2;
    fx = 955.8925;
    fy = 955.4439;
    cx = 296.9006;
    cy = 215.9074;
    k1 = -0.1523;
    k2 = 0.7722;
    k3 = 0;
    p1 = 0;
    p2 = 0;
    // 内参矩阵
    cv::Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
        fx, 0.0, cx,
        0.0, fy, cy,
        0.0, 0.0, 1.0);
    // 畸变矩阵
    cv::Mat distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);

    // 字典读取
    cv::Ptr<cv::aruco::Dictionary> dictionary =
    cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

    // 读取图片
    cv::Mat image, imageCopy;
    image = cv::imread("../../../board.png");
    image.copyTo(imageCopy);

    // board对象指针，在后面有create函数来实际创建
    // 下面这些参数需要用来计算相机位姿
    cv::Ptr<cv::aruco::GridBoard> board =
            cv::aruco::GridBoard::create(5,             //每行多少个Marker
                                         7,             //每列多少个Marker
                                         0.04,          //marker长度
                                         0.01,          //marker之间的间隔
                                         dictionary);   //字典

    std::vector<int> ids;
    std::vector<std::vector<cv::Point2f> > corners;

    std::vector<std::vector<cv::Point2f> > corners_out;

    // 检测Marker
    cv::aruco::detectMarkers(image, dictionary, corners, ids,cv::aruco::DetectorParameters::create(),corners_out);

    // 显示检测到的但是由于字典对不上被拒绝的Marker
    if (corners_out.size()>0){
        cout<<"一共有 "<<corners_out.size()<<" 个被拒绝的 Marker "<<endl;
        for (int idx_rej=0;idx_rej<corners_out.size();idx_rej++) {
            for (int i=0;i<4;i++) {
                cv::circle(imageCopy,cv::Point(corners_out[idx_rej][i].x,corners_out[idx_rej][i].y),6,cv::Scalar(0,0,255));
            }
        }
    }

    // 显示正确的Marker
    if (ids.size() > 0) {
        cout<<"track thing"<<endl;
        // 绘制检测边框
        cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);

        //board->create(corners,dictionary,ids);

//        // 估计相机位姿(相对于每一个marker)
//        std::vector<cv::Vec3d> rvecs, tvecs;
//        cv::aruco::estimatePoseSingleMarkers(corners, 0.055, cameraMatrix, distCoeffs, rvecs, tvecs);

        // 估计相机位姿(相对于 aruco 板)
        cv::Vec3d rvec, tvec;
        int valid = cv::aruco::estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, rvec, tvec);

        // draw axis for each marker
        if(valid)
        {
            /// 得到的位姿估计是：从board坐标系到相机坐标系的
            cv::Mat R;
            cv::Rodrigues(rvec,R);
            cout << "R_{camera<---marker} :" << R << endl;
            cout << "t_{camera<---marker} :" << tvec << endl;
            cout << endl;
            cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1);

            Eigen::Matrix3d R_eigen;
            cv::cv2eigen(R,R_eigen);
            Eigen::Vector3d zyx_Euler_fromR=R_eigen.eulerAngles(0,1,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012,实际上这个编号跟乘法的顺序一致就可以了(从左向又右看的顺序)
            cout<< "zyx euler from Rotation \n[输出顺序为:x,y,z]:\n"<<(180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl;
        }
    }
    cv::imshow("out", imageCopy);
    cv::waitKey(0);

    return 0;
}

运行效果：

这是我个人运行的结果，可能由于图片中的Marker是旧版本的字典，现在已经识别不了了，通过函数返回的reject判断

这是官方自己给出的运行效果

4. Aruco Board 的生成

创建 Aruco Board 对象需要指定环境中每个Marker的角位置。 然而，在许多情况下， Aruco Board 对象只是在同一个平面和网格布局中的一组Marker，所以它可以很容易地打印和使用。

将会使用一个GridBoard类，是继承自 Board 类的一个特殊类，它表示一个在同一个平面中具有所有标记并且在一个网格布局中的 Board。

GridBoard类的坐标系定位在板面上，坐标系原点在板的左下角，z 轴指向外面，如下图所示。轴色对应为

• x: 红色
• y: 绿色
• z: 蓝色

GridBoard类两个函数说明

static Ptr<GridBoard> cv::aruco::GridBoard::create(	
int 	markersX,          // X方向marker数量，即下图NumberX
int 	markersY,          // Y方向marker数量，即下图NumberY
float 	markerLength,      // marker长度，即下图MarkerLength
float 	markerSeparation,  // marker之间的间隔，即下图MarkerSeperation
const Ptr< Dictionary > & 	dictionary, // 字典
int 	firstMarker = 0    //grid board上第一个marker的ID
)

void cv::aruco::GridBoard::draw(	
Size 	outSize,        // 输出图像大小
OutputArray 	img,    // 输出图像
int 	marginSize = 0, // 即上图Margin，即最外面的marker与图像边界之间的距离
int 	borderBits = 1  // 即上图BoderBits,代表每个marker边框大小
)

4.1. 生成并输出一个Aruco Board

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include <opencv2/aruco/charuco.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

using namespace std;

int main()
{
    int markersX = 5;           //X轴上标记的数量
    int markersY = 7;           //Y轴上标记的数量   本例生成5x5的棋盘
    int markerLength = 100;     //标记的长度，单位是像素
    int markerSeparation = 20;  //每个标记之间的间隔，单位像素
    int dictionaryId = cv::aruco::DICT_6X6_250;//生成标记的字典ID
    int margins = markerSeparation;//标记与边界之间的间隔
    int borderBits = 1;         //标记的边界所占的bit位数
    int firstMarker= 0;         //板的第一个Marker id (后面的直接按顺序生成)
    bool showImage = true;

    // 计算输出图像大小
    cv::Size imageSize;
    imageSize.width = markersX * (markerLength + markerSeparation) - markerSeparation + 2 * margins;
    imageSize.height =
        markersY * (markerLength + markerSeparation) - markerSeparation + 2 * margins;

    cv::Ptr<cv::aruco::Dictionary> dictionary =
        cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));

    cv::Ptr<cv::aruco::GridBoard> board =
            cv::aruco::GridBoard::create(markersX, markersY,
                                         float(markerLength),
                                         float(markerSeparation),
                                         dictionary,
                                         firstMarker);

    // show created board
    cv::Mat boardImage;
    board->draw(imageSize, boardImage, margins, borderBits);

    if (showImage) {
        imshow("board", boardImage);
        cv::waitKey(0);
    }

    // save

    return 0;
}

运行效果