视线区域棋盘坐标到相机坐标转换脚本

最新推荐文章于 2024-09-13 16:20:00 发布

ustczhng2012

最新推荐文章于 2024-09-13 16:20:00 发布

阅读量228

点赞数

分类专栏：实操项目相关博文文章标签：数据清洗坐标转换

本文链接：https://blog.csdn.net/ustczhng2012/article/details/120369213

版权

实操项目相关博文专栏收录该内容

3 篇文章 0 订阅

订阅专栏

坐标点配置文件（yaml）

#----------------------中控屏左边---------------------------------------------
center_console_left:
  cheeseboard_coordinate_mm: [[301.751, 278.06, -948.419], [9.251, 278.06, -948.419], [9.251, 312.23, -943.66], [301.751, 312.23, -943.66]]
  direct_get_camera_coordinate_mm: [[-114.601, 95.144, -79.962], [177.898, 95.144, -80.547], [177.873, 62.992, -93.059], [-114.626, 62.992, -92.473]]
#-----------------------中控屏中间------------------------------------------------
center_console_middle:
  cheeseboard_coordinate_mm: [[-60.749, 278.06, -948.419], [-285.319, 278.06, -948.419], [-285.319, 307.278, -944.35], [-60.749, 307.278, -944.35]]
  direct_get_camera_coordinate_mm: [[247.898, 95.144, -80.687], [472.468, 95.144, -81.137], [472.446, 67.652, -91.835], [247.877, 67.652, -91.386]]
#-----------------------中控屏右边------------------------------------------------
center_console_right:
  cheeseboard_coordinate_mm: [[-483.249, 278.059, -948.413], [-675.249, 278.059, -948.413], [-675.249, 307.277, -944.344], [-483.249, 307.277, -944.344]]
  direct_get_camera_coordinate_mm: [[670.398, 95.141, -81.527], [862.397, 95.141, -81.912], [862.376, 67.65, -92.61], [670.376, 67.65, -92.226]]
#-----------------------左前挡玻璃----------------------------------------------
left_front_windshield:
  cheeseboard_coordinate_mm: [[355.827, -36.279, -683.533], [-49.531, -48.185, -749.924], [-149.249, -4.287, -862.295], [-149.249, 250.664, -1366.38], [425.713, 237.383, -1244.1]]
  direct_get_camera_coordinate_mm: [[-167.907, 240.367, 304.702], [237.346, 283.142, 251.739], [336.825, 299.626, 132.03], [335.695, 322.912, -432.38], [-239.039, 274.867, -318.002]]
#-----------------------右前挡玻璃----------------------------------------------
right_front_windshield:
  cheeseboard_coordinate_mm: [[-369.22, -1.649, -861.144], [-423.525, -32.737, -780.839], [-615.888, -28.603, -752.252], [-731.077, 250.71, -1298.366], [-369.22, 250.74, -1360.628]]
  direct_get_camera_coordinate_mm: [[556.794, 296.762, 131.307], [611.27, 284.733, 216.467], [803.679, 267.182, 239.022], [917.641, 289.7, -374.191], [555.675, 320.04, -427.836]]
#-----------------------左后视镜------------------------------------------------
letf_rear_view:
  cheeseboard_coordinate_mm: [[841.298, 203.582, -851.586], [681.792, 166.134, -904.195], [692.887, 274.217, -900.209], [786.491, 276.622, -868.118]]
  direct_get_camera_coordinate_mm: [[-653.905, 112.934, 41.977], [-494.454, 171.285, 13.995], [-505.648, 74.985, -35.218], [-599.198, 57.234, -8.188]]
#-----------------------左挡玻璃------------------------------------------------
letf_windshield:
  cheeseboard_coordinate_mm: [[562.518, 181.344, -886.068], [485.162, -9.492, -411.887], [560.456, 172.006, -461.185], [565.261, 189.179, -888.163]]
  direct_get_camera_coordinate_mm: [[-375.164, 149.166, 22.163], [-296.793, 84.492, 529.041], [-372.35, -49.911, 397.634], [-377.918, 143.347, 16.518]]
#-----------------------右挡玻璃------------------------------------------------
right_windshield:
  cheeseboard_coordinate_mm: [[-862, -52.503, -461.557], [-925.97, 89.435, -830.664], [-963.501, 187.838, -856.883], [-959.832, 174.37, -523.631]]
  direct_get_camera_coordinate_mm: [[1050.321, 146.266, 503.961], [1113.508, 202.379, 112.377], [1150.896, 129.262, 41.42], [1147.823, -21.517, 338.922]]
#-----------------------右后视镜------------------------------------------------
right_rear_view:
  cheeseboard_coordinate_mm: [[-1066.774, 168.042, -906.541], [-1216.132, 202.942, -826.465], [-1171.523, 273.959, -849.268], [-1076.548, 272.258, -901.546]]
  direct_get_camera_coordinate_mm: [[1254.102, 170.763, 7.517], [1403.566, 101.24, 60.101], [1358.848, 50.365, 5.648], [1263.783, 77.348, -38.972]]
#-----------------------内后视镜------------------------------------------------
rear_view:
  cheeseboard_coordinate_mm: [[-69.72, -9.002, -616.259], [-318.772, -5.191, -613.745], [-306.233, 40.427, -620.294], [-80.503, 38.143, -622.737]]
  direct_get_camera_coordinate_mm: [[257.731, 183.743, 349.276], [506.782, 179.19, 349.114], [494.187, 142.56, 321.172], [268.456, 145.745, 320.605]]

转换脚本


import numpy as np
import yaml


camera_intrinsic = {
    # R，旋转矩阵
    "R": [[-0.91536173, 0.40180837, 0.02574754],
          [0.05154812, 0.18037357, -0.98224649],
          [-0.39931903, -0.89778361, -0.18581953]],
    # t，平移向量
    "T": [1841.10702775, 4955.28462345, 1563.4453959],
    # 焦距，f/dx, f/dy
    "f": [1145.04940459, 1143.78109572],
    'w'# principal point，主点，主轴与像平面的交点
    "c": [512.54150496, 515.45148698]

}

def load_yaml_file(cfg_path):
    with open(cfg_path, 'r', encoding='utf-8') as cfg_file:
        cfg_cont = cfg_file.read()
        cfg = yaml.load(cfg_cont, Loader=yaml.SafeLoader)
        cfg_file.close()
        return cfg


def write_config_to_yaml(cfg, file_name):
    with open(file_name, 'w') as f:
        yaml.dump(cfg, f)


class CoordinateConvert:
    @staticmethod
    def convert_wc_to_cc(joint_world):  # joint_world: n*3
        """
        世界坐标系 -> 相机坐标系: R * (pt - T)
        :return:
        """
        joint_world = np.asarray(joint_world)
        R = np.asarray(camera_intrinsic["R"])
        T = np.asarray(camera_intrinsic["T"])
        joint_num = len(joint_world)
        # 世界坐标系 -> 相机坐标系
        # [R|t] world coords -> camera coords
        joint_cam = np.zeros((joint_num, 3))  # joint camera
        for i in range(joint_num):  # joint i
            joint_cam[i] = np.dot(R, joint_world[i] - T)  # R * (pt - T)
        return joint_cam

    @staticmethod
    def __cam2pixel(cam_coord, f, c):
        """
        相机坐标系 -> 像素坐标系: (f / dx) * (X / Z) = f * (X / Z) / dx
        cx,ppx=260.166; cy,ppy=205.197; fx=367.535; fy=367.535
        将从3D(X,Y,Z)映射到2D像素坐标P(u,v)计算公式为：
        u = X * fx / Z + cx
        v = Y * fy / Z + cy
        D(v,u) = Z / Alpha
        =====================================================
        camera_matrix = [[428.30114, 0.,   316.41648],
                        [   0.,    427.00564, 218.34591],
                        [   0.,      0.,    1.]])
        fx = camera_intrinsic[0, 0]
        fy = camera_intrinsic[1, 1]
        cx = camera_intrinsic[0, 2]
        cy = camera_intrinsic[1, 2]
        =====================================================
        :param cam_coord:
        :param f: [fx,fy]
        :param c: [cx,cy]
        :return:
        """
        # 等价于：(f / dx) * (X / Z) = f * (X / Z) / dx
        # 三角变换， / dx, + center_x
        u = cam_coord[..., 0] / cam_coord[..., 2] * f[0] + c[0]
        v = cam_coord[..., 1] / cam_coord[..., 2] * f[1] + c[1]
        d = cam_coord[..., 2]
        return u, v, d

    @staticmethod
    def convert_cc_to_ic(joint_cam):
        """
        相机坐标系 -> 像素坐标系
        :param joint_cam:
        :return:
        """
        # 相机坐标系 -> 像素坐标系，并 get relative depth
        # Subtract center depth
        # 选择 Pelvis骨盆 所在位置作为相机中心，后面用之求relative depth
        root_idx = 0
        center_cam = joint_cam[root_idx]  # (x,y,z) mm
        joint_num = len(joint_cam)
        f = camera_intrinsic["f"]
        c = camera_intrinsic["c"]
        # joint image，像素坐标系，Depth 为相对深度 mm
        joint_img = np.zeros((joint_num, 3))
        joint_img[:, 0], joint_img[:, 1], joint_img[:, 2] = CoordinateConvert.__cam2pixel(joint_cam, f, c)  # x,y
        joint_img[:, 2] = joint_img[:, 2] - center_cam[2]  # z: 相对图片某个点的深度信息
        return joint_img


if __name__ == "__main__":
    coordinate_convert = CoordinateConvert()
    cfg = load_yaml_file("template.yaml")

    for key in cfg:
        joint_world = np.array(cfg[key]['cheeseboard_coordinate_mm'])
        cfg[key]['transform_get_camera_coordinate_mm'] = coordinate_convert.convert_wc_to_cc(joint_world).tolist()

    write_config_to_yaml(cfg, 'result.yaml')