OpenCV4学习笔记（74）——ArUco模块之对aruco标记进行实时姿态估计

在之前的笔记《OpenCV4学习笔记（72）》中，记录了在OpenCV中关于aruco标记的创建和检测这方面的内容，今天就基于aruco标记检测来进一步实现对aruco标记的实时姿态估计。

首先我们需要知道，所谓姿态估计问题就是要确定某个三维物体的方位指向问题，也就是确定以该物体为中心原点的一个坐标系。姿态估计在机器人视觉、动作跟踪和单照相机定标等很多领域都有应用，当然了在不同领域中实现姿态估计的方法也有很多种。在本次笔记中，就来整理一下OpenCV中的ArUco模块对于aruco标记是如何实现姿态估计的。

那么对于aruco标记进行姿态估计，第一步是要先检测到我们的aruco标记，这就涉及到《OpenCV4学习笔记（72）》中的内容了，可以参阅一下。这里直接给出演示代码：

		auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);
		vector<vector<Point2f>>corners, rejectedImgPoints;
		vector<int>ids;
		auto parameters = aruco::DetectorParameters::create();
		aruco::detectMarkers(test_image, dictionary, corners, ids, parameters, rejectedImgPoints);

那么当我们每检测到一个aruco标记，vector>corners中就会获得一个检测到标记的角点列表，我们接下来就通过这个角点列表来对该标记进行姿态估计。

OpenCV中提供了estimatePoseSingleMarkers()这个API来实现对这种简单标记的姿态估计，其参数含义如下：
（1）参数corners： detectMarkers()返回的检测到标记的角点列表，是一个vector>类型的元素；
（2）参数markerLength：aruco标记的实际物理尺寸，也就是我们打印出来的aruco标记的实际尺寸，一般以米为单位；
（3）参数cameraMatrix：用来拍摄aruco标记的相机的内参矩阵，突然发现还没有整理过相机标定这一块内容，有空再补一下嘿嘿(o゜▽゜)o☆;
（4)参数distCoeffs：用来拍摄aruco标记的相机的畸变参数；
（5）参数rvecs：vector类型的向量，其中每个元素为每个标记相对于相机的旋转向量；
（6）参数tvecs：vector类型的向量，其中每个元素为每个标记相对于相机的平移向量。
（7）参数_objPoints：每个标记角点的对应点数组。
estimatePoseSingleMarkers()接收 detectMarkers()检测到的aruco标记（以输出角点列表的方式表示），并分别对每个标记进行姿态估计。因此，每个aruco标记都将返回一个相对于相机的旋转向量和平移矢量，返回的点数组是将标记角点从每个标记坐标系转换到相机坐标系下的表示。
标记坐标系原点位于标记的中心，Z轴垂直于标记平面，每个标记的四个角点在其坐标系中的坐标为：（-markerLength / 2，markerLength / 2，0），（markerLength / 2，markerLength / 2，0），（markerLength / 2，-markerLength / 2 ，0），（-markerLength / 2，-markerLength / 2，0），其中，markerLength是aruco标记的边长。

通过estimatePoseSingleMarkers()这个API，我们就能实现对简单aruco标记的姿态估计了，但是我们还需要把它进行可视化来便于我们查看，所以我们可以绘制出每个aruco标记的坐标轴，从而查看姿态估计的结果。

我们使用drawAxis（）这个API来绘制坐标轴，其参数含义为：
（1）参数image：绘制坐标轴的输入、输出图像（通常是进行检测标记的图像）；
（2）参数cameraMatrix：相机的内参矩阵；
（3）参数distCoeffs：相机的畸变参数；
（4）~（5）参数rvec和参数tvec：当前要绘制坐标轴的物体的姿态参数，分别是旋转向量和平移向量；
（6）参数length：绘制坐标轴的长度，单位通常为米。

那么上面就对aruco标记的姿态估计所需API进行了介绍，下面就进入代码演示和效果演示部分。

首先，我们需要加载相机的内参矩阵和畸变系数，这里是默认标定好了的，直接获取的参数

	cv::Mat cameraMatrix, distCoeffs;
	vector<double> camera = { 657.1548323619423, 0, 291.8582472145741,0, 647.384819351103, 391.254810476919,0, 0, 1 };
	cameraMatrix = Mat(camera);
	cameraMatrix = cameraMatrix.reshape(1,3);
	vector<double> dist = { 0.1961793476399528, -1.38146317350581, -0.002301820186177369, -0.001054637905895881, 2.458286937422959 };
	distCoeffs = Mat(dist);
	distCoeffs = distCoeffs.reshape(1, 1);

然后，调用摄像头

	VideoCapture capture;
	capture.open(0);
	if (!capture.isOpened())
	{
		cout << "can't open camera" << endl;
		exit(-1);
	}

接着在每一帧图像中都进行aruco标记的检测以及姿态估计，并绘制出每一帧中检测到的aruco标记的坐标轴。

	Mat frame;
	while (capture.read(frame))
	{
		Mat test_image;
		resize(frame, test_image, Size(800, 800));
		imshow("test_image", test_image);
		auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);
		vector<vector<Point2f>>corners, rejectedImgPoints;
		vector<int>ids;
		auto parameters = aruco::DetectorParameters::create();
		aruco::detectMarkers(test_image, dictionary, corners, ids, parameters, rejectedImgPoints);
		aruco::drawDetectedMarkers(test_image, corners, ids, Scalar(0, 255, 0));
		//namedWindow("dectect", WINDOW_FREERATIO);
		//imshow("dectect", test_image);

		std::vector<cv::Vec3d> rvecs;
		std::vector<cv::Vec3d> tvecs;
		cv::aruco::estimatePoseSingleMarkers(corners, 0.053, cameraMatrix, distCoeffs, rvecs, tvecs);
		for (int i = 0;i < rvecs.size();i++)
		{
			//绘制坐标轴，检查姿态估计结果
			cv::aruco::drawAxis(test_image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.02);
		}
		//namedWindow("pose", WINDOW_FREERATIO);
		imshow("pose", test_image);

		char ch = cv::waitKey(1);
		if (27 == ch)
		{
			break;
		}
	}

下面是效果演示：



到此我们就实现了对于简单的aruco标记进行实时姿态估计的功能啦，本次笔记也就到此结束。

PS：本人的注释比较杂，既有自己的心得体会也有网上查阅资料时摘抄下的知识内容，所以如有雷同，纯属我向前辈学习的致敬，如果有前辈觉得我的笔记内容侵犯了您的知识产权，请和我联系，我会将涉及到的博文内容删除，谢谢！