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