aruco码C++检测和位姿估计代码

短命女 2023-06-16 13:58 61阅读 0赞

## 1、Marker检测 ##

对于aruco的检测，是opencv中自带的检测库，进行检测时，需要先进行Marker库的定义，这个库需要与你生成的aruco码（[生成aruco码方法][aruco]）的定义一致，否则也检测不到。该代码可以检测Apriltag，只需将Marker库更换为“DICT\_APRILTAG\_\*\*\*”即可。本文代码是在Windows 环境下vs2019中运行。

// aruco_code_detect.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
    //
    
    #include "pch.h"
    #include <iostream> 
    #include <opencv2/core/core.hpp> 
    #include<opencv2/highgui/highgui.hpp> 
    #include <opencv2/aruco/charuco.hpp>
    #include "opencv2/imgproc.hpp"
    
    using namespace cv;
    using namespace std;
    
    void maker_test(Mat image)
    { 
    
    	cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
    
    	cv::Ptr<cv::aruco::DetectorParameters> params = aruco::DetectorParameters::create();
    	params->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;
    
    	cv::Mat imageCopy;
    
    	image.copyTo(imageCopy);
    	std::vector<int> ids;
    	std::vector<std::vector<cv::Point2f>> corners, rejected;
    	cv::aruco::detectMarkers(image, dictionary, corners, ids, params);
    	
    
    	// if at least one marker detected
    	if (ids.size() > 0) 
    	{ 
    		cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);
    		cv::imshow("test", imageCopy);
    		cv::waitKey();
    	}
    
    
    int main()
    { 
    	Mat img = cv::imread("test.jpg", 1);
    	maker_test(img);	
    }

检测结果如下：  
                               ![在这里插入图片描述][20191126183813734.jpg]

## 2、位姿估计 ##

根据opencv中estimatePoseSingleMarkers()可以得到Marker坐标系到相机坐标系旋转向量和平移向量。

cv::VideoCapture inputVideo;
    inputVideo.open(0);
    cv::Mat cameraMatrix, distCoeffs;
    // camera parameters are read from somewhere
    readCameraParameters(cameraMatrix, distCoeffs);//cameraMatrix为3×3矩阵，distCoeffs为4,58或12个元素，根据你的畸变情况得到
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
    while (inputVideo.grab()) { 
        cv::Mat image, imageCopy;
        inputVideo.retrieve(image);
        image.copyTo(imageCopy);
        std::vector<int> ids;
        std::vector<std::vector<cv::Point2f>> corners;
        cv::aruco::detectMarkers(image, dictionary, corners, ids);
        // if at least one marker detected
        if (ids.size() > 0) { 
            cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);
            std::vector<cv::Vec3d> rvecs, tvecs;
            cv::aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
            // draw axis for each marker
            for(int i=0; i<ids.size(); i++)
                cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
        }
        cv::imshow("out", imageCopy);
        char key = (char) cv::waitKey(waitTime);
        if (key == 27)
            break;
    }

上述得到的旋转和平移向量是，Marker坐标系到相机坐标系的转化，故有：  
![在这里插入图片描述][2020010214485451.png]  
      根据如下旋转和平移关系，可以得到摄像头在Marker坐标系的世界坐标系。![在这里插入图片描述][20200102140400526.png]  
      实际中摄像头在其自身坐标系的坐标为\[0,0,0\]，故此得到摄像头在Marker的世界坐标为：  
![在这里插入图片描述][20200102141334161.png]  
      其中\[Xm,Ym,Zm\]是指在Marker坐标系中的坐标，\[Xc,Yc,Zc\]是指在摄像头坐标系中的坐标，注意坐标轴的建立方向。  
      我自己做了几个利用单个aruco定位的测试，测试数据如下，考虑手动摆放和测量的误差，距离定位精度大致为1~4cm误差，角度定位误差大致为3°。（实际上我测试的是上述公式中的第一种情况，即定位物体中心，而不是摄像头的位置，其中物体中心在摄像头的坐标是\[0,0,L/2\]（L为物体的长度，摄像头安装位置如下图所示，该图为俯视图），但是实际上x,y坐标并不是等于0的，因此测试摄像头的结果，可能误差更小）  
                                                                                       ![在这里插入图片描述][20200108094054827.png]

<table> 
 <thead> 
  <tr> 
   <th>case</th> 
   <th>id</th> 
   <th>x_set</th> 
   <th>y_set</th> 
   <th>theta_set</th> 
   <th>x_detect</th> 
   <th>y_detect</th> 
   <th>theta_detect</th> 
   <th>x_bias</th> 
   <th>y_bias</th> 
   <th>theta_bias</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>1</td> 
   <td>11</td> 
   <td>-90</td> 
   <td>68</td> 
   <td>0</td> 
   <td>91.9</td> 
   <td>66.38</td> 
   <td>2.36</td> 
   <td>-1.9</td> 
   <td>-1.62</td> 
   <td>2.36</td> 
  </tr> 
  <tr> 
   <td>2</td> 
   <td>12</td> 
   <td>59</td> 
   <td>-104</td> 
   <td>90</td> 
   <td>63.03</td> 
   <td>-106.12</td> 
   <td>86.71</td> 
   <td>4.03</td> 
   <td>-2.12</td> 
   <td>-3.29</td> 
  </tr> 
  <tr> 
   <td>3</td> 
   <td>13</td> 
   <td>58</td> 
   <td>209.5</td> 
   <td>-90</td> 
   <td>54.2</td> 
   <td>208.84</td> 
   <td>-91.9</td> 
   <td>-3.8</td> 
   <td>-0.66</td> 
   <td>-1.9</td> 
  </tr> 
  <tr> 
   <td>4</td> 
   <td>14</td> 
   <td>214</td> 
   <td>48</td> 
   <td>180</td> 
   <td>216.3</td> 
   <td>49.8</td> 
   <td>178.79</td> 
   <td>2.3</td> 
   <td>1.8</td> 
   <td>-1.21</td> 
  </tr> 
 </tbody> 
</table>

上述测试中距离单位为厘米，角度单位为度。

[aruco]: https://blog.csdn.net/zhou4411781/article/details/103262408
[20191126183813734.jpg]: https://img-blog.csdnimg.cn/20191126183813734.jpg
[2020010214485451.png]: https://img-blog.csdnimg.cn/2020010214485451.png
[20200102140400526.png]: https://img-blog.csdnimg.cn/20200102140400526.png
[20200102141334161.png]: https://img-blog.csdnimg.cn/20200102141334161.png
[20200108094054827.png]: https://img-blog.csdnimg.cn/20200108094054827.png