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.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)