说明
判断一堆3d点哪些在一堆3d框内,与主流3d目标检测算法一样,立方体只有水平方向上的旋转,没有高度方向上的旋转,就是拍到BEV图像上是一个旋转的矩形
代码优势
基于numpy完成,直接判断一堆点和一堆3d框的包含关系,不使用for循环
算法思路
整体思路说白了其实也挺简单的,就是分两步,首先判断点是否在旋转矩形内(本人直接使用凸多边形),然后判断是否在高度范围内
针对numpy核心的代码思路是:在加和过程中True为1、False为0,因此同时满足三个条件加和应该为3。以此进行重叠的逻辑判断,最后判断一次取出满足全部条件的索引
工程细节
定义随机点
lidar_points = np.random.randint(low=0, high=10, size=(10, 3), dtype='int')
定义旋转矩形
定义一个旋转矩形,输入的box为中心点坐标x、y,矩形宽高,和围绕中心点旋转角度
def get_corners(box): # 这里本人项目yaw [-pi/4, 3*pi/4),需要映射到[0, pi)# box = box.detach().cpu().numpy()x = box[0]y = box[1]w = box[2]l = box[3]yaw = box[4]if yaw < 0: # 用来映射yaw = yaw + np.pibev_corners = np.zeros((4, 2), dtype=np.float32)cos_yaw = np.cos(yaw)sin_yaw = np.sin(yaw)bev_corners[0, 0] = (w / 2) * cos_yaw - (l / 2) * sin_yaw + xbev_corners[0, 1] = (w / 2) * sin_yaw + (l / 2) * cos_yaw + ybev_corners[1, 0] = (l / 2) * sin_yaw + (w / 2) * cos_yaw + xbev_corners[1, 1] = (w / 2) * sin_yaw - (l / 2) * cos_yaw + ybev_corners[2, 0] = (-w / 2) * cos_yaw - (-l / 2) * sin_yaw + xbev_corners[2, 1] = (-w / 2) * sin_yaw + (-l / 2) * cos_yaw + ybev_corners[3, 0] = (-l / 2) * sin_yaw + (-w / 2) * cos_yaw + xbev_corners[3, 1] = (-w / 2) * sin_yaw - (-l / 2) * cos_yaw + yreturn bev_corners
定义空间3d立方体
首先定义旋转矩形,然后为旋转矩形在z轴上赋最大值和最小值,得到的结果就是空间3d的立方体,但是没有高度上的旋转
def get_3dbox(boxes):boxes_3d = []for box in boxes:bev_corners = get_corners(box[:5])down = np.hstack([bev_corners, box[5] * np.ones((bev_corners.shape[0], 1))])up = np.hstack([bev_corners, box[6] * np.ones((bev_corners.shape[0], 1))])box_3d = np.vstack([down, up])boxes_3d.append(box_3d)return np.array(boxes_3d)boxes_3d = get_3dbox([[5, 4, 2, 3, 0, 1, 3], [3, 2, 6, 2, -10, 3, 8]])
点在旋转矩形内
就是判断点在凸多边形内,个人使用的是同侧法,或者叫叉乘法。最后就是判断点和四边形的四个顶点的叉乘是否在同一侧。
def points_in_rotatingrectangles(lidar_points, boxes):boxes_shape = boxes.shapePxy = lidar_points[:, :2].reshape(1, lidar_points.shape[0], 2)Axy = boxes[:, 0, :2].reshape(boxes_shape[0], 1, 2)Bxy = boxes[:, 1, :2].reshape(boxes_shape[0], 1, 2)Cxy = boxes[:, 2, :2].reshape(boxes_shape[0], 1, 2)Dxy = boxes[:, 3, :2].reshape(boxes_shape[0], 1, 2)cross1 = np.cross(Bxy - Axy, Pxy - Axy)cross2 = np.cross(Cxy - Bxy, Pxy - Bxy)cross3 = np.cross(Dxy - Cxy, Pxy - Cxy)cross4 = np.cross(Axy - Dxy, Pxy - Dxy)points_concat1 = np.concatenate([(cross1 > 0)[:, :, np.newaxis], (cross2 > 0)[:, :, np.newaxis],(cross3 > 0)[:, :, np.newaxis], (cross4 > 0)[:, :, np.newaxis]], axis=2)points_concat2 = np.concatenate([(cross1 < 0)[:, :, np.newaxis], (cross2 < 0)[:, :, np.newaxis],(cross3 < 0)[:, :, np.newaxis], (cross4 < 0)[:, :, np.newaxis]], axis=2)points_usability1 = points_concat1.sum(axis=2)points_usability2 = points_concat2.sum(axis=2)ww = np.concatenate([(points_usability1 == 4)[:, :, np.newaxis], (points_usability1 == 0)[:, :, np.newaxis],(points_usability2 == 4)[:, :, np.newaxis], (points_usability2 == 0)[:, :, np.newaxis]], axis=2)mm = ww.sum(axis=2)return (mm > 1)[:, :, np.newaxis]
点在3d立方体内
首先不管高度轴判断水平点是否在旋转矩形内,然后判断点的高度是否在立方体内。
def points_in_cubes(lidar_points, boxes):boxes_shape = boxes.shapeAz = boxes[:, 0, 2]Ez = boxes[:, 4, 2]Pz = lidar_points[:, 2].reshape(1, -1)zmin = np.min([Az, Ez], axis=0).reshape(boxes_shape[0], -1)zmax = np.max([Az, Ez], axis=0).reshape(boxes_shape[0], -1)p_min_z = (Pz > zmin)[:, :, np.newaxis]p_max_z = (Pz < zmax)[:, :, np.newaxis]p_in_r = points_in_rotatingrectangles(lidar_points, boxes)ii = np.concatenate([p_in_r, p_min_z, p_max_z], axis=2).sum(axis=2)usability_sum = np.sum(ii == 3, axis=0)return usability_sum > 0
完整代码
#!/usr/bin/env python# -*- coding: utf-8 -*-'''@File : main.py@Time : 2024/10/24 20:02:07@Author : xudh @Version : 1.0@Desc : 一群空间3d点在一群3d立方体内code is far away from bug with the animal protecting┏┓ ┏┓┏┛┻━━━┛┻┓┃ ┃┃ ━ ┃┃ > < ┃┃ ┃┃ . ⌒ .. ┃┃ ┃┗━┓ ┏━┛┃ ┃ Codes are far away from bugs with the animal protecting┃ ┃ 神兽保佑,代码无bug┃ ┃┃ ┃┃ ┃┃ ┃┃ ┗━━━┓┃ ┣┓┃ ┏┛┗┓┓┏━┳┓┏┛┃┫┫ ┃┫┫┗┻┛ ┗┻┛'''import numpy as npdef points_in_rotatingrectangles(lidar_points, boxes):boxes_shape = boxes.shapePxy = lidar_points[:, :2].reshape(1, lidar_points.shape[0], 2)Axy = boxes[:, 0, :2].reshape(boxes_shape[0], 1, 2)Bxy = boxes[:, 1, :2].reshape(boxes_shape[0], 1, 2)Cxy = boxes[:, 2, :2].reshape(boxes_shape[0], 1, 2)Dxy = boxes[:, 3, :2].reshape(boxes_shape[0], 1, 2)cross1 = np.cross(Bxy - Axy, Pxy - Axy)cross2 = np.cross(Cxy - Bxy, Pxy - Bxy)cross3 = np.cross(Dxy - Cxy, Pxy - Cxy)cross4 = np.cross(Axy - Dxy, Pxy - Dxy)points_concat1 = np.concatenate([(cross1 > 0)[:, :, np.newaxis], (cross2 > 0)[:, :, np.newaxis],(cross3 > 0)[:, :, np.newaxis], (cross4 > 0)[:, :, np.newaxis]], axis=2)points_concat2 = np.concatenate([(cross1 < 0)[:, :, np.newaxis], (cross2 < 0)[:, :, np.newaxis],(cross3 < 0)[:, :, np.newaxis], (cross4 < 0)[:, :, np.newaxis]], axis=2)points_usability1 = points_concat1.sum(axis=2)points_usability2 = points_concat2.sum(axis=2)ww = np.concatenate([(points_usability1 == 4)[:, :, np.newaxis], (points_usability1 == 0)[:, :, np.newaxis],(points_usability2 == 4)[:, :, np.newaxis], (points_usability2 == 0)[:, :, np.newaxis]], axis=2)mm = ww.sum(axis=2)return (mm > 1)[:, :, np.newaxis]def points_in_cubes(lidar_points, boxes):boxes_shape = boxes.shapeAz = boxes[:, 0, 2]Ez = boxes[:, 4, 2]Pz = lidar_points[:, 2].reshape(1, -1)zmin = np.min([Az, Ez], axis=0).reshape(boxes_shape[0], -1)zmax = np.max([Az, Ez], axis=0).reshape(boxes_shape[0], -1)p_min_z = (Pz > zmin)[:, :, np.newaxis]p_max_z = (Pz < zmax)[:, :, np.newaxis]p_in_r = points_in_rotatingrectangles(lidar_points, boxes)ii = np.concatenate([p_in_r, p_min_z, p_max_z], axis=2).sum(axis=2)usability_sum = np.sum(ii == 3, axis=0)return usability_sum > 0def get_corners(box): x = box[0]y = box[1]w = box[2]l = box[3]yaw = box[4]if yaw < 0: # 用来映射yaw = yaw + np.pibev_corners = np.zeros((4, 2), dtype=np.float32)cos_yaw = np.cos(yaw)sin_yaw = np.sin(yaw)bev_corners[0, 0] = (w / 2) * cos_yaw - (l / 2) * sin_yaw + xbev_corners[0, 1] = (w / 2) * sin_yaw + (l / 2) * cos_yaw + ybev_corners[1, 0] = (l / 2) * sin_yaw + (w / 2) * cos_yaw + xbev_corners[1, 1] = (w / 2) * sin_yaw - (l / 2) * cos_yaw + ybev_corners[2, 0] = (-w / 2) * cos_yaw - (-l / 2) * sin_yaw + xbev_corners[2, 1] = (-w / 2) * sin_yaw + (-l / 2) * cos_yaw + ybev_corners[3, 0] = (-l / 2) * sin_yaw + (-w / 2) * cos_yaw + xbev_corners[3, 1] = (-w / 2) * sin_yaw - (-l / 2) * cos_yaw + yreturn bev_cornersdef get_3dbox(boxes):boxes_3d = []for box in boxes:bev_corners = get_corners(box[:5])down = np.hstack([bev_corners, box[5] * np.ones((bev_corners.shape[0], 1))])up = np.hstack([bev_corners, box[6] * np.ones((bev_corners.shape[0], 1))])box_3d = np.vstack([down, up])boxes_3d.append(box_3d)return np.array(boxes_3d)if __name__ == '__main__':lidar_points = np.random.randint(low=0, high=10, size=(10, 3), dtype='int')print(lidar_points.shape)boxes_3d = get_3dbox([[5, 4, 2, 3, 0, 1, 3], [3, 2, 6, 2, -10, 3, 8]])print(boxes_3d.shape)results = points_in_cubes(lidar_points, boxes_3d)print(results)