仿射投影-以相机为参考计算空间坐标系到像素坐标系

仿射投影-以相机为参考计算空间坐标系到像素坐标系

在以相机为世界坐标系原点的情况下，假设已知物体的长宽高，以及离相机的距离，假设相机的高度与物体的中心高度一样，则我们可以将物体的世界坐标投影到像素坐标。
代码如下：

import numpy as np
import cv2

"""
在以相机为参考的世界坐标系下
模拟不同距离车辆宽度
"""

f = 752.18
cx = 315.35
cy = 254.55

"""
考虑相机与物体中心平齐
相机高为h/2
"""


# 当正对  正对设置变量x = 0
# d = 10
# x为平移变量 等于0时正对相机 为正向右 为负向左  当前假设都是基于车辆没有旋转，即没有绕中心的旋转

# x = 0

# r为旋转角度，逆时针为正
# r = np.pi / 180 * 10

# d = 30
# h = 1.60
# w = 1.80
# l = 4.30
# # 正面
# # 左下
# p1 = [-w / 2 + x, -h / 2, d]
# # 右下
# p2 = [w / 2 + x, -h / 2, d]
# # 左上
# p3 = [-w / 2 + x, h / 2, d]
# # 右上
# p4 = [w / 2 + x, h / 2, d]

def Angle2R(thetax, thetay, thetaz):
    # 翻滚 偏航 俯仰
    thetax = thetax / 180 * np.pi
    thetay = thetay / 180 * np.pi
    thetaz = thetaz / 180 * np.pi
    Rx = np.matrix([[1, 0, 0], [0, np.cos(thetaz), -np.sin(thetaz)], [0, np.sin(thetaz), np.cos(thetaz)]])
    Ry = np.matrix([[np.cos(thetay), 0, np.sin(thetay)], [0, 1, 0], [-np.sin(thetay), 0, np.cos(thetay)]])
    Rz = np.matrix([[np.cos(thetax), -np.sin(thetax), 0], [np.sin(thetax), np.cos(thetax), 0], [0, 0, 1]])
    return Rz * Ry * Rx


def caculateAllConcor(l, w, h, d, x):
    p_all = []
    # 正面
    p_all.append([-w / 2 + x, -h / 2, d - l / 2])  # 左下
    p_all.append([w / 2 + x, -h / 2, d - l / 2])  # 右下
    p_all.append([-w / 2 + x, h / 2, d - l / 2])  # 左上
    p_all.append([w / 2 + x, h / 2, d - l / 2])  # 右上
    # 后面
    p_all.append([-w / 2 + x, -h / 2, d + l / 2])
    p_all.append([w / 2 + x, -h / 2, d + l / 2])
    p_all.append([-w / 2 + x, h / 2, d + l / 2])
    p_all.append([w / 2 + x, h / 2, d + l / 2])
    return p_all


def caculateWorld2Pixel(p, f, cx, cy):
    p = p[0]
    u = f * (p[0] / p[2]) + cx
    v = f * (p[1] / p[2]) + cy
    return u, v


def caculateP2R(p, l, w, h, d, x, r):
    p_t = []
    if p[1] == h / 2:
        # 上平面的点
        ps = [x, h / 2, d]
        xx = (p[0] - ps[0]) * np.cos(r) - (p[2] - ps[2]) * np.sin(r) + ps[0]
        zz = (p[0] - ps[0]) * np.sin(r) + (p[2] - ps[2]) * np.cos(r) + ps[2]
        p_t.append(xx)
        p_t.append(p[1])
        p_t.append(zz)
    if p[1] == -h / 2:
        # 上平面的点
        ps = [x, -h / 2, d]
        xx = (p[0] - ps[0]) * np.cos(r) - (p[2] - ps[2]) * np.sin(r) + ps[0]
        zz = (p[0] - ps[0]) * np.sin(r) + (p[2] - ps[2]) * np.cos(r) + ps[2]
        p_t.append(xx)
        p_t.append(p[1])
        p_t.append(zz)
    return p_t


rr = np.arange(0, 90)
for rrr in rr:
    r = np.pi / 180 * rrr
    l = 5.256
    w = 1.878
    h = 1.776
    d = 10
    x = 0
    p_all = caculateAllConcor(l, w, h, d, x)
    # print(p_all)
    p_all2 = []
    for p in p_all:
        p_all2.append(caculateP2R(p, l, w, h, d, x, r))
    # 若有角度偏差
    p_all1 = []
    for p in p_all2:
        p_all1.append((p * Angle2R(0, 0, 10)).tolist())
    p_all = p_all1
    p_pixel = []
    for p in p_all:
        p_pixel.append(caculateWorld2Pixel(p, f, cx, cy))
    # print(p_pixel)
    img = cv2.imread("2.bmp")
    i = 0
    x_list = []
    y_list = []
    for p in p_pixel:
        # print(p)
        i += 1
        x_list.append(p[0])
        y_list.append(p[1])
        if i > 4:
            cv2.circle(img, (int(p[0]), int(p[1])), 4, color=(0, 0, 255))
        else:
            cv2.circle(img, (int(p[0]), int(p[1])), 4, color=(255, 0, 0))
    # print(x_list)
    # print(rrr, max(x_list) - min(x_list))
    print(f'宽度为：{max(x_list) - min(x_list)}')
    print(f'高度为：{max(y_list) - min(y_list)}')
    cv2.imshow("1", img)
    cv2.waitKey(0)
    x_list.clear()

图像点为box的8个点，蓝色为正面的点，红色为背面的点