OpenCV检测Marker位姿

• Marker检测采用小觅相机，可以实时检测Marker的位置和姿态，效果如下：

　　参考代码如下：

#include "pch.h"

#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2highgui.hpp>
#include <opencv2aruco.hpp>
#include <opencv2arucodictionary.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp> 
#include <opencv2/opencv.hpp>

#include <mynteyed/camera.h>
#include <mynteyed/utils.h>

#include <vector>
#include <iostream>
#include <Windows.h>
#include <fstream>

using namespace std;
using namespace cv;
using namespace Eigen;
using  namespace aruco;


int main(int argc, char *argv[]) {
    mynteyed::Camera cam;
    mynteyed::DeviceInfo dev_info;
    if (!mynteyed::util::select(cam, &dev_info)) {
        return 1;
    }
    mynteyed::util::print_stream_infos(cam, dev_info.index);

    std::cout << "Open device: " << dev_info.index << ", "
        << dev_info.name << std::endl << std::endl;
    //设置相机的参数
    mynteyed::OpenParams params(dev_info.index);
    //params.depth_mode = mynteyed::DepthMode::DEPTH_GRAY;
    //params.stream_mode = mynteyed::StreamMode::
    params.stream_mode = mynteyed::StreamMode::STREAM_2560x720;
    params.color_mode = mynteyed::ColorMode::COLOR_RECTIFIED;
    //params.ir_intensity = 4;
    params.framerate = 30;

    cam.Open(params);

    

    Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
    Mat out;
    dictionary->drawMarker(100, 600, out, 5);

    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
       

    for (;;) {
        auto Left_color = cam.GetStreamData(mynteyed::ImageType::IMAGE_LEFT_COLOR);
        if (Left_color.img)
        {
            Mat image = Left_color.img->To(mynteyed::ImageFormat::COLOR_BGR)->ToMat();
            Mat  imageCopy;
            //相机内参矩阵
            const Mat  intrinsic_matrix = (Mat_<float>(3, 3)
                << 713.12554931640625000, 0.0, 634.99163818359375000, 0.0,
                714.41278076171875000, 363.88098144531250000, 0.0, 0.0, 1.0);
            
            //畸变校正
            const Mat  arucodistCoeffs = (Mat_<float>(1, 5) << -0.29668807983398438, 0.07767868041992188,
                0.00000000000000000, -0.00012969970703125,
                0.00000000000000000);
            vector< int > ids;
            vector< vector< Point2f > > corners, rejected;
            vector< Vec3d > rvecs, tvecs;
            Mat  R;
            MatrixXd M(4, 4);
            // detect markers and estimate pose
            detectMarkers(image, dictionary, corners, ids, detectorParams, rejected);//rejected拒绝的矩形区域
            image.copyTo(imageCopy);
            if (ids.size() > 0)
            {
                drawDetectedMarkers(imageCopy, corners, ids);
                std::vector <cv::Vec3d> rvecs，tvecs;
                estimatePoseSingleMarkers(corners,0.05, intrinsic_matrix, arucodistCoeffs, rvecs, tvecs);
                //0.05为Marker的大小
                Rodrigues(rvecs[0], R, noArray());//罗德里格斯变换将旋转矩阵变为旋转向量
                MatrixXd r(3, 3);
                VectorXd t(3);
                VectorXd T_mm(3);
                cv2eigen(R, r);
                cv2eigen(tvecs[0], t);
                cv2eigen(tvecs[0], T_mm);
                T_mm = T_mm * 1000;
                M(0, 0) = r(0, 0); M(0, 1) = r(0, 1); M(0, 2) = r(0, 2); M(0, 3) = t(0, 0);
                M(1, 0) = r(1, 0); M(1, 1) = r(1, 1); M(1, 2) = r(1, 2); M(1, 3) = t(1, 0);
                M(2, 0) = r(2, 0); M(0, 1) = r(2, 1); M(2, 2) = r(2, 2); M(2, 3) = t(2, 0);
                M(3, 0) = 0; M(3, 1) = 0; M(3, 2) = 0; M(3, 3) = 1;

                cout << "R :" << r << endl;
                cout << "T :" << T_mm << endl;

                for (int i = 0; i < ids.size(); i++)
                {
                    cv::aruco::drawAxis(imageCopy, intrinsic_matrix, arucodistCoeffs, rvecs[i], tvecs[i], 0.05);
                }

            }

            imshow("out", imageCopy);
            char key = (char)waitKey(1);
            if (key == 27) break;
        }
        
    }
    return 0;
}