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
y = box
w = box
l = box
yaw = box
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 = (w / 2) * cos_yaw - (l / 2) * sin_yaw + x
bev_corners = (w / 2) * sin_yaw + (l / 2) * cos_yaw + y
bev_corners = (l / 2) * sin_yaw + (w / 2) * cos_yaw + x
bev_corners = (w / 2) * sin_yaw - (l / 2) * cos_yaw + y
bev_corners = (-w / 2) * cos_yaw - (-l / 2) * sin_yaw + x
bev_corners = (-w / 2) * sin_yaw + (-l / 2) * cos_yaw + y
bev_corners = (-l / 2) * sin_yaw + (-w / 2) * cos_yaw + x
bev_corners = (-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(
* np.ones((bev_corners.shape, 1))])
up = np.hstack(
* np.ones((bev_corners.shape, 1))])
box_3d = np.vstack()
boxes_3d.append(box_3d)
return np.array(boxes_3d)
boxes_3d = get_3dbox([, ])
点在旋转矩形内
就是判定点在凸多边形内，个人使用的是同侧法，或者叫叉乘法。末了就是判定点和四边形的四个顶点的叉乘是否在同一侧。
def points_in_rotatingrectangles(lidar_points, boxes):
boxes_shape = boxes.shape
Pxy = lidar_points[:, :2].reshape(1, lidar_points.shape, 2)
Axy = boxes[:, 0, :2].reshape(boxes_shape, 1, 2)
Bxy = boxes[:, 1, :2].reshape(boxes_shape, 1, 2)
Cxy = boxes[:, 2, :2].reshape(boxes_shape, 1, 2)
Dxy = boxes[:, 3, :2].reshape(boxes_shape, 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(, axis=0).reshape(boxes_shape, -1)
zmax = np.max(, axis=0).reshape(boxes_shape, -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(, 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, 2)
Axy = boxes[:, 0, :2].reshape(boxes_shape, 1, 2)
Bxy = boxes[:, 1, :2].reshape(boxes_shape, 1, 2)
Cxy = boxes[:, 2, :2].reshape(boxes_shape, 1, 2)
Dxy = boxes[:, 3, :2].reshape(boxes_shape, 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(, axis=0).reshape(boxes_shape, -1)
zmax = np.max(, axis=0).reshape(boxes_shape, -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(, axis=2).sum(axis=2)
usability_sum = np.sum(ii == 3, axis=0)
return usability_sum > 0 def get_corners(box): x = box y = box w = box l = box yaw = box 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 = (w / 2) * cos_yaw - (l / 2) * sin_yaw + x bev_corners = (w / 2) * sin_yaw + (l / 2) * cos_yaw + y bev_corners = (l / 2) * sin_yaw + (w / 2) * cos_yaw + x bev_corners = (w / 2) * sin_yaw - (l / 2) * cos_yaw + y bev_corners = (-w / 2) * cos_yaw - (-l / 2) * sin_yaw + x bev_corners = (-w / 2) * sin_yaw + (-l / 2) * cos_yaw + y bev_corners = (-l / 2) * sin_yaw + (-w / 2) * cos_yaw + x bev_corners = (-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( * np.ones((bev_corners.shape, 1))]) up = np.hstack( * np.ones((bev_corners.shape, 1))]) box_3d = np.vstack() 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([, ]) print(boxes_3d.shape) results = points_in_cubes(lidar_points, boxes_3d) print(results)
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。
页:
[1]