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)