OpenCV检测Marker位姿

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

　　参考代码如下：

 #include "pch.h"

 #include <Eigen/Dense>
 #include <opencv2/core.hpp>
 #include <opencv2\highgui.hpp>
 #include <opencv2\aruco.hpp>
 #include <opencv2\aruco\dictionary.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 ;
     }
     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 = ;

     cam.Open(params);

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

     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>(, )
                 << 713.12554931640625000, 0.0, 634.99163818359375000, 0.0,
                 714.41278076171875000, 363.88098144531250000, 0.0, 0.0, 1.0);

             //畸变校正
             const Mat  arucodistCoeffs = (Mat_<float>(, ) << -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(, );
             // detect markers and estimate pose
             detectMarkers(image, dictionary, corners, ids, detectorParams, rejected);//rejected拒绝的矩形区域
             image.copyTo(imageCopy);
             if (ids.size() > )
             {
                 drawDetectedMarkers(imageCopy, corners, ids);
                 std::vector <cv::Vec3d> rvecs，tvecs;
                 estimatePoseSingleMarkers(corners,0.05, intrinsic_matrix, arucodistCoeffs, rvecs, tvecs);
                 //0.05为Marker的大小
                 Rodrigues(rvecs[], R, noArray());//罗德里格斯变换将旋转矩阵变为旋转向量
                 MatrixXd r(, );
                 VectorXd t();
                 VectorXd T_mm();
                 cv2eigen(R, r);
                 cv2eigen(tvecs[], t);
                 cv2eigen(tvecs[], T_mm);
                 T_mm = T_mm * ;
                 M(, ) = r(, ); M(, ) = r(, ); M(, ) = r(, ); M(, ) = t(, );
                 M(, ) = r(, ); M(, ) = r(, ); M(, ) = r(, ); M(, ) = t(, );
                 M(, ) = r(, ); M(, ) = r(, ); M(, ) = r(, ); M(, ) = t(, );
                 M(, ) = ; M(, ) = ; M(, ) = ; M(, ) = ;

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

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

             }

             imshow("out", imageCopy);
             char key = (char)waitKey();
             if (key == ) break;
         }

     }
     return ;
 }