单、双目标定及双目立体视觉深度图获取

最终目标：获取位图（深度图）

过程：

1、单目标定（棋盘标定法）

2、双目标定，获取位图

 

一、单目标定

前期准备--两个摄像机固定到一起，镜头之间呈现一定的夹角，然后对准标定板，切换各种角度拍摄。为了方便，把棋盘图片和最终要测量的实物图一起拍完。

我的标定板一开始是用自制的，精度差的一批。后面买了个72*72毫米极小的标定板来标定。

首先标定左相机拍摄的图片，标定的目标是通过查找角点计算出相机的扭曲矩阵，内外参数矩阵。

可以将结果打印出来观察，画出角点，图片保存在提前创建好的show文件夹中。

注意代码中关于要修改棋盘格角点大小的多个位置。计算棋盘角点大小的方式一般是从左上角第一个格子黑色算起，这个格子开始计算黑白格子交点个数

import numpy as np
import cv2
import glob
import time
# 设置迭代终止条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 设置 object points, 形式为 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((8*10,3), np.float32) #我用的是8×10的棋盘格，可根据自己棋盘格自行修改相关参数
objp[:,:2] = np.mgrid[0:8,0:10].T.reshape(-1,2)

# 用arrays存储所有图片的object points 和 image points
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

#用glob匹配文件夹下所有文件名含有“.jpg"的图片
images = glob.glob(r"/home/ubuntu/Two_eyes/left/*.bmp")
t0 = time.time()
for fname in images:
    print(time.time()-t0)
    prefix=fname.split('/')[5]
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, (8,10), None)
    # 如果找到了就添加 object points, image points
    if ret == True:
        objpoints.append(objp)
        corners2=cv2.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners)
        # 对角点连接画线加以展示
        cv2.drawChessboardCorners(img, (8,10), corners2, ret)
        cv2.imwrite('/home/ubuntu/Two_eyes/left/show/'+prefix, img)
        cv2.namedWindow("img", 0)
        cv2.resizeWindow("img", 1200, 800);
        cv2.imshow('img', img)
        cv2.waitKey(5)
        
cv2.destroyAllWindows()

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print(ret)
print("############")
print(mtx)
print("############")
print(dist)
print("############")
print(rvecs)
print("############")
print(tvecs)

#对所有图片进行去畸变，有两种方法实现分别为： undistort()和remap()
images = glob.glob(r"/home/ubuntu/Two_eyes/left/*.bmp")
for fname in images:
    prefix=fname.split('/')[5]
    img = cv2.imread(fname)
    h,  w = img.shape[:2]
    newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))

    # # 使用 cv.undistort()进行畸变校正
    # dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
    # # 对图片有效区域进行剪裁
    # # x, y, w, h = roi
    # # dst = dst[y:y+h, x:x+w]
    # cv2.imwrite('/home/song/pic_1/undistort/'+prefix, dst)

    #  使用 remap() 函数进行校正
    mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
    dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
    # 对图片有效区域进行剪裁
    x, y, w, h = roi
    dst = dst[y:y + h, x:x + w]
    cv2.imwrite('/home/ubuntu/Two_eyes/left/undistort/'+prefix, dst)

#重投影误差计算，对标定结果评价
mean_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
    mean_error += error

print("total error: ", mean_error/len(objpoints))

只有少部分图片可以标定成功，大概是占了1/4的概率，所以报废了很多图片。。。

代码修改路径，对右相机也做标定

也可以将扭曲矫正后的图片保存下来查看。相机标定过程会为这个相机的镜头生成一组矫正数据，包括 内参矩阵、形变参数、旋转向量、平移向量

最后一步，将左右相机都标定成功的组图片复制到新的文件夹中，注意一组一组的对应

 

二、双目相机标定

过程：对左右摄像头图片分别做角点获取，矫正。之后使用极线对齐的方法在两幅图片上画线，来观察两幅图片是否匹配。

最后用Block Maching方法生成差异图

 

import cv2
import numpy as np
import glob
import matplotlib.pyplot as plt
from PIL import Image

# 设置迭代终止条件
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
criteria_stereo = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 设置 object points, 形式为 (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((8*10,3), np.float32) #我用的是9×7的棋盘格，可根据自己棋盘格自行修改相关参数
objp[:,:2] = np.mgrid[0:8,0:10].T.reshape(-1,2)

# 用arrays存储所有图片的object points 和 image points
objpoints = [] # 3d point in real world space
imgpointsL = [] # 2d points in image plane.
imgpointsR = [] 

string = ["12"] #, "15", "20", "22"

for i in range(0, 1):
    t = string[i]
    imgL = cv2.imread("/home/ubuntu/Two_eyes/all_img_use2/" + t + "_L.bmp")
    grayL = cv2.cvtColor(imgL, cv2.COLOR_BGR2GRAY)
    # 查找棋盘格角点
    retL, cornersL = cv2.findChessboardCorners(grayL, (8,10), None)

    imgR = cv2.imread("/home/ubuntu/Two_eyes/all_img_use2/" + t + "_R.bmp")
    grayR = cv2.cvtColor(imgR, cv2.COLOR_BGR2GRAY)
    # 查找棋盘格角点
    retR, cornersR = cv2.findChessboardCorners(grayR, (8,10), None)
    
    print(i, retL, retR)
    # print(grayL.shape[::-1], grayL.shape[:2], grayL.shape)


    # 如果找到了就添加 object points, image points
    if  (retL == True) &  (retR == True) :
        objpoints.append(objp)
        cornersL=cv2.cornerSubPix(grayL,cornersL, (11,11), (-1,-1), criteria)
        cornersR=cv2.cornerSubPix(grayR,cornersR, (11,11), (-1,-1), criteria)
        imgpointsL.append(cornersL)
        imgpointsR.append(cornersR)
        #角点提取顺序展示
        cv2.drawChessboardCorners(imgL, (8,10), cornersL, retL)
        im_L=Image.fromarray(imgL) 
        cv2.drawChessboardCorners(imgR, (8,10), cornersR, retR)
        im_R=Image.fromarray(imgR) 
        width = im_L.size[0]*2
        height = im_L.size[1]
        img_compare = Image.new('RGBA',(width, height))
        img_compare.paste(im_L,box=(0,0))
        img_compare.paste(im_R,box=(im_L.size[0],0))
        plt.imshow(img_compare)
        plt.show()
        
    
#         cv2.namedWindow("img", 0)
#         cv2.resizeWindow("img", 1200, 800);
#         cv2.imshow('img', img)
#         cv2.waitKey(50)
# cv2.destroyAllWindows()

# 左标定 #########################################
# print(len(objpoints))
# print(len(imgpointsL))

retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(objpoints, imgpointsL, grayL.shape[::-1], None, None)
print("left finished")

#   获取新的相机矩阵后续传递给initUndistortRectifyMap，以用remap生成映射关系
hL, wL = grayL.shape[:2]
OmtxL, roiL = cv2.getOptimalNewCameraMatrix(mtxL, distL, (wL, hL), 1, (wL, hL))


# 右侧标定###############################################
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(objpoints, imgpointsR, grayR.shape[::-1], None, None)
print("right finished")

hR, wR =  grayR.shape[:2]
OmtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR,(wR, hR), 1, (wR, hR))

# # 做扭曲矫正##################################################
# img1 = cv2.imread("./26_L.png")
# img2 = cv2.imread("./26_R.png")

# mapx, mapy = cv2.initUndistortRectifyMap(mtxL, distL, None, OmtxL, (wL, hL), 5)
# imgL = cv2.remap(img1, mapx, mapy, cv2.INTER_LINEAR)

# mapx, mapy = cv2.initUndistortRectifyMap(mtxR, distR, None, OmtxR, (wL, hL), 5)
# imgR = cv2.remap(img2, mapx, mapy, cv2.INTER_LINEAR)

# cv2.imwrite("./fL.bmp", imgL)
# cv2.imwrite("./fR.bmp", imgR)



# 双目相机的标定##############################################
# 设置标志位为cv2.CALIB_FIX_INTRINSIC，这样就会固定输入的cameraMatrix和distCoeffs不变，只求解𝑅,𝑇,𝐸,𝐹
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC

retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(objpoints,imgpointsL,imgpointsR,OmtxL,distL,OmtxR,distR,
                                                           grayR.shape[::-1], criteria_stereo,flags)

# 利用stereoRectify()计算立体校正的映射矩阵
rectify_scale= 1 # 设置为0的话，对图片进行剪裁，设置为1则保留所有原图像像素
RL, RR, PL, PR, Q, roiL, roiR= cv2.stereoRectify(MLS, dLS, MRS, dRS,grayR.shape[::-1], R, T,
                                                 rectify_scale,(0,0))  
# 利用initUndistortRectifyMap函数计算畸变矫正和立体校正的映射变换，实现极线对齐。
Left_Stereo_Map= cv2.initUndistortRectifyMap(MLS, dLS, RL, PL, grayL.shape[::-1], cv2.CV_16SC2)   

Right_Stereo_Map= cv2.initUndistortRectifyMap(MRS, dRS, RR, PR, grayR.shape[::-1], cv2.CV_16SC2)


#立体校正效果显示################################################
for i in range(0,1):  # 以第一对图片为例
    t = str(i)
    # frameR = cv2.imread('/home/song/pic/right_' + t + '.jpg', 0)  
    # frameL = cv2.imread('/home/song/pic/left_' + t + '.jpg', 0) 
    frameL = cv2.imread("./22_L.bmp")
    frameR = cv2.imread("./22_R.bmp")
    
    Left_rectified= cv2.remap(frameL,Left_Stereo_Map[0], Left_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)  # 使用remap函数完成映射
    im_L=Image.fromarray(Left_rectified) # numpy 转 image类
   
    Right_rectified= cv2.remap(frameR,Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)
    im_R=Image.fromarray(Right_rectified) # numpy 转 image 类

    # cv2.imshow("f", Left_rectified)
    # key = cv2.waitKey(0)
    # if key == ord("q"):
    #     cv2.destroyAllWindows()

	#创建一个能同时并排放下两张图片的区域，后把两张图片依次粘贴进去
    width = im_L.size[0]*2
    height = im_L.size[1]

    img_compare = Image.new('RGBA',(width, height))
    img_compare.paste(im_L,box=(0,0))
    img_compare.paste(im_R,box=(im_L.size[0],0))
    
    #在已经极线对齐的图片上均匀画线
    for i in range(1,20):
        len=1920/20
        plt.axhline(y=i*len, color='r', linestyle='-')
    plt.imshow(img_compare)
    plt.show()

#获取深度图################################################################3
img1 = cv2.imread("./22_L.bmp")
img2 = cv2.imread("./22_R.bmp")

cv2.namedWindow("left")
cv2.namedWindow("right")
cv2.namedWindow("depth")
cv2.resizeWindow("depth", 1200, 800);
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 600, 0)
cv2.createTrackbar("num", "depth", 0, 10, lambda x: None)
cv2.createTrackbar("blockSize", "depth", 5, 255, lambda x: None)

# 添加点击事件，打印当前点的距离
def callbackFunc(e, x, y, f, p):
    if e == cv2.EVENT_LBUTTONDOWN:        
        print(threeD[y][x])
 
cv2.setMouseCallback("depth", callbackFunc, None)

Left_rectified= cv2.remap(img1, Left_Stereo_Map[0],Left_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)
Right_rectified= cv2.remap(img2, Right_Stereo_Map[0],Right_Stereo_Map[1], cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT, 0)


# 将图片置为灰度图，为StereoBM作准备
#cv2.imshow("gray", Left_rectified)
imgL = cv2.cvtColor(Left_rectified, cv2.COLOR_BGR2GRAY)
imgR = cv2.cvtColor(Right_rectified, cv2.COLOR_BGR2GRAY)
 
# 两个trackbar用来调节不同的参数查看效果
num = cv2.getTrackbarPos("num", "depth")
blockSize = cv2.getTrackbarPos("blockSize", "depth")
if blockSize % 2 == 0:
    blockSize += 1
if blockSize < 5:
    blockSize = 5

# 根据Block Maching方法生成差异图（opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试）
stereo = cv2.StereoBM_create(numDisparities=16*num, blockSize=blockSize)
disparity = stereo.compute(imgL, imgR)
 
disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
# 将图片扩展至3d空间中，其z方向的值则为当前的距离
threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32)/16., Q)

cv2.imshow("left", imgL)
cv2.imshow("right", imgR)
cv2.imshow("depth", disp)
cv2.imwrite("./depth.bmp", disp)
 
key = cv2.waitKey(0)
if key == ord("q"):
    cv2.destroyAllWindows()

可能遇到的问题：

两张矫正后的图片大片黑色，十分扭曲 -- cv2的查找棋盘角点的函数 findchessboardcorner 有两种查找方向，顺时针和逆时针。只有当左右两张图片的查找方向都相同时，参数矩阵才有意义。

比如这样的，就是造成错误干扰的：

不是很能看的清，但是可以看出，左边角点的颜色是蓝色到紫色的渐变，但是右边角点的颜色是从紫色到蓝色

而正确匹配的角点应该是：

都是从紫色到蓝色

这个好像没什么好的解决办法，我还没找到。我的解决办法是把正确的照片组挑出来，所以最后只有4组能用的。

 

部分代码参考来自：https://blog.csdn.net/qq_22059843/article/details/103400094