VSLAM实践（二）：基于python+opencv的双目相机标定及深度图获取

VSLAM实践（二）：基于python+opencv的双目相机标定及深度图获取

本篇文章主要分享两部分的代码，第一部分为双目相机的标定，第二部分是双目相机三维坐标的获取；

关于双目相机标定，不同于单目相机标定只求相机内参及畸变系数，双目标定主要多了左右相机相对位置关系的标定，本例中所用双目左右相机横向距离为40mm,此参数将作为相机标定效果的一个衡量指标；双目标定的基本思路为：先采用棋盘分别针对左右相机进行标定，然后再用双目标定函数对上述结果进行进一步修正，从而得到更加合理准确的内参，双目标定函数中注意设置flags=cv2.CALIB_USE_INTRINSIC_GUESS。最后需要强掉的一点是，在拍摄棋盘图片时，尽量保证棋盘平整，可从多方位多角度拍10-20张图片，图片数目过少也会影响标定精度。

另一部分，关于双目深度图获取思路为：首先根据左右相机的内参，畸变系数及相对位置关系（R,t）求得立体矫正参数，包括旋转矩阵R1,R2，投影矩阵P1,P2，以及像素到世界坐标系的映射矩阵Q（4*4），然后对输入图像根据上述参数进行立体校正去畸变后，求解左右图片的视差图，最后用映射矩阵与视差图中的时差信息求得三维坐标，此三维坐标的坐标系为左侧相机的相机坐标系；

深度图需要注意的几点有：1）StereoBM_create函数中的numDisparities及blockSize对于结果影响很大，本例中相片像素为320*240，numDisparities取0，blockSize=5，取0以后怎么操作的，其实我也不清楚…；2）注意disparity的数据类型是CV_16S,还是CV_32f

-- coding:utf-8 --

import cv2
import numpy as np
import glob

设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001

criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)

获取标定板角点的位置

board_size = [6,9]
scale = 20
objp = np.zeros((board_size[0] * board_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:(board_size[0]-1)*scale:complex(0,board_size[0]), 0:(board_size[1]-1)*scale:complex(0,board_size[1])].T.reshape(-1, 2)
obj_points = [] # 存储3D点
img_points = [] # 存储左侧相机2D点
img_points_r = [] # 存储右侧相机2D点

#左侧相机内参标定
images = glob.glob("./left/.png")
#images1 = glob.glob("./left/.jpg")
#images = images+images1
print(images)
for fname in images:
img = cv2.imread(fname)
img = cv2.resize(img,(320,240))
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (board_size[0], board_size[1]), None)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (3, 3), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点
if corners2.any:
img_points.append(corners2/1.0)
else:
img_points.append(corners/1.0)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None,flags = 0 )
print(“ret:”, ret)
print(“mtx:\n”, mtx ) # 内参数矩阵
print(“dist:\n”, dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
print(“rvecs:\n”, rvecs) # 旋转向量 # 外参数
print(“tvecs:\n”, tvecs) # 平移向量 # 外参数
print("-------------------计算反向投影误差-----------------------")

#右侧相机内参标定
images = glob.glob("./right/.png")
#images1 = glob.glob("./right/.jpg")
#images = images+images1
obj_points = [] # 存储3D点
for fname in images:
img = cv2.imread(fname)
img = cv2.resize(img, (320, 240))
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (board_size[0], board_size[1]), None)
if ret:
obj_points.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (3, 3), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点
if corners2.any:
img_points_r.append(corners2/1.0)
else:
img_points_r.append(corners/1.0)
‘’‘cv2.drawChessboardCorners(img, (board_size[0], board_size[1]), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
cv2.namedWindow(‘img’, 30)
cv2.imshow(‘img’, img)
cv2.waitKey()’’’
#cv2.destroyAllWindows()

标定

#cv2.CALIB_FIX_K3
ret1, mtx1, dist1, rvecs1, tvecs1 = cv2.calibrateCamera(obj_points, img_points_r, size, None, None,flags = 0 )
print(“ret1:”, ret1)
print(“mtx1:\n”, mtx1 ) # 内参数矩阵
print(“dist1:\n”, dist1) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3)
print(“rvecs1:\n”, rvecs1) # 旋转向量 # 外参数
print(“tvecs1:\n”, tvecs1) # 平移向量 # 外参数
print("-------------------计算反向投影误差-----------------------")

#双目立体矫正及左右相机内参进一步修正
rms, C1, dist1, C2, dist2, R, T, E,F = cv2.stereoCalibrate(obj_points, img_points, img_points_r, mtx, dist,mtx1, dist1,size,
flags=cv2.CALIB_USE_INTRINSIC_GUESS )
#tx为左右相机距离，本例中为40mm
print(T)

#立体校正及深度图获取
cv2.namedWindow(“depth”)
def callbackFunc(e, x, y, f, p):
if e == cv2.EVENT_LBUTTONDOWN:
print(threeD[y][x])
cv2.setMouseCallback(“depth”, callbackFunc, None)

R1,R2,P1,P2,Q,validPixROI1,validPixROI2 = cv2.stereoRectify(C1,dist1,C2,dist2,size,R,T,alpha = -1)
left_map1, left_map2 = cv2.initUndistortRectifyMap(C1, dist1, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(C2, dist2, R2, P2, size, cv2.CV_16SC2)

frame1 = cv2.imread("./left.png")
frame2 = cv2.imread("./right.png")
img1_rectified = cv2.remap(frame1, left_map1, left_map2, cv2.INTER_LINEAR)
img2_rectified = cv2.remap(frame2, right_map1, right_map2, cv2.INTER_LINEAR)
cv2.imshow(“left”, img1_rectified)
cv2.imshow(“right”, img2_rectified)
cv2.waitKey(-1)
imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY)
num = cv2.getTrackbarPos(“num”, “depth”)
blockSize = cv2.getTrackbarPos(“blockSize”, “depth”)
if blockSize % 2 == 0:
blockSize += 1
if blockSize < 5:
blockSize = 5
stereo = cv2.StereoBM_create(numDisparities=0, blockSize=5)
disparity = stereo.compute(imgL, imgR)
disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32)/16., Q) #此三维坐标点的基准坐标系为左侧相机坐标系
cv2.imshow(“depth”, disp)
cv2.waitKey(-1)
cv2.destroyAllWindows()