目标检测与位姿估计（二十三）：OpenCV+Aruco完成目标检测

一份识别图像图像中所有Aruco的代码

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <opencv2/aruco.hpp>
#include <stdio.h>

using namespace cv;
using namespace std;

int main(int argc, char *argv[])

{
//内参与畸变矩阵，笔者在前面的博客已经给出求解方法，有需要的可以找找看看
    double fx,fy,cx,cy,k1,k2,k3,p1,p2;
    fx=955.8925;
    fy=955.4439;
    cx=296.9006;
    cy=215.9074;
    k1=-0.1523;
    k2=0.7722;
    k3=0;
    p1=0;
    p2=0;

    Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
        fx, 0.0, cx,
        0.0, fy, cy,
        0.0, 0.0, 1.0);
    Mat distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);
    cv::VideoCapture inputVideo;
    inputVideo.open(1);
    // 创建一个字典，该字典内储存了设定数量的Aruco码用于检测
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);  
    
    // 检测字典是否建立成功
    //cv::Mat MarkerImage;
    //cv::aruco::drawMarker(dictionary, 23, 200, MarkerImage, 1);
    //cv::imshow("Marker", MarkerImage);

    while (inputVideo.grab()) {
        cv::Mat image, imageCopy;
        inputVideo.retrieve(image);                                  // 抓取视频中的一张照片
        image.copyTo(imageCopy);
        std::vector<int> ids;                                        // 储存结果id
        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;                     // 储存结果R/t
            cv::aruco::estimatePoseSingleMarkers(corners, 0.055, cameraMatrix, distCoeffs, rvecs, tvecs); // 求解旋转矩阵rvecs和平移矩阵tvecs
            cout<<"R :"<<rvecs[0]<<endl;
            cout<<"T :"<<tvecs[0]<<endl;
            // 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);        // 画出坐标轴 
        }
        else {
            cout<<"No Object Detected"<<endl;
        }
        cv::imshow("out", imageCopy);
        cv::waitKey(10);
        //if (key == 27)1
        // break;
    }
return 0;
}

一份只识别特定ID的Aruco的代码

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <opencv2/aruco.hpp>
#include <stdio.h>

using namespace cv;
using namespace std;

int main(int argc, char *argv[])

{
//内参与畸变矩阵，笔者在前面的博客已经给出求解方法，有需要的可以找找看看
    double fx,fy,cx,cy,k1,k2,k3,p1,p2;
    fx=955.8925;
    fy=955.4439;
    cx=296.9006;
    cy=215.9074;
    k1=-0.1523;
    k2=0.7722;
    k3=0;
    p1=0;
    p2=0;

    int ID = 3;

    Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
        fx, 0.0, cx,
        0.0, fy, cy,
        0.0, 0.0, 1.0);
    Mat distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);
    cv::VideoCapture inputVideo;
    inputVideo.open(1);
    // 创建一个字典，该字典内储存了设定数量的Aruco码用于检测
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);  
    
    // 检测字典是否建立成功
    //cv::Mat MarkerImage;
    //cv::aruco::drawMarker(dictionary, 23, 200, MarkerImage, 1);
    //cv::imshow("Marker", MarkerImage);

    while (inputVideo.grab()) {
        cv::Mat image, imageCopy;
        inputVideo.retrieve(image);                                  // 抓取视频中的一张照片
        image.copyTo(imageCopy);
        std::vector<int> ids, op_ids;                                        // 储存结果id
        std::vector<std::vector<cv::Point2f>> corners, op_corners;               // 储存结果位置
        cv::aruco::detectMarkers(image, dictionary, corners, ids);   // 检测靶标
        // if at least one marker detected
        if (ids.size() > 0) {
            for (int t=0;t<ids.size();t++){
                if (ids[t] == ID){
                    op_ids.push_back(ids[t]);
                    op_corners.push_back(corners[t]);
                }
                //cout<<"ids"<<ids[t]<<endl;
            }
            if (op_ids.size()>0){
                cv::aruco::drawDetectedMarkers(imageCopy, op_corners, op_ids); // 绘制检测到的靶标的框
                std::vector<cv::Vec3d> rvecs, tvecs;                     // 储存结果R/t
                cv::aruco::estimatePoseSingleMarkers(op_corners, 0.055, cameraMatrix, distCoeffs, rvecs, tvecs); // 求解旋转矩阵rvecs和平移矩阵tvecs
                cout<<"R :"<<rvecs[0]<<endl;
                cout<<"T :"<<tvecs[0]<<endl;
                // draw axis for each marker
                for(int i=0; i<op_ids.size(); i++)
                    cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);        // 画出坐标轴 
            }
            else {
                cout<<"No Object Detected"<<endl;
            }
        }
        cv::imshow("out", imageCopy);
        cv::waitKey(10);
        //if (key == 27)1
        // break;
    }
return 0;
}

常用函数解析

cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);

创建一个字典，该字典内储存了设定数量的Aruco码用于检测。目前还不知是什么原因，总之4x4的Aruco比6x6的Aruco更容易检测到

参数cv::aruco有4/5/6/7几种尺寸以及50/100/250/1000多种数量可供选择，详见：OpenCV: ArUco Marker Detection

cv::aruco::drawMarker(dictionary, 23, 200, MarkerImage, 1);

用于画出某个Aruco，检测dictionary是否设置正确

void cv::aruco::detectMarkers	(	InputArray	image,
		const Ptr< Dictionary > &	dictionary,
		OutputArrayOfArrays	corners,
		OutputArray	ids,
		const Ptr< DetectorParameters > &	parameters = DetectorParameters::create(),
		OutputArrayOfArrays	rejectedImgPoints = noArray(),
		InputArray	cameraMatrix = noArray(),
		InputArray	distCoeff = noArray()
	)

检测出输入的image参数中所有由dictionary定义的Aruco
image 输入图像
dictionary 指定待搜索的字典
corners 检测到的Aruco标记角向量。 对于每个标记，提供了它的四个角（例如 std::vector<std::vector<cv::Point2f> > ）。 对于N个检测到的标记，该阵列的维度为Nx4。 角的顺序是顺时针。
ids 检测到的Aruco地id。标识符是int类型（例如 std::vector<int>）。 对于 N 个检测到的标记，id 的大小也是 N。标识符的顺序与imgPoints数组中的标记的顺序相同。
parameters 标记检测参数rejectImgPoints包含那些内部代码没有正确编码的方块的imgPoints。 用于调试目的。
cameraMatreix和distCoeff两个参数不需要传入

void cv::aruco::estimatePoseSingleMarkers	(	InputArrayOfArrays	corners,
		float	markerLength,
		InputArray	cameraMatrix,
		InputArray	distCoeffs,
		OutputArray	rvecs,
		OutputArray	tvecs,
		OutputArray	_objPoints = noArray()
	)

此函数接收检测到的标记并单独返回它们相对于相机的姿态估计。 因此，对于每个标记，返回一个旋转和平移向量。 返回的转换是将点从每个标记坐标系转换到相机坐标系的转换。 标记坐标系统以标记的中间为中心，Z 轴垂直于标记平面。 标记的四个角在其自身坐标系中的坐标为：(-markerLength/2,markerLength/2,0),(markerLength/2,markerLength/2,0),(markerLength/2,-markerLength/2 , 0), (-markerLength/2, -markerLength/2, 0)

corners 检测到的Aruco标记角向量。 对于每个标记，提供了它的四个角（例如 std::vector<std::vector<cv::Point2f> > ）。 对于N个检测到的标记，该阵列的维度为Nx4。 角的顺序是顺时针。
markerLength 标记边的长度。返回的平移向量将在同一单位中。通常单位为米。
cameraMatrix 一个3x3的相机内参矩阵
distCoeffs 失真系数向量
rvecs 输出旋转向量矩阵
tvecs 输出转移向量矩阵
_objPoints 所有标记角的对象点数组

注意这句函数是对每个被检测到的Aruco估计位姿并储存在输出中