仿射投影-以相机为参考计算空间坐标系到像素坐标系
在以相机为世界坐标系原点的情况下,假设已知物体的长宽高,以及离相机的距离,假设相机的高度与物体的中心高度一样,则我们可以将物体的世界坐标投影到像素坐标。
代码如下:
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个点,蓝色为正面的点,红色为背面的点