單目相機標定原理及影像坐標到世界坐標的求解（影像處理）

啥都不說先上代碼和圖

結果圖：

世界坐標推導程序：

import cv2
import numpy as np

calibrate_board_size = (7, 7)
calibrate_board_circle_gap = 12.5

src_img = cv2.imread('./img/1.jpg', cv2.IMREAD_ANYCOLOR)
src_img = src_img[600:1500, 100:1100]
gray_img = cv2.cvtColor(src_img, cv2.COLOR_RGB2GRAY)

###找標定板圓心###
retval, image_point = cv2.findCirclesGrid(gray_img, calibrate_board_size, cv2.CALIB_CB_SYMMETRIC_GRID)
cv2.drawChessboardCorners(src_img, calibrate_board_size, image_point, True)
cv2.imshow('img', src_img)

###對圓心進行亞像素精度運算###
stop_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
                 30, 0.001)
image_point = cv2.cornerSubPix(gray_img, image_point, calibrate_board_size, (-1, -1), stop_criteria)

###標定相機###
worldPoint = []
for i in range(calibrate_board_size[0]):
    for j in range(calibrate_board_size[1]):
        worldPoint.append([float(i * calibrate_board_circle_gap), float(j * calibrate_board_circle_gap), float(0)])
worldPoint = np.array(worldPoint, dtype='float32')
image_point = np.reshape(image_point, (49, 2))

worldPoints = []
image_points = []

worldPoints.append(worldPoint)
image_points.append(image_point)

retval, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(worldPoints, image_points,
                                                                       (gray_img.shape[1], gray_img.shape[0]), None,
                                                                       None)
print('-------相機內參矩陣--------')
print(camera_matrix)
print('-------相機畸變矩陣--------')
print(dist_coeffs)
print('-------旋轉矩陣--------')
print(rvecs)
print('-------平移矩陣--------')
print(tvecs)

###對影像進行畸變校正###
new_camera_matrix, _roi = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, gray_img.shape[::-1], 1)
print(_roi)
_x, _y, _w, _h = _roi
src_img = cv2.undistort(src_img, new_camera_matrix, dist_coeffs)
cv2.imshow('undistort img', src_img[_x:_x+_w,_y:_y+_h])

###計算沖投影誤差###
mean_error = 0
for i in range(len(worldPoints)):
    reproject_image_point, jacobian = cv2.projectPoints(worldPoints[i], rvecs[i], tvecs[i], camera_matrix, dist_coeffs)
    error = cv2.norm(image_points[i] - reproject_image_point, cv2.NORM_L2) / len(reproject_image_point)
    print(image_points[i] - reproject_image_point)
    mean_error += error
mean_error /= len(worldPoints)
print("calibrate error:" + str(mean_error))

###通過影像坐標計算世界坐標（在有機械手的專案中經常需要用到這個來計算機械手的抓取點）###
'''
前面相機標定求出 1.相機內參矩陣camera_matrix 2.相機畸變矩陣dist_coeffs  3.旋轉矩陣rvecs 4.平移矩陣tvecs
根據coordinate_transform_formula.jpg可以推出從影像坐標到世界坐標的轉換公式

注：如果是使用張正友標定法，則world_point_z=0,相當于一個平面到另一個平面的投影，假設旋轉矩陣R=[rx,ry,rz,t],world_point=[x,y,z,1],z=0可以推出
R可以寫為[rx,ry,t]
下面的只是簡單演示如何通過影像坐標反推世界坐標，具體推導程序可以看圖calibrate_world_point.jpg
'''
worldPoint_index = 30
x = [image_point[worldPoint_index][0], image_point[worldPoint_index][1], 1]
image_point = np.array(x).reshape(3, 1)
image_point = np.asmatrix(image_point)

rotate_matrix = cv2.Rodrigues(rvecs[0])[0]
translate_matrix = tvecs[0]

rotate_matrix = np.asmatrix(rotate_matrix)
translate_matrix = np.asmatrix(translate_matrix)
camera_matrix = np.asmatrix(camera_matrix)

camera_rotate = camera_matrix * rotate_matrix
camera_translate = camera_matrix * translate_matrix
camera_rotate_inv = np.linalg.inv(camera_rotate)

world_point_z = worldPoint[worldPoint_index][2]
matrix1 = camera_rotate_inv * camera_translate

matrix2 = camera_rotate_inv * image_point

world_point_x = ((matrix1[2][0] + world_point_z) * matrix2[0] / matrix2[2]) - matrix1[0]
world_point_y = ((matrix1[2][0] + world_point_z) * matrix2[1] / matrix2[2]) - matrix1[1]
print(worldPoint[worldPoint_index])
print([world_point_x[0], world_point_y[0], worldPoint[worldPoint_index][2]])
cv2.waitKey()

參考鏈接https://zhuanlan.zhihu.com/p/445872994?utm_source=wechat_timeline&utm_medium=social&utm_oi=746817410819977216

gitee代碼地址