def remove_occluded_points(self, bg_points, gt_boxes, fg_points_list,
sampled_boxes, sampled_points_list):
bg_points_spherical = box_np_ops.convert_to_spherical_coord(bg_points)
boxes = np.concatenate((gt_boxes, sampled_boxes), axis=0)
boxes_points = fg_points_list + sampled_points_list
boxes_points_spherical = box_np_ops.convert_to_spherical_coord_list(
boxes_points)
ascending_idx = sorted(range(len(boxes)), key=lambda x: boxes[x, 0])
locs = boxes[:, 0:3]
dims = boxes[:, 3:6]
angles = boxes[:, 6]
camera_box_origin = [0.5, 0.5, 0]
box_corners = box_np_ops.center_to_corner_box3d(locs,
dims,
angles,
camera_box_origin,
axis=2)
box_corners_spherical = box_np_ops.convert_to_spherical_coord(
box_corners)
scale_factor = 100
box_corners_spherical *= scale_factor
boxes_points_spherical = [
x * scale_factor for x in boxes_points_spherical
]
bg_points_spherical *= scale_factor
exclude_idx = []
for idx in ascending_idx[:-1]:
exclude_idx.append(idx)
for i, box_points in enumerate(boxes_points_spherical):
if i in exclude_idx:
continue
# remove_mask = in_hull(box_points[:, :2], ConvexHull(box_corners_spherical[idx][:, :2], qhull_options='QJ').points)
remove_mask = in_hull(box_points[:, :2],
box_corners_spherical[idx][:, :2])
boxes_points_spherical[i] = box_points[np.logical_not(
remove_mask)]
remained_boxes_idx = np.zeros((boxes.shape[0]), dtype=bool)
for i, box_points in reversed(list(enumerate(boxes_points_spherical))):
if i < len(fg_points_list):
if box_points.shape[0] > 8:
remained_boxes_idx[i] = True
bg_points_spherical = np.concatenate(
[bg_points_spherical, box_points], axis=0)
else:
if box_points.shape[0] > 8:
remained_boxes_idx[i] = True
# remove_mask = in_hull(bg_points_spherical[:, :2], ConvexHull(box_points[:, :2], qhull_options='QJ').points)
remove_mask = in_hull(bg_points_spherical[:, :2],
box_points[:, :2])
bg_points_spherical = bg_points_spherical[np.logical_not(
remove_mask)]
bg_points_spherical = np.concatenate(
[bg_points_spherical, box_points], axis=0)
bg_points_spherical /= scale_factor
points = box_np_ops.convert_to_cartesian_coord(bg_points_spherical)
return points, remained_boxes_idx
def compute_3dbox_corners(box):
centers = box[:3]
dims = box[3:6]
angles = np.array([box[6]])
corners_3d = box_np_ops.center_to_corner_box3d(centers.reshape([1, 3]),
dims.reshape([1, 3]),
angles,
origin=(0.5, 0.5, 0.5),
axis=2)
result = np.squeeze(corners_3d)
# print(result)
# return result
return np.transpose(result)
def box3d_t_2d(box3d, P2):
from second.core.box_np_ops import center_to_corner_box3d, project_to_image
locs = box3d[:, :3]
dims = box3d[:, 3:6]
angles = box3d[:,6]
camera_box_origin = [0.5,1.0,0.5]
box_corners_3d = center_to_corner_box3d(
locs, dims, angles, camera_box_origin, axis=1)
box_corners_in_image = project_to_image(box_corners_3d, P2)
minxy = np.min(box_corners_in_image, axis=1)
maxxy = np.max(box_corners_in_image, axis=1)
box_2d_preds = np.concatenate([minxy, maxxy], axis=1)
return box_2d_preds
def box3d_to_bbox(box3d, rect, Trv2c, P2):
box3d_lidar = box3d.copy()
box3d_lidar[:, 2] -= box3d_lidar[:, 5] / 2
box3d_camera = box_lidar_to_camera(box3d_lidar, rect, Trv2c)
box_corners = center_to_corner_box3d(box3d_camera[:, :3],
box3d_camera[:, 3:6],
box3d_camera[:, 6], [0.5, 1.0, 0.5],
axis=1)
box_corners_in_image = project_to_image(box_corners, P2)
# box_corners_in_image: [N, 8, 2]
minxy = np.min(box_corners_in_image, axis=1)
maxxy = np.max(box_corners_in_image, axis=1)
bbox = np.concatenate([minxy, maxxy], axis=1)
return bbox
def draw_detection(self, detection_anno, label_color=GLColor.Blue):
dt_box_color = self.w_config.get("DTBoxColor")[:3]
dt_box_color = (*dt_box_color, self.w_config.get("DTBoxAlpha"))
dt_box_lidar = np.array(
[detection_anno["box3d_lidar"].detach().cpu().numpy()])[0]
scores = np.array([detection_anno["scores"].detach().cpu().numpy()])[0]
# filter by score
keep_list = np.where(scores > 0.2)[0]
dt_box_lidar = dt_box_lidar[keep_list, :]
scores = scores[keep_list]
dt_boxes_corners = box_np_ops.center_to_corner_box3d(
dt_box_lidar[:, :3],
dt_box_lidar[:, 3:6],
dt_box_lidar[:, 6],
origin=[0.5, 0.5, 0],
axis=2)
# filter bbox by its center
centers = (dt_boxes_corners[:, 0, :] + dt_boxes_corners[:, 6, :]) / 2
keep_list = np.where((centers[:, 0] < self.points_range[0]) & (centers[:, 0] > self.points_range[3]) & \
(centers[:, 1] < self.points_range[1]) & (centers[:, 1] > self.points_range[4]) & \
(centers[:, 2] < self.points_range[2]) & (centers[:, 2] > self.points_range[5]))[0]
dt_boxes_corners = dt_boxes_corners[keep_list, :, :]
dt_box_lidar = dt_box_lidar[keep_list, :]
scores = scores[keep_list]
num_dt = dt_box_lidar.shape[0]
self.info('num_dt', num_dt)
if num_dt != 0:
for ind in range(num_dt):
self.info('scores', scores[ind])
self.info('dt_box_lidar', dt_box_lidar[ind])
dt_box_color = np.tile(
np.array(dt_box_color)[np.newaxis, ...], [num_dt, 1])
scores_rank = scores / scores[0]
# if self.w_config.get("DTScoreAsAlpha") and scores is not None:
# dt_box_color = np.concatenate([dt_box_color[:, :3], scores[..., np.newaxis]], axis=1)
dt_box_color = np.concatenate(
[dt_box_color[:, :3], scores_rank[..., np.newaxis]], axis=1)
# dt_box_color = np.concatenate([dt_box_color[:, :3], np.ones((scores[..., np.newaxis].shape))], axis=1)
self.w_pc_viewer.boxes3d("dt_boxes", dt_boxes_corners,
dt_box_color,
self.w_config.get("DTBoxLineWidth"), 1.0)
def kitti_anno_to_corners(info, annos=None):
rect = info['calib/R0_rect']
P2 = info['calib/P2']
Tr_velo_to_cam = info['calib/Tr_velo_to_cam']
if annos is None:
annos = info['annos']
dims = annos['dimensions']
loc = annos['location']
rots = annos['rotation_y']
scores = None
if 'score' in annos:
scores = annos['score']
boxes_camera = np.concatenate([loc, dims, rots[..., np.newaxis]], axis=1)
boxes_lidar = box_np_ops.box_camera_to_lidar(boxes_camera, rect,
Tr_velo_to_cam)
boxes_corners = box_np_ops.center_to_corner_box3d(boxes_lidar[:, :3],
boxes_lidar[:, 3:6],
boxes_lidar[:, 6],
origin=[0.5, 0.5, 0],
axis=2)
return boxes_corners, scores, boxes_lidar
def noise_per_object_v2_(gt_boxes,
points=None,
valid_mask=None,
rotation_perturb=np.pi / 4,
center_noise_std=1.0,
global_random_rot_range=np.pi / 4,
num_try=100):
"""random rotate or remove each groundtrutn independently.
use kitti viewer to test this function points_transform_
Args:
gt_boxes: [N, 7], gt box in lidar.points_transform_
points: [M, 4], point cloud in lidar.
"""
num_boxes = gt_boxes.shape[0]
if not isinstance(rotation_perturb, (list, tuple, np.ndarray)):
rotation_perturb = [-rotation_perturb, rotation_perturb]
if not isinstance(global_random_rot_range, (list, tuple, np.ndarray)):
global_random_rot_range = [
-global_random_rot_range, global_random_rot_range
]
if not isinstance(center_noise_std, (list, tuple, np.ndarray)):
center_noise_std = [
center_noise_std, center_noise_std, center_noise_std
]
if valid_mask is None:
valid_mask = np.ones((num_boxes, ), dtype=np.bool_)
center_noise_std = np.array(center_noise_std, dtype=gt_boxes.dtype)
loc_noises = np.random.normal(scale=center_noise_std,
size=[num_boxes, num_try, 3])
# loc_noises = np.random.uniform(
# -center_noise_std, center_noise_std, size=[num_boxes, num_try, 3])
rot_noises = np.random.uniform(rotation_perturb[0],
rotation_perturb[1],
size=[num_boxes, num_try])
gt_grots = np.arctan2(gt_boxes[:, 0], gt_boxes[:, 1])
grot_lowers = global_random_rot_range[0] - gt_grots
grot_uppers = global_random_rot_range[1] - gt_grots
global_rot_noises = np.random.uniform(grot_lowers[..., np.newaxis],
grot_uppers[..., np.newaxis],
size=[num_boxes, num_try])
origin = [0.5, 0.5, 0]
gt_box_corners = box_np_ops.center_to_corner_box3d(gt_boxes[:, :3],
gt_boxes[:, 3:6],
gt_boxes[:, 6],
origin=origin,
axis=2)
if np.abs(global_random_rot_range[0] - global_random_rot_range[1]) < 1e-3:
selected_noise = noise_per_box(gt_boxes[:, [0, 1, 3, 4, 6]],
valid_mask, loc_noises, rot_noises)
else:
selected_noise = noise_per_box_v2_(gt_boxes[:, [0, 1, 3, 4, 6]],
valid_mask, loc_noises, rot_noises,
global_rot_noises)
loc_transforms = _select_transform(loc_noises, selected_noise)
rot_transforms = _select_transform(rot_noises, selected_noise)
if points is not None:
surfaces = box_np_ops.corner_to_surfaces_3d_jit(gt_box_corners)
point_masks = points_in_convex_polygon_3d_jit(points[:, :3], surfaces)
points_transform_(points, gt_boxes[:, :3], point_masks, loc_transforms,
rot_transforms, valid_mask)
box3d_transform_(gt_boxes, loc_transforms, rot_transforms, valid_mask)
def convert_detection_to_kitti_annos(self, detection):
class_names = self._class_names
det_image_idxes = [det["metadata"]["image_idx"] for det in detection]
gt_image_idxes = [
info["image"]["image_idx"] for info in self._kitti_infos
]
annos = []
for i in range(len(detection)):
det_idx = det_image_idxes[i]
det = detection[i]
# info = self._kitti_infos[gt_image_idxes.index(det_idx)]
info = self._kitti_infos[i]
calib = info["calib"]
rect = calib["R0_rect"]
Trv2c = calib["Tr_velo_to_cam"]
P2 = calib["P2"]
final_box_preds = det["box3d_lidar"].detach().cpu().numpy()
label_preds = det["label_preds"].detach().cpu().numpy()
scores = det["scores"].detach().cpu().numpy()
if final_box_preds.shape[0] != 0:
final_box_preds[:, 2] -= final_box_preds[:, 5] / 2
box3d_camera = box_np_ops.box_lidar_to_camera(
final_box_preds, rect, Trv2c)
locs = box3d_camera[:, :3]
dims = box3d_camera[:, 3:6]
angles = box3d_camera[:, 6]
camera_box_origin = [0.5, 1.0, 0.5]
box_corners = box_np_ops.center_to_corner_box3d(
locs, dims, angles, camera_box_origin, axis=1)
box_corners_in_image = box_np_ops.project_to_image(
box_corners, P2)
# box_corners_in_image: [N, 8, 2]
minxy = np.min(box_corners_in_image, axis=1)
maxxy = np.max(box_corners_in_image, axis=1)
bbox = np.concatenate([minxy, maxxy], axis=1)
anno = kitti.get_start_result_anno()
num_example = 0
box3d_lidar = final_box_preds
for j in range(box3d_lidar.shape[0]):
image_shape = info["image"]["image_shape"]
if bbox[j, 0] > image_shape[1] or bbox[j, 1] > image_shape[0]:
continue
if bbox[j, 2] < 0 or bbox[j, 3] < 0:
continue
bbox[j, 2:] = np.minimum(bbox[j, 2:], image_shape[::-1])
bbox[j, :2] = np.maximum(bbox[j, :2], [0, 0])
anno["bbox"].append(bbox[j])
# convert center format to kitti format
# box3d_lidar[j, 2] -= box3d_lidar[j, 5] / 2
anno["alpha"].append(
-np.arctan2(-box3d_lidar[j, 1], box3d_lidar[j, 0]) +
box3d_camera[j, 6])
anno["dimensions"].append(box3d_camera[j, 3:6])
anno["location"].append(box3d_camera[j, :3])
anno["rotation_y"].append(box3d_camera[j, 6])
anno["name"].append(class_names[int(label_preds[j])])
anno["truncated"].append(0.0)
anno["occluded"].append(0)
anno["score"].append(scores[j])
num_example += 1
if num_example != 0:
anno = {n: np.stack(v) for n, v in anno.items()}
annos.append(anno)
else:
annos.append(kitti.empty_result_anno())
num_example = annos[-1]["name"].shape[0]
annos[-1]["metadata"] = det["metadata"]
return annos
def noise_per_object_v3_(gt_boxes,
points=None,
valid_mask=None,
rotation_perturb=np.pi / 4,
center_noise_std=1.0,
global_random_rot_range=np.pi / 4,
num_try=100,
group_ids=None):
"""random rotate or remove each groundtruth independently.
use kitti viewer to test this function points_transform_
Args:
gt_boxes: [N, 7], gt box in lidar.points_transform_
points: [M, 4], point cloud in lidar.
"""
num_boxes = gt_boxes.shape[0]
if not isinstance(rotation_perturb, (list, tuple, np.ndarray)):
rotation_perturb = [-rotation_perturb, rotation_perturb]
if not isinstance(global_random_rot_range, (list, tuple, np.ndarray)):
global_random_rot_range = [
-global_random_rot_range, global_random_rot_range
]
enable_grot = np.abs(global_random_rot_range[0] - # False
global_random_rot_range[1]) >= 1e-3
if not isinstance(center_noise_std, (list, tuple, np.ndarray)):
center_noise_std = [
center_noise_std, center_noise_std, center_noise_std
]
if valid_mask is None:
valid_mask = np.ones((num_boxes, ), dtype=np.bool_)
center_noise_std = np.array(center_noise_std, dtype=gt_boxes.dtype)
loc_noises = np.random.normal( # 定位扰动服从正态分布，每个对象有num_try个扰动数据，每个数据包含三个方向
scale=center_noise_std,
size=[num_boxes, num_try, 3])
# loc_noises = np.random.uniform(
# -center_noise_std, center_noise_std, size=[num_boxes, num_try, 3])
rot_noises = np.random.uniform(rotation_perturb[0],
rotation_perturb[1],
size=[num_boxes, num_try
]) # 在范围内均匀分布,每个对象都有num_try个旋转角扰动
gt_grots = np.arctan2(gt_boxes[:, 0],
gt_boxes[:, 1]) # 根据中心点激光雷达下前后与左右坐标正切值，求绝对转角
grot_lowers = global_random_rot_range[0] - gt_grots
grot_uppers = global_random_rot_range[1] - gt_grots
global_rot_noises = np.random.uniform( # 叠加全局旋转角扰动后的绝对转角扰动，由于全局转角扰动范围为[0.0,0.0]，单个对象绝对转角扰动保持不变
grot_lowers[..., np.newaxis],
grot_uppers[..., np.newaxis],
size=[num_boxes, num_try])
if group_ids is not None:
if enable_grot:
set_group_noise_same_v2_(loc_noises, rot_noises, global_rot_noises,
group_ids)
else:
set_group_noise_same_(loc_noises, rot_noises, group_ids)
group_centers, group_id_num_dict = get_group_center(
gt_boxes[:, :3], group_ids)
if enable_grot:
group_transform_v2_(loc_noises, rot_noises, gt_boxes[:, :3],
gt_boxes[:, 6], group_centers,
global_rot_noises, valid_mask)
else:
group_transform_(loc_noises, rot_noises, gt_boxes[:, :3],
gt_boxes[:, 6], group_centers, valid_mask)
group_nums = np.array(list(group_id_num_dict.values()), dtype=np.int64)
origin = [0.5, 0.5, 0] # 激光雷达坐标系下原点
# 真值框信息转换为8个角点[N,8,3](左前下，左前上，左后上，左后下，右前下，右前上，右后上，右后下)
gt_box_corners = box_np_ops.center_to_corner_box3d(gt_boxes[:, :3],
gt_boxes[:, 3:6],
gt_boxes[:, 6],
origin=origin,
axis=2)
if group_ids is not None: # None
if not enable_grot:
selected_noise = noise_per_box_group(gt_boxes[:, [0, 1, 3, 4, 6]],
valid_mask, loc_noises,
rot_noises, group_nums)
else:
selected_noise = noise_per_box_group_v2_(
gt_boxes[:, [0, 1, 3, 4, 6]], valid_mask, loc_noises,
rot_noises, group_nums, global_rot_noises)
else:
if not enable_grot: # True
selected_noise = noise_per_box(gt_boxes[:, [0, 1, 3, 4, 6]],
valid_mask, loc_noises,
rot_noises) # 每一个真值不会碰撞的定位和旋转扰动索引
else:
selected_noise = noise_per_box_v2_(gt_boxes[:, [0, 1, 3, 4, 6]],
valid_mask, loc_noises,
rot_noises, global_rot_noises)
loc_transforms = _select_transform(loc_noises, selected_noise) # 根据索引选择扰动
rot_transforms = _select_transform(rot_noises, selected_noise)
surfaces = box_np_ops.corner_to_surfaces_3d_jit(
gt_box_corners) # [N,6,4,3],6个面,每个面4个点
if points is not None:
point_masks = points_in_convex_polygon_3d_jit(
points[:, :3], surfaces) # 判断此帧点云的点是否在表面内
points_transform_(
points,
gt_boxes[:, :3],
point_masks,
loc_transforms, # 对添加了扰动的真值框内的点云坐标进行修改
rot_transforms,
valid_mask)
box3d_transform_(gt_boxes, loc_transforms, rot_transforms,
valid_mask) # 对添加了扰动的真值框坐标进行修改
# #####***** 加载车道bmp *****#####
bmp_path = r'/data/station_2_lane.bmp'
img_cv2 = cv2.imread(bmp_path)
imgg = cv2.cvtColor(img_cv2, cv2.COLOR_BGR2RGB)
bmp_img = cache_bmp(imgg)
for i in range(100):
box_file = box_adr + str(my_count) + '.npy'
lidar_file = lidar_adr + str(my_count) + '.npy'
my_count += 1
box = np.load(box_file)
point = np.load(lidar_file)
corners = box_np_ops.center_to_corner_box3d(box[:, :3],
box[:, 3:6],
box[:, 6],
origin=[0.5, 0.5, 0.5],
axis=2)
start_time = time.time()
# new_box = fix_cluster(box, bmp_img)
new_box = fix_cluster(box)
print('the running time is : ', (time.time() - start_time) * 10)
new_box[:, 1] += 1
new_corners = box_np_ops.center_to_corner_box3d(new_box[:, :3],
new_box[:, 3:6],
new_box[:, 6],
origin=[0.5, 0.5, 0.5],
axis=2)
line_box = np.array([[0, 1], [1, 2], [0, 3], [2, 3], [4, 5], [4, 7],
[5, 6], [6, 7], [0, 4], [1, 5], [2, 6], [3, 7]])
def PointPillarsInference(inference_ctx, points, points_range, scale_up,
shift_x, shift_z):
if points.shape[0] < 100:
return None
inputs = inference_ctx.get_inference_input_dict(points)
with inference_ctx.ctx():
predictions_dicts = inference_ctx.inference(inputs)
# print(predictions_dicts)
detection_anno = predictions_dicts[0]
if detection_anno["box3d_lidar"] is None:
return None
dt_box_lidar = np.array(
[detection_anno["box3d_lidar"].detach().cpu().numpy()])[0]
scores = np.array([detection_anno["scores"].detach().cpu().numpy()])[0]
# print(scores)
# filter by score
keep_list = np.where(scores > 0.4)[0]
dt_box_lidar = dt_box_lidar[keep_list, :]
scores = scores[keep_list]
dt_box_lidar[:, :6] /= scale_up
dt_box_lidar[:, 1] += shift_x
dt_box_lidar[:, 2] -= -0.12
dt_box_lidar_big = dt_box_lidar.copy()
# dt_box_lidar_big[:, 4] *= 1.1 # length
dt_box_lidar_big[:, 3] *= 1.1 # width
# dt_box_lidar[:, 4] *= 1.1 # length
dt_box_lidar[:, 3] *= 1.2 # width
dt_boxes_corners = box_np_ops.center_to_corner_box3d(dt_box_lidar[:, :3],
dt_box_lidar[:, 3:6],
dt_box_lidar[:, 6],
origin=[0.5, 0.5, 0],
axis=2)
dt_boxes_corners_big = box_np_ops.center_to_corner_box3d(
dt_box_lidar_big[:, :3],
dt_box_lidar_big[:, 3:6],
dt_box_lidar_big[:, 6],
origin=[0.5, 0.5, 0],
axis=2)
# filter bbox by its center
centers = dt_box_lidar[:, :3]
keep_list = np.where((centers[:, 0] < points_range[0]) & (centers[:, 0] > points_range[3]) & \
(centers[:, 1] < points_range[1]) & (centers[:, 1] > points_range[4]))[0]
dt_boxes_corners = dt_boxes_corners[keep_list, :, :]
dt_boxes_corners_big = dt_boxes_corners_big[keep_list, :]
# dt_box_lidar = dt_box_lidar[keep_list]
scores = scores[keep_list]
num_dt = dt_boxes_corners.shape[0]
if num_dt == 0:
print('miss')
return None
boxes_select = np.zeros((18, 3))
boxes_select[0:8, :] = dt_boxes_corners[0, :, :] # tight bbox
boxes_select[8:16, :] = dt_boxes_corners_big[0, :, :] # big bbox
boxes_select[16, :] = dt_box_lidar[keep_list[0], :3] # center position
boxes_select[17, 0] = dt_box_lidar[keep_list[0], 6] # orientation
print('scores: ', scores, 'angles: ', dt_box_lidar[keep_list, 6])
# print(dt_box_lidar[keep_list[0], 6])
return boxes_select
def main():
np.set_printoptions(threshold=np.inf)
# pc_file = '/home/lucerna/MEGA/project/AVP/2427439726050.npy'
# pc = np.transpose(np.load(pc_file))
context_cloud = zmq.Context()
socket_cloud = context_cloud.socket(zmq.SUB)
socket_cloud.setsockopt(zmq.SUBSCRIBE, b"")
socket_cloud.setsockopt(zmq.RCVHWM, 1)
socket_cloud.connect("tcp://localhost:5556")
print("Collecting point clouds...")
context_result = zmq.Context()
socket_result = context_result.socket(zmq.SUB)
socket_result.setsockopt(zmq.SUBSCRIBE, b"")
socket_result.setsockopt(zmq.RCVHWM, 1)
socket_result.connect("tcp://localhost:5560")
print("Collecting inference...")
context_odom = zmq.Context()
socket_odom = context_odom.socket(zmq.SUB)
socket_odom.setsockopt(zmq.SUBSCRIBE, b"")
socket_odom.setsockopt(zmq.RCVHWM, 1)
print("Collecting odom info...")
socket_odom.connect("tcp://192.168.1.2:5559")
context_box = zmq.Context()
socket_box = context_box.socket(zmq.PUB)
socket_box.setsockopt(zmq.SNDHWM, 1)
socket_box.bind("tcp://*:5557")
print('Sending bbox')
context_grid = zmq.Context()
socket_grid = context_grid.socket(zmq.PUB)
socket_grid.setsockopt(zmq.SNDHWM, 1)
socket_grid.bind("tcp://*:5558")
print('Sending occupancy grid')
x_clip = np.array([0, 1.5])
y_clip = np.array([-1.5, 1.5])
z_clip = 0.14
grid_res = 100
object_res = 50
# create occupancy grid
dim_x = int((x_clip[1] - x_clip[0]) * grid_res)
dim_y = int((y_clip[1] - y_clip[0]) * grid_res)
dim_x_object = int((x_clip[1] - x_clip[0]) * object_res)
dim_y_object = int((y_clip[1] - y_clip[0]) * object_res)
object_matrix = np.zeros([dim_x_object, dim_y_object])
object_matrix_inflated = object_matrix.copy()
car_matrix = np.zeros([dim_x_object, dim_y_object])
car_matrix_object = np.zeros([dim_x_object, dim_y_object])
car_peri_coords_x = []
car_peri_coords_y = []
pc_grid = np.zeros((1, 2))
pc_in = None
inference_in = None
odom_info = None
last_orientation = 0
last_postion = np.zeros((1, 3))
last_time = 0
flip_count = 0
detection_count = 1
calibration_count = 10
filter_flag = 0
first_time = 1
point_update = 0
mu_now = np.zeros((4, ))
mu_pre = np.zeros((4, ))
z_now = np.zeros((4, ))
z_pre = np.zeros((4, ))
odom_now = np.zeros((4, ))
odom_pre = np.zeros((4, ))
while True:
if socket_cloud.poll(timeout=5) != 0:
# add objects in the grid
point_update = 1
pc_in = np.transpose(recv_array(socket_cloud))
pc = pc_in[np.where((pc_in[:, 1] > y_clip[0])
& (pc_in[:, 1] < y_clip[1]))]
pc = pc[np.where(pc[:, 0] < x_clip[1])]
pc[:, 2] += -z_clip
pc = pc[np.where((pc[:, 2] > 0))]
pc = pc[np.where((pc[:, 3] > 150))]
pc_grid = pc[:, 0:2]
pc_grid[:, 0] = (np.floor(pc_grid[:, 0] * object_res))
pc_grid[:, 1] = (np.floor(pc_grid[:, 1] * object_res) +
dim_y_object / 2)
pc_grid = pc_grid.astype(int)
if pc_grid.shape[0] > 1:
object_matrix = np.zeros([dim_x_object, dim_y_object])
pc_grid, counts = np.unique(pc_grid,
return_counts=True,
axis=0)
pc_grid = pc_grid[np.where(counts > grid_res / object_res)]
object_matrix[pc_grid[:, 0], pc_grid[:, 1]] = 1
object_matrix_inflated = object_matrix.copy()
if socket_result.poll(timeout=5) != 0:
inference_in = recv_array(socket_result)
if inference_in[1, 2] == 1:
first_time = 0
dt_box_lidar = inference_in[0, :].copy()
if len(dt_box_lidar.shape) == 1:
dt_box_lidar = np.expand_dims(dt_box_lidar, axis=0)
# dt_box_lidar[:, 3] *= 1.2 # width
# dt_box_lidar[:, 4] *= 1.1 # length
dt_box_lidar[:, 3] = 0.28 # width
dt_box_lidar[:, 4] = 0.57 # length
# if we encounter a lost of track, recalibrate
if inference_in[1, 0] - last_time > 3:
last_orientation = 0
flip_count = 0
detection_count = 1
print('Calibrating...')
if detection_count == calibration_count:
print('Calibrated')
detection_count += 1
elif detection_count < calibration_count and inference_in[
1, 1] > 0.6:
# print(flip_count, detection_count)
detection_count += 1
if last_time != 0:
delta_time = inference_in[1, 0] - last_time
# dealing with orientation fluctuation
if np.abs(np.pi -
np.abs(last_orientation -
dt_box_lidar[:, 6])) < 0.6 and delta_time:
if detection_count < calibration_count and inference_in[
1, 1] > 0.6:
flip_count += 1
if not (detection_count == calibration_count
and flip_count / detection_count >= 0.5):
dt_box_lidar[:, 6] -= np.pi
delta_angular_speed = np.abs(
last_orientation - dt_box_lidar[:, 6]) / delta_time
delta_linear_speed = np.abs(
last_postion - dt_box_lidar[:, :3]) / delta_time
# print('delta_time', delta_time, 'delta_angular_speed', delta_angular_speed, 'delta_linear_speed', delta_linear_speed)
# filter jetter
if delta_time < 0.2 and (
delta_angular_speed > 2
or np.any(delta_linear_speed > 2)
) and detection_count > calibration_count:
# print(delta_angular_speed, delta_linear_speed)
print('filtered')
filter_flag = 1
if filter_flag == 0:
# record current detection
last_orientation = dt_box_lidar[:, 6]
last_time = inference_in[1, 0]
last_postion = dt_box_lidar[:, :3]
elif filter_flag == 1:
# print('filtered')
dt_box_lidar[:, 6] = last_orientation
last_time = inference_in[1, 0]
dt_box_lidar[:, :3] = last_postion
filter_flag = 0
z_now[0] = dt_box_lidar[:, 0]
z_now[1] = dt_box_lidar[:, 1]
z_now[2] = dt_box_lidar[:, 6]
z_now[3] = inference_in[1, 0]
if socket_odom.poll(
timeout=1) != 0 and detection_count > calibration_count:
odom_info = recv_array(socket_odom)
odom_now[3] = odom_info[2]
linear_speed_correction = 0.67
angular_speed_correction = 0.9
delta_time_odom = odom_now[3] - odom_pre[3]
short_time = 0
if delta_time_odom < 0.01:
short_time = delta_time_odom
if delta_time_odom > 0.01 and z_now[2] != 0:
delta_time_odom += short_time
odom_now = convert_odom(
odom_info[0] * linear_speed_correction,
odom_info[1] * angular_speed_correction, z_now[2],
odom_info[2])
# predict step
if odom_now[3] > odom_pre[3]:
if delta_time_odom > 1000:
delta_time_odom = 0.03
mu_now[0:3] = delta_time_odom * odom_now[0:3] + mu_pre[0:3]
odom_pre = odom_now.copy()
# update step
if z_now[3] > z_pre[3]:
K = 0.8
mu_now[0:3] = mu_now[0:3] + K * (z_now[0:3] - mu_now[0:3])
z_pre = z_now.copy()
mu_pre = mu_now
if first_time == 0 and pc_grid.shape[0] > 1:
dt_box_lidar[:, 0:2] = mu_now[0:2]
dt_box_lidar[:, 6] = mu_now[2]
# get the cornor coords from the bbox
dt_boxes_corners = box_np_ops.center_to_corner_box3d(
dt_box_lidar[:, :3],
dt_box_lidar[:, 3:6],
dt_box_lidar[:, 6],
origin=[0.5, 0.5, 0],
axis=2)
bbox_array = np.zeros((10, 3))
bbox_array[0:8, :] = dt_boxes_corners
bbox_array[8, :] = dt_box_lidar[0, :3] # center position
bbox_array[9, 0] = dt_box_lidar[0, 6] # orientation
send_array(socket_box, bbox_array)
# add car detection in the grid
rect_x = np.zeros((4, ))
rect_y = np.zeros((4, ))
rect_x[0] = find_coords(bbox_array[0, 0], object_res)
rect_y[0] = find_coords(bbox_array[0, 1], object_res,
dim_y_object / 2)
rect_x[1] = find_coords(bbox_array[4, 0], object_res)
rect_y[1] = find_coords(bbox_array[4, 1], object_res,
dim_y_object / 2)
rect_x[2] = find_coords(bbox_array[6, 0], object_res)
rect_y[2] = find_coords(bbox_array[6, 1], object_res,
dim_y_object / 2)
rect_x[3] = find_coords(bbox_array[2, 0], object_res)
rect_y[3] = find_coords(bbox_array[2, 1], object_res,
dim_y_object / 2)
car_coords_x, car_coords_y = np.array(
polygon(rect_x, rect_y, shape=(dim_x_object, dim_y_object)))
car_matrix = np.zeros([dim_x_object, dim_y_object])
car_matrix[car_coords_x, car_coords_y] = 1
rect_x[0] = find_coords(bbox_array[0, 0], grid_res)
rect_y[0] = find_coords(bbox_array[0, 1], grid_res, dim_y / 2)
rect_x[1] = find_coords(bbox_array[4, 0], grid_res)
rect_y[1] = find_coords(bbox_array[4, 1], grid_res, dim_y / 2)
rect_x[2] = find_coords(bbox_array[6, 0], grid_res)
rect_y[2] = find_coords(bbox_array[6, 1], grid_res, dim_y / 2)
rect_x[3] = find_coords(bbox_array[2, 0], grid_res)
rect_y[3] = find_coords(bbox_array[2, 1], grid_res, dim_y / 2)
car_peri_coords_x, car_peri_coords_y = np.array(
polygon_perimeter(rect_x,
rect_y,
shape=(dim_x, dim_y),
clip=True))
# if an occupied grid's neighbor is in car, then that grid is also in car
car_matrix_object = np.zeros([dim_x_object, dim_y_object])
pc_grid = np.transpose(np.array(np.where(object_matrix == 1)))
for ind in range(pc_grid.shape[0]):
if car_matrix[pc_grid[ind, 0], pc_grid[ind, 1]] == 1:
car_matrix_object[pc_grid[ind, 0], pc_grid[ind, 1]] = 1
else:
find_neighbor = 0
neighbors = find_neighbours(pc_grid[ind, 0],
pc_grid[ind, 1],
dim_x_object,
dim_y_object,
option=0)
for neighbor in neighbors:
if object_matrix[neighbor[0], neighbor[1]] == 1:
find_neighbor = 1
if car_matrix[neighbor[0], neighbor[1]] == 1:
car_matrix_object[pc_grid[ind, 0], pc_grid[ind,
1]] = 1
find_neighbor = 1
continue
if find_neighbor == 0:
# remove isolated points
object_matrix_inflated[pc_grid[ind, 0], pc_grid[ind,
1]] = 0
# use connected body to find the car
matrix_labels, num = label(object_matrix_inflated,
connectivity=2,
return_num=True)
find_flag = 0
for num_ind in range(num + 1):
label_xy = np.array(np.where(matrix_labels == num_ind))
if num_ind != 0:
for ind in range(label_xy.shape[1]):
if car_matrix_object[label_xy[0, ind],
label_xy[1, ind]] == 1:
car_matrix_object[label_xy[0, :],
label_xy[1, :]] = 1
find_flag = 1
if find_flag == 1:
break
# print(label_xy.shape[1], ind)
pc_grid = np.transpose(np.array(np.where(car_matrix_object == 1)))
object_matrix_inflated[pc_grid[:, 0], pc_grid[:, 1]] = 0
# inflate the rest of the object matrix
# if point_update == 1:
# pc_grid = np.transpose(np.array(np.where(object_matrix_inflated == 1)))
# for ind in range(pc_grid.shape[0]):
# neighbors = find_neighbours(pc_grid[ind, 0], pc_grid[ind, 1], dim_x_object, dim_y_object, option = 1)
# for neighbor in neighbors:
# object_matrix_inflated[neighbor[0], neighbor[1]] = 1
# pc_grid = np.transpose(np.array(np.where(object_matrix_inflated == 1)))
# point_update = 0
car_matrix_object_big = rescale(car_matrix_object,
grid_res / object_res,
anti_aliasing=False)
object_matrix_big = rescale(object_matrix_inflated,
grid_res / object_res,
anti_aliasing=False)
occupancy_grid = OccupancyGrid(dim_x, dim_y)
occupancy_grid.matrix[np.where(car_matrix_object_big > 0)] = 2
occupancy_grid.matrix[car_peri_coords_x,
car_peri_coords_y] = 3 # perimeter line
occupancy_grid.matrix[np.where(object_matrix_big > 0)] = 1
occupancy_grid.update_image()
send_array(socket_grid, occupancy_grid.image)
