【目标跟踪】跨相机如何匹配像素

import yaml
import numpy as np
import cv2


def read_yaml(path):
    with open(path, 'r', encoding='utf-8') as f:
        result = yaml.load(f.read(), Loader=yaml.FullLoader)
    return result


def get_r_t_mtx(path, f_r_b_l):
    sensor_list = ["front_center", "right_center", "back_center", "left_center"]
    yaml_result = read_yaml(path)  # 读取yaml配置文件h
    res_pitch = yaml_result[sensor_list[f_r_b_l]]["pitch"]
    res_h = yaml_result[sensor_list[f_r_b_l]]["height"]
    res_r = np.array(yaml_result[sensor_list[f_r_b_l]]["rotation"]).reshape(3, 3)
    res_t = np.array(yaml_result[sensor_list[f_r_b_l]]["translation"]).reshape(3, 1)
    res_mtx = np.array(yaml_result[sensor_list[f_r_b_l]]["K"]).reshape(3, 3)
    return res_pitch, res_h, res_mtx, res_r, res_t


# 近似计算相机坐标系 Zcamera
def get_camera_z(children, pixe_y):
    pitch, h, K, *_ = children
    sigma = np.arctan((pixe_y - K[1][2]) / K[1][1])
    z = h * np.cos(sigma) / np.sin(sigma + pitch)  # 深度
    return z


def get_sensor_pixe(children, parent, x, y, distance):
    r, t = get_two_camera_r_t(children, parent)
    children_pitch, children_h, children_mtx, *c = children
    parent_pitch, parent_h, parent_mtx, *p = parent
    distance_init = distance
    x = (x - children_mtx[0][2]) / children_mtx[0][0]
    y = (y - children_mtx[1][2]) / children_mtx[1][1]
    coor = np.array([x, y, 1]).reshape(3, 1) * distance_init
    res_coor = r @ coor + t  # 车体坐标系
    res_x = (res_coor[0] / res_coor[2]) * parent_mtx[0][0] + parent_mtx[0][2]
    res_y = (res_coor[1] / res_coor[2]) * parent_mtx[1][1] + parent_mtx[1][2]
    return res_x, res_y


def show_img(img):
    cv2.namedWindow("show")
    cv2.imshow("show", img)
    cv2.waitKey(0)


def get_two_camera_r_t(children, parent):
    *children, children_mtx, children_r, children_t = children
    *parent, parent_mtx, parent_r, parent_t = parent
    res_r = np.array(parent_r).T @ np.array(children_r)
    res_t = np.array(parent_r).T @ (np.array(children_t) - np.array(parent_t)).reshape(3, 1)
    return res_r, res_t


def get_uv(point, param):
    *p, mtx, r, t = param
    coor_camera = r.T @ (np.array(point).reshape(3, 1) - t)
    coor_pixe = mtx @ coor_camera * (1 / coor_camera[2])
    return coor_pixe[0][0], coor_pixe[1][0]


if __name__ == '__main__':
    front_img = cv2.imread("front_img.jpg")
    left_img = cv2.imread("left_img.jpg")
    img = np.concatenate((left_img, front_img), axis=1)  # 横向拼接
    front_param = get_r_t_mtx("./sensor_param.yaml", 0)
    left_param = get_r_t_mtx("./sensor_param.yaml", 3)
    color = np.random.randint(0, 255, (3000, 3))  # 随机颜色

    car_coor = [5.41, 6.5, 1.3]
    camera1 = np.ravel(get_uv(car_coor, left_param))
    camera2 = np.ravel(get_uv(car_coor, front_param))
    print(camera1, camera2)
    cv2.circle(img, (int(camera1[0]), int(camera1[1])), 1, color[0].tolist(), 2)
    cv2.circle(img, (int(camera2[0]) + 1920, int(camera2[1])), 1, color[1].tolist(), 2)
    cv2.line(img, (int(camera1[0]), int(camera1[1])), (int(camera2[0] + 1920), int(camera2[1])), color[0].tolist(), 2)
    show_img(img)

    # print(get_two_camera_r_t(front_param, left_param))
    # print(front_to_left_r.reshape(-1), "\n", front_to_left_t)
    # distance = get_camera_z(left_param, 640)
    # x1, y1 = 1429, 488
    # x2, y2 = 1509, 637
    # for x in range(x1, x2, 20):
    #     for y in range(y1, y2, 20):
    #         res_x, res_y = get_sensor_pixe(left_param, front_param, x, y, distance)
    #         cv2.circle(img, (int(x), int(y)), 1, color[x].tolist(), 5)
    #         cv2.circle(img, (int(res_x) + 1920, int(res_y)), 1, color[x].tolist(), 5)
    # cv2.line(img, (int(x) , int(y)), (int(res_x)+ 1920, int(res_y)), color[x].tolist(), 2)
    # distance = get_camera_z(front_param, 649)
    # x1, y1 = 271, 469
    # x2, y2 = 353, 649
    # for x in range(x1, x2, 20):
    #     for y in range(y1, y2, 20):
    #         res_x, res_y = get_sensor_pixe(front_param, left_param, x, y, distance)
    #         cv2.circle(img, (int(x) + 1920, int(y)), 1, color[x].tolist(), 2)
    #         cv2.circle(img, (int(res_x), int(res_y)), 1, color[x].tolist(), 2)
    # cv2.line(img, (int(x) + 1920, int(y)), (int(res_x), int(res_y)), color[x].tolist(), 2)
    # show_img(img)