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

代码

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

功能说明

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

流程分析

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

代码使用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_size
        self.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)*2
        return obj_corner * square_size

    def 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}!')
            return
        objs_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 image
            chess_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 gray
            gray = cv.cvtColor(chess_img, cv.COLOR_BGR2GRAY)
            # find chessboard corners
            ret, img_corners = cv.findChessboardCorners(gray, (corner['height'], corner['width']))

            # append to img_corners
            if 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))

        # calibration
        ret, 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 ret

    def 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 False

        if 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 False
        fourcc = 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 False
        if not cap.isOpened():
            print("Unable to open camera.")
            return False
        while 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 None

        img = 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

    # @timefunc
    def get_world_point(self, point):
        point = list(point) + [1]  # 注意要加１
        # print(point)
        # 计算参数S
        s = (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]  # 转成列表输出

    # @timefunc
    def wcpoint2point(self, wcpoints):
        points, _ = cv.projectPoints(wcpoints, self.rvec, self.tvec, self.cam_mat, 0)
        return points

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

# ======main.py
# 先开线程打开图片
def add():
    global Flag
    if not Flag:
        img_file = filedialog.askopenfile()
        threading.Thread(target=cb.point_get_from_image, args=(img_file.name, pos_im)).start()
        Flag = True
    else:
        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:
            break
    cv.destroyAllWindows()
            
# 左键点击，在图像上标记，并将点传给标记点工具界面数据，右键退出
def on_mouse(event, x, y, flags, param):
    image, pos, flag = param
    if 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))