1.get_dms_camera_coord_by_cheeseboard

import numpy as np
import yaml


dms_camera_parameter = {
    # R，旋转矩阵
    "R": [[0.99888374, 0.01836553, 0.04351849],
          [-0.02197058, 0.99623484, 0.08386436],
          [-0.04181441, -0.08472689, 0.99552628]],
    # t，平移向量
    "T": [-38.37505778, 28.7546632, 170.40619774],
    # 焦距，f/dx, f/dy
    "f": [1145.04940459, 1143.78109572],
    # principal point，主点，主轴与像平面的交点
    "c": [512.54150496, 515.45148698]
}

depth_camera_parameter = {
    # R，旋转矩阵
    "R": [[0.99888374, 0.01836553, 0.04351849],
          [-0.02197058, 0.99623484, 0.08386436],
          [-0.04181441, -0.08472689, 0.99552628]],
    # t，平移向量
    "T": [-38.37505778, 28.7546632, 170.40619774],
    # 焦距，f/dx, f/dy
    "f": [1145.04940459, 1143.78109572],
    # 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(camera_parameter, joint_world):  # joint_world: n*3
        """
        世界坐标系 -> 相机坐标系:  R * pt + T
        :return:
        """
        joint_world = np.asarray(joint_world)
        R = np.asarray(camera_parameter["R"])
        T = np.asarray(camera_parameter["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(camera_parameter, 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_parameter["f"]
        c = camera_parameter["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

    @staticmethod
    def convert_uv_to_cc(camera_parameter, joint_uv1, joint_Zc):
        intrinsic = np.zeros((3, 3), np.float32)
        intrinsic[0][0] = camera_parameter['f'][0]
        intrinsic[1][1] = camera_parameter['f'][1]
        intrinsic[0][2] = camera_parameter['c'][0]
        intrinsic[1][2] = camera_parameter['c'][1]
        intrinsic[2][2] = 1
        inv_intrinsic = np.linalg.inv(intrinsic)
        assert joint_uv1.shape[0] == joint_Zc.shape[0]
        camera_coord = []
        for id, zc in enumerate(joint_Zc):
            temp = inv_intrinsic * zc
            camera_coord_id = np.matmul(temp, joint_uv1[id].T)
            camera_coord.append(camera_coord_id)

        return np.array(camera_coord)

    @staticmethod
    def convert_cc_to_wc(camera_parameter, joint_camera):  # joint_world: n*3
        """
        相机坐标系 -> 世界坐标系:  inv_R * (pt - T)
        :return:
        """
        joint_camera = np.asarray(joint_camera)
        R = np.asarray(camera_parameter["R"])
        T = np.asarray(camera_parameter["T"])
        joint_num = len(joint_camera)
        # 相机坐标系 -> 世界坐标系
        joint_world = np.zeros((joint_num, 3))  # joint camera
        for i in range(joint_num):
            joint_world[i] = np.dot(np.linalg.inv(R), (joint_camera[i] - T)) # inv_R * (pt - T)
        return joint_world

if __name__ == "__main__":
    import os
    path = os.getcwd()
    path = os.path.join(path, "1.depth_camera_points.yaml")
    cfg = load_yaml_file(path)
    depth_points = cfg["depth_camera_points_uvz"]

    uv1_points = np.ones_like(depth_points)
    uv1_points[..., 0] = depth_points[..., 0]
    uv1_points[..., 1] = depth_points[..., 1]

    zc_points = depth_points[..., 2]

    coord_convert = CoordinateConvert()

    depth_camera_points = coord_convert.convert_uv_to_cc(depth_camera_parameter, uv1_points, zc_points)
    world_points = coord_convert.convert_cc_to_wc(depth_camera_parameter, depth_camera_points)
    dms_camera_points = coord_convert.convert_wc_to_cc(dms_camera_parameter, world_points).tolist()

    cfg = {}
    cfg['dms_camera_points'] = dms_camera_points
    write_config_to_yaml(cfg, '1.dms_camera_points.yaml')