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