3d点在立方体内（numpy，不使用for循环）
说明
判断一堆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.pi bev_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 + x bev_corners[0, 1] = (w / 2) * sin_yaw + (l / 2) * cos_yaw + y bev_corners[1, 0] = (l / 2) * sin_yaw + (w / 2) * cos_yaw + x bev_corners[1, 1] = (w / 2) * sin_yaw - (l / 2) * cos_yaw + y bev_corners[2, 0] = (-w / 2) * cos_yaw - (-l / 2) * sin_yaw + x bev_corners[2, 1] = (-w / 2) * sin_yaw + (-l / 2) * cos_yaw + y bev_corners[3, 0] = (-l / 2) * sin_yaw + (-w / 2) * cos_yaw + x bev_corners[3, 1] = (-w / 2) * sin_yaw - (-l / 2) * cos_yaw + y return 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.shape Pxy = 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.shape Az = 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 np def points_in_rotatingrectangles(lidar_points, boxes): boxes_shape = boxes.shape Pxy = 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.shape Az = 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 def get_corners(box): x = box[0] y = box[1] w = box[2] l = box[3] yaw = box[4] if yaw < 0: # 用来映射 yaw = yaw + np.pi bev_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 + x bev_corners[0, 1] = (w / 2) * sin_yaw + (l / 2) * cos_yaw + y bev_corners[1, 0] = (l / 2) * sin_yaw + (w / 2) * cos_yaw + x bev_corners[1, 1] = (w / 2) * sin_yaw - (l / 2) * cos_yaw + y bev_corners[2, 0] = (-w / 2) * cos_yaw - (-l / 2) * sin_yaw + x bev_corners[2, 1] = (-w / 2) * sin_yaw + (-l / 2) * cos_yaw + y bev_corners[3, 0] = (-l / 2) * sin_yaw + (-w / 2) * cos_yaw + x bev_corners[3, 1] = (-w / 2) * sin_yaw - (-l / 2) * cos_yaw + y return bev_corners 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) 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)