简介：在第五节的内容中，我们学习了使用rqt工具集观看摄像头视频流的方法，细心的同学应该会发现camera_node发布的视频数据中的图像有变形现象，图像变形会导致直线不直，部分区域变大，部分区域缩小，导致无法准确计算出我们需要的数据，为了解决这个问题，我们需要先校准相机，另外在虚拟仿真的学习过程中，我们了解到在后续的图像处理过程中，我们还需要知道相机的内参矩阵，以及投影变换矩阵，在这一章节中，我们详细介绍这些参数的求取过程。
在求取参数过程中，我们需要借助棋盘格来辅助完成相机的校准，我们使用的棋盘格大小是一张标准的A3纸的尺寸，样式如下图：
棋盘格也标示了车辆的坐标系统。x轴向前正，y轴向左为正，如果你手上没有标准的棋盘格需要自制的话，有以下几个参数需要注意：
1、棋盘格为6（行）*8（列），我们实际使用的是内角点，所以后续配置参数为5*7
2、每个棋盘格中的小方块是标准的31mm，不能缩放
3、以车辆坐标为参考，棋盘格右下角坐标为（160mm，-124mm）
未校准情况下，摄像头中的棋盘格如下图，明显存在变形，而我们需要的是横平竖直，格子大小相等的图像。
在虚拟仿真环境中我们是通过虚拟仿真环境截取了一些棋盘格图像然后计算相关参数的，在真实环境中，我们可以手动来获取这些图像，然后统一加载图像再计算相关参数，为了减少工作量，我们通过编程实现一次性完成图像采集和参数的计算，具体流程如下：
目录
1、创建工作空间以及功能包
2、主要功能函数说明
2.1 cv2.findChessboardCorners 查找图像中棋盘格角点信息
2.2 cv2.cornerSubPix 亚像素角点检测
2.3 cv2.drawChessboardCorners 绘制标定的角点
2.4 cv2.calibrateCamera 计算相机内参系数
2.5 cv2.getOptimalNewCameraMatrix 获取视场调节参数
2.6 cv2.undistort 图像去畸变
3、编程实现图像校准，计算校准参数
4、上传校准参数
由于需要采集图像，我们需要在ROS平台上完成以下功能，先创建新的ROS工作空间
$ cd ~
$ mkdir -p duckietown/catkin_ws/src
$ cd duckietown/catkin_ws/src
$ catkin_init_workspace
$ cd ..
$ catkin_make新建摄像头校准功能包，并新建源码文件
$ cd src
$ catkin_create_pkg calibrate rospy sensor_msgs
$ touch calibrate/src/calibrate_node.py修改编译配置文件如下图：
$ gedit calibrate/CMakeLists.txt
retval, corners = cv.findChessboardCorners(image, patternSize[, corners[, flags]])
第一个参数Image，传入拍摄的棋盘图Mat图像，必须是8位的灰度或者彩色图像；
第二个参数patternSize，每个棋盘图上内角点的行列数，一般情况下，行列数不要相同，便于后续标定程序识别标定板的方向；
第三个参数corners，用于存储检测到的内角点图像坐标位置，一般是数组形式；
第四个参数flage：用于定义棋盘图上内角点查找的不同处理方式，有默认值。
返回值corners中包含了检测到的角点信息
为了提高标定精度，需要在初步提取的角点信息上进一步提取亚像素信息，降低相机标定偏差，常用的方法是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表示有未被探测到的内角点，这时候函数会以（红色）圆圈标记处检测到的内角点；
获取到棋盘标定图的内角点图像坐标之后，就可以使用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：相机内参矩阵
返回值distCoeffs：相机径向畸变参数
优化内参数和畸变系数，通过设定自由比例因子alpha。当alpha设为0的时候，将会返回一个剪裁过的将去畸变后不想要的像素去掉的内参数和畸变系数；当alpha设为1的时候，将会返回一个包含额外黑色像素点的内参数和畸变系数，并返回一个ROI用于将其剪裁掉。
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),alpha,(w,h))
参数1：mtx，相机内参矩阵
参数2：dist，相机径向畸变参数
参数3：（w,h）原图像尺寸
参数4：alpha 视场大小调节，0放大，1不变
参数5：（w,h）新图像大小
返回值newcameramtx：视场调节参数矩阵
返回值roi：感兴趣区域
得到去畸变的图像，使用上面的ROI可以对其进行剪裁
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
参数1：img 原图
参数2：mtx 相机内参矩阵
参数3：dist 相机径向畸变参数
参数4：None
参数5：newcameramtx 视场调节参数矩阵
返回值dst 去畸变的图像
$ gedit calibrate/src/calibrate_node.py#!/usr/bin/env python3
#encoding=utf-8
import rospy
import cv2
import math
import numpy as np
from PIL import Image,ImageDraw,ImageFont
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridge
class CalibrateNode():
def __init__(self):
rospy.init_node("calibrate_node",anonymous=False)
self.bridge = CvBridge()
self.nx = 5 #棋盘格内角点行数
self.ny = 7 #棋盘格内角点列数
#世界坐标系中的三维点
self.objp = np.zeros((self.nx*self.ny,3), np.float32)
self.objp[:,:2] = np.mgrid[0:self.nx,0:self.ny].T.reshape(-1,2)
self.objpoints = [] #三维点列表
self.imgpoints = [] #二维点列表
#提示信息
self.notes = ['0.请将棋盘纸放入摄像头视野中',
'1.请将棋盘格置于摄像头视野中间，面积占整体画面1/5左右',
'2.请将棋盘格向前移动，尽量占满摄像头视野',
'3.请将棋盘格向后移动，直到面积占整体画面1/20左右',
'4.请将棋盘格置于摄像头视野中间，并向上移动至视野上边缘',
'5.请将棋盘格置于摄像头视野中间，并向下移动至视野下边缘',
'6.请将棋盘格置于摄像头视野中间，并向右移动至视野左边缘',
'7.请将棋盘格置于摄像头视野中间，并向左移动至视野右边缘',
'8.请将棋盘格置于摄像头视野中间，并移动至视野左上边缘',
'9.请将棋盘格置于摄像头视野中间，并移动至视野左下边缘',
'10.请将棋盘格置于摄像头视野中间，并移动至视野右上边缘',
'11.请将棋盘格置于摄像头视野中间，并移动至视野右下边缘',
'12.请将棋盘格置于摄像头视野中间，并向前倾斜60度角',
'13.请将棋盘格置于摄像头视野中间，并向后倾斜60度角',
'14.请将棋盘格置于摄像头视野中间，并向左倾斜60度角',
'15.请将棋盘格置于摄像头视野中间，并向右倾斜60度角',
'16.数据采集完毕，开始校准摄像头',
'17.校准完成，请保存校准数据']
self.step = 0 #操作步骤计数
self.mtx = None #内参矩阵
self.dist = None #径向畸变矩阵
self.newcameramtx = None #感兴趣区域（ROI）截取矩阵
self.roi = None #感兴趣区域
#求角点的迭代过程的终止条件，采用的停止准则是最大循环次数31和最大误差容限0.001
self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 31, 0.001)
self.skip = False #为减少卡顿，设定图片跳帧标志
self.count = 0 #图片跳帧计数
#订阅小车图像话题，需要根据小车主机名做相应修改
rospy.Subscriber("/duckiebot1/camera_node/image/compressed", CompressedImage, self.cb_image)
def cb_image(self, msg):
if self.skip or self.count<4: #设定跳帧条件，不符合则跳过当前帧
self.skip = False
self.count += 1
return
else:
self.count = 0
self.skip = True
#图片消息数据转化为opencv2图片格式
image = self.bridge.compressed_imgmsg_to_cv2(msg)
if self.step<16: #操作步骤前16步工作内容是数据采集
image = self.find_corners(image)
elif self.step==16: #第17步根据前16步采集的数据计算摄像头各项参数
shape = image.shape
ret, self.mtx, self.dist, rvecs, tvecs = cv2.calibrateCamera(self.objpoints, self.imgpoints, (shape[0],shape[1]), None, None)
self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(self.mtx,self.dist,(shape[1],shape[0]),0,(shape[1],shape[0]))
self.step += 1
print("mtx:", self.mtx)
print("dist:", self.dist)
elif self.step==17: #利用计算出的校准参数校准图片
image = cv2.undistort(image, self.mtx, self.dist, None, self.newcameramtx)
#为图片添加提示文字信息
image = self.cv2ImgAddText(image, self.notes[self.step], 10, 10, (255, 0, 0), 20)
cv2.imshow("image",image)
cv2.waitKey(1)
#利用cv2.findChessboardCorners和cv2.cornerSubPix函数查找棋盘格内角点信息，并绘制到图片上
def find_corners(self, image):
#转化灰度图片
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
#查找内角点信息
ret, corners = cv2.findChessboardCorners(gray, (self.nx, self.ny), None)
if ret==True:
#查找亚像素角点信息
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),self.criteria)
#绘制角点图像
return self.draw_conners(image, corners)
else: #如果没有查找到角点信息，则下一帧图片跳过不处理
self.skip = True
return image
#绘制角点数据，根据角点信息判断棋盘格位置，符合筛选条件的位置保存相应角点信息
def draw_conners(self, image, corners):
shape = image.shape
cns = np.zeros((7,5,2),np.float32) #新建数组存储角点坐标
for idx, cn in enumerate(corners):
[[x,y]] = cn
cns[idx//5][idx%5] = [x, y]
#第0步检测到棋盘格即可
if self.step==0:
self.step += 1
#第1和3步需要棋盘格距离相机较近和较远，近需要占据总图片面积15%以上区域，远需要占据图片面积5%以下
elif self.step==1 or self.step==3:
w = cns[6][4][0]-cns[0][0][0]
h = cns[6][4][1]-cns[0][0][1]
if self.step==1 and w*h/(shape[0]*shape[1])>=0.15:
self.saveCornersData(corners)
elif self.step==3 and w*h/(shape[0]*shape[1])<=0.05:
self.saveCornersData(corners)
#第2步棋盘格尽量占满摄像头视野，第4~11步需要棋盘格分别出现在相机视野的上、下、左、右、左上、左下、右上、右下。
elif self.step==2 or (self.step>=4 and self.step<=11):
margin_top = np.sum(cns[0], 0)[1]/5 #棋盘格距离上边缘平均距离
margin_bottom = shape[0] - np.sum(cns[6], 0)[1]/5 #棋盘格距离下边缘平均距离
margin_left = np.sum(cns[:,:1,:],0)[0][0]/7 #棋盘格距离左边缘平均距离
margin_right = shape[1] - np.sum(cns[:,4:,:],0)[0][0]/7 #棋盘格距离右边缘平均距离
#第2步、上下接近边缘或者左右接近边缘表示棋盘格占满图像
if self.step==2 and ((margin_top<shape[0]/8 and margin_bottom<shape[0]/8) or (margin_left<shape[1]/8 and margin_right<shape[1]/8)):
self.saveCornersData(corners)
#第4步、棋盘格接近上边缘
elif self.step==4 and margin_top<shape[0]/8:
self.saveCornersData(corners)
#第5步、棋盘格接近下边缘
elif self.step==5 and margin_bottom<shape[0]/8:
self.saveCornersData(corners)
#第6步、棋盘格接近左边缘
elif self.step==6 and margin_left<shape[1]/8:
self.saveCornersData(corners)
#第7步、棋盘格接近右边缘
elif self.step==7 and margin_right<shape[1]/8:
self.saveCornersData(corners)
#第8步、棋盘格接近左上边缘
elif self.step==8 and margin_top<shape[0]/8 and margin_left<shape[1]/8:
self.saveCornersData(corners)
#第9步、棋盘格接近左下边缘
elif self.step==9 and margin_bottom<shape[0]/8 and margin_left<shape[1]/8:
self.saveCornersData(corners)
#第10步、棋盘格接近右上边缘
elif self.step==10 and margin_top<shape[0]/8 and margin_right<shape[1]/8:
self.saveCornersData(corners)
#第11步、棋盘格接近右下边缘
elif self.step==11 and margin_bottom<shape[0]/8 and margin_right<shape[1]/8:
self.saveCornersData(corners)
#第12-15步，需要棋盘格在图像中做出倾斜状态，当一个正方形被倾斜后，其宽高比会发生变化
elif self.step>=12 and self.step<=15:
w_top = cns[0][4][0]-cns[0][0][0] #棋盘格顶部宽度
w_bottom = cns[6][4][0]-cns[6][0][0] #棋盘格底部宽度
h_left = cns[6][0][1] - cns[0][0][1] #棋盘格左侧宽度
h_right = cns[6][4][1] - cns[0][4][1] #棋盘格右侧宽度
w_mean = (w_top + w_bottom)/8 #计算横向方格平均宽度
h_mean = (h_left +h_right)/12 #计算纵向方格平均高度
#第12步、前倾
if self.step==12 and w_top>w_bottom and w_mean/h_mean>=2:
self.saveCornersData(corners)
#第13步、后倾
elif self.step==13 and w_top<w_bottom and w_mean/h_mean>=2:
self.saveCornersData(corners)
#第14步、左倾
elif self.step==14 and h_left>h_right and h_mean/w_mean>=2:
self.saveCornersData(corners)
#第15步、右倾
elif self.step==15 and h_left<h_right and h_mean/w_mean>=2:
self.saveCornersData(corners)
#内角点信息画到图片上
cv2.drawChessboardCorners(image, (self.nx, self.ny), corners, True)
return image
#为图片附加提示文字
def cv2ImgAddText(self, img, text, left, top, textColor, textSize):
#图像转化为PLT格式
img = Image.fromarray(cv2.cvtColor(img,cv2.COLOR_BGR2RGB))
draw = ImageDraw.Draw(img)
#设置字体格式
ttf = "/usr/share/fonts/opentype/noto/NotoSansCJK-Bold.ttc"
fontStyle = ImageFont.truetype(ttf, textSize, encoding="utf-8")
#将文字附加到图片上
draw.text((left,top),text,textColor,font=fontStyle)
#图片再转化回opencv格式
return cv2.cvtColor(np.asarray(img),cv2.COLOR_RGB2BGR)
#存储内角点信息
def saveCornersData(self, corners):
self.objpoints.append(self.objp)
self.imgpoints.append(corners)
self.step += 1
if __name__=='__main__':
node = CalibrateNode()
rospy.spin()编译源码
$ cd ~/duckietown/catkin_ws
$ catkin_make设置环境变量
$ source devel/setup.bash运行程序
$ rosrun calibrate calibrate_node.py按照提示依次操作棋盘格做出指定动作
最后在终端打印相机内参以及径向畸变参数：
mtx: [[306.09044878 0. 328.29132065] [ 0. 304.98753442 235.57672176] [ 0. 0. 1. ]]
dist: [[-3.11854407e-01 9.77819171e-02 1.91544813e-03 1.56072741e-04 -1.38483714e-02]]
计算出相机矩阵和径向畸变参数后，对图像进行校准，下图可见棋盘格已经校正，所有格子都恢复为正方形。
注：相机校准过程中，需要保证棋盘格平整，所以棋盘格需要附着到一个刚性的物体上，如果棋盘格有变形情况，那么校准参数就会失真，会影响后续车道线识别准确度。
相机校准参数是个性化参数，每辆车都会不相同，所以需要上传到小车上保存，操作步骤如下：
远程登录小车：
$ ssh duckie@duckiebot1.local
Are you sure you want to continue connecting (yes/no/[fingerprint])? yes
duckie@duckiebot1.local's password: quackquack编辑相机参数配置文件：
$ vim /data/config/calibrations/camera_intrinsic/default.yaml修改其中的camera_matrix和distortion_coefficients为我们计算出的参数
image_width: 640
image_height: 480
camera_name: /porsche911/rosberrypi_cam
camera_matrix:
rows: 3
cols: 3
data:
- 306.09044878
- 0
- 328.29132065
- 0
- 304.98753442
- 235.57672176
- 0
- 0
- 1
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data:
- -3.11854407e-01
- 9.77819171e-02
- 1.91544813e-03
- 1.56072741e-04
- -1.38483714e-02
rectification_matrix:
rows: 3
cols: 3
data:
- 1
- 0
- 0
- 0
- 1
- 0
- 0
- 0
- 1
projection_matrix:
rows: 3
cols: 4
data:
- 210.1107940673828
- 0
- 327.2577820024981
- 0
- 0
- 253.8408660888672
- 239.9969353923052
- 0
- 0
- 0
- 1
- 0vim基本操作命令简介：打开后按i键进入编辑模式，按方向键操作光标移动，编辑后，按esc退出编辑模式，输入:wq保存并退出，输入:q!不保存退出。
重启小车
$ sudo reboot等待小车重启完成后，在虚拟机查看小车摄像头参数信息：
$ rostopic echo /duckiebot1/camera_node/camera_info
其中的K和D就是我们计算出的参数，在后续图像处理中需要根据这两个参数对图像进行校准后再识别。