https://blog.csdn.net/chenshiming1995/article/details/106546455
雙目相機標定+去畸變+獲得視差+深度
一共需要解決幾個問題：
棋格板或者圓點板，代碼有細微差別
怎樣對比標定結果： 觀察像素誤差
需不需要棋盤格子的尺寸：內參不需要，只有計算雙目間的外參R和T時，T與棋盤格大小相關，成正比關系。另外T[0]代表基線長度
代碼的正確性：沒有錯誤，已檢查過，當然一些是復制別人的工作
導入你自己的標定圖片需要更改：
角點個數（目前是7*7）
文件夾名稱（顯然）
世界坐標系的圓心點距離（目前是15mm）
確定是棋盤格還是圓心標定板（目前用的圓點板）
一共有三段代碼，建議在ipython notebook下運行
5. 第一段：雙目標定
6. 第二段：配置內參參數
7. 第三段：去畸變+獲得視差+深度
import cv2
import matplotlib.pyplot as plt # plt 用於顯示圖片
import matplotlib.image as mpimg # mpimg 用於讀取圖片
import sys
import numpy as np
import glob
class shuangmu:
def __init__(self):
self.m1 = 0
self.m2 = 0
self.d1 = 0
self.d2 = 0
self.R = 0
self.T = 0
stereo = shuangmu()
class StereoCalibration(object):
def __init__(self):
self.imagesL = self.read_images('camL')
self.imagesR = self.read_images('camR')
def read_images(self , cal_path):
filepath = glob.glob(cal_path + '/*.bmp')
filepath.sort()
return filepath
#標定圖像
def calibration_photo(self):
#設置要標定的角點個數
x_nums = 7 #x方向上的角點個數
y_nums = 7
# 設置(生成)標定圖在世界坐標中的坐標
world_point = np.zeros((x_nums * y_nums,3),np.float32) #生成x_nums*y_nums個坐標，每個坐標包含x,y,z三個元素
world_point[:,:2] = np.mgrid[:x_nums,:y_nums].T.reshape(-1, 2) #mgrid[]生成包含兩個二維矩陣的矩陣，每個矩陣都有x_nums列,y_nums行
#.T矩陣的轉置
#reshape()重新規划矩陣，但不改變矩陣元素
#保存角點坐標
world_position = []
image_positionl = []
image_positionr = []
#設置角點查找限制
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,30,0.001)
#獲取所有標定圖
for ii in range(20):
image_path_l = self.imagesL[ii]
image_path_r = self.imagesR[ii]
image_l = cv2.imread(image_path_l)
image_r = cv2.imread(image_path_r)
gray_l = cv2.cvtColor(image_l,cv2.COLOR_RGB2GRAY)
gray_r = cv2.cvtColor(image_r,cv2.COLOR_RGB2GRAY)
#查找角點
# ok,corners = cv2.findChessboardCorners(gray,(x_nums,y_nums),None)
# ok1,cornersl = cv2.findChessboardCorners(gray_l,(x_nums,y_nums),None)
# ok2,cornersr = cv2.findChessboardCorners(gray_r,(x_nums,y_nums),None)
ok1,cornersl = cv2.findCirclesGrid(gray_l,(x_nums,y_nums),None)
ok2,cornersr = cv2.findCirclesGrid(gray_r,(x_nums,y_nums),None)
self.world = world_point
print(ok1&ok2)
if ok1&ok2:
#把每一幅圖像的世界坐標放到world_position中
center_spacing = 15 ## 圓心的位置距離，這一個其實不重要
world_position.append(world_point*center_spacing)
#獲取更精確的角點位置
exact_cornersl = cv2.cornerSubPix(gray_l,cornersl,(11,11),(-1,-1),criteria)
exact_cornersr = cv2.cornerSubPix(gray_r,cornersr,(11,11),(-1,-1),criteria)
#把獲取的角點坐標放到image_position中
image_positionl.append(exact_cornersl)
image_positionr.append(exact_cornersr)
#可視化角點
# image = cv2.drawChessboardCorners(image,(x_nums,y_nums),exact_corners,ok)
# cv2.imshow('image_corner',image)
# cv2.waitKey(0)
#計算內參數
image_shape = gray_l.shape[::-1]
retl, mtxl, distl, rvecsl, tvecsl = cv2.calibrateCamera(world_position, image_positionl, image_shape , None,None)
retr, mtxr, distr, rvecsr, tvecsr = cv2.calibrateCamera(world_position, image_positionr, image_shape , None,None)
print('ml = ',mtxl)
print('mr = ',mtxr)
print('dl = ' , distl)
print('dr = ' , distr)
stereo.m1 = mtxl
stereo.m2 = mtxr
stereo.d1 = distl
stereo.d2 = distr
#計算誤差
self.cal_error(world_position , image_positionl , mtxl , distl , rvecsl , tvecsl)
self.cal_error(world_position , image_positionr , mtxr, distr , rvecsr , tvecsr)
##雙目標定
self.stereo_calibrate( world_position ,image_positionl , image_positionr , mtxl, distl, mtxr, distr, image_shape)
def cal_error(self , world_position , image_position , mtx , dist , rvecs , tvecs):
#計算偏差
mean_error = 0
for i in range(len(world_position)):
image_position2, _ = cv2.projectPoints(world_position[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(image_position[i], image_position2, cv2.NORM_L2) / len(image_position2)
mean_error += error
print("total error: ", mean_error / len(image_position))
def stereo_calibrate( self , objpoints ,imgpoints_l , imgpoints_r , M1, d1, M2, d2, dims):
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
flags |= cv2.CALIB_FIX_FOCAL_LENGTH
flags |= cv2.CALIB_ZERO_TANGENT_DIST
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER +cv2.TERM_CRITERIA_EPS, 100, 1e-5)
ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
objpoints, imgpoints_l,
imgpoints_r, M1, d1, M2,
d2, dims,
criteria=stereocalib_criteria, flags=flags)
print(R)
print(T)
stereo.R = R
stereo.T = T
if __name__ == '__main__':
# calibration_photo()
biaoding = StereoCalibration()
biaoding.calibration_photo()
import numpy as np
import cv2
#雙目相機參數
class stereoCameral(object):
def __init__(self):
#左相機內參數
self.cam_matrix_left = stereo.m1
#右相機內參數
self.cam_matrix_right = stereo.m2
#左右相機畸變系數:[k1, k2, p1, p2, k3]
self.distortion_l = stereo.d1
self.distortion_r = stereo.d2
#旋轉矩陣
self.R = stereo.R
#平移矩陣
self.T = stereo.T
self.baseline = stereo.T[0]
import cv2
import numpy as np
import matplotlib.pyplot as plt # plt 用於顯示圖片
import matplotlib.image as mpimg # mpimg 用於讀取圖片
# 預處理
def preprocess(img1, img2):
# 彩色圖->灰度圖
im1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
im2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# 直方圖均衡
im1 = cv2.equalizeHist(im1)
im2 = cv2.equalizeHist(im2)
return im1, im2
# 消除畸變
def undistortion(image, camera_matrix, dist_coeff):
undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)
return undistortion_image
# 獲取畸變校正和立體校正的映射變換矩陣、重投影矩陣
# @param：config是一個類，存儲着雙目標定的參數:config = stereoconfig.stereoCamera()
def getRectifyTransform(height, width, config):
# 讀取內參和外參
left_K = config.cam_matrix_left
right_K = config.cam_matrix_right
left_distortion = config.distortion_l
right_distortion = config.distortion_r
R = config.R
T = config.T
# 計算校正變換
height = int(height)
width = int(width)
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion, (width, height), R, T, alpha=-1)
map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_16SC2)
map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_16SC2)
print(width,height)
return map1x, map1y, map2x, map2y, Q
# 畸變校正和立體校正
def rectifyImage(image1, image2, map1x, map1y, map2x, map2y):
rectifyed_img1 = cv2.remap(image1, map1x, map1y, cv2.INTER_LINEAR)
rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_LINEAR)
return rectifyed_img1, rectifyed_img2
# 立體校正檢驗----畫線
def draw_line1(image1, image2):
# 建立輸出圖像
height = max(image1.shape[0], image2.shape[0])
width = image1.shape[1] + image2.shape[1]
output = np.zeros((height, width,3), dtype=np.uint8)
output[0:image1.shape[0], 0:image1.shape[1]] = image1
output[0:image2.shape[0], image1.shape[1]:] = image2
for k in range(15):
cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA) # 直線間隔：100
return output
# 立體校正檢驗----畫線
def draw_line2(image1, image2):
# 建立輸出圖像
height = max(image1.shape[0], image2.shape[0])
width = image1.shape[1] + image2.shape[1]
output = np.zeros((height, width), dtype=np.uint8)
output[0:image1.shape[0], 0:image1.shape[1]] = image1
output[0:image2.shape[0], image1.shape[1]:] = image2
for k in range(15):
cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA) # 直線間隔：100
return output
# 視差計算
def disparity_SGBM(left_image, right_image, down_scale=False):
# SGBM匹配參數設置
if left_image.ndim == 2:
img_channels = 1
else:
img_channels = 3
blockSize = 3
param = {'minDisparity': 0,
'numDisparities': 128,
'blockSize': blockSize,
'P1': 8 * img_channels * blockSize ** 2,
'P2': 32 * img_channels * blockSize ** 2,
'disp12MaxDiff': 1,
'preFilterCap': 63,
'uniquenessRatio': 15,
'speckleWindowSize': 100,
'speckleRange': 1,
'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
}
# 構建SGBM對象
sgbm = cv2.StereoSGBM_create(**param)
# 計算視差圖
size = (left_image.shape[1], left_image.shape[0])
if down_scale == False:
disparity_left = sgbm.compute(left_image, right_image)
disparity_right = sgbm.compute(right_image, left_image)
else:
left_image_down = cv2.pyrDown(left_image)
right_image_down = cv2.pyrDown(right_image)
factor = size[0] / left_image_down.shape[1]
disparity_left_half = sgbm.compute(left_image_down, right_image_down)
disparity_right_half = sgbm.compute(right_image_down, left_image_down)
disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
disparity_left *= factor
disparity_right *= factor
return disparity_left, disparity_right
if __name__ == '__main__':
imgL = cv2.imread("camL/L10.bmp")
imgR = cv2.imread("camR/R10.bmp")
# imgL , imgR = preprocess(imgL ,imgR )
height, width = imgL.shape[0:2]
config = stereoCameral() # 讀取相機內參和外參
# 去畸變
imgL = undistortion(imgL ,config.cam_matrix_left , config.distortion_l )
imgR = undistortion(imgR ,config.cam_matrix_right, config.distortion_r )
# 去畸變和幾何極線對齊
map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width, config)
iml_rectified, imr_rectified = rectifyImage(imgL, imgR, map1x, map1y, map2x, map2y)
linepic = draw_line1(iml_rectified , imr_rectified)
plt.imshow(linepic)
# 計算視差
lookdispL,lookdispR = disparity_SGBM(iml_rectified , imr_rectified )
linepic2 = draw_line2(lookdispL,lookdispR)
plt.imshow(linepic2)
# points_3d = cv2.reprojectImageTo3D(lookdispL, Q)
相機1參數-左 內參stereoParams_v2.CameraParameters1.IntrinsicMatrix 1437.23707719718 0 0 -0.163005525961248 1436.86470419033 0 966.786371484151 540.390795809632 1 畸變 stereoParams_v2.CameraParameters1.RadialDistortion 0.0203815336602082 -0.0892188525805868 0.0905320816763131 stereoParams_v2.CameraParameters1.TangentialDistortion 0.00102781329079596 -0.000854815661759533 相機2參數-右 內參stereoParams_v2.CameraParameters1.IntrinsicMatrix 1437.23707719618 0 0 -0.163005527144222 1436.86470418913 0 966.786371477582 540.390795813682 1 畸變 stereoParams_v2.CameraParameters1.RadialDistortion 0.0203815336535712 -0.0892188525415953 0.0905320816167176 stereoParams_v2.CameraParameters1.TangentialDistortion 0.00102781329168718 -0.000854815663150387 相機2到相機1的變換 旋轉stereoParams_v2.RotationOfCamera2 1 0 0 0 1 0 0 0 1 位移stereoParams_v2.TranslationOfCamera2 6.38975788941902e-10 -9.79644888346105e-10 -1.09875374991737e-09
使用matlab可視化深度
https://blog.csdn.net/a6333230/article/details/88245102