python cv2摄像头校准，坐标系转换

本文主要是介绍python cv2摄像头校准，坐标系转换，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

代码

先上代码链接：
链接: https://pan.baidu.com/s/1vk1hYcOHdfadU-XwJQQS6g 提取码: cn2h

功能说明

摄像头校准：内参，外参获取，测试校准点
图片视频畸变还原
2D像素坐标坐标转3D世界坐标
3D世界坐标转2D像素坐标

流程分析

使用相机拍摄或直接使用现有的内参和外参图片
张友正标定法获取内参参数
获取外参标记点的世界坐标和像素坐标
使用PNP算法获取相机畸变系数
根据得到的参数做还原和坐标系转换

代码使用tkinter写成了一个小工具，有兴趣的尝试优化：
在这里插入图片描述

本例以摄像头为世界坐标系原点，以汽车车头中间作为测量点
offset是摄像头与测量点的偏移量

1. 校准

通过选取拍摄好的图片或直接调用摄像头拍照获取校准图片，然后将测得的基准点的世界坐标和像素坐标添加到基准点数据中后校准获得camera_params_XXX.xml文件

在这里插入图片描述
注意： 测量距离的时候先平行车和垂直车贴好标线方便测量标记点坐标
如图，根据图像在绿色线对应位置摆放多点然后沿多点贴上胶带即获得垂直坐标线
横坐标在纵坐标上取一点，取垂直纵坐标的坐标线（测试使用单位mm，垂直为x轴，横向左侧为正右侧为负）
在这里插入图片描述

2. 测试

将测得的3D坐标转为对应的像素坐标，然后与源像素坐标比较
如图，绿色是源像素点坐标6个像素点范围圆，红色是通过对应的3D坐标转换的3个像素的实心点

如果红色实心点在绿色圈附近则认为相机参数基本符合要求（精度根据自己的需求调整，测试数据的准确性很重要，测试了很多组数据才将精度调整到需求）

在这里插入图片描述

3. 主要代码

校准部分

class CameraCalibrate(object):def __init__(self, image_size: tuple):super(CameraCalibrate, self).__init__()self.image_size = image_sizeself.matrix = np.zeros((3, 3), np.float)self.new_camera_matrix = np.zeros((3, 3), np.float)self.dist = np.zeros((1, 5))self.roi = np.zeros(4, np.int)self.element = ET.Element('cameraParams')self.elementTree = ET.ElementTree(self.element)self.time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())comment = ET.Element('about')comment.set('author', 'shadow')comment.set('email', 'yinlu@flyaudio.cn')comment.set('date', self.time)self.element.append(comment)self.add_param('imageSize', dict(zip(('width', 'height'), image_size)))# 添加保存摄像头参数def add_param(self, name, param):node = ET.Element(name)self.element.append(node)if isinstance(param, dict):for key, value in param.items():child = ET.Element(key)child.text = str(value)node.append(child)else:for i, elem in enumerate(param.flatten()):child = ET.Element(f'data{i}')child.text = str(elem)node.append(child)# 保存摄像头参数def save_params(self):save_path = 'camera_params_' + self.time.split()[0] + '.xml'self.elementTree.write(save_path, 'UTF-8')print("Saved params in {}.".format(save_path))# 校准角点def cal_real_corner(self, corner_height, corner_width, square_size):obj_corner = np.zeros([corner_height * corner_width, 3], np.float32)obj_corner[:, :2] = np.mgrid[0:corner_height, 0:corner_width].T.reshape(-1, 2)  # (w*h)*2return obj_corner * square_sizedef calibration(self, corner: dict, img_path='./image'):file_names = glob.glob(img_path+'/*.jpg') + glob.glob(img_path+'./*.png')if not file_names:showerror("ERROR", f'No picture find in {img_path}!')returnobjs_corner = []imgs_corner = []criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)obj_corner = self.cal_real_corner(corner['height'], corner['width'], corner['size'])for file_name in file_names:# read imagechess_img = cv.imread(file_name)assert (chess_img.shape[0] == self.image_size[1] and chess_img.shape[1] == self.image_size[0]), \"Image size does not match the given value {}.".format(self.image_size)# to graygray = cv.cvtColor(chess_img, cv.COLOR_BGR2GRAY)# find chessboard cornersret, img_corners = cv.findChessboardCorners(gray, (corner['height'], corner['width']))# append to img_cornersif ret:objs_corner.append(obj_corner)img_corners = cv.cornerSubPix(gray, img_corners, winSize=(corner['size']//2, corner['size']//2),zeroZone=(-1, -1), criteria=criteria)imgs_corner.append(img_corners)else:print("Fail to find corners in {}.".format(file_name))# calibrationret, self.matrix, self.dist, rvecs, tveces = cv.calibrateCamera(objs_corner, imgs_corner, self.image_size, None, None)if ret:self.add_param('camera_matrix', self.matrix)self.add_param('camera_distortion', self.dist)# self.add_param('roi', self.roi)return retdef get_camera_param(self, points):objPoints = np.array(points['worldPoints'], dtype=np.float64)imgPoints = np.array(points['imagePoints'], dtype=np.float64)retval, rvec, tvec = cv.solvePnP(objPoints, imgPoints, self.matrix, self.dist)if retval:# rotMatrix = cv.Rodrigues(rvec)[0]self.add_param('camera_rvec', rvec)self.add_param('camera_tvec', tvec)# self.add_param('rotation_matrix', rotMatrix)self.add_param('points', points)return retval

图像矫正，坐标转换部分

class Rectify(object):def __init__(self, param_file='/home/fly/ros2_ws/src/camera_calibration/camera_params.xml'):super(Rectify, self).__init__()self.image_size = (1280, 720)self.camera_params = {'camera_matrix': None,  # 相机内参矩阵'camera_distortion': None,  # 畸变系数'camera_rvec': None,  # 旋转矢量'camera_tvec': None,  # 平移矢量}self.load_params(param_file)# 加载摄像头参数def load_params(self, param_file):if not os.path.exists(param_file):print("File {} does not exist.", format(param_file))exit(-1)tree = ET.parse(param_file)root = tree.getroot()# 获取到测试基准点数据p = root.find('points')if p:self.points = {data.tag: eval(data.text) for data in p}# print(self.points)# 获取摄像头内参。外参for i in self.camera_params.keys():datas = root.find(i)if datas:paras = [data.text for data in datas]self.camera_params[i] = np.array(paras, np.float)# print(i, paras)if i =='camera_matrix':self.camera_params[i] = mat(paras, np.float).reshape((3, 3))elif i in ['camera_tvec', 'camera_rvec']:self.camera_params[i] = mat(paras, np.float).reshape((3, 1))elif i == 'camera_distortion':self.camera_params[i] = mat(paras, np.float)else:print(i, self.camera_params)return Falseif root.find('imageSize'):params = {data.tag: int(data.text) for data in root.find('imageSize')}self.image_size = tuple((params['width'], params['height']))self.rvec = self.camera_params['camera_rvec']self.cam_mat = self.camera_params['camera_matrix']self.tvec = self.camera_params['camera_tvec']self.cam_dist = self.camera_params['camera_distortion']self.rot_mat = mat(cv.Rodrigues(self.rvec)[0])self.cam_mat_new, roi = cv.getOptimalNewCameraMatrix(self.cam_mat, self.cam_dist, self.image_size, 1, self.image_size)self.roi = np.array(roi)# for k, v in self.camera_params.items():#     print(k, v)return True# 矫正视频def rectify_video(self, video_in: str, video_out: str):cap = cv.VideoCapture(video_in)print(cap)if not cap.isOpened():print("Unable to open video.")return Falsefourcc = int(cap.get(cv.CAP_PROP_FOURCC))fps = int(cap.get(cv.CAP_PROP_FPS))out = cv.VideoWriter(filename=video_out, fourcc=fourcc, fps=fps, frameSize=self.image_size)frame_count = int(cap.get(cv.CAP_PROP_FRAME_COUNT))print('rectify_video start...')for _ in range(frame_count):ret, img = cap.read()if ret:dst = self.rectify_image(img)out.write(dst)cv.waitKey(1)cap.release()out.release()cv.destroyAllWindows()return True# 矫正摄像头def rectify_camera(self, camera: dict):try:cap = cv.VideoCapture(camera['id'])print(cap)except:print("Unable to open camera.")return Falseif not cap.isOpened():print("Unable to open camera.")return Falsewhile camera['run']:ret, img = cap.read()yield ret, self.rectify_image(img)cap.release()cv.destroyAllWindows()# 矫正图片def rectify_image(self, img):if not isinstance(img, np.ndarray) or not img.any():AssertionError("Image is None or type '{}' is not numpy.ndarray .".format(type(img)))return Noneimg = cv.resize(img, self.image_size)dst = cv.undistort(img, self.cam_mat, self.cam_dist, self.cam_mat_new)# x, y, w, h = self.roi# dst = dst[y:y + h, x:x + w]dst = cv.resize(dst, self.image_size)# cv.waitKey(0)return dst# @timefuncdef get_world_point(self, point):point = list(point) + [1]  # 注意要加１# print(point)# 计算参数Ss = (self.rot_mat.I * self.tvec)[2] / (self.rot_mat.I * self.cam_mat.I * mat(point).T)[2]# 计算世界坐标wcpoint = self.rot_mat.I * (s[0, 0] * self.cam_mat.I * mat(point).T - self.tvec)# print("sourcePoint:", point, "worldpoint:", wcpoint)return wcpoint.flatten().getA()[0]  # 转成列表输出# @timefuncdef wcpoint2point(self, wcpoints):points, _ = cv.projectPoints(wcpoints, self.rvec, self.tvec, self.cam_mat, 0)return points

另外做了在图像上直接选取像素点的功能但没有加到工具中，可以帮助省略通过其他图像工具获取像素点的步骤，有兴趣的同学可以自己尝试替换手动输入像素坐标的功能

# ======main.py
# 先开线程打开图片
def add():global Flagif not Flag:img_file = filedialog.askopenfile()threading.Thread(target=cb.point_get_from_image, args=(img_file.name, pos_im)).start()Flag = Trueelse:add_point()# ======calibration.py
# 显示图片，监控左键点击
def point_get_from_image(img_file, pos):# 打开要校准的基准图片，选取测试基准点flag = []img = cv.imread(img_file)cv.namedWindow("image")cv.setMouseCallback("image", on_mouse, param=(img, pos, flag))while True:cv.imshow("image", img)cv.line(img, (640, 0), (640, 720), (0, 255, 0), 1)if cv.waitKey(1) == ord('q') or flag:breakcv.destroyAllWindows()# 左键点击，在图像上标记，并将点传给标记点工具界面数据，右键退出
def on_mouse(event, x, y, flags, param):image, pos, flag = paramif event == cv.EVENT_LBUTTONDOWN:# print(event, x, y, param, flags)cv.circle(image, (x, y), 2, (0, 255, 0), -1)# cv.putText(image, f"pos:({x},{y})", (x, y - 10), cv.FONT_HERSHEY_SIMPLEX, 0.5, (200, 0, 0), 2)pos.set(f"{x}, {y}")elif event == cv.EVENT_RBUTTONDOWN:flag.append((x, y))