常用方法： AABB检测、OBB检测、python shapely 库方法等
AABB 与 OBB区别
OBB
AABB
重点： 是否 带 旋转
obb检测原理
OBB间的相交测试基于分离轴理论（separating axis theory）。若两个OBB在一条轴线上（不一定是坐标轴）上的投影不重叠，则这条轴称为分离轴。若一对OBB间存在一条分离轴，则可以判定这两个OBB不相交。对任何两个不相交的凸三维多面体，其分离轴要么垂直于任何一个多面体的某一个面，要么同时垂直于每个多面体的某一条边。因此，对一对OBB，只需测试15条可能是分离轴的轴（每个OBB的3个面方向再加上每个OBB的3个边方面的两两组合），只要找到一条这样的分离轴，就可以判定这两个OBB是不相交的，如果这15条轴都不能将这两个OBB分离，则它们是相交的。
Shapely api
https://www.osgeo.cn/shapely/manual.html
Shapely （2d） 与 OBB (3d) 程序及可视化包围框实现
注：代码只是个人项目使用，这里只做记录，读者可能拷贝不能直接使用。
import numpy as np
import pandas as pd
import random
import os
import sys ,cv2, mmcv
from shapely.geometry import Polygon
import math
class OBB:
def __init__(self,point_set:np.ndarray): # （point_set: [8,3]）
self.pos=(point_set[0]+point_set[6])/2 #包围盒中心点位置
self.axisX=self.__norm(point_set[1]-point_set[0]) #包围盒x轴的方向向量
self.axisY=self.__norm(point_set[3]-point_set[0]) #包围盒y轴的方向向量
self.axisZ=self.__norm(point_set[0]-point_set[4]) #包围盒z轴的方向向量
self.half_size=np.array([self.__get_distance(point_set[0],point_set[1])/2,
self.__get_distance(point_set[0],point_set[3])/2,
self.__get_distance(point_set[0],point_set[4])/2])
def __norm(self,vector): #将向量归一化为标准向量
s=0
for e in vector:
s+=e*e
return vector/(s**0.5)
def __get_distance(self,point_1,point_2): #计算两个点的距离
return ((point_1[0]-point_2[0])**2+(point_1[1]-point_2[1])**2+(point_1[2]-point_2[2])**2)**0.5
def cross_product(vector1,vector2): #向量积
return np.array([vector1[1]*vector2[2]-vector1[2]*vector2[1],vector1[2]*vector2[0]-vector1[0]*vector2[2],vector1[0]*vector2[1]-vector1[1]*vector2[0]])
def getSeparatingPlane(r_pos,plane,box1:OBB,box2:OBB): #判断在选定的坐标平面是否有分割平面
return ((abs(sum(r_pos*plane)) >
(abs(sum((box1.axisX*box1.half_size[0])*plane)) +
abs(sum((box1.axisY*box1.half_size[1])*plane)) +
abs(sum((box1.axisZ*box1.half_size[2])*plane)) +
abs(sum((box2.axisX*box2.half_size[0])*plane)) +
abs(sum((box2.axisY*box2.half_size[1])*plane)) +
abs(sum((box2.axisZ*box2.half_size[2])*plane)))))
def isCollision(box1:OBB,box2:OBB): #判断两个OBB是否发生碰撞
r_pos=box2.pos-box1.pos
if not (getSeparatingPlane(r_pos, box1.axisX, box1, box2) or getSeparatingPlane(r_pos, box1.axisY, box1, box2) or
getSeparatingPlane(r_pos, box1.axisZ, box1, box2) or getSeparatingPlane(r_pos, box2.axisX, box1, box2) or
getSeparatingPlane(r_pos, box2.axisY, box1, box2) or getSeparatingPlane(r_pos, box2.axisZ, box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisX,box2.axisX), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisX,box2.axisY), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisX,box2.axisZ), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisY,box2.axisX), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisY,box2.axisY), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisY,box2.axisZ), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisZ,box2.axisX), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisZ,box2.axisY), box1, box2) or
getSeparatingPlane(r_pos, cross_product(box1.axisZ,box2.axisZ), box1, box2)):
return True
else:
return False
def create_bb_3d(Um_L,Um_W,Um_H,trans_pos,trans_rot):
T_x = pos_to_transmatrix(trans_rot, trans_pos)
R_mat = T_x[0:3, 0:3]
T_mat = T_x[0:3, 3]
l = Um_L/2
w = Um_W /2
h = Um_H / 2
bb = np.zeros((8,3))
xm = 0
ym = 0
zm = 0
bb[0] = np.array([xm + l,ym -w, zm + h])
bb[1] = np.array([xm + l,ym +w, zm + h])
bb[2] = np.array([xm - l,ym +w, zm + h])
bb[3] = np.array([xm - l,ym -w, zm + h])
bb[4] = np.array([xm + l,ym -w, zm - h])
bb[5] = np.array([xm + l,ym +w, zm - h])
bb[6] = np.array([xm - l,ym +w, zm - h])
bb[7] = np.array([xm - l,ym -w, zm - h])
bb = R_mat.dot(bb.T).T + T_mat
return bb
def create_bb_2d(Um_L,Um_W,Um_H,trans_pos,trans_rot):
T_x = pos_to_transmatrix(trans_rot, trans_pos)
R_mat = T_x[0:3, 0:3]
T_mat = T_x[0:3, 3]
l = Um_L / 2
w = Um_W / 2
h = Um_H / 2
res = []
bb = np.zeros((8,3))
xm = 0
ym = 0
bb[0] = np.array([xm + l,ym -w, 0])
bb[1] = np.array([xm + l,ym +w, 0])
bb[2] = np.array([xm - l,ym +w, 0])
bb[3] = np.array([xm - l,ym -w, 0])
bb = R_mat.dot(bb.T).T + T_mat
res.append((bb[0][0],bb[0][1]))
res.append((bb[1][0],bb[1][1]))
res.append((bb[2][0],bb[2][1]))
res.append((bb[3][0],bb[3][1]))
return res
def InitCanvas(width, height, color=(255, 255, 255)):
canvas = np.ones((height, width, 3), dtype="uint8")
canvas[:] = color
return canvas
COLOR_MAP = {
"white": (255, 255, 255),
"green": (0, 255, 0),
"red": (0, 0, 255),
"blue" :(255, 0, 0),
}
canvas = InitCanvas(600, 600, color=COLOR_MAP['white'])
def vis(box,img,angle):
bounds = box.bounds
cx = (bounds[2] + bounds[0]) / 2
cy = (bounds[1] + bounds[3]) / 2
w = bounds[2] - bounds[0]
h = bounds[3] - bounds[1]
cosA = math.cos(angle)
sinA = math.sin(angle)
x1 = cx - 0.5 * w
y1 = cy - 0.5 * h
x0 = cx + 0.5 * w
y0 = y1
x2 = x1
y2 = cy + 0.5 * h
x3 = x0
y3 = y2
x0n = (x0 - cx) * cosA - (y0 - cy) * sinA + cx
y0n = (x0 - cx) * sinA + (y0 - cy) * cosA + cy
x1n = (x1 - cx) * cosA - (y1 - cy) * sinA + cx
y1n = (x1 - cx) * sinA + (y1 - cy) * cosA + cy
x2n = (x2 - cx) * cosA - (y2 - cy) * sinA + cx
y2n = (x2 - cx) * sinA + (y2 - cy) * cosA + cy
x3n = (x3 - cx) * cosA - (y3 - cy) * sinA + cx
y3n = (x3 - cx) * sinA + (y3 - cy) * cosA + cy
# 根据得到的点，画出矩形框
r = random.randint(0, 255)
g = random.randint(0, 255)
b = random.randint(0, 255)
c = (r, g, b)
thickness = 1
cv2.line(img, (int(x0n), int(y0n)), (int(x1n), int(y1n)), c,thickness,lineType=cv2.LINE_AA)
cv2.line(img, (int(x1n), int(y1n)), (int(x2n), int(y2n)), c,thickness,lineType=cv2.LINE_AA)
cv2.line(img, (int(x2n), int(y2n)), (int(x3n), int(y3n)), c,thickness,lineType=cv2.LINE_AA)
cv2.line(img, (int(x0n), int(y0n)), (int(x3n), int(y3n)), c,thickness,lineType=cv2.LINE_AA)
file_names = glob.glob("/home/liuq13/Documents/xworld_drive/xtraffic/cross/*.wpb")
world_proto = xworld_data.Xworld_session()
python_ue5_data = pd.read_csv("/home/liuq13/Documents/xworld_drive/python_ue5_data.csv")
obj_list_all = python_ue5_data['python_path'].to_list()
for file_name in file_names:
print(file_name)
with open(file_name, "rb") as f:
msg = f.read()
world_proto.ParseFromString(msg)
# print('-----------------obj len', len(world_proto.obj_firm_data))
flag_2d = False
flag_3d = False
bb_list = []
bb_list_2d = []
angle_list =[]
if "right" in file_name:
offset = 180
else:
offset = 0
for i in range(0,len(world_proto.obj_firm_data)):
item = world_proto.obj_firm_data[i]
index = obj_list_all.index(item.obj_path)
# print(item.obj_path)
# print(python_ue5_data.iloc[index, 4])
Um_L , Um_H,Um_W = python_ue5_data.iloc[index, 9],python_ue5_data.iloc[index, 10],python_ue5_data.iloc[index, 11]
if i == 0: # 自车
Um_L, Um_H, Um_W = 4.860 ,1.450,2.123
trans_rot = vec2ndpos(item.trans_rot)
trans_pos = vec2ndpos(item.trans_pos)
trans_rot[2] += offset
bb = create_bb_3d(Um_L,Um_W,Um_H,trans_pos, trans_rot)
bb_list.append(bb)
bb_2d = create_bb_2d(Um_L,Um_W,Um_H,trans_pos,trans_rot)
bb_list_2d.append(bb_2d)
angle_list.append(trans_rot[2])
for i in range(len(bb_list)-1):
if flag_3d == True:
break
for j in range(i+1,len(bb_list)):
# print("i, j:",i,j)
box1 = OBB(bb_list[i])
box2 = OBB(bb_list[j])
flag_3d = isCollision(box1,box2)
if flag_3d == True:
break
for box,angle in zip(bb_list_2d,angle_list):
# print(angle )
bb = Polygon(box)
vis(bb, canvas, angle)
# cv2.imshow("Canvas", canvas)
# cv2.waitKey(0)
for i in range(len(bb_list_2d)-1):
if flag_2d == True:
break
box1 = Polygon(bb_list_2d[i])
for j in range(i+1,len(bb_list_2d)):
box2 = Polygon(bb_list_2d[j])
flag_2d = box1.intersects(box2)
# print(flag_2d)
if flag_2d == True:
break
print("flag_2d,flag_3d：",flag_2d,flag_3d)
# cv2.imshow("Canvas", canvas)
# mmcv.imshow(canvas,"res",wait_time=100000)
# cv2.imwrite("draw_rectangle.png", canvas)
# cv2.waitKey(0)
