非常详细的相机标定（六）（2维坐标点转为3维坐标点）

根据提取的相机的参数，2维坐标点转为3维坐标点，代码如下：

import argparse
from argparse import RawTextHelpFormatter
import numpy as np
import cv2# 寻找焦点
def cam_calib_find_corners(img, rlt_dir, col, row):# 灰度化图片，减少参数的运算gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)# 寻找角点ret, corners = cv2.findChessboardCorners(gray, (col, row), None)# 为了得到稍微精确一点的角点坐标，进一步对角点进行亚像素寻找corners2 = cv2.cornerSubPix(gray, corners, (7, 7), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 10, 0.001))if ret == True:# 保存角点图像sav_path = rlt_dir + "/1_corner.jpg"# 绘制角点cv2.drawChessboardCorners(img, (col, row), corners2, ret)cv2.imwrite(sav_path, img)return (ret, corners2)# 相机标定
def cam_calib_calibrate(img_dir, rlt_dir, col, row, img_num):w = 0h = 0all_corners = []patterns = []x, y = np.meshgrid(range(col), range(row))prod = row * colpattern_points = np.hstack((x.reshape(prod, 1), y.reshape(prod, 1), np.zeros((prod, 1)))).astype(np.float32)img_path = r"C:\Users\17930\Desktop\1.jpg"img = cv2.imread(img_path)if img is None:print(f"Error reading image at {img_path}")passelse:(h, w) = img.shape[:2]ret, corners = cam_calib_find_corners(img, rlt_dir, col, row)all_corners.append(corners)patterns.append(pattern_points)rms, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(patterns, all_corners, (w, h), None, None)print('相机内参', cameraMatrix)print('相机外参旋转向量', rvecs)print('相机外参平移向量', tvecs)return cameraMatrix, distCoeffs, rvecs, tvecsif __name__ == "__main__":parser = argparse.ArgumentParser(description="读取标定的图片并保存结果", formatter_class=RawTextHelpFormatter)parser.add_argument("--img_dir", help="标定图片路径", type=str,metavar='', default=r'D:\螺丝数据集与相机标定代码\camera\calib_img')parser.add_argument("--rlt_dir", help="保存路径", type=str,metavar='', default="rlt_dir")parser.add_argument("--crct_img_dir", help="待矫正图像路径",type=str, metavar='', default="crct_img")parser.add_argument("--row_num", help="每一行有多少个角点，边缘处的不算",type=int, metavar='', default="7")parser.add_argument("--col_num", help="每一列有多少个角点，边缘处的不算",type=int, metavar='', default="6")parser.add_argument("--img_num", help="多少幅图像",type=int, metavar='', default="4")args = parser.parse_args()cameraMatrix, distCoeffs, rvecs, tvecs = cam_calib_calibrate(args.img_dir, args.rlt_dir, args.row_num, args.col_num, args.img_num)image_points = np.array([[55, 66], [77, 88]], dtype=np.float32)world_z = 0world_points = []for i in range(len(rvecs)):rotation_matrix, _ = cv2.Rodrigues(rvecs[i])projection_matrix = np.dot(cameraMatrix, np.hstack((rotation_matrix, tvecs[i])))inv_projection_matrix = np.linalg.pinv(projection_matrix)for pt in image_points:img_pt = np.array([pt[0], pt[1], 1])ray_dir = np.dot(inv_projection_matrix, img_pt)scale = (world_z - tvecs[i][2]) / ray_dir[2]world_pt = tvecs[i].reshape(3) + scale * ray_dir[:3]world_points.append(world_pt)world_points = np.array(world_points)print(world_points)