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);	
}

    检测结果如下:
                                                         aruco码C++检测和位姿估计代码_第1张图片

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

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

你可能感兴趣的:(图像处理)