class Voxelization(object): def __init__(self, **kwargs): cfg = kwargs.get("cfg", None) self.range = cfg.range self.voxel_size = cfg.voxel_size self.max_points_in_voxel = cfg.max_points_in_voxel self.max_voxel_num = cfg.max_voxel_num self.include_pt_to_voxel = cfg.get("include_pt_to_voxel", False) self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, return_pt_to_voxel=self.include_pt_to_voxel, ) def __call__(self, res, info): voxel_size = self.voxel_generator.voxel_size pc_range = self.voxel_generator.point_cloud_range grid_size = self.voxel_generator.grid_size if res["mode"] == "train": gt_dict = res["lidar"]["annotations"] bv_range = pc_range[[0, 1, 3, 4]] mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range) _dict_select(gt_dict, mask) res["lidar"]["annotations"] = gt_dict # points = points[:int(points.shape[0] * 0.1), :] points = res["lidar"]["points"] if self.include_pt_to_voxel: voxels, coordinates, num_points, pt_to_voxel = self.voxel_generator.generate( points) else: voxels, coordinates, num_points = self.voxel_generator.generate( points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) res["lidar"]["voxels"] = dict( voxels=voxels, coordinates=coordinates, num_points=num_points, num_voxels=num_voxels, shape=grid_size, ) if self.include_pt_to_voxel: res["lidar"]["voxels"]["pt_to_voxel"] = pt_to_voxel if "valid_idx" in res["lidar"]: # panoview projector is used valid_idx = res["lidar"]["valid_idx"] return res, info
class Voxelization(object): def __init__(self, **kwargs): cfg = kwargs.get("cfg", None) self.range = cfg.range # [0, -40.0, -3.0, 70.4, 40.0, 1.0] self.voxel_size = cfg.voxel_size # [0.05, 0.05, 0.1] self.max_points_in_voxel = cfg.max_points_in_voxel # 5 self.max_voxel_num = cfg.max_voxel_num # 20000 self.far_points_first = cfg.far_points_first self.shuffle = False self.voxel_generator = VoxelGenerator( point_cloud_range=self.range, voxel_size=self.voxel_size, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) def __call__(self, res, info): pc_range = self.voxel_generator.point_cloud_range # [0, -40, -3, 70.4, 40, 1] grid_size = self.voxel_generator.grid_size # [1408, 1600, 40] if res["mode"] == "train" and res['labeled']: gt_dict = res["lidar"]["annotations"] bv_range = pc_range[[0, 1, 3, 4]] # [ 0. , -40. , 70.4, 40. ], mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range) _dict_select(gt_dict, mask) res["lidar"]["annotations"] = gt_dict self.shuffle = True points = res["lidar"]["points"] voxels, coordinates, num_points_per_voxel = self.voxel_generator.generate( points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) res["lidar"]["voxels"] = dict( voxels=voxels, coordinates=coordinates, num_points=num_points_per_voxel, num_voxels=num_voxels, shape=grid_size, ) # voxelization of raw points without transformation if "points_raw" in res["lidar"].keys(): points_raw = res["lidar"]["points_raw"] voxels_raw, coordinates_raw, num_points_per_voxel_raw = self.voxel_generator.generate( points_raw) num_voxels_raw = np.array([voxels_raw.shape[0]], dtype=np.int64) res["lidar"]["voxels_raw"] = dict( voxels=voxels_raw, coordinates=coordinates_raw, num_points=num_points_per_voxel_raw, num_voxels=num_voxels_raw, shape=grid_size, ) return res, info
def getDataFromPcd(path, cfg): points = pcl.load(path).to_array() points = np.matmul(cfg.rotMat, points.T).T vgcfg = cfg.voxel_generator # create voxel generator vg = VoxelGenerator( voxel_size=vgcfg.voxel_size, point_cloud_range=vgcfg.range, max_num_points=vgcfg.max_points_in_voxel, max_voxels=vgcfg.max_voxel_num, ) grid_size = vg.grid_size voxels, coordinates, num_points = vg.generate(points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) data = dict(points=points, voxels=voxels, shape=grid_size, num_points=num_points, num_voxels=num_voxels, coordinates=coordinates) return data
class Voxelization(object): ''' configs: range=[0, -40.0, -3.0, 70.4, 40.0, 1.0], voxel_size=[0.05, 0.05, 0.1], max_points_in_voxel=5, max_voxel_num=20000, ''' def __init__(self, **kwargs): cfg = kwargs.get("cfg", None) self.range = cfg.range self.voxel_size = cfg.voxel_size self.max_points_in_voxel = cfg.max_points_in_voxel self.max_voxel_num = cfg.max_voxel_num self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) def __call__(self, res, info): voxel_size = self.voxel_generator.voxel_size # [0.05, 0.05, 0.1 ] pc_range = self.voxel_generator.point_cloud_range # [0, -40, -3, 70.4, 40, 1] grid_size = self.voxel_generator.grid_size # [1408, 1600, 40] # remove gt_boxes out of bv_range if res["mode"] == "train": gt_dict = res["lidar"]["annotations"] bv_range = pc_range[[0, 1, 3, 4]] # [ 0. , -40. , 70.4, 40. ], # todo: try get_valid_mask_by_pc_valid_range in preprocess.py, how about z-axis mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range) _dict_select(gt_dict, mask) res["lidar"]["annotations"] = gt_dict # todo: how about the points out of pc_valid_range voxels, coordinates, num_points = self.voxel_generator.generate( res["lidar"]["points"]) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) # pack voxelization result res["lidar"]["voxels"] = dict( voxels=voxels, coordinates=coordinates, num_points=num_points, num_voxels=num_voxels, shape=grid_size, ) return res, info
class Voxelization(object): def __init__(self, **kwargs): cfg = kwargs.get("cfg", None) self.range = cfg.range # [0, -40.0, -3.0, 70.4, 40.0, 1.0] self.voxel_size = cfg.voxel_size # [0.05, 0.05, 0.1] self.max_points_in_voxel = cfg.max_points_in_voxel # 5 self.max_voxel_num = cfg.max_voxel_num # 20000 self.far_points_first = cfg.far_points_first self.shuffle = False self.voxel_generator = VoxelGenerator( point_cloud_range=self.range, voxel_size=self.voxel_size, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) def __call__(self, res, info): voxel_size = self.voxel_generator.voxel_size # [0.05, 0.05, 0.1 ] pc_range = self.voxel_generator.point_cloud_range # [0, -40, -3, 70.4, 40, 1] grid_size = self.voxel_generator.grid_size # [1408, 1600, 40] # remove those gt_boxes (after gt-aug, per-object-aug, and global-aug) out of valid bv_range if res["mode"] == "train": gt_dict = res["lidar"]["annotations"] bv_range = pc_range[[0, 1, 3, 4]] # [ 0. , -40. , 70.4, 40. ], # todo: try get_valid_mask_by_pc_valid_range in preprocess.py, how about z-axis mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range) _dict_select(gt_dict, mask) res["lidar"]["annotations"] = gt_dict self.shuffle = True points = res["lidar"]["points"] if self.far_points_first: points = box_np_ops.far_points_first(points, 40.0, self.max_voxel_num, self.shuffle) voxels, coordinates, num_points_per_voxel = self.voxel_generator.generate( points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) # pack voxelization result res["lidar"]["voxels"] = dict( voxels=voxels, coordinates=coordinates, num_points=num_points_per_voxel, num_voxels=num_voxels, shape=grid_size, ) return res, info
class Voxelization(object): def __init__(self, **kwargs): cfg = kwargs.get("cfg", None) self.range = cfg.range self.voxel_size = cfg.voxel_size self.max_points_in_voxel = cfg.max_points_in_voxel self.max_voxel_num = cfg.max_voxel_num self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) def __call__(self, res, info): # [0, -40, -3, 70.4, 40, 1] voxel_size = self.voxel_generator.voxel_size pc_range = self.voxel_generator.point_cloud_range grid_size = self.voxel_generator.grid_size # [352, 400] if res["mode"] == "train": gt_dict = res["lidar"]["annotations"] bv_range = pc_range[[0, 1, 3, 4]] mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range) _dict_select(gt_dict, mask) res["lidar"]["annotations"] = gt_dict # points = points[:int(points.shape[0] * 0.1), :] voxels, coordinates, num_points = self.voxel_generator.generate( res["lidar"]["points"] ) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) res["lidar"]["voxels"] = dict( voxels=voxels, coordinates=coordinates, num_points=num_points, num_voxels=num_voxels, shape=grid_size, ) return res, info
def getDataFromDepth(path, cfg): ''' get point cloud data from depth image and calibration file and prepare the data for inputs path: depth image path calibPath: camera intrinsic path ''' fs = cv.FileStorage(cfg.intrinsic, cv.FILE_STORAGE_READ) cameraMat = fs.getNode('CameraMat').mat() # distCoeff = fs.getNode('DistCoeff').mat() fs.release() fx = cameraMat[0, 0] fy = cameraMat[1, 1] cx = cameraMat[0, 2] cy = cameraMat[1, 2] points = depth2points(cv.imread(path, -1), fx, fy, cx, cy, cfg.maxDepth).reshape((-1, 3)) points = np.matmul(cfg.rotMat, points.T).T vgcfg = cfg.voxel_generator # create voxel generator vg = VoxelGenerator( voxel_size=vgcfg.voxel_size, point_cloud_range=vgcfg.range, max_num_points=vgcfg.max_points_in_voxel, max_voxels=vgcfg.max_voxel_num, ) grid_size = vg.grid_size voxels, coordinates, num_points = vg.generate(points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) data = dict(points=points, voxels=voxels, shape=grid_size, num_points=num_points, num_voxels=num_voxels, coordinates=coordinates) return data
class Processor_ROS: def __init__(self, config_path, model_path): self.points = None self.config_path = config_path self.model_path = model_path self.device = None self.net = None self.voxel_generator = None self.inputs = None def initialize(self): self.read_config() def read_config(self): config_path = self.config_path cfg = Config.fromfile(self.config_path) self.device = torch.device( "cuda" if torch.cuda.is_available() else "cpu") self.net = build_detector(cfg.model, train_cfg=None, test_cfg=cfg.test_cfg) self.net.load_state_dict(torch.load(self.model_path)["state_dict"]) self.net = self.net.to(self.device).eval() self.range = cfg.voxel_generator.range self.voxel_size = cfg.voxel_generator.voxel_size self.max_points_in_voxel = cfg.voxel_generator.max_points_in_voxel self.max_voxel_num = cfg.voxel_generator.max_voxel_num self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) def run(self, points): t_t = time.time() print(f"input points shape: {points.shape}") num_features = 5 self.points = points.reshape([-1, num_features]) self.points[:, 4] = 0 # timestamp value voxels, coords, num_points = self.voxel_generator.generate(self.points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) grid_size = self.voxel_generator.grid_size coords = np.pad(coords, ((0, 0), (1, 0)), mode='constant', constant_values=0) voxels = torch.tensor(voxels, dtype=torch.float32, device=self.device) coords = torch.tensor(coords, dtype=torch.int32, device=self.device) num_points = torch.tensor(num_points, dtype=torch.int32, device=self.device) num_voxels = torch.tensor(num_voxels, dtype=torch.int32, device=self.device) self.inputs = dict(voxels=voxels, num_points=num_points, num_voxels=num_voxels, coordinates=coords, shape=[grid_size]) torch.cuda.synchronize() t = time.time() with torch.no_grad(): outputs = self.net(self.inputs, return_loss=False)[0] # print(f"output: {outputs}") torch.cuda.synchronize() print(" network predict time cost:", time.time() - t) outputs = remove_low_score_nu(outputs, 0.45) boxes_lidar = outputs["box3d_lidar"].detach().cpu().numpy() print(" predict boxes:", boxes_lidar.shape) scores = outputs["scores"].detach().cpu().numpy() types = outputs["label_preds"].detach().cpu().numpy() boxes_lidar[:, -1] = -boxes_lidar[:, -1] - np.pi / 2 print(f" total cost time: {time.time() - t_t}") return scores, boxes_lidar, types
class Voxelization(object): def __init__(self, **kwargs): cfg = kwargs.get("cfg", None) self.range = cfg.range self.voxel_size = cfg.voxel_size self.max_points_in_voxel = cfg.max_points_in_voxel self.max_voxel_num = [cfg.max_voxel_num, cfg.max_voxel_num] if isinstance(cfg.max_voxel_num, int) else cfg.max_voxel_num self.double_flip = cfg.get('double_flip', False) self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num[0], ) def __call__(self, res, info): voxel_size = self.voxel_generator.voxel_size pc_range = self.voxel_generator.point_cloud_range grid_size = self.voxel_generator.grid_size if res["mode"] == "train": gt_dict = res["lidar"]["annotations"] bv_range = pc_range[[0, 1, 3, 4]] mask = prep.filter_gt_box_outside_range(gt_dict["gt_boxes"], bv_range) _dict_select(gt_dict, mask) res["lidar"]["annotations"] = gt_dict max_voxels = self.max_voxel_num[0] else: max_voxels = self.max_voxel_num[1] voxels, coordinates, num_points = self.voxel_generator.generate( res["lidar"]["points"], max_voxels=max_voxels ) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) res["lidar"]["voxels"] = dict( voxels=voxels, coordinates=coordinates, num_points=num_points, num_voxels=num_voxels, shape=grid_size, range=pc_range, size=voxel_size ) double_flip = self.double_flip and (res["mode"] != 'train') if double_flip: flip_voxels, flip_coordinates, flip_num_points = self.voxel_generator.generate( res["lidar"]["yflip_points"] ) flip_num_voxels = np.array([flip_voxels.shape[0]], dtype=np.int64) res["lidar"]["yflip_voxels"] = dict( voxels=flip_voxels, coordinates=flip_coordinates, num_points=flip_num_points, num_voxels=flip_num_voxels, shape=grid_size, range=pc_range, size=voxel_size ) flip_voxels, flip_coordinates, flip_num_points = self.voxel_generator.generate( res["lidar"]["xflip_points"] ) flip_num_voxels = np.array([flip_voxels.shape[0]], dtype=np.int64) res["lidar"]["xflip_voxels"] = dict( voxels=flip_voxels, coordinates=flip_coordinates, num_points=flip_num_points, num_voxels=flip_num_voxels, shape=grid_size, range=pc_range, size=voxel_size ) flip_voxels, flip_coordinates, flip_num_points = self.voxel_generator.generate( res["lidar"]["double_flip_points"] ) flip_num_voxels = np.array([flip_voxels.shape[0]], dtype=np.int64) res["lidar"]["double_flip_voxels"] = dict( voxels=flip_voxels, coordinates=flip_coordinates, num_points=flip_num_points, num_voxels=flip_num_voxels, shape=grid_size, range=pc_range, size=voxel_size ) return res, info
class CenterPoint: def __init__(self, config_path, model_path, savingpath): self.points = None self.config_path = config_path self.model_path = model_path self.device = None self.net = None self.voxel_generator = None self.inputs = None self.savepth = savingpath def initialize(self): self.read_config() def read_config(self): config_path = self.config_path cfg = Config.fromfile(self.config_path) self.device = torch.device( "cuda" if torch.cuda.is_available() else "cpu") self.net = build_detector(cfg.model, train_cfg=None, test_cfg=cfg.test_cfg) self.net.load_state_dict(torch.load(self.model_path)["state_dict"]) self.net = self.net.to(self.device).eval() self.range = cfg.voxel_generator.range self.voxel_size = cfg.voxel_generator.voxel_size self.max_points_in_voxel = cfg.voxel_generator.max_points_in_voxel self.max_voxel_num = cfg.voxel_generator.max_voxel_num self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) # def run(self, points): # t_t = time.time() # print(type(points)) # print(points.shape) # print(f"input points shape: {points.shape}") # num_features = 4 # self.points = points.reshape([-1, num_features]) # self.points[:, 3] = 0 # timestamp value #self.points = np.hstack((aself.points, np.zeros((self.points.shape[0], 1), dtype=self.points.dtype))) # voxels, coords, num_points = self.voxel_generator.generate(self.points) # num_voxels = np.array([voxels.shape[0]], dtype=np.int64) # grid_size = self.voxel_generator.grid_size # coords = np.pad(coords, ((0, 0), (1, 0)), mode='constant', constant_values = 0) # voxels = torch.tensor(voxels, dtype=torch.float32, device=self.device) # coords = torch.tensor(coords, dtype=torch.int32, device=self.device) # num_points = torch.tensor(num_points, dtype=torch.int32, device=self.device) # num_voxels = torch.tensor(num_voxels, dtype=torch.int32, device=self.device) # self.inputs = dict( # voxels = voxels, # num_points = num_points, # num_voxels = num_voxels, # coordinates = coords, # shape = [grid_size] # ) # torch.cuda.synchronize() # t = time.time() # with torch.no_grad(): # outputs = self.net(self.inputs, return_loss=False)[0] # # print(f"output: {outputs}") # torch.cuda.synchronize() # print(" network predict time cost:", time.time() - t) # outputs = remove_low_score_nu(outputs, 0.45) # boxes_lidar = outputs["box3d_lidar"].detach().cpu().numpy() # print(" predict boxes:", boxes_lidar.shape) # scores = outputs["scores"].detach().cpu().numpy() # types = outputs["label_preds"].detach().cpu().numpy() # boxes_lidar[:, -1] = -boxes_lidar[:, -1] - np.pi / 2 # print(f" total cost time: {time.time() - t_t}") # return scores, boxes_lidar, types def run(self, pc_folder): for ipfile in os.listdir(pc_folder): t_t = time.time() print("Processing File is {}".format(ipfile)) self.points = np.fromfile(os.path.join(pc_folder, ipfile), dtype=np.float32) # print(points.shape) # print(f"input points shape: {points.shape}") ##The below lines are for default pc num_features = 5 self.points = points.reshape([-1, num_features]) self.points[:, 4] = 0 # timestamp value ##The above modification is required on custom data #num_features = 5 #self.points = self.points.reshape([-1, 4]) # self.points[:, 4] = 0 # timestamp value # #self.points = np.hstack((self.points, np.zeros((self.points.shape[0], 1), dtype=self.points.dtype))) voxels, coords, num_points = self.voxel_generator.generate( self.points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) grid_size = self.voxel_generator.grid_size coords = np.pad(coords, ((0, 0), (1, 0)), mode='constant', constant_values=0) voxels = torch.tensor(voxels, dtype=torch.float32, device=self.device) coords = torch.tensor(coords, dtype=torch.int32, device=self.device) num_points = torch.tensor(num_points, dtype=torch.int32, device=self.device) num_voxels = torch.tensor(num_voxels, dtype=torch.int32, device=self.device) self.inputs = dict(voxels=voxels, num_points=num_points, num_voxels=num_voxels, coordinates=coords, shape=[grid_size]) torch.cuda.synchronize() t = time.time() with torch.no_grad(): outputs = self.net(self.inputs, return_loss=False)[0] # print(f"output: {outputs}") torch.cuda.synchronize() print(" network predict time cost:", time.time() - t) outputs = remove_low_score_nu(outputs, 0.45) boxes_lidar = outputs["box3d_lidar"].detach().cpu().numpy() print(" predict boxes:", boxes_lidar.shape) scores = outputs["scores"].detach().cpu().numpy() types = outputs["label_preds"].detach().cpu().numpy() boxes_lidar[:, -1] = -boxes_lidar[:, -1] - np.pi / 2 print(f" total cost time: {time.time() - t_t}") pred2string(scores, types, boxes_lidar, ipfile, self.savepth) # return scores, boxes_lidar, types def log_predkitti(self, scores, detbox, types): pass
def crop2assign(example, predictions, cfg=None, device=torch.device("cpu"), training=True): rank, world_size = get_dist_info() # STEP 2: crop enlarged point clouds; organise as new batch' # 2.1 Prepare batch targets, filter invalid target batch_targets = [] # 2.1 Prepare batch data batch_data = defaultdict(list) if training: gt_boxes3d = defaultdict(list) for idx, sample in enumerate(example["annos"]): for branch in sample["gt_boxes"]: mask = np.ma.masked_equal(branch, 0).mask if np.any(mask): gt_boxes3d[idx].extend( branch[np.where(mask.sum(axis=1) == 0)]) else: gt_boxes3d[idx].extend(branch) # Batch size cnt = 0 for idx, dt_boxes3d in enumerate(predictions): sample_points = ( example["points"][example["points"][:, 0] == idx][:, 1:].cpu().numpy()) if sample_points[:, -1].min() < 0: sample_points[:, -1] += 0.5 if training: gp = example["ground_plane"][idx] sample_gt_boxes3d = np.array(gt_boxes3d[idx]) if not sample_gt_boxes3d.shape[0] == 0: gt_bevs = sample_gt_boxes3d[:, [0, 1, 3, 4, -1]] else: gt_bevs = np.zeros((0, 5)) sample_dt_boxed3d = dt_boxes3d["box3d_lidar"].cpu().numpy() if training: dt_bevs = sample_dt_boxed3d[:, [0, 1, 3, 4, -1]] gp_height = cfg.anchor.center - cfg.anchor.height / 2 # Find max match gt ious = riou_cc(gt_bevs, dt_bevs) dt_max_matched_gt = ious.argmax(axis=0) max_ious = ious.max(axis=0) selected = np.where(max_ious >= 0.7) max_ious = max_ious[selected] dt_max_matched_gt = dt_max_matched_gt[selected] # remove fp sample_dt_boxed3d = sample_dt_boxed3d[selected] # enlarge box sample_dt_boxed3d[:, [3, 4]] += cfg.roi_context indices = points_in_rbbox(sample_points, sample_dt_boxed3d) num_points_in_gt = indices.sum(0) # remove empty dt boxes selected_by_points = np.where(num_points_in_gt > 0) boxes = sample_dt_boxed3d[selected_by_points] indices = indices.transpose()[selected_by_points].transpose() if training: dt_max_matched_gt = dt_max_matched_gt[selected_by_points] cnt += len(boxes) # voxel_generators to form fixed_size batch input fixed_size = cfg.dense_shape # w, l, h for i, box in enumerate(boxes): if training: batch_data["ground_plane"].append(gp) # 1. generate regression targets matched_gt = sample_gt_boxes3d[dt_max_matched_gt[i]] height_a = cfg.anchor.height z_center_a = cfg.anchor.center # z_g = matched_gt[2] + matched_gt[5]/2. - (-0.14) # z top z_g = matched_gt[2] - z_center_a h_g = matched_gt[5] - height_a g_g = (matched_gt[2] - matched_gt[5] / 2) - (float(gp)) dt_target = np.array([idx, i, z_g, h_g, g_g]) batch_targets.append(dt_target) if not training: batch_data["stage_one_output_boxes"].append(box) # 2. prepare data box_points = sample_points[indices[:, i]].copy() # img = kitti_vis(box_points, np.array(box).reshape(1, -1)) # img.tofile(open("./bev.bin", "wb")) # move to center box_center = box[:2] box_points[:, :2] -= box_center # rotate to canonical box_yaw = box[-1:] box_points_canonical = rotation_3d_in_axis( box_points[:, :3][np.newaxis, ...], -box_yaw, axis=2)[0] box_points_canonical = np.hstack( (box_points_canonical, box_points[:, -1:])) if box_points_canonical.shape[0] > 0: point_cloud_range = [ -box[3] / 2, -box[4] / 2, -3.5, box[3] / 2, box[4] / 2, 1.5, ] else: import pdb pdb.set_trace() voxel_size = [ (point_cloud_range[3] - point_cloud_range[0]) / fixed_size[0], (point_cloud_range[4] - point_cloud_range[1]) / fixed_size[1], (point_cloud_range[5] - point_cloud_range[2]) / fixed_size[2], ] for vs in voxel_size: if not vs > 0: import pdb pdb.set_trace() vg = VoxelGenerator( voxel_size=voxel_size, point_cloud_range=point_cloud_range, max_num_points=20, max_voxels=2000, ) voxels, coordinates, num_points = vg.generate(box_points_canonical, max_voxels=2000) batch_data["voxels"].append(voxels) batch_data["coordinates"].append(coordinates) batch_data["num_points"].append(num_points) if training: batch_targets = torch.tensor(np.array(batch_targets), dtype=torch.float32, device=device) batch_data["targets"] = batch_targets batch_data["ground_plane"] = torch.tensor(batch_data["ground_plane"], dtype=torch.float32, device=device) for k, v in batch_data.items(): if k in ["voxels", "num_points"]: batch_data[k] = np.concatenate(v, axis=0) elif k in ["stage_one_output_boxes"]: batch_data[k] = np.array(v) elif k in ["coordinates"]: coors = [] for i, coor in enumerate(v): coor_pad = np.pad(coor, ((0, 0), (1, 0)), mode="constant", constant_values=i) coors.append(coor_pad) batch_data[k] = np.concatenate(coors, axis=0) for k, v in batch_data.items(): if k in ["coordinates", "num_points"]: batch_data[k] = torch.tensor(v, dtype=torch.int32, device=device) elif k in ["voxels", "stage_one_output_boxes"]: batch_data[k] = torch.tensor(v, dtype=torch.float32, device=device) if training: if (not cnt == (batch_data["coordinates"][:, 0].max() + 1) == batch_data["targets"].shape[0]): import pdb pdb.set_trace() batch_data["shape"] = fixed_size return batch_data
class Processor_ROS: def __init__(self, config_path, model_path): self.points = None self.config_path = config_path self.model_path = model_path self.device = None self.net = None self.voxel_generator = None self.lidar_deque = deque(maxlen=5) self.current_frame = { "lidar_stamp": None, "lidar_seq": None, "points": None, "odom_seq": None, "odom_stamp": None, "translation": None, "rotation": None } self.pc_list = deque(maxlen=5) self.inputs = None def initialize(self): self.read_config() def read_config(self): config_path = self.config_path cfg = Config.fromfile(self.config_path) self.device = torch.device( "cuda" if torch.cuda.is_available() else "cpu") self.net = build_detector( cfg.model, train_cfg=None, test_cfg=cfg.test_cfg) self.net.load_state_dict(torch.load(self.model_path)["state_dict"]) self.net = self.net.to(self.device).eval() self.range = cfg.voxel_generator.range self.voxel_size = cfg.voxel_generator.voxel_size self.max_points_in_voxel = cfg.voxel_generator.max_points_in_voxel self.max_voxel_num = cfg.voxel_generator.max_voxel_num self.voxel_generator = VoxelGenerator( voxel_size=self.voxel_size, point_cloud_range=self.range, max_num_points=self.max_points_in_voxel, max_voxels=self.max_voxel_num, ) # nuscenes dataset lidar2imu_t = np.array([0.985793, 0.0, 1.84019]) lidar2imu_r = Quaternion([0.706749235, -0.01530099378, 0.0173974518, -0.7070846]) ## UDI dataset # lidar2imu_t = np.array([1.50, 0., 1.42]) # lidar2imu_r = Quaternion([1., 0., 0., 0.]) self.lidar2imu = transform_matrix(lidar2imu_t, lidar2imu_r, inverse=True) self.imu2lidar = transform_matrix(lidar2imu_t, lidar2imu_r, inverse=False) def run(self): # print(f"input points shape: {points.shape}") # num_features = 5 # self.points = points.reshape([-1, num_features]) voxels, coords, num_points = self.voxel_generator.generate(self.points) num_voxels = np.array([voxels.shape[0]], dtype=np.int64) grid_size = self.voxel_generator.grid_size coords = np.pad(coords, ((0, 0), (1, 0)), mode='constant', constant_values=0) voxels = torch.tensor(voxels, dtype=torch.float32, device=self.device) coords = torch.tensor(coords, dtype=torch.int32, device=self.device) num_points = torch.tensor( num_points, dtype=torch.int32, device=self.device) num_voxels = torch.tensor( num_voxels, dtype=torch.int32, device=self.device) # grid_size = torch.tensor(grid_size, dtype=torch.float32, device=self.device) # t = time.time() self.inputs = dict( voxels=voxels, num_points=num_points, num_voxels=num_voxels, coordinates=coords, shape=grid_size ) torch.cuda.synchronize() t = time.time() outputs = self.net(self.inputs)[0] torch.cuda.synchronize() print(" network predict time cost:", time.time() - t) outputs = remove_low_score_nu(outputs, 0.45) boxes_lidar = outputs["box3d_lidar"].detach().cpu().numpy() print(" predict boxes:", boxes_lidar.shape) scores = outputs["scores"].detach().cpu().numpy() types = outputs["label_preds"].detach().cpu().numpy() boxes_lidar[:, -1] = -boxes_lidar[:, -1] - np.pi / 2 return scores, boxes_lidar, types def get_lidar_data(self, input_points: dict): print("get one frame lidar data.") self.current_frame["lidar_stamp"] = input_points['stamp'] self.current_frame["lidar_seq"] = input_points['seq'] self.current_frame["points"] = input_points['points'].T self.lidar_deque.append(deepcopy(self.current_frame)) if len(self.lidar_deque) == 5: ref_from_car = self.imu2lidar car_from_global = transform_matrix(self.lidar_deque[-1]['translation'], self.lidar_deque[-1]['rotation'], inverse=True) ref_from_car_gpu = cp.asarray(ref_from_car) car_from_global_gpu = cp.asarray(car_from_global) for i in range(len(self.lidar_deque) - 1): last_pc = self.lidar_deque[i]['points'] last_pc_gpu = cp.asarray(last_pc) global_from_car = transform_matrix(self.lidar_deque[i]['translation'], self.lidar_deque[i]['rotation'], inverse=False) car_from_current = self.lidar2imu global_from_car_gpu = cp.asarray(global_from_car) car_from_current_gpu = cp.asarray(car_from_current) transform = reduce( cp.dot, [ref_from_car_gpu, car_from_global_gpu, global_from_car_gpu, car_from_current_gpu], ) # tmp_1 = cp.dot(global_from_car_gpu, car_from_current_gpu) # tmp_2 = cp.dot(car_from_global_gpu, tmp_1) # transform = cp.dot(ref_from_car_gpu, tmp_2) last_pc_gpu = cp.vstack((last_pc_gpu[:3, :], cp.ones(last_pc_gpu.shape[1]))) last_pc_gpu = cp.dot(transform, last_pc_gpu) self.pc_list.append(last_pc_gpu[:3, :]) current_pc = self.lidar_deque[-1]['points'] current_pc_gpu = cp.asarray(current_pc) self.pc_list.append(current_pc_gpu[:3,:]) all_pc = np.zeros((5, 0), dtype=float) for i in range(len(self.pc_list)): tmp_pc = cp.vstack((self.pc_list[i], cp.zeros((2, self.pc_list[i].shape[1])))) tmp_pc = cp.asnumpy(tmp_pc) ref_timestamp = self.lidar_deque[-1]['lidar_stamp'].to_sec() timestamp = self.lidar_deque[i]['lidar_stamp'].to_sec() tmp_pc[3, ...] = self.lidar_deque[i]['points'][3, ...] tmp_pc[4, ...] = ref_timestamp - timestamp all_pc = np.hstack((all_pc, tmp_pc)) all_pc = all_pc.T print(f" concate pointcloud shape: {all_pc.shape}") self.points = all_pc sync_cloud = xyz_array_to_pointcloud2(all_pc[:, :3], stamp=self.lidar_deque[-1]["lidar_stamp"], frame_id="lidar_top") pub_sync_cloud.publish(sync_cloud) return True def get_odom_data(self, input_odom): self.current_frame["odom_stamp"] = input_odom.header.stamp self.current_frame["odom_seq"] = input_odom.header.seq x_t = input_odom.pose.pose.position.x y_t = input_odom.pose.pose.position.y z_t = input_odom.pose.pose.position.z self.current_frame["translation"] = np.array([x_t, y_t, z_t]) x_r = input_odom.pose.pose.orientation.x y_r = input_odom.pose.pose.orientation.y z_r = input_odom.pose.pose.orientation.z w_r = input_odom.pose.pose.orientation.w self.current_frame["rotation"] = Quaternion([w_r, x_r, y_r, z_r])