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

1、Marker检测

       对于aruco的检测，是opencv中自带的检测库，进行检测时，需要先进行Marker库的定义，这个库需要与你生成的aruco码（生成aruco码方法）的定义一致，否则也检测不到。该代码可以检测Apriltag，只需将Marker库更换为“DICT_APRILTAG_***”即可。

// aruco_code_detect.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include "pch.h"
#include   
#include   
#include  
#include 
#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);	
}

    检测结果如下：
                                                         

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坐标系到相机坐标系的转化，故有：

      根据如下旋转和平移关系，可以得到摄像头在Marker坐标系的世界坐标系。
      实际中摄像头在其自身坐标系的坐标为[0,0,0]，故此得到摄像头在Marker的世界坐标为：

      其中[Xm,Ym,Zm]是指在Marker坐标系中的坐标，[Xc,Yc,Zc]是指在摄像头坐标系中的坐标，注意坐标轴的建立方向。
      我自己做了几个利用单个aruco定位的测试，测试数据如下，考虑手动摆放和测量的误差，距离定位精度大致为1~4cm误差，角度定位误差大致为3°。（实际上我测试的是上述公式中的第一种情况，即定位物体中心，而不是摄像头的位置，其中物体中心在摄像头的坐标是[0,0,L/2]（L为物体的长度，摄像头安装位置如下图所示，该图为俯视图），但是实际上x,y坐标并不是等于0的，因此测试摄像头的结果，可能误差更小）
                                                                                       

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

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