bev投影

点云bev投影代码

pillar/voxel

#!/usr/bin/env python
# -*- encoding: utf-8 -*-import os
import numpy as np
import mathn_max_points_per_pillar = 32
n_max_pillars = 20736  # 144*144 = 20736
n_in_features = 7  # x y z i xc yc zcx_step = 0.32  # 32cm 32*144 = 4608 = 46.08m
y_step = 0.16  # 16cm 16*144 =  = 15.36mmax_intensity = 255def read_pcd(pcd_file, readline=11):# np.fromfile read pcd 文本/二进制# cloud = np.fromfile(str(pcd_file), dtype=np.float32, count=-1)data = []with open(pcd_file, 'r') as f:lines = f.readlines()[readline:]          for line in lines:line = np.array(line.strip('\n').split(' '), dtype=np.float32)# print(line.shape, line)x,y,z,i = line[0], line[1], line[2], line[3]# print(x,y,z,i)# r = math.sqrt(x**2 + y**2 + z**2) * i  # for rslidar# r = float(line[4]) # for ouster lidar data.append([x,y,z,i])# points = list(map(lambda line: list(map(lambda x: float(x), line.split(' '))), lines))points = np.array(data)return pointsdef points_to_pillars(points, list_roi_xyz = [0.0, 69.12, -11.52, 11.52, -2.5, 0.5],list_pillar_xy = [0.48, 0.16],list_grid_xy = [144, 144],  # (69.12 - 0) / 0.48list_pillar_szie = [20736, 32, 7]):x_min, x_max, y_min, y_max, z_min, z_max = list_roi_xyzx_grid, y_grid = list_grid_xyx_pillar, y_pillar = list_pillar_xyn_max_pillars, n_max_points_per_pillar, n_in_features = list_pillar_szie# ROIidx = np.where((points[:, 0] > x_min) & (points[:, 0] < x_max) &(points[:, 1] > y_min) & (points[:, 1] < y_max) &(points[:, 2] > z_min) & (points[:, 2] < z_max))points_roi = points[idx[0]]# intensity normlizepoints_roi[:,3] /= 255# 计算每个点所在的voxel索引# Compute the indices of the pillars that each point belongs to.x_img = (points_roi[:,0] - x_min) // x_pillary_img = (points_roi[:,1] - y_min) // y_pillarx_img = x_img.reshape(-1, 1).astype(int)y_img = y_img.reshape(-1, 1).astype(int)# pillar_coords = np.concatenate((x_img, y_img), axis=1)# 计算pillar的索引pillar_idx = x_img * y_grid + y_img  # 找到每个pillar的点数unique_pillars, num_points_per_pillars = np.unique(pillar_idx, return_counts=True)# print("num pillars: ",unique_pillars, num_points_per_pillars)num_points_per_pillars = np.minimum(num_points_per_pillars, n_max_points_per_pillar)# Initialize the pillar representation with zeros.pillars = np.zeros([n_max_pillars, n_max_points_per_pillar, n_in_features]).astype(np.float32)for i, pillar_id in enumerate(unique_pillars):# 找出pillar对应的点indices = np.where(pillar_idx == pillar_id)[0]  # 每个pillar内所有点的索引indices = indices[:num_points_per_pillars[i]]  # 每个pillar内取前n个点的索引# 计算pillar的中心坐标pillar_c = np.mean(points_roi[indices, :3], axis=0)        # 计算pillar的中心坐标和每个点的偏移offsets = points_roi[indices, :3] - pillar_c# 将点和偏移添加到pillar中pillars[pillar_id, :num_points_per_pillars[i], :4] = points_roi[indices]pillars[pillar_id, :num_points_per_pillars[i], 4:7] = offsetsprint(pillars[pillar_id, ...])if __name__=="__main__":path = "/home/data"for i,f in enumerate(os.listdir(path)):file = os.path.join(path, f)if i < 20:points = read_pcd(file)points_to_pillars(points)