SLAM学习——使用ARUCO_marker进行AR投影

一、简介

1.1 目标

增强现实技术（Augmented Reality，简称 AR），是一种实时地计算摄影机影像的位置及角度并加上相应图像、视频、3D模型的技术，这种技术的目标是在屏幕上把虚拟世界套在现实世界并进行互动。

将三维模型投影到ARUCO marker上，并获取投影效果。

1.2 实现思路

制作并识别ARUCO marker
估计旋转矩阵和平移矩阵
获取模型数据，并根据需要进行缩放和坐标调整
通过projectPoints重投影将三维坐标转换到图像二维坐标
显示图像

二、ARUCO marker

2.1 简介

ARUCO marker为汉明(海明)码的格子图，如下所示：
在这里插入图片描述

汉明码（Hamming Code）利用奇偶校验位的概念，通过在数据位后面增加一些比特，可以验证数据的有效性。利用一个以上的校验位，汉明码不仅可以验证数据是否有效，还能在数据出错的情况下指明错误位置。

2.2 原理解析

一个ArUco marker是一个二进制平方标记，它由一个宽的黑边和一个内部的二进制矩阵组成，内部的矩阵决定了它们的id。黑色的边界有利于快速检测到图像，二进制编码可以验证id，并且允许错误检测和矫正技术的应用。marker的大小决定了内部矩阵的大小。例如，一个4x4的marker由16bits组成。
在这里插入图片描述

上图为一个典型的ArUco marker，去除白色边框后为5X5的格子（黑色表示0，白色表示1），5X5的格子的外边缘为黑色。

黑白格子遵循下面的排列规则

\begin{matrix} p & d & p & d & p \\ p & d & p & d & p \\ p & d & p & d & p \\ p & d & p & d & p \\ p & d & p & d & p \end{matrix} \begin{matrix}
p & d & p &d &p\\
p & d & p &d &p\\
p & d & p &d &p\\
p & d & p &d &p\\
p & d & p &d &p\\
\end{matrix}

ppppp?ddddd?ppppp?ddddd?ppppp?

p为校验位：1,3,5列
d为数据位：2,4列，共10位能表示0-1023的数字

用数字01可表示为下面的排列，去除验位后可获得右侧排列，

\begin{matrix} 0 & 1 & 0 & 0 & 1 \\ 1 & 0 & 1 & 1 & 1 \\ 1 & 0 & 0 & 0 & 0 \\ 1 & 0 & 1 & 1 & 1 \\ 0 & 1 & 0 & 0 & 1 \end{matrix} ? \begin{matrix} 1 & 0 \\ 0 & 1 \\ 0 & 0 \\ 0 & 1 \\ 1 & 0 \end{matrix} \begin{matrix}
0 & 1 & 0 &0 &1\\
1 & 0 & 1 &1 &1\\
1 & 0 & 0 &0 &0\\
1 & 0 & 1 &1 &1\\
0 & 1 & 0 &0 &1\\
\end{matrix}\Rightarrow
\begin{matrix}
1 & 0 \\
0 & 1 \\
0 & 0 \\
0 & 1 \\
1 & 0 \\
\end{matrix}

01110?10001?01010?01010?11011??10001?01010?
将数据行行首尾相接后可获得该ArUco marker的id为582

1001000110 ? 582 10 01 00 01 10\Rightarrow582

1001000110?582

2.3 ARUCO marker的生成

注意：此处采用opencv的aruco库实现，opencv版本需要3以上，此处使用的是OpenCV3.3.1，下同

函数解析

1
2
3
4
5

void cv::aruco::drawMarker(const Ptr< Dictionary > & dictionary,
int id,
int sidePixels,
OutputArray img,
int borderBits = 1)

dictionary，包含marker对象的字典：

cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

DICT_6X6_250为6x6的marker，该字典包含id为0-249的marker。
id，返回marker的id，需要在设置的字典的范围内。
sidePixels，返回图像的像素值。
img，输出marker图像。
borderBits，marker图像边缘向像素值。

完整程序

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui.hpp>

#include <fstream>
#include <iostream>
#include <sstream>

using namespace std;
using namespace cv;

void createArucoMarkers() {
Mat outputMarker;
int marker_nums = 10;

// 6x6,id:0~249
Ptr<aruco::Dictionary> markerDictionary =
aruco::getPredefinedDictionary(aruco::DICT_6X6_250);

for (int i = 0; i < marker_nums; i++) {
// generate
cv::aruco::drawMarker(markerDictionary, i, 200, outputMarker, 1);

// save
ostringstream convent;
string imageName = "6X6_Marker_";
convent << "marker/" << imageName << i << ".jpg";
imwrite(convent.str(), outputMarker);
}
}

int main(int argc, char *argv[]) {
createArucoMarkers();
return 0;
}

三、姿态估计

姿态估计（Pose estimation）在计算机视觉领域扮演着十分重要的角色：机器人导航、增强现实以及其它。这一过程的基础是找到现实世界和图像投影之间的对应点。

最为常用的方法是基于二进制平方的标记，这种Marker的主要便利之处在于，一个Marker提供了足够多的对应（四个角）来获取相机的信息。同样的，内部的二进制编码使得算法非常健壮，允许应用错误检测和校正技术的可能性。

此处的姿态估计采用PnP（Perspective-n-Point）的方法获得Marker相对相机的旋转矩阵和平移矩阵。

PnP（Perspective-n-Point）是求解3D到2D点对运动的方法。它描述了当我们知道n个3D空间点以及它们的投影位置时，如何估计相机所在的位姿。

3.1 关键函数解析

1. 识别Marker

1
2
3
4
5
6
7
8

void cv::aruco::detectMarkers(InputArray image,
Dictionary dictionary,
OutputArrayOfArrays corners,
OutputArray ids,
DetectorParameters parameters = DetectorParameters(),
OutputArrayOfArrays rejectedImgPoints = noArray()
InputArray cameraMatrix = noArray(),
InputArray distCoeff = noArray())

image，待检测marker的图像。
dictionary，字典对象，为需要识别的marker类型，与生成marker时设置的字典对象一致即可。
corners，检测marker获得的包含所有marker四个角点的数组，数据类型为std::vector >，若检测到N个marker，则数组大小为Nx4，角点顺序为顺时针，第一个点为左上角。
ids，检测marker获得的id，大小与corners一致，只能检测包含在字典内的id。
parameters，marker检测所需参数，输入对象类型为 DetectionParameters。
rejectedCandidates，返回被检测出来但不是有效编码的marker，每个返回的marker同样通过四个角点定义，数据格式与corners一致，该参数可忽略，参用于debug模式和refineDetectedMarkers()
cameraMatrix，相机内参数，可省略。
distCoeff，相机畸变参数，可省略。

2. 姿态估计

1
2
3
4
5
6

void cv::aruco::estimatePoseSingleMarkers(InputArrayOfArrays corners,
float markerLength,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArrayOfArrays rvecs,
OutputArrayOfArrays tvecs)

corners，识别的包含marker的角点的数组，为detectMarkers() 函数中corners参数的返回值。
markerLength，marker的边长，单位为米。
cameraMatrix，相机内参数，标定获得。
distCoeffs，相机畸变参数，标定获得。
rvecs，markers相对相机的旋转矩阵。
tvecs，markers相对相机的平移矩阵。

markers的坐标系为：矩形中心为坐标原点，红色为X轴，绿色为Y轴，蓝色为Z轴，轴的指向为该轴的正方向。
在这里插入图片描述

3.2 程序实现

姿态估计

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

bool poseEstimation(Mat image, Vec3d &markerRvec, Vec3d &markerTvec, int detectID) {
// detect marker
vector<int> ids;
vector<vector<Point2f>> corners;
aruco::detectMarkers(image, dictionary, corners, ids);

// if at least one marker detected
if (ids.size() > 0) {
// estimate Pose
vector<Vec3d> rvecs, tvecs;
aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs,
rvecs, tvecs);

// screen out designed marker rvecs and tvecs
for (int i = 0; i < ids.size(); i++) {
if (ids[i] == detectID) {
// save pose
markerRvec = rvecs[i];
markerTvec = tvecs[i];
return true;
} else
return false;
}
} else
return false;
}

四、显示模型

4.1 模型准备与读入

三维模型采用普林斯顿三维模型库的数据，该数据库的模型为.off格式，程序内部通过fstream读入。读入前需对文件进行修改处理：直接修改.off格式为.txt，同时删除首行的OFF。

off文件简析

Object File Format(off)文件通过描述物体表面的多边形来表示一个模型的几何结构

格式为：

OFF

顶点数面数边数

x y z

x y z

…

n个顶点顶点1的索引顶点2的索引 … 顶点n的索引

…
程序实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

// model data
vector<Point3f> pointData;
vector<vector<int>> plantData;

// read model
fstream modelfile;
modelfile.open("./marker/m100.txt");
int pointSize, plantSize, lineSize;
modelfile >> pointSize;
modelfile >> plantSize;
modelfile >> lineSize;

// point data
for (int i = 0; i < pointSize; i++) {
Point3f pointTmp;
modelfile >> pointTmp.x;
modelfile >> pointTmp.y;
modelfile >> pointTmp.z;

// resize model
pointTmp.x = pointTmp.x / 0.5 * markerLength - (markerLength / 2.0);
pointTmp.y = pointTmp.y / 0.5 * markerLength - (markerLength / 2.0);
pointTmp.z = pointTmp.z / 0.5 * markerLength - (markerLength / 2.0);
pointData.push_back(pointTmp);
}

// plant data
for (int i = 0; i < plantSize; i++) {
vector<int> plantTmp;
for (int j = 0; j < 4; j++) {
int data;
modelfile >> data;
plantTmp.push_back(data);
}
plantData.push_back(plantTmp);
}

4.2 重投影

projectPoints()可根据所给的三维坐标和已知的相机内外参数求解投影到图像坐标系上的二维坐标。

1
2
3
4
5
6
7
8

void projectPoints(InputArray objectPoints,
InputArray rvec,
InputArray tvec,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArray imagePoints,
OutputArray jacobian=noArray(),
double aspectRatio=0 )

objectPoints，三维坐标数组。
rvec，旋转向量，通过estimatePoseSingleMarkers获得。
tvec，平移向量，通过estimatePoseSingleMarkers获得。
cameraMatrix，相机内参数。
distCoeffs，相机畸变参数。
imagePoints，返回图像坐标数组。
jacobian，雅克比行列式，
aspectRatio，相机传感器的感光单元有关的可选参数，如果设置为非0，则函数默认感光单元的dx/dy是固定的，会依此对雅可比矩阵进行调整。

4.3 绘制模型

4.3.1 绘制函数解析

1. 绘制坐标

1
2
3
4
5
6

void cv::aruco::drawAxis(InputOutputArray image,
InputArray cameraMatrix,
InputArray distCoeffs,
InputArray rvec,
InputArray tvec,
float length)

image ，是输入/输出图像，坐标将会绘制在该图像上（通常就是检测marker的那张图像）。
cameraMatrix，相机内参数，标定获得。
distCoeffs ，相机畸变参数，标定获得。
rvec ，外参数，旋转向量。
tvec ，外参数，平移向量。
length，坐标轴的长度，单位为米。

2. 绘制轮廓

1
2
3
4
5
6
7
8
9

void drawContours(InputOutputArray image,
InputArrayOfArrays contours,
int contourIdx,
const Scalar& color,
int thickness=1,
int lineType=LINE_8,
InputArray hierarchy=noArray(),
int maxLevel=INT_MAX,
Point offset=Point() )

3. 绘制线

1
2
3
4
5
6
7

void line(InputOutputArray img,
Point pt1,
Point pt2,
const Scalar& color,
int thickness=1,
int lineType=LINE_8,
int shift=0 )

4. 绘制点

1
2
3
4
5
6
7

void circle(InputOutputArray img,
Point center,
int radius,
const Scalar& color,
int thickness=1,
int lineType=LINE_8,
int shift=0 )

4.3.2 程序实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190

#include <opencv2/aruco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>

#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>

#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>

using namespace std;
using namespace cv;

#define markerLength 0.05 // ar码实际边长，单位m
#define modelSize 1 //模型大小0-1

ros::Publisher image_pub;

// init data
Mat cameraMatrix, distCoeffs;
Ptr<cv::aruco::Dictionary> dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

// model data
vector<Point3f> pointData;
vector<vector<int>> plantData;

// 绘制模型
Mat drawModel(Mat image, Vec3d rvec, Vec3d tvec, vector<Point3f> modelPoints,
vector<vector<int>> modelPlants, bool judge) {
Mat showImage;
cvtColor(image, showImage, CV_GRAY2BGR);

if (judge) {
// draw axis
aruco::drawAxis(showImage, cameraMatrix, distCoeffs, rvec, tvec, 0.05);

// draw model
// projectPoints
std::vector<cv::Point2f> imagePoints;
projectPoints(modelPoints, rvec, tvec, cameraMatrix, distCoeffs,
imagePoints);

// draw plant
vector<vector<Point>> plantPoints(modelPlants.size());
vector<Point> tmpPoints(3);
for (int i = 0; i < modelPlants.size(); i++) {
tmpPoints[0] = imagePoints[modelPlants[i][1]];
tmpPoints[1] = imagePoints[modelPlants[i][2]];
tmpPoints[2] = imagePoints[modelPlants[i][3]];
plantPoints[i] = tmpPoints;
}
for (int i = 0; i < plantPoints.size(); i++) {
drawContours(showImage, plantPoints, i, Scalar(203, 192, 255), FILLED);
}

// draw line
for (int i = 0; i < modelPlants.size(); i++) {
line(showImage, imagePoints[modelPlants[i][1]],
imagePoints[modelPlants[i][2]], Scalar(0, 0, 0), 1);
line(showImage, imagePoints[modelPlants[i][2]],
imagePoints[modelPlants[i][3]], Scalar(0, 0, 0), 1);
line(showImage, imagePoints[modelPlants[i][3]],
imagePoints[modelPlants[i][1]], Scalar(0, 0, 0), 1);
}

// draw point
for (int i = 0; i < imagePoints.size(); i++) {
circle(showImage, imagePoints[i], 1, Scalar(0, 0, 0), 1);
}
}
return showImage;
}

// 状态估计
bool poseEstimation(Mat image, Vec3d &markerRvec, Vec3d &markerTvec) {
Mat imageCopy = image.clone();

// detect marker
vector<int> ids;
vector<vector<Point2f>> corners;
aruco::detectMarkers(imageCopy, dictionary, corners, ids);

// if at least one marker detected
if (ids.size() > 0) {
// aruco::drawDetectedMarkers(showImage, corners, ids);
vector<Vec3d> rvecs, tvecs;
aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs,
rvecs, tvecs);

// screen out designed marker rvecs and tvecs
for (int i = 0; i < ids.size(); i++) {
if (ids[i] == 2) {
markerRvec = rvecs[i];
markerTvec = tvecs[i];
return true;
} else
return false;
}
} else
return false;
}

// 回调函数
void imageCb(const sensor_msgs::ImageConstPtr &msg) {
// get image
cv_bridge::CvImagePtr cv_ptr =
cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
Mat cameraImage = cv_ptr->image;

// estimate pose
Vec3d markerRvec, markerTvec;
bool judge = poseEstimation(cameraImage, markerRvec, markerTvec);

// draw model
Mat showImage = drawModel(cameraImage, markerRvec, markerTvec, pointData,
plantData, judge);

// pub result
cv_bridge::CvImage out_msg;
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
out_msg.image = showImage;
image_pub.publish(out_msg.toImageMsg());
}

int main(int argc, char *argv[]) {
// init ros
ros::init(argc, argv, "aruco");
ros::NodeHandle nh;
ros::Subscriber image_sub =
nh.subscribe<sensor_msgs::Image>("/camera/image", 1, imageCb);
image_pub = nh.advertise<sensor_msgs::Image>("/camera/aruco", 1);

// init parms
vector<double> cameraMatrixData, distCoeffsData;
ros::param::get("/camera_matrix/data", cameraMatrixData);
ros::param::get("/distortion_coefficients/data", distCoeffsData);

Mat cameraMatrixTmp =
(Mat_<double>(3, 3) << cameraMatrixData[0], cameraMatrixData[1],
cameraMatrixData[2], cameraMatrixData[3], cameraMatrixData[4],
cameraMatrixData[5], cameraMatrixData[6], cameraMatrixData[7],
cameraMatrixData[8]);
cameraMatrix = cameraMatrixTmp;

Mat distCoeffsTmp =
(Mat_<double>(1, 5) << distCoeffsData[0], distCoeffsData[1],
distCoeffsData[2], distCoeffsData[3], distCoeffsData[4]);
distCoeffs = distCoeffsTmp;

// read model
fstream modelfile;
modelfile.open("./marker/m100.txt");
int pointSize, plantSize, lineSize;
modelfile >> pointSize;
modelfile >> plantSize;
modelfile >> lineSize;

for (int i = 0; i < pointSize; i++) {
Point3f pointTmp;
modelfile >> pointTmp.x;
modelfile >> pointTmp.y;
modelfile >> pointTmp.z;

// resize model
pointTmp.x = pointTmp.x / 0.5 * markerLength - (markerLength / 2.0);
pointTmp.y = pointTmp.y / 0.5 * markerLength - (markerLength / 2.0);
pointTmp.z = pointTmp.z / 0.5 * markerLength - (markerLength / 2.0);
pointData.push_back(pointTmp);
}

for (int i = 0; i < plantSize; i++) {
vector<int> plantTmp;
for (int j = 0; j < 4; j++) {
int data;
modelfile >> data;
plantTmp.push_back(data);
}
plantData.push_back(plantTmp);
}

ros::spin();
return 0;
}

4.4 实现效果

在这里插入图片描述

参考

aruco

ARUCO marker的解释

[OpenCV] aruco Markers识别

tutorial_aruco_detection
程序参考

https://github.com/fdcl-gwu/aruco-markers

https://github.com/jeradesign/ar-challenge
原理参考

《视觉SLAM十四讲》——第7讲视觉里程计1
模型数据

off文件格式(Object File Format)

常见的三维点云数据下载链接

普林斯顿三维模型库