深度相机同时拍摄xyz点云文件和jpg图像文件。xyz文件里面包含三维坐标[x,y,z]和jpg图像文件包含二维坐标[x,y],但是不能直接进行变换,需要一定的步骤来推演。
下面函数是通过box二维框[xmin, ymin, xmax, ymax, _, _ ]去截取xyz文件中对应box里面的点云,返回点云的numpy数组
def get_point_from_xyz_and_box(self, Image_XYZ_Name, box, p=0.1, Sampling_interval=4):#改进的读取点云的函数,可以节省很多时间# 使用内存映射读取文件with open(Image_XYZ_Name, "rb") as fd:# 计算映射大小和偏移dtype = np.dtype(np.uint16)offset = dtype.itemsize * self.width * self.height * 3 #根据文件格式调整fd.seek(0, 2) # 移动到文件末尾file_size = fd.tell()assert file_size >= offset, "文件大小不匹配"# 创建内存映射mm = np.memmap(fd, dtype=dtype, mode='r', shape=(self.height, self.width, 3), order="C")# 转换为3D点云,只读取box范围内的xmin, ymin, xmax, ymax, _, _ = boxy_indices = np.arange(ymin, ymax, Sampling_interval, dtype=int)x_indices = np.arange(xmin, xmax, Sampling_interval, dtype=int)# 构建网格索引y_grid, x_grid = np.meshgrid(y_indices, x_indices, indexing='ij')depth_points = mm[y_grid, x_grid].reshape(-1, 3) * p# print('mm',mm) # print(y_grid, x_grid) # 过滤无效点(假设深度值为0表示无效点)valid_points = depth_points[depth_points[:, 2] != 0]return valid_points
下面函数是知道点云中的一个点[x,y,z]来计算出这个点对应的二维坐标[y,x]
def calculate_box_from_points_and_xyz(self, point, p=0.1):with open(self.XYZ_Name, "rb") as fd:# 计算映射大小和偏移dtype = np.dtype(np.uint16)offset = dtype.itemsize * self.width * self.height * 3 # 根据文件格式调整fd.seek(0, 2) # 移动到文件末尾file_size = fd.tell()assert file_size >= offset, "文件大小不匹配"# 创建内存映射a = np.memmap(fd, dtype=dtype, mode='r', shape=(self.height, self.width, 3), order="C")# 将所有点的坐标转换为KD树需要的格式,并除以ppoints = np.reshape(a, (-1, 3)) kd_tree = KDTree(points)# 查找最接近的点point_idx = kd_tree.query(point / p)[1]# 将一维索引转换回二维索引matching_indice = np.unravel_index(point_idx, (self.height, self.width))return matching_indice