简介:在实际交通地图环境中,我们预设了交通标志牌,可以通过交通标志牌对小车进行更加细致丰富的控制,标志牌形式如下:
每个交通标志牌都是由功能图片+编码图片结合的方式生成(虚拟仿真环境下功能图片和编码图片位置相反,不影响识别),每一个标志牌代表一种对车辆行为的指示,比如:
停止:车辆需要在指示牌范围内停止形式
人行横道:车辆需观察行人
前方慢形:车辆需要减速
避让:车辆需要避让一段道路
路口标识:按照设定是否转弯或者直行
......
用户可以按照实际需求,自行拟定交通标志牌,此类编码图片有500多种,用户可以指定编码图片与车辆行为来进行匹配,当检测到标志牌时,对车辆进行相应的功能指示。
在虚拟仿真环境中,我们也可以加入交通标志牌,从而实现与车辆的交互,在这一节中,我们将学习如何识别交通标志牌,以及通过上图交通标志牌对车辆进行控制。
目录
1、制作地图
首先,我们需要制作含有交通标志牌的虚拟仿真地图(test.yaml,保存路径参考第一章):
tiles:
- [floor , floor , floor , floor , floor , floor , floor , floor ]
- [floor , curve_left/W, straight/W, 3way_left/W, straight/W , straight/W , curve_left/N , floor ]
- [floor , straight/S , floor , straight/N , floor , floor , straight/N , floor ]
- [floor , 3way_left/S , straight/W, 4way , straight/E , straight/E , 3way_left/N , floor ]
- [floor , straight/S , floor , straight/S , floor , floor , straight/N , floor ]
- [floor , straight/S , floor , straight/S , floor , curve_right/N , curve_left/E , floor ]
- [floor , curve_left/S, straight/E, 3way_left/E, straight/E , curve_left/E , floor , floor ]
- [floor , floor , floor , floor , floor , floor , floor , floor ]
objects:
trafficlight1:
kind: trafficlight
place:
tile: [3,4]
relative:
~SE2Transform:
p: [-0.18,-0.18]
theta_deg: 135
height: 0.3
optional: true
trafficlight2:
kind: trafficlight
place:
tile: [6,4]
relative:
~SE2Transform:
p: [-0.18,-0.18]
theta_deg: 135
height: 0.3
optional: true
trafficlight3:
kind: trafficlight
place:
tile: [1,4]
relative:
~SE2Transform:
p: [-0.18,-0.18]
theta_deg: 135
height: 0.3
optional: true
trafficlight4:
kind: trafficlight
place:
tile: [3,6]
relative:
~SE2Transform:
p: [-0.18,-0.18]
theta_deg: 135
height: 0.3
optional: true
trafficlight5:
kind: trafficlight
place:
tile: [3,1]
relative:
~SE2Transform:
p: [-0.18,-0.18]
theta_deg: 135
height: 0.3
optional: true
sign1:
kind: sign_stop
pos: [5, -0.05]
rotate: 0
height: 0.18
sign2:
kind: sign_left_T_intersect
pos: [3, -0.05]
rotate: 0
height: 0.18
sign3:
kind: sign_yield
pos: [0.95, 3.4]
rotate: -90
height: 0.18
sign4:
kind: sign_duck_crossing
pos: [2.5, 5.95]
rotate: 180
height: 0.18
sign5:
kind: sign_T_intersect
pos: [0.95, 2]
rotate: 0
height: 0.18
tile_size: 0.585
地图中包含了T型路口,避让、慢行、停止标志牌。
2、截取含交通标志牌的图片
$ cd gym-duckietown
$ python3 manual_control.py --env-name Duckietown --map-name test
通过方向键控制车辆,移动到标志牌附近,按enter键截图
3、安装dt-apriltags库
$ pip3 install dt-apriltags
4、摄像头标定
当我们在一张图片中识别到编码图片(类似二维码)可以计算出编码图片的相对于整体图片的大小(像素)、位置(像素)、旋转情况,另外编码图片实际大小是已知的,要计算摄像头与编码图片的距离还需要一些摄像头本身的参数,包括焦距、光学中心等,所以我们需要先计算摄像头的参数,摄像头参数相关名词有以下几种:
1)相机矩阵:包括焦距(fx,fy),光学中心(Cx,Cy),完全取决于相机本身,是相机的固有属性,只需要计算一次,可用矩阵表示如下:[fx, 0, Cx; 0, fy, cy; 0,0,1];
2)畸变系数:畸变数学模型的5个参数 D = (k1,k2, P1, P2, k3);
3)相机内参:相机矩阵和畸变系数统称为相机内参,在不考虑畸变的时候,相机矩阵也会被称为相机内参;
4)相机外参:通过旋转和平移变换将3D的坐标转换为相机2维的坐标,其中的旋转矩阵和平移矩阵就被称为相机的外参;描述的是将世界坐标系转换成相机坐标系的过程。
摄像头的标定过程实际上就是在4个坐标系转化的过程中求出相机的内参和外参的过程。这4个坐标系分别是:世界坐标系(描述物体真实位置),相机坐标系(摄像头镜头中心),图像坐标系(图像传感器成像中心,图片中心,影布中心,单位mm),像素坐标系(图像左上角为原点,描述像素的位置,单位是多少行,多少列)。
(1)世界坐标系 -->相机坐标系:求解摄像头外参(旋转和平移矩阵);
(2)相机坐标系 -->图像坐标系:求解相机内参(摄像头矩阵和畸变系数);
(3)图像坐标系 -->像素坐标系:求解像素转化矩阵(可简单理解为原点从图片中心到左上角,单位厘米变行列)
在虚拟仿真环境中,摄像头不存在畸变,所以我们只需要求解摄像头的相机矩阵,具体流程如下:
4.1 截取棋盘格图片
虚拟仿真环境中有相机标定专用环境,通过手动控制脚本打开,用enter键截图,因为需要截取多张图片,每截取一张都需要修改文件名,否则会覆盖先前截取的。
$ cd gym-duckietown
$ python3 manual_control.py --env-name Duckietown --map-name calibration_map_int
需要截取多张图片(10张以上),需要包含7个角度(可以多截几张):居中、较远、较近、偏左、偏右、偏上、偏下,要求截图中棋盘格角点完整,不能缺行:
注:如果出现地图全黑色没有棋盘格的情况,请查看calibration_map_int.yaml地图文件,如果地图图块配置如下图:
则修改为:
4.2 初始化参数及图片导入
OpenCV使用棋盘格板进行标定,为了标定相机,我们需要输入一系列三维点和它们对应的二维图像点。在黑白相间的棋盘格上,二维图像点很容易通过角点检测找到。而对于真实世界中的三维点,由于我们采集中,是将相机放在一个地方,而将棋盘格定标板进行移动变换不同的位置,然后对其进行拍摄,所以我们需要知道(X,Y,Z)的值。但是简单来说,我们定义棋盘格所在平面为XY平面,即Z=0。对于定标板来说,我们可以知道棋盘格的方块尺寸,例如30mm,这样我们就可以把棋盘格上的角点坐标定义为(0,0,0),(30,0,0),(60,0,0),···,这个结果的单位是mm。
#!/usr/bin/env python3
import cv2
import numpy as np
nx = 5 #棋盘格角点行数
ny = 7 #棋盘格角点列数
#世界坐标系中的三维点
objp = np.zeros((nx*ny,3), np.float32)
objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2)
#依据棋盘上单个黑白矩阵的大小,计算出(初始化)每一个内角点的世界坐标
#我们使用的棋盘格方块尺寸是31mm
objp = objp*31
objpoints = [] #三维点列表
imgpoints = [] #二维点列表
#求角点的迭代过程的终止条件,采用的停止准则是最大循环次数31和最大误差容限0.001
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 31, 0.001)
#循环读取图片并显示
for i in range(1,8):
imageFile = 'images/13-'+str(i)+'.png'
img = cv2.imread(imageFile)
cv2.imshow(imageFile,img)
cv2.waitKey(0)
cv2.destroyAllWindows()
4.3 寻找角点
寻找角点我们需要使用findChessboardCorners函数提取角点,这里的角点专指的是标定板上的内角点,这些角点与标定板的边缘不接触,涉及到以下几个opencv函数:
retval, corners = cv.findChessboardCorners(image, patternSize[, corners[, flags]])
第一个参数Image,传入拍摄的棋盘图Mat图像,必须是8位的灰度或者彩色图像;
第二个参数patternSize,每个棋盘图上内角点的行列数,一般情况下,行列数不要相同,便于后续标定程序识别标定板的方向;
第三个参数corners,用于存储检测到的内角点图像坐标位置,一般是数组形式;
第四个参数flage:用于定义棋盘图上内角点查找的不同处理方式,有默认值。
为了提高标定精度,需要在初步提取的角点信息上进一步提取亚像素信息,降低相机标定偏差,常用的方法是cornerSubPix函数:
cv2.cornerSubPix(image, corners, winSize, zeroZone, criteria)
第一个参数image,输入图像的像素矩阵,最好是8位灰度图像,检测效率更高;
第二个参数corners,初始的角点坐标向量,同时作为亚像素坐标位置的输出,所以需要是浮点型数据;
第三个参数winSize,大小为搜索窗口的一半;
第四个参数zeroZone,死区的一半尺寸,死区为不对搜索区的中央位置做求和运算的区域。它是用来避免自相关矩阵出现某些可能的奇异性。当值为(-1,-1)时表示没有死区;
第五个参数criteria,定义求角点的迭代过程的终止条件,可以为迭代次数和角点精度两者的组合;
找到角点信息后,我们可以用 drawChessboardCorners函数用绘制被成功标定的角点:
cv2.drawChessboardCorners(image, patternSize, corners, patternWasFound)
第一个参数image,8位灰度或者彩色图像;
第二个参数patternSize,每张标定棋盘上内角点的行列数;
第三个参数corners,初始的角点坐标向量,同时作为亚像素坐标位置的输出,所以需要是浮点型数据;
第四个参数patternWasFound,标志位,用来指示定义的棋盘内角点是否被完整的探测到,true表示别完整的探测到,函数会用直线依次连接所有的内角点,作为一个整体,false表示有未被探测到的内角点,这时候函数会以(红色)圆圈标记处检测到的内角点;
def findConner(image):
#转化为灰度图
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
#寻找角点
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
if ret==True:
#寻找亚像素角点
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
#存储三维点和二维点矩阵
objpoints.append(objp)
imgpoints.append(corners)
#绘制寻找到的角点
cv2.drawChessboardCorners(image, (nx, ny), corners, True)
cv2.imshow("corners",image)
4.4 计算相机矩阵
获取到棋盘标定图的内角点图像坐标之后,就可以使用calibrateCamera函数进行标定,计算相机内参和外参系数:
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs[, rvecs[, tvecs[, flags[, criteria]]]])
第一个参数objectPoints,为世界坐标系中的三维点。需要依据棋盘上单个黑白矩阵的大小,计算出(初始化)每一个内角点的世界坐标;
第二个参数imagePoints,为每一个内角点对应的图像坐标点;
第三个参数imageSize,为图像的像素尺寸大小,在计算相机的内参和畸变矩阵时需要使用到该参数;
第四个参数cameraMatrix为相机的内参矩阵;
第五个参数distCoeffs为畸变矩阵;
第六个参数rvecs为旋转向量;
第七个参数tvecs为位移向量;
第八个参数flags为标定时所采用的算法。有如下几个参数:
CV_CALIB_USE_INTRINSIC_GUESS:使用该参数时,在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话,将初始化(u0,v0)图像的中心点,使用最小二乘估算出fx,fy。
CV_CALIB_FIX_PRINCIPAL_POINT:在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置,光轴点将保持在中心或者某个输入的值。
CV_CALIB_FIX_ASPECT_RATIO:固定fx/fy的比值,只将fy作为可变量,进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置,fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。
CV_CALIB_ZERO_TANGENT_DIST:设定切向畸变参数(p1,p2)为零。
CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6:对应的径向畸变在优化中保持不变。
CV_CALIB_RATIONAL_MODEL:计算k4,k5,k6三个畸变参数。如果没有设置,则只计算其它5个畸变参数。
第九个参数criteria是最优迭代终止条件设定。
标定的结果是生成相机的内参矩阵cameraMatrix、相机的5个畸变系数distCoeffs(本节只需要内参矩阵),另外每张图像都会生成属于自己的平移向量和旋转向量。
def getMatrix():
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (640,480), None, None)
print("mtx:", mtx)
print("dist:", dist)
return mtx, dist
4.5 查看相机矩阵
附以上内容整理后的源码(getCamMatrix.py):
#!/usr/bin/env python3
import cv2
import numpy as np
def findConner(image):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
if ret==True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
objpoints.append(objp)
imgpoints.append(corners)
cv2.drawChessboardCorners(image, (nx, ny), corners, True)
cv2.imshow("corners",image)
def getMatrix():
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (640,480), None, None)
print("mtx:", mtx)
print("dist:", dist)
return mtx, dist
nx = 5
ny = 7
objp = np.zeros((nx*ny,3), np.float32)
objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2)
objp = objp*31
objpoints = []
imgpoints = []
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 31, 0.001)
for i in range(1,8):
imageFile = 'images/13-'+str(i)+'.png'
img = cv2.imread(imageFile)
cv2.imshow(imageFile,img)
findConner(img)
cv2.waitKey(0)
cv2.destroyAllWindows()
mtx, dist = getMatrix()
$ python3 getCamMatrix.py
其中的mtx即我们需要的摄像头内参矩阵。
5、识别标志牌
截取两张带标签的图片:
#!/usr/bin/env python3
import cv2
from dt_apriltags import Detector
import numpy as np
#读取图片
image = cv2.imread("images/tag1.png")
#创建识别器
at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
#图像转灰度图
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
#摄像头内参矩阵
matrix = np.array([[397.06923648, 0, 319.53462827], [0, 409.40521919, 246.53066753], [0, 0, 1]])
#识别标志牌
tags = at_detector.detect(gray, True, (matrix[0][0],matrix[1][1],matrix[0][2],matrix[1][2]), 0.06)
#在图中画出标志牌位置
for tag in tags:
cv2.line(image,tag.corners[0].astype(int),tag.corners[1].astype(int),(0,255,0))
cv2.line(image,tag.corners[1].astype(int),tag.corners[2].astype(int),(0,255,0))
cv2.line(image,tag.corners[2].astype(int),tag.corners[3].astype(int),(0,255,0))
cv2.line(image,tag.corners[3].astype(int),tag.corners[0].astype(int),(0,255,0))
cv2.putText(image, str(tag.tag_id), tag.center.astype(int), cv2.FONT_HERSHEY_COMPLEX, 1.0, (100, 200, 200), 1)
print("识别到的标志牌ID:", tag.tag_id, " 标志牌与车的距离:",tag.pose_t[2][0])
cv2.imshow("tag",image)
cv2.waitKey(0)
cv2.destroyAllWindows()