ArUco

一、ArUco简介

  姿态估计（Pose estimation）在计算机视觉领域扮演着十分重要的角色：如机器人导航、目标跟踪和相机定标等。所谓姿态估计问题就是要确定某个三维物体的方位指向问题，也就是确定以该物体为中心原点的一个坐标系。这一过程的基础是找到现实世界和图像投影之间的对应点。这通常是困难的，因此我们使用Marker解决这一问题。
  aruco模块基于ArUco库，这是一个检测二进制marker的非常流行的库。
  aruco标记其实就是一种编码，就和我们日常生活中的二维码是相似的，只不过由于编码方式的不同，导致它们存储信息的方式、容量等等有所差异。由于单个aruco标记就可以提供足够的对应关系，例如有四个明显的角点及内部的二进制编码，所以aruco标记被广泛用来增加从二维世界映射到三维世界时的信息量，便于发现二维世界与三维世界之间的投影关系，从而实现姿态估计、相机矫正等等应用。

aruco的函数包含在#include <opencv2/aruco.hpp>，该库主要的类主要有：
aruco::Marker----视觉标志类；
aruco::MarkerDetector----视觉标志检测类；
aruco::MarkerPoseTracker----视觉标志姿态预估类；
aruco::MarkerMap-----视觉标志地图类；
aruco::MarkerMapPoseTracker----视觉标志地图姿态预估类；
aruco::CvDrawingUtils----绘图类；

二、Marker和字典

  ArUco marker是一个由二进制矩阵组成的正方形标记。它由一个宽黑色边框和一个内部的二进制矩阵组成。黑色边框有利于快速检测到图像，内部二进制编码用于识别标记和提供错误检测和纠正。marker的尺寸的大小决定了内部矩阵的大小。例如，一个4x4的marker由16 位二进制数（16bits）组成。
  
一些marker图像示例：

  应当注意到，我们需要检测到一个Marker在空间中发生了旋转，但是，检测的过程需要确定它的初始角度，所以每个角落需要是明确的，不能有歧义，保证上述这点也是靠二进制编码完成的。
  markers的字典是在一个特殊应用中使用到的marker的集合。这仅仅是每个marker的二进制编码的链表。
  字典的主要性质是字典的大小和marker的大小：字典的大小是组成字典的marker的数量；marker的大小是这些marker的尺寸（位的个数）。
  aruco模块包含了一些预定义的字典，这些字典涵盖了一系列的字典大小和Marker尺寸。
  一个marker的 id 仅是marker在它所在的字典的下标，由内部的矩阵决定。例如，一个字典里的五个marker的 id 是：0,1,2,3 和 4。

三、步骤

1.创建Marker（Marker Creation）

cv::Mat markerImage; 
cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); 
cv::aruco::drawMarker(dictionary, 23, 200, markerImage, 1);

getPredefinedDictionary():
  在我们创建aruco标记之前，需要先指定一个字典。这个字典表示的是创建出来的aruco标记具有怎样的尺寸、怎样的编码等等内容。我们使用这个API来声明我们使用的字典。在OpenCV中，提供了多种预定义字典，我们可以通过PREDEFINED_DICTIONARY_NAME来查看有哪些预定义字典。而且字典名称表示了该字典的aruco标记数量和尺寸，例如DICT_6X6_250表示一个包含了250种6x6位标记的字典。
  
drawMarker()：
  确定好我们需要的字典后，就可以通过这个API来生成aruco标记，其参数含义如下：
（1）参数dictionary: 之前创建的 Dictionary 对象；
（2）参数id：marker 的 id，表示绘制字典中的哪一个aruco标记。每个字典由不同数量的标记组成，id 有效范围是 [ 0，字典包含的标记数 ），任何超出有效范围的特定 id 都会产生异常；
（3）参数sidepixel： 输出标记图像的尺寸，输出标记图像的尺寸为Size(sidepixel，sidepixel)（此参数应足够大以存储特定字典的位数，至少需要满足（sidepixel - 标记的边长）>= 2；并且为了避免输出标记图像变形，sidepixel应与位数 + 边界大小成比例，或者至少比标记尺寸大得多，以使变形不明显）；
（4）参数img：输出的标记图像；
（5）参数borderBist：可选，用于指定标记黑色边框的宽度，例如borderBist=2 表示边框的宽度等于两个内部像素的大小，默认值 borderBist=1。
  
  这样，我们就创建了一个选定字典的aruco标记了，下面我们创建5个不同的aruco标记来对比下，演示代码如下：

	//创建aruco标记
	Mat marker;
	auto dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME::DICT_6X6_250);
	for (int i = 0;i < 5;i++)
	{
		aruco::drawMarker(dictionary, i, 200, marker, 1);
		string windowName = "marker" + to_string(i);
		imshow(windowName, marker);
		string path ="D:\\"+ windowName + ".jpg";
		imwrite(path, marker);
	}

  效果如下：

2.检测Marker（Marker Detection）

cv::Mat inputImage; 
vector< int > markerIds;
vector< vector<Point2f> > markerCorners, rejectedCandidates;
cv::aruco::DetectorParameters parameters; 
cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); 
cv::aruco::detectMarkers(inputImage, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);

detectMarkers()：
  当我们把aruco标记打印出来后贴在任意其它地方后，再进行拍摄，就可以对aruco标记进行检测。这个API用于检测aruco标记，其参数含义如下：
（1）参数image：输入的需要检测标记的图像；
（2）参数dictionary：进行检测的字典对象，这里的字典就是我们创建aruco标记时所使用的字典，检测什么类型的aruco标记就使用什么类型的字典；
（3）参数corners：检测到的aruco标记的角点列表，对于每个标记，其四个角点均按其原始顺序返回（从右上角开始顺时针旋转），第一个角是右上角，然后是右下角，左下角和左上角；

注意这里的返回角点顺序，在OpenCV官方文档中记录的是从左上角开始进行旋转，而经过测试，发现是以右上角为起点的。
（4）检测到的每个标记的 id，需要注意的是第三个参数和第四个参数具有相同的大小；
（5）参数parameters： DetectionParameters 类的对象，该对象包括在检测过程中可以自定义的所有参数；
（6）参数rejectedImgPoints：抛弃的候选标记列表，即检测到的、但未提供有效编码的正方形。每个候选标记也由其四个角定义，其格式与第三个参数相同，该参数若无特殊要求可以省略。
  
drawDetectedMarkers():
  当我们进行aruco标记检测后，为了方便我们观察，通常需要进行可视化操作，也就是把检测到的aruco标记绘制出来，我们使用这个API来绘制检测到的aruco标记，其参数含义如下：
（1）参数image: 是将绘制标记的输入 / 输出图像（通常就是检测到标记的图像）；
（2）参数corners：检测到的aruco标记的角点列表；
（3）参数ids：检测到的每个标记对应到其所属字典中的id；
（4）参数borderColor：绘制标记外框的颜色。
  
  对aruco标记进行检测并绘制的演示代码如下：

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

  效果如下：



  可以看到每个aruco标记的边框上，都有一个绿色矩形框，而在标记的中心，有一个 id 值，这个 id 就是该标记在其所属字典中的 id。
  到这里，就完成了对aruco标记的创建和检测。后续的姿态估计、相机标定等等操作都是建立在此基础之上的。

3.姿态估计（Pose Estimation）

Mat cameraMatrix, distCoeffs;  
vector< Vec3d > rvecs, tvecs; 
cv::aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);

  接下来要通过Marker检测来获取相机pose。
  为了获取相机pose，需要知道相机的校准（Calibration）参数，即相机矩阵和畸变系数。
  注：当用ArUco marker来检测相机Pose时，可以单独地检测每个Marker的pose。如果想要从一堆Marker里检测出一个pose，需要的是aruco板。（参见ArUco板教程）
  涉及到marker的相机pose是一个从marker坐标系统到相机坐标系统的三维变换。这是由一个旋转和一个平移向量确定的（参见 solvePnP() 函数）。

  当我们每检测到一个aruco标记，vector<vector<Point2f>>corners中就会获得一个检测到标记的角点列表，我们接下来就通过这个角点列表来对该标记进行姿态估计。
  
estimatePoseSingleMarkers()：
  OpenCV中提供了这个API来实现对这种简单标记的姿态估计，其参数含义如下：
（1）参数corners： detectMarkers()返回的检测到标记的角点列表，是一个vector<vector<Point2f>>类型的元素；
（2）参数markerLength：aruco标记的实际物理尺寸，也就是我们打印出来的aruco标记的实际尺寸，一般以米为单位；
（3）参数cameraMatrix：相机的内参矩阵；
（4）参数distCoeffs：相机的畸变参数；
（5）参数rvecs：vector<cv::Vec3d>类型的向量，其中每个元素为每个标记相对于相机的旋转向量；
（6）参数tvecs：vector<cv::Vec3d>类型的向量，其中每个元素为每个标记相对于相机的平移向量。
（7）参数_objPoints：每个标记角点的对应点数组。
  
  接收 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标记的边长。
  
  通过这个API，我们就能实现对简单aruco标记的姿态估计了，但是我们还需要把它进行可视化来便于我们查看，所以我们可以绘制出每个aruco标记的坐标轴，从而查看姿态估计的结果。
  
drawAxis()：
  我们使用这个API来绘制坐标轴，其参数含义为：
（1）参数image：绘制坐标轴的输入、输出图像（通常是进行检测标记的图像）；
（2）参数cameraMatrix：相机的内参矩阵；
（3）参数distCoeffs：相机的畸变参数；
（4）~（5）参数rvec和参数tvec：当前要绘制坐标轴的物体的姿态参数，分别是旋转向量和平移向量；
（6）参数length：绘制坐标轴的长度，单位通常为米。

  我们需要加载相机的内参矩阵和畸变系数，这里是默认标定好了的，直接获取的参数，演示代码如下：

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

  下面是效果演示：