互联网+项目中的功能——AR码的定位

一、项目简要

拿下！互联网＋国金

本次比赛因涉及一下协议内容，我在这里只能将一些我在项目中所做的东西，嘻嘻。

二、项目功能简介

①做了一个底部AGV定位的功能，基于AR码的定位，可以使AGV能精准进入箱体底部

②做了一个基于巡回的相关软件后台可控，类似于我之前做的一些c#+视觉相关的内容

三，二维码码定位相关

1、zbar码定位

刚开始使用zbar二维码做的定位，相机标定+视觉处理+深度以及xy轴的偏差值，使其一直存在于相机中心，下面代码为简易版的demo 

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
#include </home/dl/InternetPlus/lib/zbarcode.h>

void Zbarcode::prProcess(Mat gray){
      int threadvlaue = 255;
   Mat enhancedImage;
   equalizeHist(gray, enhancedImage);
   Mat binary;
	 threshold( enhancedImage, binary,152, threadvlaue,THRESH_BINARY ); //二值化
   Mat morphedImage;
   Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
   morphologyEx(binary, morphedImage, MORPH_OPEN, kernel);
   imshow("3",morphedImage);
 
}
void Zbarcode::QrcodeSearch(Mat src){
ImageScanner scanner;
scanner.set_config(ZBAR_QRCODE, ZBAR_CFG_ENABLE, 1);
Mat gray;
cvtColor(src, gray, COLOR_BGR2GRAY);
prProcess(gray);
int width =gray.cols;
int height = gray.rows;
uchar *raw = gray.data;
Image zbarImage(width, height, "Y800", raw, width * height);
scanner.scan(zbarImage);

 if (zbarImage.symbol_begin() != zbarImage.symbol_end()) {
for (Image::SymbolIterator symbol = zbarImage.symbol_begin(); symbol != zbarImage.symbol_end(); ++symbol) {
    vector<Point> points;
    vector<Point2f> imagePoints;
    // 获取二维码的角点
    for (int i = 0; i < symbol->get_location_size(); ++i) {
        points.push_back(Point(symbol->get_location_x(i), symbol->get_location_y(i)));
    }

    
    for (int i = 0; i < 4; ++i) {
        line(src, points[i], points[(i + 1) % 4], Scalar(0, 255, 0), 2);
    }
for (; symbol != zbarImage.symbol_end(); ++symbol)
	{
		cout << "类型：" << endl << symbol->get_type_name() << endl << endl;
		cout << "条码：" << endl << symbol->get_data() << endl << endl;
	}

for (int i = 0; i < symbol->get_location_size(); ++i) {
    imagePoints.push_back(Point2f(symbol->get_location_x(i), symbol->get_location_y(i)));
}

}}
else {
    cout << "查询二维码失败！" << endl;
	imshow("4",src);
}
}

void Zbarcode::QrcodeSearch(Mat src){
           ImageScanner scanner;
          scanner.set_config(ZBAR_QRCODE, ZBAR_CFG_ENABLE, 1);
          Mat gray;
          cvtColor(src, gray, COLOR_BGR2GRAY);
          prProcess(gray);
          int width =gray.cols;
int height = gray.rows;
uchar *raw = gray.data;
Image zbarImage(width, height, "Y800", raw, width * height);
scanner.scan(zbarImage);

 if (zbarImage.symbol_begin() != zbarImage.symbol_end()) {
for (Image::SymbolIterator symbol = zbarImage.symbol_begin(); symbol != zbarImage.symbol_end(); ++symbol) {
    vector<Point> points;
    vector<Point2f> imagePoints;
    // 获取二维码的角点
    for (int i = 0; i < symbol->get_location_size(); ++i) {
        points.push_back(Point(symbol->get_location_x(i), symbol->get_location_y(i)));
    }

    // 在图像上绘制二维码的角点
    for (int i = 0; i < 4; ++i) {
        line(src, points[i], points[(i + 1) % 4], Scalar(0, 255, 0), 2);
    }

 // 二维码角点坐标
vector<Point3f> objectPoints;
objectPoints.push_back(Point3f(-1.5, 3, 0));  // 左上角
objectPoints.push_back(Point3f(1.5, 3, 0));   // 右上角
objectPoints.push_back(Point3f(1.5,0, 0));    // 右下角
objectPoints.push_back(Point3f(-1.5,0, 0));   // 左下角

// 图像中的角点坐标

for (int i = 0; i < symbol->get_location_size(); ++i) {
    imagePoints.push_back(Point2f(symbol->get_location_x(i), symbol->get_location_y(i)));
}

    cv::Mat cameraMatrix, distCoeffs;
    std::vector<double> camera = {                         
                             968.4681, 
                             0,
                             988.8516,
                             0,
                             968.1535,
                             549.3471, 
                             0,
                             0,
                            1 };
    cameraMatrix = cv::Mat(camera);
    cameraMatrix = cameraMatrix.reshape(1, 3);
    std::vector<double> dist = {
                              0.1869,
                            -0.2228,
                              0,
                              0,
                              0};
    distCoeffs = cv::Mat(dist);
    distCoeffs = distCoeffs.reshape(1, 1);
// 位姿估计
        Mat rvecs ;
		Mat tvecs ;
solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs, tvecs);
		//创建旋转矩阵和平移矩阵
		
// 输出旋转矩阵和平移向量
	Mat rotM = Mat::eye(3, 3, CV_64F);
		Mat rotT = Mat::eye(3, 3, CV_64F);
		Rodrigues(rvecs, rotM);  //将旋转向量变换成旋转矩阵
		Rodrigues(tvecs, rotT);
		//计算角度
		Mat adjustTvecs;
		Mat rot_camera2ptz = cv::Mat::eye(3, 3, CV_64FC1);
		Mat trans_camera2ptz = cv::Mat::zeros(3, 1, CV_64FC1);
		adjustTvecs = rot_camera2ptz * tvecs - trans_camera2ptz;
		adjustAngle(adjustTvecs,angle_x,angle_y,1,0);

		//计算深度
		Mat distance=Mat::zeros(Size(3,3), CV_64F);
		distance = rotM.t() * tvecs;
 imshow("4",src);
   printf("angle_x:%.4lf ,angle_y:%.4lf\n,distance: %.2lf\n", angle_x,angle_y,abs(distance.at<double>(2)));
}}
else {
    cout<<"error"<<endl;
	imshow("4",src);
}
} 

/*----------include------------*/
#include <opencv2/opencv.hpp>
#include <iostream>
#include </home/dl/InternetPlus/lib/zbarcode.h>
#include </home/dl/InternetPlus/lib/ArUcode.h>
//#include </home/dl/InternetPlus/lib/ArUco.h>
/*-----------------------------*/

/*----------namespace----------*/
using namespace cv;
/*-----------------------------*/
/***************************************************************************************************
文件名：main.cpp
介绍:程序启动入口
***************************************************************************************************/
int main()
{
  Zbarcode cam;
	VideoCapture cap(0);
	
	while (1)
	{
		Mat src;
		cap >> src;
     cam.QrcodeSearch(src);
	//	imshow("相机",src);//显示当前帧图像
		waitKey(1);//延时30秒
	}
	return 0;

}

2、AR码定位

很厉害方便的做定位的码

首先需要一个AR码的生成器

推荐生成器https://chev.me/arucogen/

1.AR码的基本概念

AR码是一种特殊的二维图像（如二维码或AprilTag），具有以下特点：

  • 高对比度：黑白相间，便于图像处理算法识别。
  • 唯一性：每个AR码有唯一的ID，用于区分不同的标记。
  • 几何特征：具有明显的角点和边缘，便于计算位姿。

2. AR码三维定位的核心步骤

(1) 图像采集
  • 使用摄像头或深度相机采集包含AR码的图像。
  • 如果是深度相机，还可以获取深度信息（即每个像素点到相机的距离）。
  • 本次使用的是免驱相机。自己进行标定做深度。
(2) 图像预处理
  • 灰度化：将彩色图像转换为灰度图像，简化计算。
  • 二值化：通过阈值处理，将图像转换为黑白二值图像。
  • 去噪：使用滤波算法（如高斯滤波）去除噪声。
(3) AR码检测
  • 轮廓检测：使用边缘检测算法（如Canny）找到图像中的轮廓。
  • 四边形拟合：从轮廓中筛选出四边形（AR码通常是矩形）。
  • 解码：识别AR码内部的编码信息，获取其唯一ID。
  • 本次项目调用opencv-aruco
(4) 角点检测
  • 提取AR码的四个角点（即四个顶点）。
  • 这些角点将用于计算AR码的三维位姿。
(5) 相机参数
  • 内参矩阵：描述相机的焦距、主点等参数。
  • 畸变系数：描述镜头的畸变情况。
  • 这些参数通过相机标定获得。
(6) 三维位姿计算

  • PnP问题：利用AR码的已知物理尺寸（如边长）和图像中的角点坐标，通过PnP（Perspective-n-Point）算法计算AR码的三维位姿。

  • 输出结果：

    • 平移向量（Translation Vector）：AR码相对于相机的位置（[x,y,z][x,y,z]）。
    • 旋转向量（Rotation Vector）：AR码相对于相机的旋转（通常转换为旋转矩阵或四元数）。

    3. 数学原理

    (1) 相机模型

    • 相机将三维空间中的点投影到二维图像平面，投影公式为：

      s[uv1]=K[RT][XYZ1]suv1=K[R**T]XYZ1

      其中：

      • (u,v)(u,v)：图像平面上的像素坐标。
      • (X,Y,Z)(X,Y,Z)：三维空间中的点坐标。
      • KK：相机内参矩阵。
      • RR：旋转矩阵。
      • TT：平移向量。
      • ss：尺度因子。

    (2) PnP算法

    • PnP算法通过已知的3D点（AR码角点）和对应的2D点（图像中的角点），求解相机的位姿（RR和TT）。
    • 常用算法：
      • EPnP：高效且稳定的PnP算法。
      • Iterative PnP：基于迭代优化的PnP算法。

    (3) 位姿表示

    • 旋转矩阵：3x3矩阵，表示AR码的旋转。
    • 四元数：一种紧凑的旋转表示方法。
    • 欧拉角：将旋转分解为绕X、Y、Z轴的旋转角度。

    4. 代码实现

    在ROS中，AR码的三维定位通常使用ar_track_alvar或apriltag_ros等包。以下是ar_track_alvar的工作流程：

    (1) 订阅AR码话题

    1
    self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb)
    • 订阅/ar_pose_marker话题，获取AR码的位姿信息。

    (2) 回调函数处理

    1
    2
    3
    4
    5
    6
    def ar_cb(self, data):
        for marker in data.markers:
            id = marker.id
            pose = marker.pose.pose
            position = pose.position  # 位置 (x, y, z)
            orientation = pose.orientation  # 旋转 (四元数)
    • 从消息中提取AR码的ID、位置和旋转。

    (3) 位姿转换

    • 如果需要将AR码的位姿转换到其他坐标系（如地图坐标系），可以使用TF库：
1
2
3
4
import tf
listener = tf.TransformListener()
listener.waitForTransform("map", "camera_frame", rospy.Time(), rospy.Duration(4.0))
(trans, rot) = listener.lookupTransform("map", "camera_frame", rospy.Time())

这是我写的c++版部分代码，

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
#include </home/dl/InternetPlus/lib/ArUcode.h>

void ArUcode::prProcess(Mat gray){
   int threadvlaue = 255;
   Mat enhancedImage;
   equalizeHist(gray, enhancedImage);
   Mat binary;
	 threshold( enhancedImage, binary,112, threadvlaue,THRESH_BINARY ); //二值化
   Mat morphedImage;
   Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
   morphologyEx(binary, morphedImage, MORPH_OPEN, kernel);
   imshow("3",morphedImage);
 
}

void ArUcode::QrcodeSearch(cv::Mat src){
    Mat gray;
    cvtColor(src, gray, COLOR_BGR2GRAY);
    Mat morphedImage;
    prProcess(gray);
    qrmatch(src);
}
 void ArUcode::qrmatch( cv::Mat src){
    cv::Mat cameraMatrix, distCoeffs;
    std::vector<double> camera = {                         
                             968.4681, 
                             0,
                             988.8516,
                             0,
                             968.1535,
                             549.3471, 
                             0,
                             0,
                            1 };
    cameraMatrix = cv::Mat(camera);
    cameraMatrix = cameraMatrix.reshape(1, 3);
    std::vector<double> dist = {
                              0.1869,
                            -0.2228,
                              0,
                              0,
                              0};
    distCoeffs = cv::Mat(dist);
    distCoeffs = distCoeffs.reshape(1, 1);
    //  对标记图像都进行aruco标记的检测以及姿态估计
    cv::Mat test_image;
    cv::resize(src, test_image, cv::Size(800, 600));
 //   cv::imshow("test_image", test_image);
    auto dictionary = cv::aruco::getPredefinedDictionary(
    cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_4X4_50);
    std::vector<std::vector<cv::Point2f>> corners, rejectedImgPoints;
    std::vector<int> ids;
    auto parameters = cv::aruco::DetectorParameters::create();
    cv::aruco::detectMarkers(test_image, dictionary, corners, ids, parameters,
                             rejectedImgPoints);
   // cv::aruco::drawDetectedMarkers(test_image, corners, ids, cv::Scalar(0, 255, 0));

    std::vector<cv::Vec3d> rvecs;
    std::vector<cv::Vec3d> tvecs;
    cv::aruco::estimatePoseSingleMarkers(corners, 0.053, cameraMatrix,
                                         distCoeffs, rvecs, tvecs);
//       // 获取图像的宽度和高度
// int imageWidth = test_image.cols;
// int imageHeight = test_image.rows;

// // 计算图像中心坐标
// double centerX = imageWidth / 2.0;
// double centerY = imageHeight / 2.0;                                   
 // 计算偏移的长度
for (int i = 0; i < rvecs.size(); i++) {
    cv::Mat rotM;
    cv::Rodrigues(rvecs[i], rotM);  // 将旋转向量转换为旋转矩阵

    // 计算 x 和 y 偏移的长度
  double xLength = rotM.at<double>(0, 2) ;
    double yLength = rotM.at<double>(1, 2) ;
    double depth = cv::norm(rotM.t() * cv::Mat(tvecs[i]));
   cout << "Marker Position - X: " <<xLength*100 << " Y: " <<yLength*100<< " Z: " <<depth*100<< endl;

}
    // step 3: 绘制坐标轴并进行可视化显示
    for (int i = 0; i < rvecs.size(); i++) {
         // 在图像上绘制标记的边界框
        cv::aruco::drawDetectedMarkers(test_image, corners,ids);
        cv::aruco::drawAxis(test_image, cameraMatrix, distCoeffs, rvecs[i],
                            tvecs[i], 0.02);
    }
 
    cv::imshow("pose", test_image);
   }