def transform_world_to_image_coordinate(word_coord_array, camera_token: str, lyftd: LyftDataset):
sd_record = lyftd.get("sample_data", camera_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
cam_intrinsic_mtx = np.array(cs_record["camera_intrinsic"])
# homogeneous coordinate to non-homogeneous one
pose_to_sense_rot_mtx = Quaternion(cs_record['rotation']).rotation_matrix.T
world_to_pose_rot_mtx = Quaternion(pose_record['rotation']).rotation_matrix.T
ego_coord_array = np.dot(world_to_pose_rot_mtx, word_coord_array)
t = np.array(pose_record['translation'])
for i in range(3):
ego_coord_array[i, :] = ego_coord_array[i, :] - t[i]
sense_coord_array = np.dot(pose_to_sense_rot_mtx, ego_coord_array)
t = np.array(cs_record['translation'])
for i in range(3):
sense_coord_array[i, :] = sense_coord_array[i, :] - t[i]
return view_points(sense_coord_array, cam_intrinsic_mtx, normalize=True)
def transform_pc_to_camera_coord(cam: dict, pointsensor: dict, point_cloud_3d: LidarPointCloud, lyftd: LyftDataset):
warnings.warn("The point cloud is transformed to camera coordinates in place", UserWarning)
# Points live in the point sensor frame. So they need to be transformed via global to the image plane.
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = lyftd.get("calibrated_sensor", pointsensor["calibrated_sensor_token"])
point_cloud_3d.rotate(Quaternion(cs_record["rotation"]).rotation_matrix)
point_cloud_3d.translate(np.array(cs_record["translation"]))
# Second step: transform to the global frame.
poserecord = lyftd.get("ego_pose", pointsensor["ego_pose_token"])
point_cloud_3d.rotate(Quaternion(poserecord["rotation"]).rotation_matrix)
point_cloud_3d.translate(np.array(poserecord["translation"]))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
poserecord = lyftd.get("ego_pose", cam["ego_pose_token"])
point_cloud_3d.translate(-np.array(poserecord["translation"]))
point_cloud_3d.rotate(Quaternion(poserecord["rotation"]).rotation_matrix.T)
# Fourth step: transform into the camera.
cs_record = lyftd.get("calibrated_sensor", cam["calibrated_sensor_token"])
point_cloud_3d.translate(-np.array(cs_record["translation"]))
point_cloud_3d.rotate(Quaternion(cs_record["rotation"]).rotation_matrix.T)
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
point_cloud_2d = view_points(point_cloud_3d.points[:3, :],
np.array(cs_record["camera_intrinsic"]), normalize=True)
return point_cloud_3d, point_cloud_2d
def transform_box_from_world_to_flat_sensor_coordinates(first_train_sample_box: Box, sample_data_token: str,
lyftd: LyftDataset):
sample_box = first_train_sample_box.copy()
sd_record = lyftd.get("sample_data", sample_data_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
# Move box to ego vehicle coord system parallel to world z plane
ypr = Quaternion(pose_record["rotation"]).yaw_pitch_roll
yaw = ypr[0]
sample_box.translate(-np.array(pose_record["translation"]))
sample_box.rotate(Quaternion(scalar=np.cos(yaw / 2), vector=[0, 0, np.sin(yaw / 2)]).inverse)
# Move box to sensor vehicle coord system parallel to world z plane
# We need to steps, because camera coordinate is x(right), z(front), y(down)
inv_ypr = Quaternion(cs_record["rotation"]).inverse.yaw_pitch_roll
angz = inv_ypr[0]
angx = inv_ypr[2]
sample_box.translate(-np.array(cs_record['translation']))
# rotate around z-axis
sample_box.rotate(Quaternion(scalar=np.cos(angz / 2), vector=[0, 0, np.sin(angz / 2)]))
# rotate around x-axis (by 90 degrees)
angx = 90
sample_box.rotate(Quaternion(scalar=np.cos(angx / 2), vector=[np.sin(angx / 2), 0, 0]))
return sample_box
def generateXYZRGB(ds: LyftDataset, sample_token: str, config: dict,
folder: str):
sample = ds.get("sample", sample_token)
sample_lidar_token = sample["data"]["LIDAR_TOP"]
lidar_data = ds.get("sample_data", sample_lidar_token)
lidar_filepath = ds.get_sample_data_path(sample_lidar_token)
ego_pose = ds.get("ego_pose", lidar_data["ego_pose_token"])
calibrated_sensor = ds.get("calibrated_sensor",
lidar_data["calibrated_sensor_token"])
global_from_car = transform_matrix(ego_pose['translation'],
Quaternion(ego_pose['rotation']),
inverse=False)
car_from_sensor = transform_matrix(calibrated_sensor['translation'],
Quaternion(
calibrated_sensor['rotation']),
inverse=False)
try:
lidar_pointcloud = LidarPointCloud.from_file(lidar_filepath)
pc = copy.deepcopy(lidar_pointcloud)
lidar_pointcloud.transform(car_from_sensor)
except Exception as e:
print("Failed to load Lidar Pointcloud for {}: {}:".format(
sample_token, e))
pass
XYZRGB = getXYZRGB(ds, pc, sample, lidar_data)
np.savez(folder + sample_lidar_token + ".xyzrgb")
def get_box_from_inference(lyftd: LyftDataset, heading_cls, heading_res,
rot_angle, size_cls, size_res, center_coord,
sample_data_token, score, type_name: str) -> Box:
heading_angle = get_heading_angle(heading_cls, heading_res, rot_angle)
size = get_size(size_cls, size_res)
rot_angle += np.pi / 2
center_sensor_coord = get_center_in_sensor_coord(center_coord=center_coord,
rot_angle=rot_angle)
# Make Box
# The rationale of doing this: to conform the convention of Box class, the car is originally heading to +x axis,
# with y(left) and z(top). To make the car heading to the angle it should be in the camera coordinate,
# we have to rotate it by 90 degree around x axis and [theta] degree around y axis, where [theta] is the heading angle
l, w, h = size
first_rot = Quaternion(axis=[1, 0, 0], angle=np.pi / 2)
second_rot = Quaternion(axis=[0, -1, 0], angle=-heading_angle)
box_in_sensor_coord = Box(center=center_sensor_coord,
size=[w, l, h],
orientation=second_rot * first_rot,
score=score,
name=type_name)
box_in_world_coord = convert_box_to_world_coord_with_sample_data_token(
box_in_sensor_coord, sample_data_token, lyftd)
# assert np.abs(box_in_world_coord.orientation.axis[0]) <= 0.02
# assert np.abs(box_in_world_coord.orientation.axis[1]) <= 0.02
return box_in_world_coord
def to_glb(box, info):
# lidar -> ego -> global
# info should belong to exact same element in `gt` dict
box.rotate(Quaternion(info['lidar2ego_rotation']))
box.translate(np.array(info['lidar2ego_translation']))
box.rotate(Quaternion(info['ego2global_rotation']))
box.translate(np.array(info['ego2global_translation']))
return box
def get_mapped_points(self):
###############################################################################
pointsensor = self.level5data.get('sample_data',
self.my_sample['data'][self.ldName])
cam = self.level5data.get("sample_data",
self.my_sample['data'][self.caName])
if 1 > 0:
pc = self.origin
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = self.level5data.get(
"calibrated_sensor",
pointsensor["calibrated_sensor_token"]) #需要判
pc.rotate(Quaternion(cs_record["rotation"]).rotation_matrix)
pc.translate(np.array(cs_record["translation"]))
# Second step: transform to the global frame.
poserecord = self.level5data.get("ego_pose",
pointsensor["ego_pose_token"])
pc.rotate(Quaternion(poserecord["rotation"]).rotation_matrix)
pc.translate(np.array(poserecord["translation"]))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
poserecord = self.level5data.get("ego_pose", cam["ego_pose_token"])
pc.translate(-np.array(poserecord["translation"]))
pc.rotate(Quaternion(poserecord["rotation"]).rotation_matrix.T)
# Fourth step: transform into the camera.
cs_record = self.level5data.get("calibrated_sensor",
cam["calibrated_sensor_token"])
pc.translate(-np.array(cs_record["translation"]))
pc.rotate(Quaternion(cs_record["rotation"]).rotation_matrix.T)
depths = pc.points[2, :]
# Retrieve the color from the depth.
coloring = depths
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points[:3, :],
np.array(cs_record["camera_intrinsic"]),
normalize=True)
# Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
mask = np.ones(depths.shape[0], dtype=bool)
mask = np.logical_and(mask, depths > 0)
mask = np.logical_and(mask, points[0, :] > 1)
mask = np.logical_and(mask, points[0, :] < 1920 - 1)
mask = np.logical_and(mask, points[1, :] > 1)
mask = np.logical_and(mask, points[1, :] < 1080 - 1)
#points[:, mask]
temp_points = points
coloring = coloring[mask]
self.cam_points = []
for i in range(temp_points[0].size):
point = [
temp_points[0][i], temp_points[1][i], temp_points[2][i]
]
self.cam_points.append(point)
def get_sensor_to_world_transform_matrix_from_sample_data_token(sample_data_token, lyftd: LyftDataset):
sd_record = lyftd.get("sample_data", sample_data_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
sensor_to_ego_mtx = transform_matrix(translation=np.array(cs_record["translation"]),
rotation=Quaternion(cs_record["rotation"]))
ego_to_world_mtx = transform_matrix(translation=np.array(pose_record["translation"]),
rotation=Quaternion(pose_record["rotation"]))
return np.dot(ego_to_world_mtx, sensor_to_ego_mtx)
def prepare_training_data_for_scene(first_sample_token, output_folder, level5data, classes, bev_shape, voxel_size, z_offset, box_scale):
"""
Given a first sample token (in a scene), output rasterized input volumes and targets in birds-eye-view perspective.
"""
# get the first sample tken for the scene
sample_token = first_sample_token
while sample_token:
sample = level5data.get("sample", sample_token)
sample_lidar_token = sample["data"]["LIDAR_TOP"]
lidar_data = level5data.get("sample_data", sample_lidar_token)
lidar_filepath = level5data.get_sample_data_path(sample_lidar_token)
ego_pose = level5data.get("ego_pose", lidar_data["ego_pose_token"])
calibrated_sensor = level5data.get("calibrated_sensor", lidar_data["calibrated_sensor_token"])
global_from_car = transform_matrix(ego_pose['translation'],
Quaternion(ego_pose['rotation']), inverse=False)
car_from_sensor = transform_matrix(calibrated_sensor['translation'], Quaternion(calibrated_sensor['rotation']),
inverse=False)
try:
lidar_pointcloud = LidarPointCloud.from_file(lidar_filepath)
lidar_pointcloud.transform(car_from_sensor)
except Exception as e:
print ("Failed to load Lidar Pointcloud for {}: {}:".format(sample_token, e))
sample_token = sample["next"]
continue
bev = create_voxel_pointcloud(lidar_pointcloud.points, bev_shape, voxel_size=voxel_size, z_offset=z_offset)
bev = normalize_voxel_intensities(bev)
boxes = level5data.get_boxes(sample_lidar_token)
target = np.zeros_like(bev)
move_boxes_to_car_space(boxes, ego_pose)
scale_boxes(boxes, box_scale)
target = draw_boxes(target, voxel_size, boxes=boxes, classes=classes, z_offset=z_offset)
bev_im = np.round(bev*255).astype(np.uint8)
target_im = target[:,:,0] # take one channel only
cv2.imwrite(os.path.join(output_folder, "{}_input.png".format(sample_token)), bev_im)
cv2.imwrite(os.path.join(output_folder, "{}_target.png".format(sample_token)), target_im)
# go to the next sample token
sample_token = sample["next"]
def transform_box_from_world_to_sensor_coordinates(first_train_sample_box: Box, sample_data_token: str,
lyftd: LyftDataset):
sample_box = first_train_sample_box.copy()
sd_record = lyftd.get("sample_data", sample_data_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
# Move box to ego vehicle coord system
sample_box.translate(-np.array(pose_record["translation"]))
sample_box.rotate(Quaternion(pose_record["rotation"]).inverse)
# Move box to sensor coord system
sample_box.translate(-np.array(cs_record["translation"]))
sample_box.rotate(Quaternion(cs_record["rotation"]).inverse)
return sample_box
def convert_box_to_world_coord_with_sample_data_token(input_sample_box, sample_data_token, lyftd: LyftDataset):
sample_box = input_sample_box.copy()
sd_record = lyftd.get("sample_data", sample_data_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
# Move box from sensor to vehicle ego coord system
sample_box.rotate(Quaternion(cs_record["rotation"]))
sample_box.translate(np.array(cs_record["translation"]))
# Move box from ego vehicle to world coord system
sample_box.rotate(Quaternion(pose_record["rotation"]))
sample_box.translate(np.array(pose_record["translation"]))
return sample_box
def get_pred_str(pred, sample_token):
boxes_lidar = pred["box3d_lidar"]
boxes_class = pred["label_preds"]
scores = pred['scores']
preds_classes = [classes[x] for x in boxes_class]
box_centers = boxes_lidar[:, :3]
box_yaws = boxes_lidar[:, -1]
box_wlh = boxes_lidar[:, 3:6]
info = token2info[sample_token] # a `sample` token
boxes = []
pred_str = ''
for idx in range(len(boxes_lidar)):
translation = box_centers[idx]
yaw = -box_yaws[idx] - pi / 2
size = box_wlh[idx]
name = preds_classes[idx]
detection_score = scores[idx]
quat = Quaternion(scalar=np.cos(yaw / 2),
vector=[0, 0, np.sin(yaw / 2)])
box = Box(center=box_centers[idx],
size=size,
orientation=quat,
score=detection_score,
name=name,
token=sample_token)
box = to_glb(box, info)
pred = str(box.score) + ' ' + str(box.center[0]) + ' ' \
+ str(box.center[1]) + ' ' + str(box.center[2]) + ' ' \
+ str(box.wlh[0]) + ' ' + str(box.wlh[1]) + ' ' + \
str(box.wlh[2]) + ' ' + str(box.orientation.yaw_pitch_roll[0]) \
+ ' ' + str(name) + ' '
pred_str += pred
return pred_str.strip()
def get_semantic_map_around_ego(map_mask, ego_pose, voxel_size,
output_shape):
def crop_image(image: np.array, x_px: int, y_px: int,
axes_limit_px: int) -> np.array:
x_min = int(x_px - axes_limit_px)
x_max = int(x_px + axes_limit_px)
y_min = int(y_px - axes_limit_px)
y_max = int(y_px + axes_limit_px)
cropped_image = image[y_min:y_max, x_min:x_max]
return cropped_image
pixel_coords = map_mask.to_pixel_coords(ego_pose['translation'][0],
ego_pose['translation'][1])
extent = voxel_size * output_shape[0] * 0.5
scaled_limit_px = int(extent * (1.0 / (map_mask.resolution)))
mask_raster = map_mask.mask()
cropped = crop_image(mask_raster, pixel_coords[0], pixel_coords[1],
int(scaled_limit_px * np.sqrt(2)))
ypr_rad = Quaternion(ego_pose['rotation']).yaw_pitch_roll
yaw_deg = -np.degrees(ypr_rad[0])
rotated_cropped = np.array(Image.fromarray(cropped).rotate(yaw_deg))
ego_centric_map = crop_image(rotated_cropped,
rotated_cropped.shape[1] / 2,
rotated_cropped.shape[0] / 2,
scaled_limit_px)[::-1]
ego_centric_map = cv2.resize(ego_centric_map, output_shape[:2],
cv2.INTER_NEAREST)
return ego_centric_map.astype(np.float32) / 255
def transform_box_from_world_to_flat_vehicle_coordinates(first_train_sample_box: Box, sample_data_token: str,
lyftd: LyftDataset):
sample_box = first_train_sample_box.copy()
sd_record = lyftd.get("sample_data", sample_data_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
# Move box to ego vehicle coord system parallel to world z plane
ypr = Quaternion(pose_record["rotation"]).yaw_pitch_roll
yaw = ypr[0]
sample_box.translate(-np.array(pose_record["translation"]))
sample_box.rotate(Quaternion(scalar=np.cos(yaw / 2), vector=[0, 0, np.sin(yaw / 2)]).inverse)
return sample_box
def convert_boxes3d_from_sensor_to_global_frame(boxes3d, ego_pose,
calibrated_sensor):
'''
Convert boxes3d from the sensor frame to the global frame.
'''
# From the sensor frame to the car frame.
for box3d in boxes3d:
box3d.rotate(Quaternion(calibrated_sensor["rotation"]))
box3d.translate(np.array(calibrated_sensor["translation"]))
# From the car frame to the global frame.
for box3d in boxes3d:
box3d.rotate(Quaternion(ego_pose['rotation']))
box3d.translate(np.array(ego_pose['translation']))
return boxes3d
def transform_image_to_world_coordinate(image_array: np.array, camera_token: str, lyftd: LyftDataset):
"""
:param image_array: 3xN np.ndarray
:param camera_token:
:return:
"""
sd_record = lyftd.get("sample_data", camera_token)
cs_record = lyftd.get("calibrated_sensor", sd_record["calibrated_sensor_token"])
sensor_record = lyftd.get("sensor", cs_record["sensor_token"])
pose_record = lyftd.get("ego_pose", sd_record["ego_pose_token"])
# inverse the viewpoint transformation
cam_intrinsic_mtx = np.array(cs_record["camera_intrinsic"])
image_in_cam_coord = np.dot(np.linalg.inv(cam_intrinsic_mtx), image_array)
print(image_in_cam_coord)
# TODO: think of how to do normalization properly
# image_in_cam_coord = image_in_cam_coord / image_in_cam_coord[3:].ravel()
# homogeneous coordinate to non-homogeneous one
image_in_cam_coord = image_in_cam_coord[0:3, :]
sens_to_pose_rot_mtx = Quaternion(cs_record['rotation']).rotation_matrix
image_in_pose_coord = np.dot(sens_to_pose_rot_mtx, image_in_cam_coord)
t = np.array(cs_record['translation'])
for i in range(3):
image_in_pose_coord[i, :] = image_in_cam_coord[i, :] + t[i]
print(cs_record)
print("in pose record:", image_in_pose_coord)
pose_to_world_rot_mtx = Quaternion(pose_record['rotation']).rotation_matrix
image_in_world_coord = np.dot(pose_to_world_rot_mtx,
image_in_pose_coord)
t = np.array(pose_record['translation'])
for i in range(3):
image_in_world_coord[i, :] = image_in_world_coord[i, :] + t[i]
return image_in_world_coord
def get_train_data_sample_token_and_box(idx, train_objects):
first_train_id = train_objects.iloc[idx, 0]
# Make box
orient_q = Quaternion(axis=[0, 0, 1], angle=float(train_objects.loc[idx, 'yaw']))
center_pos = train_objects.iloc[idx, 2:5].values
wlh = train_objects.iloc[idx, 5:8].values
obj_name = train_objects.iloc[idx, -1]
first_train_sample_box = Box(center=list(center_pos), size=list(wlh),
orientation=orient_q, name=obj_name)
return first_train_id, first_train_sample_box
def get_sample_data(lyft, sample_data_token):
sd_rec = lyft.get("sample_data", sample_data_token)
cs_rec = lyft.get("calibrated_sensor", sd_rec["calibrated_sensor_token"])
sensor_rec = lyft.get("sensor", cs_rec["sensor_token"])
pose_rec = lyft.get("ego_pose", sd_rec["ego_pose_token"])
boxes = lyft.get_boxes(sample_data_token)
box_list = []
for box in boxes:
box.translate(-np.array(pose_rec["translation"]))
box.rotate(Quaternion(pose_rec["rotation"]).inverse)
box.translate(-np.array(cs_rec["translation"]))
box.rotate(Quaternion(cs_rec["rotation"]).inverse)
box_list.append(box)
return box_list, pose_rec
def load_position_raw(self, sample_lidar_token):
lidar_data = self.level5data.get("sample_data", sample_lidar_token)
ego_pose = self.level5data.get("ego_pose", lidar_data["ego_pose_token"])
calibrated_sensor = self.level5data.get("calibrated_sensor", lidar_data["calibrated_sensor_token"])
# Homogeneous transformation matrix from car frame to world frame.
global_from_car = transform_matrix(ego_pose['translation'],
Quaternion(ego_pose['rotation']), inverse=False)
return global_from_car
def lidar_lyft_box_to_global(lyft, boxes, sample_token):
s_record = lyft.get('sample', sample_token)
sample_data_token = s_record['data']['LIDAR_TOP']
sd_record = lyft.get('sample_data', sample_data_token)
cs_record = lyft.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
sensor_record = lyft.get('sensor', cs_record['sensor_token'])
pose_record = lyft.get('ego_pose', sd_record['ego_pose_token'])
box_list = []
for box in boxes:
# Move box to ego vehicle coord system
box.rotate(Quaternion(cs_record['rotation']))
box.translate(np.array(cs_record['translation']))
# Move box to global coord system
box.rotate(Quaternion(pose_record['rotation']))
box.translate(np.array(pose_record['translation']))
box_list.append(box)
return box_list
def move_boxes_to_car_space(boxes, ego_pose):
"""
Move boxes from world space to car space.
Note: mutates input boxes.
"""
translation = -np.array(ego_pose['translation'])
rotation = Quaternion(ego_pose['rotation']).inverse
for box in boxes:
# Bring box to car space
box.translate(translation)
box.rotate(rotation)
def project_point_clouds_to_image(camera_token: str, pointsensor_token: str, lyftd: LyftDataset, use_multisweep=False):
"""
:param camera_token:
:param pointsensor_token:
:return: (image array, transformed 3d point cloud to camera coordinate, 2d point cloud array)
"""
cam = lyftd.get("sample_data", camera_token)
pointsensor = lyftd.get("sample_data", pointsensor_token)
pcl_path = lyftd.data_path / pointsensor["filename"]
assert pointsensor["sensor_modality"] == "lidar"
if use_multisweep:
sample_of_pc_record = lyftd.get("sample", cam['sample_token'])
pc, _ = LidarPointCloud.from_file_multisweep(lyftd, sample_of_pc_record, chan='LIDAR_TOP',
ref_chan='LIDAR_TOP', num_sweeps=5)
else:
pc = LidarPointCloud.from_file(pcl_path)
im = Image.open(str(lyftd.data_path / cam["filename"]))
# Points live in the point sensor frame. So they need to be transformed via global to the image plane.
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = lyftd.get("calibrated_sensor", pointsensor["calibrated_sensor_token"])
pc.rotate(Quaternion(cs_record["rotation"]).rotation_matrix)
pc.translate(np.array(cs_record["translation"]))
# Second step: transform to the global frame.
poserecord = lyftd.get("ego_pose", pointsensor["ego_pose_token"])
pc.rotate(Quaternion(poserecord["rotation"]).rotation_matrix)
pc.translate(np.array(poserecord["translation"]))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
poserecord = lyftd.get("ego_pose", cam["ego_pose_token"])
pc.translate(-np.array(poserecord["translation"]))
pc.rotate(Quaternion(poserecord["rotation"]).rotation_matrix.T)
# Fourth step: transform into the camera.
cs_record = lyftd.get("calibrated_sensor", cam["calibrated_sensor_token"])
pc.translate(-np.array(cs_record["translation"]))
pc.rotate(Quaternion(cs_record["rotation"]).rotation_matrix.T)
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
point_cloud_2d = view_points(pc.points[:3, :], np.array(cs_record["camera_intrinsic"]), normalize=True)
return im, pc, point_cloud_2d
def moveBoxesToCar(boxes, ego_pose):
"""
Move boxes from world space to car space.
Note: mutates input boxes.
"""
translation = -np.array(ego_pose['translation'])
rotation = Quaternion(ego_pose['rotation']).inverse
for box in boxes:
box.translate(translation)
box.rotate(rotation)
return boxes
def get_sp_points(self, box_area):
pointsensor = self.level5data.get('sample_data',
self.my_sample['data'][self.ldName])
cam = self.level5data.get("sample_data",
self.my_sample['data'][self.caName])
pc = self.origin
temp = pc.points[:, :]
cs_record0 = self.level5data.get(
"calibrated_sensor", pointsensor["calibrated_sensor_token"])
cs_record1 = self.level5data.get("calibrated_sensor",
cam["calibrated_sensor_token"])
poserecord0 = self.level5data.get("ego_pose",
pointsensor["ego_pose_token"])
poserecord1 = self.level5data.get("ego_pose", cam["ego_pose_token"])
for i in range(self.size):
pc.points = temp[:4, i:i + 1]
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep. #需要判
pc.rotate(Quaternion(cs_record0["rotation"]).rotation_matrix)
pc.translate(np.array(cs_record0["translation"]))
# Second step: transform to the global frame.
pc.rotate(Quaternion(poserecord0["rotation"]).rotation_matrix)
pc.translate(np.array(poserecord0["translation"]))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
pc.translate(-np.array(poserecord1["translation"]))
pc.rotate(Quaternion(poserecord1["rotation"]).rotation_matrix.T)
# Fourth step: transform into the camera.
pc.translate(-np.array(cs_record1["translation"]))
pc.rotate(Quaternion(cs_record1["rotation"]).rotation_matrix.T)
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points[:3, :],
np.array(cs_record1["camera_intrinsic"]),
normalize=True)
#self.test_points.append(points)
if points[0] < box_area[3] and points[0] > box_area[2] and points[
1] < box_area[1] and points[1] > box_area[0] and points[
2] > 0:
if self.points[i][0] < 0:
self.sp_points.append(self.points[i])
def boxes_lidar_to_lyft(boxes3d, scores=None, labels=None):
box_list = []
for k in range(boxes3d.shape[0]):
quat = Quaternion(axis=[0, 0, 1], radians=boxes3d[k, 6])
box = Box(
boxes3d[k, :3],
boxes3d[k, [4, 3, 5]], # wlh
quat,
label=labels[k] if labels is not None else np.nan,
score=scores[k] if scores is not None else np.nan,
)
box_list.append(box)
return box_list
def load_cloud_raw(self, sample_lidar_token):
lidar_data = self.level5data.get("sample_data", sample_lidar_token)
lidar_filepath = self.level5data.get_sample_data_path(sample_lidar_token)
ego_pose = self.level5data.get("ego_pose", lidar_data["ego_pose_token"])
calibrated_sensor = self.level5data.get("calibrated_sensor", lidar_data["calibrated_sensor_token"])
# Homogeneous transformation matrix from sensor coordinate frame to ego car frame.
car_from_sensor = transform_matrix(calibrated_sensor['translation'], Quaternion(calibrated_sensor['rotation']),
inverse=False)
lidar_pointcloud = LidarPointCloud.from_file(lidar_filepath)
# The lidar pointcloud is defined in the sensor's reference frame.
# We want it in the car's reference frame, so we transform each point
lidar_pointcloud.transform(car_from_sensor)
return lidar_pointcloud.points[:3,:].T
def make_box_db(train_df,l5d):
first_samples = train_df.first_sample_token.values
box_db = {}
box_scale = cfg.DATA.BOX_SCALE
num_sweeps = cfg.DATA.NUM_SWEEPS
min_distance = cfg.DATA.MIN_DISTANCE
for sample_token in tqdm(first_samples):
while sample_token:
sample = l5d.get('sample',sample_token)
sample_lidar_token = sample['data']['LIDAR_TOP']
lidar_data = l5d.get('sample_data',sample_lidar_token)
ego_pose = l5d.get("ego_pose", lidar_data["ego_pose_token"])
calibrated_sensor = l5d.get("calibrated_sensor", lidar_data["calibrated_sensor_token"])
car_from_sensor = transform_matrix(calibrated_sensor['translation'], Quaternion(calibrated_sensor['rotation']),
inverse=False)
try:
lidar_pointcloud = LidarPointCloud.from_file_multisweep(l5d,sample,'LIDAR_TOP','LIDAR_TOP',num_sweeps=num_sweeps,min_distance=min_distance)[0]
lidar_pointcloud.transform(car_from_sensor)
except Exception as e:
print ("Failed to load Lidar Pointcloud for {}: {}:".format(sample_token, e))
sample_token = sample["next"]
continue
boxes = l5d.get_boxes(sample_lidar_token)
move_boxes_to_car_space(boxes, ego_pose)
scale_boxes(boxes, box_scale)
for box in boxes:
point_mask = points_in_box(box,lidar_pointcloud.points[:3,:])
box_points = lidar_pointcloud.points[:,point_mask]
if box.name not in box_db:
box_db[box.name] = [{'lidar':box_points,'box':box}]
else:
box_db[box.name].append({'lidar':box_points,'box':box})
sample_token = sample['next']
pickle.dump(box_db,open(cfg.DATA.BOX_DB_FILE,'wb'))
def augment_pc(pc,boxes,box_db):
classes = cfg.DATA.CLASSES
sample_dict = cfg.DATA.RANDOM_SAMPLES
for c in classes:
if not sample_dict[c] or not box_db[c]: continue
to_sample = sample_dict[c]
sample_inds = random.sample(range(0,len(box_db[c])),to_sample)
samples_to_add = [box_db[c][i] for i in sample_inds]
for sample in samples_to_add:
sample_box = sample['box']
# we don't add the sample_box to the lidar cloud if it intersects with another
# object in the cloud
# we use xy intersection of the bottom corners of the 3d box to check for overlap
# this method is not perfect and could be refactored - however it is unlikely that
# two objects would intersect on xy yet be completely separated - objects would need to stacked
# on top of each other
sample_polygon = Polygon([sample_box.bottom_corners[:2,:]])
if not any([sample_polygon.intersection(Polygon(box.bottom_corners[:2,:])).area for box in boxes]):
# apply transformations to box and points, then add to lidar
pc.append(sample['lidar'])
boxes.append(sample_box)
for box in boxes:
# apply random rotation and translation to the
# ground truth boxes and their points
random_translation = .25 * np.random.randn(3)
box.center += random_translation
points_mask = points_in_box(box,pc[:3,:])
box_points = pc[:,points_mask]
box_t = np.transpose(box_points[:3,:])
box_t += random_translation
random_angle = np.random.uniform(-np.pi,np.pi)
quat = Quaternion(axis=[0,0,1],angle=random_angle)
box.orientation *= quat
rot_matrix = quat.rotation_matrix
box_points = np.dot(rot_matrix,box_points)
return pc,boxes
def get_semantic_map_around_ego(map_mask, ego_pose, voxel_size=0.2, output_shape=(768, 768)):
"""Gets semantic map around ego vehicle, transfrom to voxil coordinates and then to match BEV shape"""
pixel_coords = map_mask.to_pixel_coords(ego_pose['translation'][0], ego_pose['translation'][1])
extent = voxel_size*output_shape[0]*0.5
scaled_limit_px = int(extent * (1.0 / (map_mask.resolution)))
mask_raster = map_mask.mask()
cropped = crop_image(mask_raster, pixel_coords[0], pixel_coords[1], int(scaled_limit_px * np.sqrt(2)))
ypr_rad = Quaternion(ego_pose['rotation']).yaw_pitch_roll
yaw_deg = -np.degrees(ypr_rad[0])
rotated_cropped = np.array(Image.fromarray(cropped).rotate(yaw_deg))
ego_centric_map = crop_image(rotated_cropped, rotated_cropped.shape[1] / 2, rotated_cropped.shape[0] / 2,
scaled_limit_px)[::-1]
ego_centric_map = cv2.resize(ego_centric_map, output_shape[:2], cv2.INTER_NEAREST)
return ego_centric_map.astype(np.float32)/255
def get_pointcloud(sample_token, level5data):
"""Get sample of lidar point cloud
Transform it to car coordinates
"""
sample = level5data.get("sample", sample_token)
sample_lidar_token = sample["data"]["LIDAR_TOP"]
lidar_data = level5data.get("sample_data", sample_lidar_token)
lidar_filepath = level5data.get_sample_data_path(sample_lidar_token)
lidar_pointcloud = LidarPointCloud.from_file(lidar_filepath)
# sensor (lidar) token
calibrated_sensor = level5data.get("calibrated_sensor", lidar_data["calibrated_sensor_token"])
# Homogeneous transformation matrix from sensor coordinate frame to ego car frame.
car_from_sensor = transform_matrix(calibrated_sensor['translation'], Quaternion(calibrated_sensor['rotation']),
inverse=False)
# The lidar pointcloud is defined in the sensor's reference frame.
# We want it in the car's reference frame, so we transform each point
lidar_pointcloud.transform(car_from_sensor)
return lidar_pointcloud