def cubic_rpn_grid_pyfc(lidarPoints, rpnBoxes, method): res = [] display_stack = [] if DEBUG: print 'Start vispy ...' display_stack.append( pcd_vispy(lidarPoints, boxes=rpnBoxes, visible=False, multi_vis=True)) for box in rpnBoxes: rpn_points, min_vertex, ctr_vertex = bounding_filter(lidarPoints, box) points_mv_min = np.subtract(rpn_points, min_vertex) # using fot coordinate points_mv_ctr = np.subtract(rpn_points, ctr_vertex) # using as feature x_cub = np.divide(points_mv_min[:, 0], cfg.CUBIC_RES[0]).astype(np.int32) y_cub = np.divide(points_mv_min[:, 1], cfg.CUBIC_RES[1]).astype(np.int32) z_cub = np.divide(points_mv_min[:, 2], cfg.CUBIC_RES[2]).astype(np.int32) feature = np.hstack((np.ones([len(points_mv_ctr[:, 3]), 1]), points_mv_ctr[:, 3:])) cubic_feature = np.zeros(shape=cubic_size, dtype=np.float32) cubic_feature[ x_cub, y_cub, z_cub] = feature # TODO:select&add feature # points_mv_ctr # using center coordinate system res.append(cubic_feature) if DEBUG: box_mv = [ box[0] - box[0], box[1] - box[1], box[2] - box[2], shape(0), shape(1), shape(2), box[6], box[7], 0 ] # box_mv = [box[0] - box[0], box[1] - box[1], box[2] - box[2],cfg.ANCHOR[0], cfg.ANCHOR[1], # cfg.ANCHOR[2], box[6],box[7],0] display_stack.append( pcd_vispy(cubic_feature.reshape(-1, 4), boxes=np.array(box_mv), visible=False, multi_vis=True)) display_stack.append( pcd_vispy(points_mv_ctr.reshape(-1, 4), boxes=np.array(box_mv), visible=False, multi_vis=True)) break if DEBUG: pcd_show_now() stack_size = np.concatenate((np.array([-1]), cubic_size)) return np.array(res, dtype=np.float32).reshape(stack_size)
def cubic_rpn_grid(self, data_idx, box_idx, angel, scalar, translation,DEBUG = False): blobs = self.dataset.get_minibatch(data_idx) # get one batch lidarPoints = blobs['lidar3d_data'] rpnBoxes_ = blobs['gt_boxes_3d'][box_idx] rpnBoxes=np.array([rpnBoxes_[0],rpnBoxes_[1],rpnBoxes_[2],4.,4.,2.,rpnBoxes_[6],rpnBoxes_[7],]) box=dict({"center":rpnBoxes[0:3].reshape(1,3),"size":rpnBoxes[3:6].reshape(1,3),'cls_rpn':rpnBoxes[6].reshape(1,1)*4,"yaw":rpnBoxes[7].reshape(1,1),"score":np.ones([1,1],dtype=np.float32)}) # rpnBoxes = np.array([17.832, -3.65, -0.3726, 4, 4, 2, 0.9]) res = [] display_stack = [] if DEBUG: pass display_stack.append(pcd_vispy(lidarPoints, boxes=box, visible=False, multi_vis=True)) for iidx, box in enumerate([rpnBoxes, rpnBoxes]): rpn_points, min_vertex, ctr_vertex = bounding_filter(lidarPoints, box) points_mv_ctr = np.subtract(rpn_points, ctr_vertex) # using as feature # angel = random.rand()*np.pi*2 #[ 0~360] # scalar = 1.2 - random.rand()*0.4 # translation = np.random.rand(3, 1) * 0.5 angel = angel * 0.017453292 + rpnBoxes_[7] # angle to radius translation = np.array(translation, dtype=np.float32).reshape(3, 1) points_mv_ctr_rot_nobound = rot_sca_pc(points_mv_ctr, angel, scalar, translation) points_mv_ctr_rot, min_p, ctr_p = bounding_filter(points_mv_ctr_rot_nobound, [0, 0, 0]) x_cub = np.divide(points_mv_ctr_rot[:, 0] - min_p[0], cfg.CUBIC_RES[0]).astype(np.int32) y_cub = np.divide(points_mv_ctr_rot[:, 1] - min_p[1], cfg.CUBIC_RES[1]).astype(np.int32) z_cub = np.divide(points_mv_ctr_rot[:, 2] - min_p[2], cfg.CUBIC_RES[2]).astype(np.int32) feature = np.hstack((np.ones([len(points_mv_ctr_rot[:, 3]), 1]),points_mv_ctr_rot[:, 3].reshape(-1, 1))) # points_mv_ctr_rot cubic_feature = np.zeros(shape=cubic_size, dtype=np.float32) cubic_feature[ x_cub, y_cub, z_cub] = feature # TODO:select&add feature # points_mv_ctr # using center coordinate system res.append(cubic_feature) if DEBUG and iidx == 0: box_mv = [box[0] - box[0], box[1] - box[1], box[2] - box[2], shape(0), shape(1), shape(2), 1, 0, 1] box_gt_mv = [box[0] - box[0], box[1] - box[1], box[2] - box[2], cfg.ANCHOR[0], cfg.ANCHOR[1], cfg.ANCHOR[2], 1,0, 1] show_feature = np.hstack((x_cub.reshape(-1, 1) - (shape(0) / 2), y_cub.reshape(-1, 1) - (shape(1) / 2), z_cub.reshape(-1, 1) - (shape(2) / 2), points_mv_ctr_rot[:, 3].reshape(-1, 1))) # points_mv_ctr_rot cubic_show_feature = np.zeros(shape=cubic_show_size, dtype=np.float32) cubic_show_feature[ x_cub, y_cub, z_cub] = show_feature # TODO:select&add feature # points_mv_ctr # using center coordinate system display_stack.append( pcd_vispy(cubic_show_feature.reshape(-1, 4), name='grid_' + str(iidx), boxes=boxary2dic(np.array(box_mv).reshape(1,-1)), visible=False, point_size=0.02, multi_vis=True)) display_stack.append( pcd_vispy(points_mv_ctr.reshape(-1, 4), name='origin_' + str(iidx), boxes=boxary2dic(np.array(box_gt_mv).reshape(1,-1)), visible=False, point_size=0.02, multi_vis=True)) if DEBUG: pcd_show_now() stack_size = np.concatenate((np.array([-1]), cubic_size)) return {self.cube:np.array(res, dtype=np.float32).reshape(stack_size), self.index:np.array(angel,dtype=np.float32).reshape(1)}
def cubic_rpn_grid_pyfc(lidarPoints, rpnBoxes,method): # rpnBoxes:(x1,y1,z1),(x2,y2,z2),cls_label,yaw res = [] display_stack=[] if DEBUG: pass display_stack.append(pcd_vispy(lidarPoints, boxes=boxary2dic(rpnBoxes),visible=False,multi_vis=True)) rpn_new_yaw=[] for iidx,box in enumerate(rpnBoxes): rpn_points,min_vertex,ctr_vertex = bounding_filter(lidarPoints,box) points_mv_min = np.subtract(rpn_points,min_vertex) # using fot coordinate points_mv_ctr = np.subtract(rpn_points,ctr_vertex) # using as feature if method =='train' and cfg.TRAIN.USE_AUGMENT_IN_CUBIC_GEN: angel = (random.rand()-0.500) * np.pi * 0.9# *np.pi*2 #[0~360]#TODO: check ;to change 0.01->big scalar # angel = 0.7854 #counter clockwise rotation scalar = 1.05 - random.rand()*0.1 translation = np.random.rand(3, 1) * 0.01 points_mv_ctr_rot_nobound = rot_sca_pc(points_mv_ctr,angel,scalar,translation) points_mv_ctr_rot,min_p,ctr_p = bounding_filter(points_mv_ctr_rot_nobound, [0,0,0]) x_cub = np.divide(points_mv_ctr_rot[:,0] - min_p[0], cfg.CUBIC_RES[0]).astype(np.int32) y_cub= np.divide(points_mv_ctr_rot[:,1] - min_p[1], cfg.CUBIC_RES[1]).astype(np.int32) z_cub = np.divide(points_mv_ctr_rot[:,2] - min_p[2], cfg.CUBIC_RES[2]).astype(np.int32) if not DEBUG: feature = np.hstack((np.ones([len(points_mv_ctr_rot[:,3]),1]),points_mv_ctr_rot[:,3].reshape(-1,1))) #points_mv_ctr_rot else: feature = np.hstack((x_cub.reshape(-1,1)-(cfg.CUBIC_SIZE[0]/2),y_cub.reshape(-1,1)-(cfg.CUBIC_SIZE[1]/2),z_cub.reshape(-1,1)-(cfg.CUBIC_SIZE[2]/2),points_mv_ctr_rot[:,3].reshape(-1,1))) #points_mv_ctr_rot else: # method: test angel = 0.0 x_cub = np.divide(points_mv_min[:, 0], cfg.CUBIC_RES[0]).astype(np.int32) y_cub = np.divide(points_mv_min[:, 1], cfg.CUBIC_RES[1]).astype(np.int32) z_cub = np.divide(points_mv_min[:, 2], cfg.CUBIC_RES[2]).astype(np.int32) feature = np.hstack((np.ones([len(points_mv_ctr[:,3]),1]),points_mv_ctr[:,3:])) rpn_new_yaw.append(angel)# gt_yaw - rotation: because gt is clockwise and rotation is counter clockwise#TODO hxd:check cubic_feature = np.zeros(shape=cubic_size, dtype=np.float32) cubic_feature[x_cub, y_cub, z_cub] = feature# TODO:select&add feature # points_mv_ctr # using center coordinate system res.append(cubic_feature) if DEBUG: box_mv = [box[0] - box[0], box[1] - box[1], box[2] - box[2],cfg.CUBIC_SIZE[0], cfg.CUBIC_SIZE[1],cfg.CUBIC_SIZE[2],1,0,0] box_gt_mv = [box[0] - box[0], box[1] - box[1], box[2] - box[2], cfg.ANCHOR[0], cfg.ANCHOR[1], cfg.ANCHOR[2],1, 0, 0] display_stack.append(pcd_vispy(cubic_feature.reshape(-1, 4),name='grid_'+str(iidx), boxes=boxary2dic(np.array(box_mv)),visible=False,point_size =0.1,multi_vis=True)) display_stack.append(pcd_vispy(points_mv_ctr.reshape(-1, 4),name='origin_'+str(iidx), boxes=boxary2dic(np.array(box_gt_mv)),visible=False,point_size =0.1,multi_vis=True)) # break if DEBUG: pcd_show_now() stack_size = np.concatenate((np.array([-1]), cubic_size)) return np.array(res, dtype=np.float32).reshape(stack_size),np.array(rpn_new_yaw,dtype=np.float32)
def boundingbox_filter(self, lidarPoints, gt_box): def bounding_filter(points, box): # one box: type,xyz,lwh,yaw, [score,reserve1,reserve2] x_min = box[1] - arg.anchor[0] / 2. x_max = box[1] + arg.anchor[0] / 2. y_min = box[2] - arg.anchor[1] / 2. y_max = box[2] + arg.anchor[1] / 2. z_min = box[3] - arg.anchor[2] / 2. z_max = box[3] + arg.anchor[2] / 2. x_points = points[:, 0] y_points = points[:, 1] z_points = points[:, 2] f_filt = np.logical_and((x_points > x_min), (x_points < x_max)) s_filt = np.logical_and((y_points > y_min), (y_points < y_max)) z_filt = np.logical_and((z_points > z_min), (z_points < z_max)) fliter = np.logical_and(np.logical_and(f_filt, s_filt), z_filt) indice = np.flatnonzero(fliter) filter_points = points[indice] return filter_points, np.array([x_min, y_min, z_min, 0.], dtype=np.float32), np.array( [box[1], box[2], box[3], 0.], dtype=np.float32) # one box: type,xyz,lwh,yaw, [score,reserve1,reserve2] cube_points = [] cube_points_label = [] display_stack=[] for iidx,box in enumerate(gt_box): bounding_points,min_vertex,ctr_vertex = bounding_filter(lidarPoints,box) bounding_ctr_points=bounding_points - ctr_vertex cube_points.append(bounding_ctr_points) cube_points_label.append(box) if DEBUG: display_stack.append(pcd_vispy_standard(bounding_ctr_points,name='points_'+str(iidx), boxes=box-[0,box[1],box[2],box[3],0,0,0,0],visible=False,point_size =0.1,multi_vis=True)) # break if DEBUG: display_stack.append(pcd_vispy_standard([lidarPoints,self.conners],name="lidar full scope", boxes=gt_box,visible=False,multi_vis=True)) pcd_show_now() return cube_points,cube_points_label
def box_np_view(data1, data2=None): stack = [] data1 = data1.reshape([30, 30, 15, 1]) coordinate1 = np.array(np.where(data1[:, :, :, 0] == 1)).transpose(1, 0) coordinate1 -= [ cfg.CUBIC_SIZE[0] / 2, cfg.CUBIC_SIZE[1] / 2, cfg.CUBIC_SIZE[2] / 2 ] if data2 is not None: data2 = data2.reshape([30, 30, 15, 1]) coordinate2 = np.array(np.where(data2[:, :, :, 0] == 1)).transpose(1, 0) coordinate2 -= [ cfg.CUBIC_SIZE[0] / 2, cfg.CUBIC_SIZE[1] / 2, cfg.CUBIC_SIZE[2] / 2 ] stack.append( pcd_vispy(coordinate2, name='WINDOW:2', boxes=boxary2dic( np.array([ 0, 0, 0, cfg.CUBIC_SIZE[0], cfg.CUBIC_SIZE[1], cfg.CUBIC_SIZE[2], 0, 0 ])), point_size=0.1, visible=False, multi_vis=True)) stack.append( pcd_vispy(coordinate1, name='WINDOW:1', boxes=boxary2dic( np.array([ 0, 0, 0, cfg.CUBIC_SIZE[0], cfg.CUBIC_SIZE[1], cfg.CUBIC_SIZE[2], 0, 0 ])), point_size=0.1, visible=False, multi_vis=True)) pcd_show_now()
def cubic_rpn_grid_pyfc(lidarPoints, rpnBoxes): x_points = lidarPoints[:, 0] y_points = lidarPoints[:, 1] z_points = lidarPoints[:, 2] # reflectance = lidarPoints[:, 3] if DEBUG: print 'Start vispy ...' display_stack = [pcd_vispy(lidarPoints, boxes=rpnBoxes, visible=False)] res = [] for box in rpnBoxes: x_min = box[1] - float(cfg.ANCHOR[0]) / 2 x_max = box[1] + float(cfg.ANCHOR[0]) / 2 y_min = box[2] - float(cfg.ANCHOR[1]) / 2 y_max = box[2] + float(cfg.ANCHOR[1]) / 2 z_min = box[3] - float(cfg.ANCHOR[2]) / 2 z_max = box[3] + float(cfg.ANCHOR[2]) / 2 f_filt = np.logical_and((x_points > x_min), (x_points < x_max)) s_filt = np.logical_and((y_points > y_min), (y_points < y_max)) z_filt = np.logical_and((z_points > z_min), (z_points < z_max)) fliter = np.logical_and(np.logical_and(f_filt, s_filt), z_filt) indice = np.flatnonzero(fliter) rpn_points = lidarPoints[indice] points_mv_min = np.subtract( rpn_points, np.array([x_min, y_min, z_min, 0.], dtype=np.float32)) # using fot coordinate points_mv_ctr = np.subtract(rpn_points, np.array( [box[1], box[2], box[3], 0.], dtype=np.float32)) # using as feature xi = points_mv_min[:, 0] yi = points_mv_min[:, 1] zi = points_mv_min[:, 2] x_cub = np.divide(xi, cfg.CUBIC_RES[0]).astype(np.int32) y_cub = np.divide(yi, cfg.CUBIC_RES[1]).astype(np.int32) z_cub = np.divide(zi, cfg.CUBIC_RES[2]).astype(np.int32) cubic_feature = np.ones(shape=cubic_size, dtype=np.float32) # a= points_mv_ctr[:,3:] feature = np.hstack((np.ones([len(indice), 1]), points_mv_ctr[:, 3:])) cubic_feature[ x_cub, y_cub, z_cub] = points_mv_ctr # TODO:select&add feature # points_mv_ctr # using center coordinate system res.append(cubic_feature) if DEBUG: box_mv = [ box[0], box[1] - box[1], box[2] - box[2], box[3] - box[3], cfg.ANCHOR[0], cfg.ANCHOR[1], cfg.ANCHOR[2], box[7], 0 ] display_stack.append( pcd_vispy(cubic_feature.reshape(-1, 4), boxes=np.array(box_mv), visible=False)) if DEBUG: pcd_show_now() stack_size = np.concatenate((np.array([-1]), cubic_size)) return np.array(res, dtype=np.float32).reshape(stack_size)