OpenCV检测Marker位姿

Marker检测采用小觅相机，可以实时检测Marker的位置和姿态，效果如下：
　　参考代码如下：
  1 #include "pch.h"
  2 
  3 #include <Eigen/Dense>
  4 #include <opencv2/core.hpp>
  5 #include <opencv2\highgui.hpp>
  6 #include <opencv2\aruco.hpp>
  7 #include <opencv2\aruco\dictionary.hpp>
  8 #include <opencv2/calib3d.hpp>
  9 #include <opencv2/core/eigen.hpp> 
 10 #include <opencv2/opencv.hpp>
 11 
 12 #include <mynteyed/camera.h>
 13 #include <mynteyed/utils.h>
 14 
 15 #include <vector>
 16 #include <iostream>
 17 #include <Windows.h>
 18 #include <fstream>
 19 
 20 using namespace std;
 21 using namespace cv;
 22 using namespace Eigen;
 23 using  namespace aruco;
 24 
 25 
 26 int main(int argc, char *argv[]) {
 27     mynteyed::Camera cam;
 28     mynteyed::DeviceInfo dev_info;
 29     if (!mynteyed::util::select(cam, &dev_info)) {
 30         return 1;
 31     }
 32     mynteyed::util::print_stream_infos(cam, dev_info.index);
 33 
 34     std::cout << "Open device: " << dev_info.index << ", "
 35         << dev_info.name << std::endl << std::endl;
 36     //设置相机的参数
 37     mynteyed::OpenParams params(dev_info.index);
 38     //params.depth_mode = mynteyed::DepthMode::DEPTH_GRAY;
 39     //params.stream_mode = mynteyed::StreamMode::
 40     params.stream_mode = mynteyed::StreamMode::STREAM_2560x720;
 41     params.color_mode = mynteyed::ColorMode::COLOR_RECTIFIED;
 42     //params.ir_intensity = 4;
 43     params.framerate = 30;
 44 
 45     cam.Open(params);
 46 
 47     
 48 
 49     Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250);
 50     Mat out;
 51     dictionary->drawMarker(100, 600, out, 5);
 52 
 53     Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
 54        
 55 
 56     for (;;) {
 57         auto Left_color = cam.GetStreamData(mynteyed::ImageType::IMAGE_LEFT_COLOR);
 58         if (Left_color.img)
 59         {
 60             Mat image = Left_color.img->To(mynteyed::ImageFormat::COLOR_BGR)->ToMat();
 61             Mat  imageCopy;
 62             //相机内参矩阵
 63             const Mat  intrinsic_matrix = (Mat_<float>(3, 3)
 64                 << 713.12554931640625000, 0.0, 634.99163818359375000, 0.0,
 65                 714.41278076171875000, 363.88098144531250000, 0.0, 0.0, 1.0);
 66             
 67             //畸变校正
 68             const Mat  arucodistCoeffs = (Mat_<float>(1, 5) << -0.29668807983398438, 0.07767868041992188,
 69                 0.00000000000000000, -0.00012969970703125,
 70                 0.00000000000000000);
 71             vector< int > ids;
 72             vector< vector< Point2f > > corners, rejected;
 73             vector< Vec3d > rvecs, tvecs;
 74             Mat  R;
 75             MatrixXd M(4, 4);
 76             // detect markers and estimate pose
 77             detectMarkers(image, dictionary, corners, ids, detectorParams, rejected);//rejected拒绝的矩形区域
 78             image.copyTo(imageCopy);
 79             if (ids.size() > 0)
 80             {
 81                 drawDetectedMarkers(imageCopy, corners, ids);
 82                 std::vector <cv::Vec3d> rvecs，tvecs;
 83                 estimatePoseSingleMarkers(corners,0.05, intrinsic_matrix, arucodistCoeffs, rvecs, tvecs);
 84                 //0.05为Marker的大小
 85                 Rodrigues(rvecs[0], R, noArray());//罗德里格斯变换将旋转矩阵变为旋转向量
 86                 MatrixXd r(3, 3);
 87                 VectorXd t(3);
 88                 VectorXd T_mm(3);
 89                 cv2eigen(R, r);
 90                 cv2eigen(tvecs[0], t);
 91                 cv2eigen(tvecs[0], T_mm);
 92                 T_mm = T_mm * 1000;
 93                 M(0, 0) = r(0, 0); M(0, 1) = r(0, 1); M(0, 2) = r(0, 2); M(0, 3) = t(0, 0);
 94                 M(1, 0) = r(1, 0); M(1, 1) = r(1, 1); M(1, 2) = r(1, 2); M(1, 3) = t(1, 0);
 95                 M(2, 0) = r(2, 0); M(0, 1) = r(2, 1); M(2, 2) = r(2, 2); M(2, 3) = t(2, 0);
 96                 M(3, 0) = 0; M(3, 1) = 0; M(3, 2) = 0; M(3, 3) = 1;
 97 
 98                 cout << "R :" << r << endl;
 99                 cout << "T :" << T_mm << endl;
100 
101                 for (int i = 0; i < ids.size(); i++)
102                 {
103                     cv::aruco::drawAxis(imageCopy, intrinsic_matrix, arucodistCoeffs, rvecs[i], tvecs[i], 0.05);
104                 }
105 
106             }
107 
108             imshow("out", imageCopy);
109             char key = (char)waitKey(1);
110             if (key == 27) break;
111         }
112         
113     }
114     return 0;
115 }
116 
117