判断点是否位于3D检测框中

最新推荐文章于 2023-06-07 11:09:09 发布

张飞飞~

最新推荐文章于 2023-06-07 11:09:09 发布

阅读量1.6k

点赞数 5

分类专栏：点云处理文章标签： python

本文链接：https://blog.csdn.net/qq_36814762/article/details/120791486

版权

点云处理专栏收录该内容

21 篇文章 39 订阅

订阅专栏

给定一个检测框[cx,cy,cz,w,h,l,head]，判断点(x,y,z)是否位于该检测框内。
在这里插入图片描述

c++版本

bool check_point_in_box(float* points, float* box, float local_x, float local_y){
  //points:[x,y,z]
  //box:[c_x,c_y,c_z,dx,dy,dz,heading]
  float x = points[0], y = points[1], z = points[2];
  float cx = box[0], cy = box[1], cz = box[2];
  float dx = box[3], dy = box[4], dz = box[5], rz = box[6];
  // trans the coordinate to the local coordinate
  float shift_x = x - cx;
  float shift_y = y - cy;
  float cos_a = cos(rz), sin_a = sin(rz);
  local_x = shift_x * cos_a + shift_y * sin_a;
  local_y = shift_y * cos_a - shift_x * sin_a;
  if (abs(z - cz) > dz / 2.0)return false;
  if(abs(local_x) > dx / 2.0 || abs(local_y)>dy/2.0)return false;
  return true;
}

python 版本

import math
def check_point_in_box(pts, box):
	"""
	pts[x,y,z]
	box[c_x,c_y,c_z,dx,dy,dz,heading]
"""
shift_x = pts[0] - box[0]
shift_y = pts[1] - box[1]
shift_z = pts[2] - box[2]
cos_a = math.cos(box[6])
sin_a = math.sin(box[6])
dx,dy,dz = box[3], box[4], box[5]
local_x = shift_x * cos_a + shift_y * sin_a;
local_y = shift_y * cos_a - shift_x * sin_a;
if(abs(shift_z)>dz/2.0||abs(local_x)>dx/2.0||abs(local_y)>dy/2.0):
	return False
return True