def test_load_pointclouds(self):
"""
Loads up lidar and radar pointclouds.
"""
assert 'NUSCENES' in os.environ, 'Set NUSCENES env. variable to enable tests.'
dataroot = os.environ['NUSCENES']
nusc = NuScenes(version='v1.0-mini', dataroot=dataroot, verbose=False)
sample_rec = nusc.sample[0]
lidar_name = nusc.get('sample_data',
sample_rec['data']['LIDAR_TOP'])['filename']
radar_name = nusc.get('sample_data',
sample_rec['data']['RADAR_FRONT'])['filename']
lidar_path = os.path.join(dataroot, lidar_name)
radar_path = os.path.join(dataroot, radar_name)
pc1 = LidarPointCloud.from_file(lidar_path)
pc2 = RadarPointCloud.from_file(radar_path)
pc3, _ = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
'LIDAR_TOP',
'LIDAR_TOP',
nsweeps=2)
pc4, _ = RadarPointCloud.from_file_multisweep(nusc,
sample_rec,
'RADAR_FRONT',
'RADAR_FRONT',
nsweeps=2)
# Check for valid dimensions.
assert pc1.points.shape[0] == pc3.points.shape[
0] == 4, 'Error: Invalid dimension for lidar pointcloud!'
assert pc2.points.shape[0] == pc4.points.shape[
0] == 18, 'Error: Invalid dimension for radar pointcloud!'
assert pc1.points.dtype == pc3.points.dtype, 'Error: Invalid dtype for lidar pointcloud!'
assert pc2.points.dtype == pc4.points.dtype, 'Error: Invalid dtype for radar pointcloud!'
def get_data(sensor_data):
sd_record = nusc.get('sample_data', sensor_data)
sensor_modality = sd_record['sensor_modality']
nsweeps = 1
sample_rec = nusc.get('sample', sd_record['sample_token'])
ref_chan = 'LIDAR_TOP'
chan = sd_record['channel']
if sensor_modality == 'lidar':
pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=nsweeps)
points = pc.points
return points # (4, 24962) max 30000
elif sensor_modality == 'radar':
pc, times = RadarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=nsweeps)
points = pc.points
return points # (18, -1) # max 100
elif sensor_modality == 'camera':
data_path, boxes, camera_intrinsic = nusc.get_sample_data(
sensor_data) #, box_vis_level=box_vis_level)
data = Image.open(data_path)
img = np.array(data)
return img # (900, 1600, 3)
def getPoints(self, index):
sample = self.nusc.get('sample', self.filtered_sample_tokens[index])
sample_lidar_token = sample['data']['LIDAR_TOP']
lidar_data = self.nusc.get('sample_data', sample_lidar_token)
ego_pose = self.nusc.get('ego_pose', lidar_data['ego_pose_token'])
calibrated_sensor = self.nusc.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, times = LidarPointCloud.from_file_multisweep(
self.nusc, sample, 'LIDAR_TOP', 'LIDAR_TOP')
lidar_pointcloud.transform(car_from_sensor)
except Exception as e:
print(f"Failed to load Lidar Pointcloud for {sample}:{e}")
points = lidar_pointcloud.points
points[3, :] /= 255
points[3, :] -= 0.5
# points_cat = np.concatenate([points, times], axis=0).transpose()
points_cat = points.transpose()
points_cat = points_cat[~np.isnan(points_cat).any(axis=1)]
return points_cat
def get_points_data(nusc, all_sample_tokens):
all_points = []
all_ego_poses = []
for sample_token in all_sample_tokens:
sample = nusc.get('sample', sample_token)
lidar_data = nusc.get('sample_data', sample['data']['LIDAR_TOP'])
chan = 'LIDAR_TOP'
ref_chan = 'LIDAR_TOP'
pc, _ = LidarPointCloud.from_file_multisweep(nusc,
sample,
chan,
ref_chan,
nsweeps=10)
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor',
lidar_data['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.
ps_record = nusc.get('ego_pose', lidar_data['ego_pose_token'])
pc.rotate(Quaternion(ps_record['rotation']).rotation_matrix)
pc.translate(np.array(ps_record['translation']))
points = pc.points
points[3, :] = points[3, :] / 255.0
all_points.append(points)
all_ego_poses.append(ps_record)
return all_points, all_ego_poses
def get_lidar_pointcloud(self):
"""
Extracting lidar pointcloud for current timestep in current scene
Author: Javier
:return Point cloud [Position(x,y,z) X n_points]
"""
# Select sensor data record
channel = 'LIDAR_TOP'
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
# Get aggregated point cloud in lidar frame.
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(self.nusc, self.sample, chan, channel, nsweeps=1)
#calibration of LiDAR points
ref_sd_record = self.nusc.get('sample_data', sample_data_token)
cs_record = self.nusc.get('calibrated_sensor', ref_sd_record['calibrated_sensor_token'])
pose_record = self.nusc.get('ego_pose', ref_sd_record['ego_pose_token'])
ref_to_ego = transform_matrix(translation=cs_record['translation'],
rotation=Quaternion(cs_record["rotation"]))
# Compute rotation between 3D vehicle pose and "flat" vehicle pose (parallel to global z plane).
ego_yaw = Quaternion(pose_record['rotation']).yaw_pitch_roll[0]
rotation_vehicle_flat_from_vehicle = np.dot(
Quaternion(scalar=np.cos(ego_yaw / 2), vector=[0, 0, np.sin(ego_yaw / 2)]).rotation_matrix,
Quaternion(pose_record['rotation']).inverse.rotation_matrix)
vehicle_flat_from_vehicle = np.eye(4)
vehicle_flat_from_vehicle[:3, :3] = rotation_vehicle_flat_from_vehicle
viewpoint = np.dot(vehicle_flat_from_vehicle, ref_to_ego)
points = view_points(pc.points[:3, :], viewpoint, normalize=False)
self.points_lidar = points
return points
def get_lidar_pointcloud(self, nsweeps=5):
"""
Extracting lidar pointcloud for current timestep in current scene
:return Point cloud [Position(x,y,z,i,t) X n_points]
"""
# Select sensor data record
channel = 'LIDAR_TOP'
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
# Get aggregated point cloud in lidar frame.
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(
self.nusc, self.sample, chan, channel, nsweeps)
points = np.concatenate([pc.points, times], axis=0)
return points
def get_lidar_instances(self, nsweeps=1, enlarge=1):
channel = 'LIDAR_TOP'
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
_, boxes, _ = self.nusc.get_sample_data(
sample_data_token, use_flat_vehicle_coordinates=False)
# Get aggregated point cloud in lidar frame.
chan = sd_record['channel']
pc, _ = LidarPointCloud.from_file_multisweep(self.nusc, self.sample,
chan, channel, nsweeps)
points = pc.points
anno_points = []
anno_boxes = []
for box in boxes:
mask = points_in_box(box, points[0:3, :], wlh_factor=enlarge)
if True in mask:
anno_points.append(points[:, mask])
anno_boxes.append(box)
return anno_points, anno_boxes
def _get_lidar_data(self,
sample_rec: dict,
sample_data: dict,
pose_rec: dict,
nsweeps: int = 1) -> LidarPointCloud:
"""
Returns the LIDAR pointcloud for this frame in vehicle/global coordniates
:param sample_rec: sample record dictionary from nuscenes
:param sample_data: sample data dictionary from nuscenes
:param pose_rec: ego pose record dictionary from nuscenes
:param nsweeps: number of sweeps to return for the LIDAR
:return: LidarPointCloud containing this sample and all sweeps
"""
lidar_path = os.path.join(self.nusc_path, sample_data['filename'])
cs_record = self.nusc.get('calibrated_sensor',
sample_data['calibrated_sensor_token'])
if nsweeps > 1:
## Returns in vehicle
lidar_pc, _ = LidarPointCloud.from_file_multisweep(
self.nusc,
sample_rec,
sample_data['channel'],
sample_data['channel'],
nsweeps=nsweeps)
else:
## returns in sensor coordinates
lidar_pc = LidarPointCloud.from_file(lidar_path)
## Sensor to vehicle
lidar_pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
lidar_pc.translate(np.array(cs_record['translation']))
## Vehicle to global
if self.coordinates == 'global':
lidar_pc.rotate(Quaternion(pose_rec['rotation']).rotation_matrix)
lidar_pc.translate(np.array(pose_rec['translation']))
return lidar_pc, cs_record
def get_pcl():
nusc = NuScenes(version='v1.0-mini', dataroot='/data/sets/nuscenes', verbose=True)
explorer = NuScenesExplorer(nusc)
imglist = []
count = 0
for scene in nusc.scene:
token = scene['first_sample_token']
while token != '':
count += 1
sample = nusc.get('sample',token)
sample_data_cam = nusc.get('sample_data', sample['data']['CAM_FRONT'])
sample_data_pcl = nusc.get('sample_data', sample['data']['LIDAR_TOP'])
# get CAM_FRONT datapath
data_path, boxes, camera_intrinsic = explorer.nusc.get_sample_data(sample_data_cam['token'], box_vis_level = BoxVisibility.ANY)
imglist += [data_path]
# get LiDAR point cloud
sample_rec = explorer.nusc.get('sample', sample_data_pcl['sample_token'])
chan = sample_data_pcl['channel']
ref_chan = 'LIDAR_TOP'
pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan)
radius = np.sqrt((pc.points[0])**2 + (pc.points[1])**2 + (pc.points[2])**2)
pc = pc.points.transpose()
pc = pc[np.where(radius<20)]
print(pc)
display(pc.transpose(), count)
token = sample['next']
if count == 2:
break
if count == 2:
break
plt.show()
print(len(imglist))
def get_each_sweep_lidar_pointcloud(self, nsweeps=5):
"""
Extracting lidar pointcloud for current timestep in current scene
:return
Point cloud [[Position(x,y,z, a) X n_points] * timestamps]
Time stamps a list of int for timestamps in microsecond
"""
# Select sensor data record
channel = 'LIDAR_TOP'
sample_data_token = self.sample['data'][channel]
sd_record = self.nusc.get('sample_data', sample_data_token)
# Get aggregated point cloud in lidar frame.
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(
self.nusc, self.sample, chan, channel, nsweeps)
times_interval = np.unique(times)[::-1]
unique_times = self.get_timestamp() - (times_interval * 1e6).astype(
np.int)
points = []
for t in times_interval:
points.append(pc.points[:, times[0] == t])
return points, unique_times
def get_lidar_points(lyftdata, lidar_token):
'''Get lidar point cloud in the frame of the ego vehicle'''
sd_record = lyftdata.get("sample_data", lidar_token)
sensor_modality = sd_record["sensor_modality"]
# Get aggregated point cloud in lidar frame.
sample_rec = lyftdata.get("sample", sd_record["sample_token"])
chan = sd_record["channel"]
ref_chan = "LIDAR_TOP"
pc, times = LidarPointCloud.from_file_multisweep(lyftdata,
sample_rec,
chan,
ref_chan,
nsweeps=1)
# Compute transformation matrices for lidar point cloud
cs_record = lyftdata.get("calibrated_sensor",
sd_record["calibrated_sensor_token"])
pose_record = lyftdata.get("ego_pose", sd_record["ego_pose_token"])
vehicle_from_sensor = np.eye(4)
vehicle_from_sensor[:3, :3] = Quaternion(
cs_record["rotation"]).rotation_matrix
vehicle_from_sensor[:3, 3] = cs_record["translation"]
ego_yaw = Quaternion(pose_record["rotation"]).yaw_pitch_roll[0]
rot_vehicle_flat_from_vehicle = np.dot(
Quaternion(scalar=np.cos(ego_yaw / 2),
vector=[0, 0, np.sin(ego_yaw / 2)]).rotation_matrix,
Quaternion(pose_record["rotation"]).inverse.rotation_matrix,
)
vehicle_flat_from_vehicle = np.eye(4)
vehicle_flat_from_vehicle[:3, :3] = rot_vehicle_flat_from_vehicle
points = view_points(pc.points[:3, :],
np.dot(vehicle_flat_from_vehicle,
vehicle_from_sensor),
normalize=False)
return points
def dataset_mapper(self, frame):
"""
Add sensor data in vehicle or global coordinates to the frame
:param frame (dict): A frame dictionary from the db (no sensor data)
:return frame (dict): frame with all sensor data
"""
sample_record = self.get('sample', frame['sample_token'])
ref_sd_record = self.get(
'sample_data', sample_record['data'][self.cfg.REF_POSE_CHANNEL])
ref_pose_record = self.get('ego_pose', ref_sd_record['ego_pose_token'])
ref_cs_record = self.get('calibrated_sensor',
ref_sd_record['calibrated_sensor_token'])
frame['ref_pose_record'] = ref_pose_record
## Load camera data
cams = frame.get('camera', [])
for cam in cams:
sd_record = self.get('sample_data', cam['token'])
filename = osp.join(self.dataroot, sd_record['filename'])
image = self.get_camera_data(filename)
cam['image'] = image
cam['height'] = sd_record['height']
cam['width'] = sd_record['width']
cam['filename'] = filename
cam['cs_record'] = self.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
cam['pose_record'] = self.get('ego_pose',
sd_record['ego_pose_token'])
## Load annotations in vehicle coordinates
frame['anns'] = self._get_anns(frame['anns'], ref_pose_record)
## Filter anns outside image if in 'camera' mode:
if self.cfg.SAMPLE_MODE == 'camera':
filtered_anns = []
for ann in frame['anns']:
box_cam = nsutils.map_annotation_to_camera(
ann['box_3d'], cam['cs_record'], cam['pose_record'],
ref_pose_record, self.cfg.COORDINATES)
cam_intrinsic = np.array(cam['cs_record']['camera_intrinsic'])
if not box_in_image(box_cam, cam_intrinsic, (1600, 900)):
continue
if self.cfg.GEN_2D_BBOX:
ann['box_2d'] = nsutils.box_3d_to_2d_simple(
box_cam, cam_intrinsic, (1600, 900))
filtered_anns.append(ann)
frame['anns'] = filtered_anns
## Load LIDAR data in vehicle coordinates
if 'lidar' in frame:
lidar = frame['lidar']
sd_record = self.get('sample_data', lidar['token'])
lidar['cs_record'] = self.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
lidar['pose_record'] = self.get('ego_pose',
sd_record['ego_pose_token'])
lidar_pc, _ = LidarPointCloud.from_file_multisweep(
self,
sample_record,
lidar['channel'],
lidar['channel'],
nsweeps=self.cfg.LIDAR_SWEEPS,
min_distance=self.cfg.PC_MIN_DIST)
## Take from sensor to vehicle coordinates
lidar_pc = nsutils.sensor_to_vehicle(lidar_pc, lidar['cs_record'])
## filter points outside the image if in 'camera' mode
if self.cfg.SAMPLE_MODE == "camera":
cam = frame['camera'][0]
cam_intrinsic = np.array(cam['cs_record']['camera_intrinsic'])
lidar_pc_cam, _ = nsutils.map_pointcloud_to_camera(
lidar_pc,
cam_cs_record=cam['cs_record'],
cam_pose_record=cam['pose_record'],
pointsensor_pose_record=frame['lidar']['pose_record'],
coordinates=frame['coordinates'])
_, _, mask = nsutils.map_pointcloud_to_image(lidar_pc_cam,
cam_intrinsic,
img_shape=(1600,
900))
lidar_pc.points = lidar_pc.points[:, mask]
frame['lidar']['pointcloud'] = lidar_pc
## Load Radar data in vehicle coordinates
if 'radar' in frame:
all_radar_pcs = RadarPointCloud(np.zeros((18, 0)))
for radar in frame['radar']:
radar_pc, _ = RadarPointCloud.from_file_multisweep(
self,
sample_record,
radar['channel'],
self.cfg.REF_POSE_CHANNEL,
nsweeps=self.cfg.RADAR_SWEEPS,
min_distance=self.cfg.PC_MIN_DIST)
radar_pc = nsutils.sensor_to_vehicle(radar_pc, ref_cs_record)
## filter points outside the image if in 'camera' mode
if self.cfg.SAMPLE_MODE == "camera":
cam = frame['camera'][0]
cam_intrinsic = np.array(
cam['cs_record']['camera_intrinsic'])
radar_pc_cam, _ = nsutils.map_pointcloud_to_camera(
radar_pc,
cam_cs_record=cam['cs_record'],
cam_pose_record=cam['pose_record'],
pointsensor_pose_record=ref_pose_record,
coordinates=frame['coordinates'])
_, _, mask = nsutils.map_pointcloud_to_image(
radar_pc_cam, cam_intrinsic, img_shape=(1600, 900))
radar_pc.points = radar_pc.points[:, mask]
all_radar_pcs.points = np.hstack(
(all_radar_pcs.points, radar_pc.points))
frame['radar'] = {}
frame['radar']['pointcloud'] = all_radar_pcs
frame['radar']['pose_record'] = ref_pose_record
frame['radar']['cs_record'] = ref_cs_record
return frame
def visualize_sample(nusc: NuScenes,
sample_token: str,
gt_boxes: EvalBoxes,
pred_boxes: EvalBoxes,
nsweeps: int = 1,
conf_th: float = 0.15,
eval_range: float = 50,
verbose: bool = True,
savepath: str = None) -> None:
"""
Visualizes a sample from BEV with annotations and detection results.
:param nusc: NuScenes object.
:param sample_token: The nuScenes sample token.
:param gt_boxes: Ground truth boxes grouped by sample.
:param pred_boxes: Prediction grouped by sample.
:param nsweeps: Number of sweeps used for lidar visualization.
:param conf_th: The confidence threshold used to filter negatives.
:param eval_range: Range in meters beyond which boxes are ignored.
:param verbose: Whether to print to stdout.
:param savepath: If given, saves the the rendering here instead of displaying.
"""
# Retrieve sensor & pose records.
sample_rec = nusc.get('sample', sample_token)
sd_record = nusc.get('sample_data', sample_rec['data']['LIDAR_TOP'])
cs_record = nusc.get('calibrated_sensor',
sd_record['calibrated_sensor_token'])
pose_record = nusc.get('ego_pose', sd_record['ego_pose_token'])
# Get boxes.
boxes_gt_global = gt_boxes[sample_token]
boxes_est_global = pred_boxes[sample_token]
# Map GT boxes to lidar.
boxes_gt = boxes_to_sensor(boxes_gt_global, pose_record, cs_record)
# Map EST boxes to lidar.
boxes_est = boxes_to_sensor(boxes_est_global, pose_record, cs_record)
# Add scores to EST boxes.
for box_est, box_est_global in zip(boxes_est, boxes_est_global):
box_est.score = box_est_global.detection_score
# Get point cloud in lidar frame.
pc, _ = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
'LIDAR_TOP',
'LIDAR_TOP',
nsweeps=nsweeps)
# Init axes.
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :]**2, axis=0))
colors = np.minimum(1, dists / eval_range)
ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show GT boxes.
for box in boxes_gt:
box.render(ax, view=np.eye(4), colors=('g', 'g', 'g'), linewidth=2)
# Show EST boxes.
for box in boxes_est:
# Show only predictions with a high score.
assert not np.isnan(box.score), 'Error: Box score cannot be NaN!'
if box.score >= conf_th:
box.render(ax, view=np.eye(4), colors=('b', 'b', 'b'), linewidth=1)
# Limit visible range.
axes_limit = eval_range + 3 # Slightly bigger to include boxes that extend beyond the range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
# Show / save plot.
if verbose:
print('Rendering sample token %s' % sample_token)
plt.title(sample_token)
if savepath is not None:
plt.savefig(savepath)
plt.close()
else:
plt.show()
def create_dataset(dataroot,
save_dir,
pretrained_model,
width=672,
height=672,
grid_range=70.,
nusc_version='v1.0-mini',
use_constant_feature=True,
use_intensity_feature=True,
end_id=None):
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
bcnn_model = BCNN(in_channels=6, n_class=5).to(device)
# bcnn_model = torch.nn.DataParallel(bcnn_model) # multi gpu
if os.path.exists(pretrained_model):
print('Use pretrained model')
bcnn_model.load_state_dict(
fix_model_state_dict(torch.load(pretrained_model)))
bcnn_model.eval()
else:
return
os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True)
os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True)
os.makedirs(os.path.join(save_dir, 'inference_feature'), exist_ok=True)
nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True)
ref_chan = 'LIDAR_TOP'
if width == height:
size = width
else:
raise Exception(
'Currently only supported if width and height are equal')
grid_length = 2. * grid_range / size
label_half_length = 0
data_id = 0
grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length)
grid_centers \
= (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1]
for my_scene in nusc.scene:
first_sample_token = my_scene['first_sample_token']
token = first_sample_token
# try:
while (token != ''):
print('--- {} '.format(data_id) + token + ' ---')
my_sample = nusc.get('sample', token)
sd_record = nusc.get('sample_data', my_sample['data'][ref_chan])
sample_rec = nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=1)
_, boxes, _ = nusc.get_sample_data(sd_record['token'],
box_vis_level=0)
out_feature = np.zeros((size, size, 8), dtype=np.float32)
for box_idx, box in enumerate(boxes):
label = 0
if box.name.split('.')[0] == 'vehicle':
if box.name.split('.')[1] == 'car':
label = 1
elif box.name.split('.')[1] == 'bus':
label = 1
elif box.name.split('.')[1] == 'truck':
label = 1
elif box.name.split('.')[1] == 'construction':
label = 1
elif box.name.split('.')[1] == 'emergency':
label = 1
elif box.name.split('.')[1] == 'trailer':
label = 1
elif box.name.split('.')[1] == 'bicycle':
label = 2
elif box.name.split('.')[1] == 'motorcycle':
label = 2
elif box.name.split('.')[0] == 'human':
label = 3
# elif box.name.split('.')[0] == 'movable_object':
# label = 1
# elif box.name.split('.')[0] == 'static_object':
# label = 1
else:
continue
height_pt = np.linalg.norm(box.corners().T[0] -
box.corners().T[3])
corners2d = box.corners()[:2, :]
box2d = corners2d.T[[2, 3, 7, 6]]
box2d_center = box2d.mean(axis=0)
yaw, pitch, roll = box.orientation.yaw_pitch_roll
generate_out_feature(width, height, size, grid_centers, box2d,
box2d_center, height_pt, label,
label_half_length, yaw, out_feature)
# import pdb; pdb.set_trace()
out_feature = out_feature.astype(np.float16)
in_feature_generator = Feature_generator(grid_range, width, height,
use_constant_feature,
use_intensity_feature)
in_feature_generator.generate(pc.points.T)
in_feature = in_feature_generator.feature
if use_constant_feature and use_intensity_feature:
in_feature = in_feature.reshape(size, size, 8)
elif use_constant_feature or use_intensity_feature:
in_feature = in_feature.reshape(size, size, 6)
else:
in_feature = in_feature.reshape(size, size, 4)
# instance_pt is flipped due to flip
# out_feature = np.flip(np.flip(out_feature, axis=0), axis=1)
# out_feature[:, :, 1:3] *= -1
np.save(os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)),
in_feature)
np.save(
os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)),
out_feature)
# inference feature
# in_feature = torch.from_numpy(in_feature)
in_feature = in_feature.astype(np.float32)
transform = transforms.Compose([transforms.ToTensor()])
in_feature = transform(in_feature)
in_feature = in_feature.to(device)
in_feature = torch.unsqueeze(in_feature, 0)
print(in_feature.size())
inference_feature = bcnn_model(in_feature)
np.save(
os.path.join(save_dir,
'inference_feature/{:05}'.format(data_id)),
inference_feature.cpu().detach().numpy())
token = my_sample['next']
data_id += 1
if data_id == end_id:
return
def render_sample_data(self,
sample_data_token: str,
with_anns: bool = True,
box_vis_level: BoxVisibility = BoxVisibility.ANY,
axes_limit: float = 40,
ax: Axes = None,
nsweeps: int = 1) -> None:
"""
Render sample data onto axis.
:param sample_data_token: Sample_data token.
:param with_anns: Whether to draw annotations.
:param box_vis_level: If sample_data is an image, this sets required visibility for boxes.
:param axes_limit: Axes limit for lidar and radar (measured in meters).
:param ax: Axes onto which to render.
:param nsweeps: Number of sweeps for lidar and radar.
"""
# Get sensor modality.
sd_record = self.nusc.get('sample_data', sample_data_token)
sensor_modality = sd_record['sensor_modality']
if sensor_modality == 'lidar':
# Get boxes in lidar frame.
_, boxes, _ = self.nusc.get_sample_data(sample_data_token, box_vis_level=box_vis_level)
# Get aggregated point cloud in lidar frame.
sample_rec = self.nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
ref_chan = 'LIDAR_TOP'
pc, times = LidarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps)
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists/axes_limit/np.sqrt(2))
ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=(c, c, c))
# Limit visible range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'radar':
# Get boxes in lidar frame.
sample_rec = self.nusc.get('sample', sd_record['sample_token'])
lidar_token = sample_rec['data']['LIDAR_TOP']
_, boxes, _ = self.nusc.get_sample_data(lidar_token, box_vis_level=box_vis_level)
# Get aggregated point cloud in lidar frame.
# The point cloud is transformed to the lidar frame for visualization purposes.
chan = sd_record['channel']
ref_chan = 'LIDAR_TOP'
pc, times = RadarPointCloud.from_file_multisweep(self.nusc, sample_rec, chan, ref_chan, nsweeps=nsweeps)
# Transform radar velocities (x is front, y is left), as these are not transformed when loading the point
# cloud.
radar_cs_record = self.nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token'])
lidar_sd_record = self.nusc.get('sample_data', lidar_token)
lidar_cs_record = self.nusc.get('calibrated_sensor', lidar_sd_record['calibrated_sensor_token'])
velocities = pc.points[8:10, :] # Compensated velocity
velocities = np.vstack((velocities, np.zeros(pc.points.shape[1])))
velocities = np.dot(Quaternion(radar_cs_record['rotation']).rotation_matrix, velocities)
velocities = np.dot(Quaternion(lidar_cs_record['rotation']).rotation_matrix.T, velocities)
velocities[2, :] = np.zeros(pc.points.shape[1])
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists / axes_limit / np.sqrt(2))
sc = ax.scatter(points[0, :], points[1, :], c=colors, s=3)
# Show velocities.
points_vel = view_points(pc.points[:3, :] + velocities, np.eye(4), normalize=False)
max_delta = 10
deltas_vel = points_vel - points
deltas_vel = 3 * deltas_vel # Arbitrary scaling
deltas_vel = np.clip(deltas_vel, -max_delta, max_delta) # Arbitrary clipping
colors_rgba = sc.to_rgba(colors)
for i in range(points.shape[1]):
ax.arrow(points[0, i], points[1, i], deltas_vel[0, i], deltas_vel[1, i], color=colors_rgba[i])
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=np.eye(4), colors=(c, c, c))
# Limit visible range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
elif sensor_modality == 'camera':
# Load boxes and image.
data_path, boxes, camera_intrinsic = self.nusc.get_sample_data(sample_data_token,
box_vis_level=box_vis_level)
data = Image.open(data_path)
# Init axes.
if ax is None:
_, ax = plt.subplots(1, 1, figsize=(9, 16))
# Show image.
ax.imshow(data)
# Show boxes.
if with_anns:
for box in boxes:
c = np.array(self.get_color(box.name)) / 255.0
box.render(ax, view=camera_intrinsic, normalize=True, colors=(c, c, c))
# Limit visible range.
ax.set_xlim(0, data.size[0])
ax.set_ylim(data.size[1], 0)
else:
raise ValueError("Error: Unknown sensor modality!")
ax.axis('off')
ax.set_title(sd_record['channel'])
ax.set_aspect('equal')
#model.eval()
im_token = image_token[0]
sample_data = nusc.get('sample_data', im_token)
sample_token = sample_data['sample_token']
sample =nusc.get('sample', sample_token)
image_name = sample_data['filename']
img = imread('/data/sets/nuscenes/' + image_name)
# load in the LiDAR data for the sample
sample_data_pcl = nusc.get('sample_data', sample['data']['LIDAR_TOP'])
sample_rec = explorer.nusc.get('sample', sample_data_pcl['sample_token'])
# get LiDAR point cloud
chan = sample_data_pcl['channel']
ref_chan = 'LIDAR_TOP'
pc, times = LidarPointCloud.from_file_multisweep(nusc, sample_rec, chan, ref_chan)
points= pc.points[:3,:]
points = points.astype(np.float32, copy=False)
points = np.expand_dims(points, axis=0)
points= torch.from_numpy(points)
pc2, c, im = map_pointcloud_to_image(sample_data_pcl['token'], im_token)
pointfeat = PointNetfeat(global_feat=True)
_, global_feat= pointfeat(points)
#classifier = classifier.train()
#global_feat = model(points)
pdb.set_trace()
def create_dataset(dataroot,
save_dir,
width=672,
height=672,
grid_range=70.,
nusc_version='v1.0-mini',
use_constant_feature=False,
use_intensity_feature=True,
end_id=None,
augmentation_num=0,
add_noise=False):
"""Create a learning dataset from Nuscens
Parameters
----------
dataroot : str
Nuscenes dataroot path.
save_dir : str
Dataset save directory.
width : int, optional
feature map width, by default 672
height : int, optional
feature map height, by default 672
grid_range : float, optional
feature map range, by default 70.
nusc_version : str, optional
Nuscenes version. v1.0-mini or v1.0-trainval, by default 'v1.0-mini'
use_constant_feature : bool, optional
Whether to use constant feature, by default False
use_intensity_feature : bool, optional
Whether to use intensity feature, by default True
end_id : int, optional
How many data to generate. If None, all data, by default None
augmentation_num : int, optional
How many data augmentations for one sample, by default 0
add_noise : bool, optional
Whether to add noise to pointcloud, by default True
Raises
------
Exception
Width and height are not equal
"""
os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True)
os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True)
nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True)
ref_chan = 'LIDAR_TOP'
if width == height:
size = width
else:
raise Exception(
'Currently only supported if width and height are equal')
grid_length = 2. * grid_range / size
z_trans_range = 0.5
sample_id = 0
data_id = 0
grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length)
grid_centers \
= (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1]
for my_scene in nusc.scene:
first_sample_token = my_scene['first_sample_token']
token = first_sample_token
# try:
while (token != ''):
print('sample:{} {} created_data={}'.format(
sample_id, token, data_id))
my_sample = nusc.get('sample', token)
sd_record = nusc.get('sample_data', my_sample['data'][ref_chan])
sample_rec = nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
pc_raw, _ = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=1)
_, boxes_raw, _ = nusc.get_sample_data(sd_record['token'],
box_vis_level=0)
z_trans = 0
q = Quaternion()
for augmentation_idx in range(augmentation_num + 1):
pc = copy.copy(pc_raw)
if add_noise:
pc.points = add_noise_points(pc.points.T).T
boxes = copy.copy(boxes_raw)
if augmentation_idx > 0:
z_trans = (np.random.rand() - 0.5) * 2 * z_trans_range
pc.translate([0, 0, z_trans])
z_rad = np.random.rand() * np.pi * 2
q = Quaternion(axis=[0, 0, 1], radians=z_rad)
pc.rotate(q.rotation_matrix)
pc_points = pc.points.astype(np.float32)
out_feature = np.zeros((size, size, 8), dtype=np.float32)
for box_idx, box in enumerate(boxes):
if augmentation_idx > 0:
box.translate([0, 0, z_trans])
box.rotate(q)
label = 0
if box.name.split('.')[0] == 'vehicle':
if box.name.split('.')[1] == 'car':
label = 1
elif box.name.split('.')[1] == 'bus':
label = 2
elif box.name.split('.')[1] == 'truck':
label = 2
elif box.name.split('.')[1] == 'construction':
label = 2
elif box.name.split('.')[1] == 'emergency':
label = 2
elif box.name.split('.')[1] == 'trailer':
label = 2
elif box.name.split('.')[1] == 'bicycle':
label = 3
elif box.name.split('.')[1] == 'motorcycle':
label = 3
elif box.name.split('.')[0] == 'human':
label = 4
# elif box.name.split('.')[0] == 'movable_object':
# label = 1
# elif box.name.split('.')[0] == 'static_object':
# label = 1
else:
continue
height_pt = np.linalg.norm(box.corners().T[0] -
box.corners().T[3])
box_corners = box.corners().astype(np.float32)
corners2d = box_corners[:2, :]
box2d = corners2d.T[[2, 3, 7, 6]]
box2d_center = box2d.mean(axis=0)
yaw, pitch, roll = box.orientation.yaw_pitch_roll
out_feature = generate_out_feature(size, grid_centers,
box_corners, box2d,
box2d_center, pc_points,
height_pt, label, yaw,
out_feature)
# feature_generator = fg.FeatureGenerator(
# grid_range, width, height,
# use_constant_feature, use_intensity_feature)
feature_generator = fgpb.FeatureGenerator(
grid_range, size, size)
in_feature = feature_generator.generate(
pc_points.T, use_constant_feature, use_intensity_feature)
if use_constant_feature and use_intensity_feature:
channels = 8
elif use_constant_feature or use_intensity_feature:
channels = 6
else:
channels = 4
in_feature = np.array(in_feature).reshape(
channels, size, size).astype(np.float16)
in_feature = in_feature.transpose(1, 2, 0)
np.save(
os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)),
in_feature)
np.save(
os.path.join(save_dir,
'out_feature/{:05}'.format(data_id)),
out_feature)
token = my_sample['next']
data_id += 1
if data_id == end_id:
return
sample_id += 1
camera_token_FRONT_LEFT = sample_rec['data']['CAM_FRONT_LEFT']
camera_token_FRONT_RIGHT = sample_rec['data']['CAM_FRONT_RIGHT']
camera_token_BACK = sample_rec['data']['CAM_BACK']
pointsensor_token = sample_rec['data']['LIDAR_TOP']
pointsensor = nusc.get('sample_data', pointsensor_token)
#get camera info
cam = nusc.get('sample_data', camera_token)
cam_front_left = nusc.get('sample_data', camera_token_FRONT_LEFT)
cam_front_right = nusc.get('sample_data', camera_token_FRONT_RIGHT)
cam_back = nusc.get('sample_data', camera_token_BACK)
orig_pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
'LIDAR_TOP',
'LIDAR_TOP',
nsweeps=10)
#get ego vehicle pose
pose_record = nusc.get('ego_pose', pointsensor['ego_pose_token'])
#get point cloud in camera frame prior to putting inside image plane
pc, global_frame_pc = project_points_to_image(orig_pc, pointsensor,
cam)
pc_front_left, _ = project_points_to_image(orig_pc, pointsensor,
cam_front_left)
pc_front_right, _ = project_points_to_image(orig_pc, pointsensor,
cam_front_right)
pc_back, _ = project_points_to_image(orig_pc, pointsensor, cam_back)
def get_pointcloud(nusc,
bottom_left,
top_right,
box,
pointsensor_token: str,
camera_token: str,
min_dist: float = 1.0) -> Tuple:
"""
Given a point sensor (lidar/radar) token and camera sample_data token, load point-cloud and map it to the image
plane.
:param pointsensor_token: Lidar/radar sample_data token.
:param camera_token: Camera sample_data token.
:param min_dist: Distance from the camera below which points are discarded.
:return (pointcloud <np.float: 2, n)>, coloring <np.float: n>, image <Image>).
"""
sample_data = nusc.get("sample_data", camera_token)
explorer = NuScenesExplorer(nusc)
cam = nusc.get('sample_data', camera_token)
pointsensor = nusc.get('sample_data', pointsensor_token)
im = Image.open(osp.join(nusc.dataroot, cam['filename']))
sample_rec = explorer.nusc.get('sample', pointsensor['sample_token'])
chan = pointsensor['channel']
ref_chan = 'LIDAR_TOP'
pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=10)
data_path, boxes, camera_intrinsic = nusc.get_sample_data(
pointsensor_token, selected_anntokens=[box.token])
pcl_box = boxes[0]
original_points = pc.points.copy()
# 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 = nusc.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 = nusc.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 = nusc.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 = nusc.get('calibrated_sensor', cam['calibrated_sensor_token'])
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
# Fifth step: actually take a "picture" of the point cloud.
depths = pc.points[2, :]
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points[:3, :],
np.array(cs_record['camera_intrinsic']),
normalize=True)
center = np.array([[box.center[0]], [box.center[1]], [box.center[2]]])
box_center = view_points(center,
np.array(cs_record['camera_intrinsic']),
normalize=True)
box_corners = box.corners()
# Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
# Also make sure points are at least 1m in front of the camera to avoid seeing the lidar points on the camera
# casing for non-keyframes which are slightly out of sync.
mask = np.ones(depths.shape[0], dtype=bool)
mask = np.logical_and(mask, depths > min_dist)
mask = np.logical_and(mask, points[0, :] > 1)
mask = np.logical_and(mask, points[0, :] < im.size[0] - 1)
mask = np.logical_and(mask, points[1, :] > 1)
mask = np.logical_and(mask, points[1, :] < im.size[1] - 1)
original_points = original_points[:, mask]
points = points[:, mask]
points_2 = pc.points[:, mask]
crop_mask = np.ones(points.shape[1], dtype=bool)
crop_mask = np.logical_and(crop_mask, points[1, :] > bottom_left[1])
crop_mask = np.logical_and(crop_mask, points[1, :] < top_right[1])
crop_mask = np.logical_and(crop_mask, points[0, :] > bottom_left[0])
crop_mask = np.logical_and(crop_mask, points[0, :] < top_right[0])
original_points = original_points[:, crop_mask]
points = points[:, crop_mask]
points_3 = points_2[:, crop_mask]
image_center = np.asarray([((bottom_left[0] + top_right[0]) / 2),
((bottom_left[1] + top_right[1]) / 2), 1])
# rotate to make the ray passing through the image_center into the z axis
z_axis = np.linalg.lstsq(np.array(cs_record['camera_intrinsic']),
image_center,
rcond=None)[0]
v = np.asarray([z_axis[0], z_axis[1], z_axis[2]])
z = np.asarray([0., 0., 1.])
normal = np.cross(v, z)
theta = np.arccos(np.dot(v, z) / np.sqrt(v.dot(v)))
new_pts = []
new_corner = []
old_pts = []
points_3 = points_3[:3, :]
translate = np.dot(rotation_matrix(normal, theta), image_center)
for point in points_3.T:
new_pts = new_pts + [np.dot(rotation_matrix(normal, theta), point)]
for corner in box_corners.T:
new_corner = new_corner + [
np.dot(rotation_matrix(normal, theta), corner)
]
points = np.asarray(new_pts)
original_points = original_points[:3, :].T
new_corners = np.asarray(new_corner)
reverse_matrix = rotation_matrix(normal, -theta)
# Sample 400 points
if np.shape(new_pts)[0] > 400:
mask = np.random.choice(points.shape[0], 400, replace=False)
points = points[mask, :]
original_points = original_points[mask, :]
shift = np.expand_dims(np.mean(points, axis=0), 0)
points = points - shift # center
new_corners = new_corners - shift # center
dist = np.max(np.sqrt(np.sum(points**2, axis=1)), 0)
points = points / dist #scale
new_corners = new_corners / dist #scale
# Compute offset from point to corner
n = np.shape(points)[0]
offset = np.zeros((n, 8, 3))
for i in range(0, n):
for j in range(0, 8):
offset[i][j] = new_corners[j] - points[i]
# Compute mask on which points lie inside of the box
m = []
for point in original_points:
if in_box(point, pcl_box.corners()) == True:
m = m + [1]
else:
m = m + [0]
m = np.asarray(m)
return points.T, m, offset, reverse_matrix, new_corners
def visualize_sample(nusc: NuScenes, sample_token: str, all_annotations: Dict, all_results: Dict, nsweeps: int=1,
conf_th: float=0.15, eval_range: float=40, verbose=True) -> None:
"""
Visualizes a sample from BEV with annotations and detection results.
:param nusc: NuScenes object.
:param sample_token: The nuScenes sample token.
:param all_annotations: Maps each sample token to its annotations.
:param all_results: Maps each sample token to its results.
:param nsweeps: Number of sweeps used for lidar visualization.
:param conf_th: The confidence threshold used to filter negatives.
:param eval_range: Range in meters beyond which boxes are ignored.
:param verbose: Whether to print to stdout.
"""
# Retrieve sensor & pose records.
sample_rec = nusc.get('sample', sample_token)
sd_record = nusc.get('sample_data', sample_rec['data']['LIDAR_TOP'])
cs_record = nusc.get('calibrated_sensor', sd_record['calibrated_sensor_token'])
pose_record = nusc.get('ego_pose', sd_record['ego_pose_token'])
# Get boxes.
boxes_gt_global = all_annotations[sample_token]
boxes_est_global = all_results[sample_token]
# Map GT boxes to lidar.
boxes_gt = boxes_to_sensor(boxes_gt_global, pose_record, cs_record)
# Map EST boxes to lidar.
boxes_est = boxes_to_sensor(boxes_est_global, pose_record, cs_record)
# Get point cloud in lidar frame.
pc, _ = LidarPointCloud.from_file_multisweep(nusc, sample_rec, 'LIDAR_TOP', 'LIDAR_TOP', nsweeps=nsweeps)
# Init axes.
_, ax = plt.subplots(1, 1, figsize=(9, 9))
# Show point cloud.
points = view_points(pc.points[:3, :], np.eye(4), normalize=False)
dists = np.sqrt(np.sum(pc.points[:2, :] ** 2, axis=0))
colors = np.minimum(1, dists / eval_range)
ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# Show ego vehicle.
ax.plot(0, 0, 'x', color='black')
# Show GT boxes.
for box in boxes_gt:
box.render(ax, view=np.eye(4), colors=('g', 'g', 'g'), linewidth=2)
# Show EST boxes.
for box in boxes_est:
# Show only predictions with a high score.
if box.score >= conf_th:
box.render(ax, view=np.eye(4), colors=('b', 'b', 'b'), linewidth=1)
# Limit visible range.
axes_limit = eval_range + 3 # Slightly bigger to include boxes that extend beyond the range.
ax.set_xlim(-axes_limit, axes_limit)
ax.set_ylim(-axes_limit, axes_limit)
# Show plot.
if verbose:
print('Showing sample token %s' % sample_token)
plt.title(sample_token)
plt.show()
def create_dataset(dataroot,
save_dir,
width=672,
height=672,
grid_range=70.,
nusc_version='v1.0-mini',
use_constant_feature=True,
use_intensity_feature=True,
end_id=None):
os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True)
os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True)
nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True)
ref_chan = 'LIDAR_TOP'
if width == height:
size = width
else:
raise Exception(
'Currently only supported if width and height are equal')
grid_length = 2. * grid_range / size
label_half_length = 0
data_id = 0
grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length)
grid_centers \
= (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1]
for my_scene in nusc.scene:
first_sample_token = my_scene['first_sample_token']
token = first_sample_token
# try:
while (token != ''):
print('--- {} '.format(data_id) + token + ' ---')
my_sample = nusc.get('sample', token)
sd_record = nusc.get('sample_data', my_sample['data'][ref_chan])
sample_rec = nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=1)
_, boxes, _ = nusc.get_sample_data(sd_record['token'],
box_vis_level=0)
out_feature = np.zeros((size, size, 8), dtype=np.float32)
start = time.time()
for box_idx, box in enumerate(boxes):
label = 0
if box.name.split('.')[0] == 'vehicle':
if box.name.split('.')[1] == 'car':
label = 1
elif box.name.split('.')[1] == 'bus':
label = 2
elif box.name.split('.')[1] == 'truck':
label = 2
elif box.name.split('.')[1] == 'construction':
label = 2
elif box.name.split('.')[1] == 'emergency':
label = 2
elif box.name.split('.')[1] == 'trailer':
label = 2
elif box.name.split('.')[1] == 'bicycle':
label = 3
elif box.name.split('.')[1] == 'motorcycle':
label = 3
elif box.name.split('.')[0] == 'human':
label = 4
# elif box.name.split('.')[0] == 'movable_object':
# label = 1
# elif box.name.split('.')[0] == 'static_object':
# label = 1
else:
continue
height_pt = np.linalg.norm(box.corners().T[0] -
box.corners().T[3])
box_corners = box.corners().astype(np.float32)
corners2d = box_corners[:2, :]
# corners2d = box.corners()[:2, :].astype(np.float32)
box2d = corners2d.T[[2, 3, 7, 6]]
box2d_center = box2d.mean(axis=0)
yaw, pitch, roll = box.orientation.yaw_pitch_roll
# print('--')
# print(box.name)
p1_reshape = box_corners
out_feature = generate_out_feature(
width, height, size, grid_centers,
box_corners, box2d, box2d_center,
pc.points.astype(np.float32), height_pt, label,
label_half_length, yaw, out_feature)
# generate_out_feature(width, height, size, grid_centers, pc.points,
# box.corners(), height_pt,
# label, label_half_length, yaw,
# out_feature)
# out_feature = out_feature.astype(np.float32)
out_end = time.time()
# feature_generator = fg.FeatureGenerator(
# grid_range, width, height,
# use_constant_feature, use_intensity_feature)
feature_generator = fgpb.FeatureGenerator(grid_range, width,
height)
in_feature = feature_generator.generate(pc.points.T,
use_constant_feature,
use_intensity_feature)
in_end = time.time()
print('time total {} out {} in {}'.format(in_end - start,
out_end - start,
in_end - out_end))
if use_constant_feature and use_intensity_feature:
channels = 8
elif use_constant_feature or use_intensity_feature:
channels = 6
else:
channels = 4
in_feature = np.array(in_feature).reshape(channels, size,
size).astype(np.float16)
in_feature = in_feature.transpose(1, 2, 0)
# instance_pt is flipped due to flip
# out_feature = np.flip(np.flip(out_feature, axis=0), axis=1)
# out_feature[:, :, 1:3] *= -1
np.save(os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)),
in_feature)
np.save(
os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)),
out_feature)
token = my_sample['next']
data_id += 1
if data_id == end_id:
return
def create_dataset(dataroot,
save_dir,
width=672,
height=672,
grid_range=70.,
nusc_version='v1.0-mini',
use_constant_feature=True,
use_intensity_feature=True,
end_id=None):
os.makedirs(os.path.join(save_dir, 'in_feature'), exist_ok=True)
os.makedirs(os.path.join(save_dir, 'out_feature'), exist_ok=True)
nusc = NuScenes(version=nusc_version, dataroot=dataroot, verbose=True)
ref_chan = 'LIDAR_TOP'
if width == height:
size = width
else:
raise Exception(
'Currently only supported if width and height are equal')
grid_length = 2. * grid_range / size
label_half_length = 10
data_id = 0
grid_ticks = np.arange(-grid_range, grid_range + grid_length, grid_length)
grid_centers \
= (grid_ticks + grid_length / 2)[:len(grid_ticks) - 1]
for my_scene in nusc.scene:
first_sample_token = my_scene['first_sample_token']
token = first_sample_token
while (token != ''):
print('--- {} '.format(data_id) + token + ' ---')
my_sample = nusc.get('sample', token)
sd_record = nusc.get('sample_data', my_sample['data'][ref_chan])
sample_rec = nusc.get('sample', sd_record['sample_token'])
chan = sd_record['channel']
pc, times = LidarPointCloud.from_file_multisweep(nusc,
sample_rec,
chan,
ref_chan,
nsweeps=10)
_, boxes, _ = nusc.get_sample_data(sd_record['token'],
box_vis_level=0)
out_feature = np.zeros((size, size, 7), dtype=np.float32)
for box_idx, box in enumerate(boxes):
label = 0
if box.name.split('.')[0] == 'vehicle':
if box.name.split('.')[1] == 'car':
label = 1
elif (box.name.split('.')[1]
in ['bus', 'construction', 'trailer', 'truck']):
label = 2
elif box.name.split('.')[1] == 'emergency':
if box.name.split('.')[2] == 'ambulance':
label = 2
# police
else:
label = 1
elif box.name.split('.')[1] in ['bicycle', 'motorcycle']:
label = 3
elif box.name.split('.')[0] == 'human':
label = 4
elif (box.name.split('.')[0]
in ['animal', 'movable_object', 'static_object']):
label = 5
else:
continue
height_pt = np.linalg.norm(box.corners().T[0] -
box.corners().T[3])
corners2d = box.corners()[:2, :]
box2d = corners2d.T[[2, 3, 7, 6]]
box2d_center = box2d.mean(axis=0)
generate_out_feature(width, height, size, grid_centers, box2d,
box2d_center, height_pt, label,
label_half_length, out_feature)
out_feature = out_feature.astype(np.float16)
feature_generator = fg.Feature_generator(grid_range, width, height,
use_constant_feature,
use_intensity_feature)
feature_generator.generate(pc.points.T)
in_feature = feature_generator.feature
if use_constant_feature and use_intensity_feature:
in_feature = in_feature.reshape(size, size, 8)
elif use_constant_feature or use_intensity_feature:
in_feature = in_feature.reshape(size, size, 6)
else:
in_feature = in_feature.reshape(size, size, 4)
# instance_pt is flipped due to flip
out_feature = np.flip(np.flip(out_feature, axis=0), axis=1)
out_feature[:, :, 1:3] *= -1
np.save(os.path.join(save_dir, 'in_feature/{:05}'.format(data_id)),
in_feature)
np.save(
os.path.join(save_dir, 'out_feature/{:05}'.format(data_id)),
out_feature)
token = my_sample['next']
data_id += 1
if data_id == end_id:
return
def nuscenes_gt_to_kitti(self) -> None:
"""
Converts nuScenes GT annotations to KITTI format.
"""
kitti_to_nu_lidar = Quaternion(axis=(0, 0, 1), angle=np.pi / 2)
kitti_to_nu_lidar_inv = kitti_to_nu_lidar.inverse
imsize = (1600, 900)
token_idx = 0 # Start tokens from 0.
# Create output folders.
label_folder = os.path.join(self.output_dir, self.split, 'label_2')
calib_folder = os.path.join(self.output_dir, self.split, 'calib')
image_folder = os.path.join(self.output_dir, self.split, 'image_2')
lidar_folder = os.path.join(self.output_dir, self.split, 'velodyne')
radar_folder = os.path.join(self.output_dir, self.split, 'radar')
for folder in [
label_folder, calib_folder, image_folder, lidar_folder,
radar_folder
]:
if not os.path.isdir(folder):
os.makedirs(folder)
id_to_token_file = os.path.join(self.output_dir, self.split,
'id2token.txt')
id2token = open(id_to_token_file, "w+")
# Use only the samples from the current split.
split_logs = create_splits_logs(self.split, self.nusc)
sample_tokens = self._split_to_samples(split_logs)
sample_tokens = sample_tokens[:self.image_count]
out_filenames = []
for sample_token in tqdm(sample_tokens):
# Get sample data.
sample = self.nusc.get('sample', sample_token)
sample_annotation_tokens = sample['anns']
cam_token = sample['data'][self.cam_name]
lidar_token = sample['data'][self.lidar_name]
radar_tokens = []
for radar_name in _C.RADARS.keys():
radar_tokens.append(sample['data'][radar_name])
# Retrieve sensor records.
sd_record_cam = self.nusc.get('sample_data', cam_token)
sd_record_lid = self.nusc.get('sample_data', lidar_token)
cs_record_cam = self.nusc.get(
'calibrated_sensor', sd_record_cam['calibrated_sensor_token'])
cs_record_lid = self.nusc.get(
'calibrated_sensor', sd_record_lid['calibrated_sensor_token'])
sd_record_rad = []
cs_record_rad = []
for i, radar_token in enumerate(radar_tokens):
sd_record_rad.append(self.nusc.get('sample_data', radar_token))
cs_record_rad.append(
self.nusc.get('calibrated_sensor',
sd_record_rad[i]['calibrated_sensor_token']))
# Combine transformations and convert to KITTI format.
# Note: cam uses same conventions in KITTI and nuScenes.
lid_to_ego = transform_matrix(cs_record_lid['translation'],
Quaternion(
cs_record_lid['rotation']),
inverse=False)
ego_to_cam = transform_matrix(cs_record_cam['translation'],
Quaternion(
cs_record_cam['rotation']),
inverse=True)
rad_to_ego = []
for cs_rec_rad in cs_record_rad:
rad_to_ego.append(
transform_matrix(cs_rec_rad['translation'],
Quaternion(cs_rec_rad['rotation']),
inverse=False))
velo_to_cam = np.dot(ego_to_cam, lid_to_ego)
# # TODO: Assuming Radar point are going to be in ego coordinates
# radar_to_cam = ego_to_cam
# Convert from KITTI to nuScenes LIDAR coordinates, where we apply velo_to_cam.
velo_to_cam_kitti = np.dot(velo_to_cam,
kitti_to_nu_lidar.transformation_matrix)
# # Nuscenes radars use same convention as KITTI lidar
# radar_to_cam_kitti = radar_to_cam
# Currently not used.
imu_to_velo_kitti = np.zeros((3, 4)) # Dummy values.
r0_rect = Quaternion(axis=[1, 0, 0], angle=0) # Dummy values.
# Projection matrix.
p_left_kitti = np.zeros((3, 4))
# Cameras are always rectified.
p_left_kitti[:3, :3] = cs_record_cam['camera_intrinsic']
# Create KITTI style transforms.
velo_to_cam_rot = velo_to_cam_kitti[:3, :3]
velo_to_cam_trans = velo_to_cam_kitti[:3, 3]
# radar_to_cam_rot = radar_to_cam_kitti[:3, :3]
# radar_to_cam_trans = radar_to_cam_kitti[:3, 3]
# Check that the lidar rotation has the same format as in KITTI.
assert (velo_to_cam_rot.round(0) == np.array([[0, -1,
0], [0, 0, -1],
[1, 0, 0]])).all()
assert (velo_to_cam_trans[1:3] < 0).all()
# Retrieve the token from the lidar.
# Note that this may be confusing as the filename of the camera will
# include the timestamp of the lidar, not the camera.
filename_cam_full = sd_record_cam['filename']
filename_lid_full = sd_record_lid['filename']
filename_rad_full = []
for sd_rec_rad in sd_record_rad:
filename_rad_full.append(sd_rec_rad['filename'])
out_filename = '%06d' % token_idx # Alternative to use KITTI names.
# out_filename = sample_token
# Write token to disk.
text = sample_token
id2token.write(text + '\n')
id2token.flush()
token_idx += 1
# Convert image (jpg to png).
src_im_path = os.path.join(self.nusc.dataroot, filename_cam_full)
dst_im_path = os.path.join(image_folder, out_filename + '.png')
if self.use_symlinks:
# Create symbolic links to nuscenes images
os.symlink(os.path.abspath(src_im_path), dst_im_path)
else:
im = Image.open(src_im_path)
im.save(dst_im_path, "PNG")
# Convert lidar.
src_lid_path = os.path.join(self.nusc.dataroot, filename_lid_full)
dst_lid_path = os.path.join(lidar_folder, out_filename + '.bin')
assert not dst_lid_path.endswith('.pcd.bin')
# pcl = LidarPointCloud.from_file(src_lid_path)
pcl, _ = LidarPointCloud.from_file_multisweep(
nusc=self.nusc,
sample_rec=sample,
chan=self.lidar_name,
ref_chan=self.lidar_name,
nsweeps=self.lidar_sweeps,
min_distance=1)
pcl.rotate(
kitti_to_nu_lidar_inv.rotation_matrix) # In KITTI lidar frame.
pcl.points = pcl.points.astype('float32')
with open(dst_lid_path, "w") as lid_file:
pcl.points.T.tofile(lid_file)
# # Visualize pointclouds
# _, ax = plt.subplots(1, 1, figsize=(9, 9))
# points = view_points(pcl.points[:3, :], np.eye(4), normalize=False)
# dists = np.sqrt(np.sum(pcl.points[:2, :] ** 2, axis=0))
# colors = np.minimum(1, dists / 50)
# ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# # plt.show(block=False)
# plt.show()
# Convert radar.
src_rad_path = []
for filename_radar in filename_rad_full:
src_rad_path.append(
os.path.join(self.nusc.dataroot, filename_radar))
dst_rad_path = os.path.join(radar_folder, out_filename + '.bin')
assert not dst_rad_path.endswith('.pcd.bin')
pcl = RadarPointCloud(np.zeros((18, 0)))
## Get Radar points in Lidar coordinate system
for i, rad_path in enumerate(src_rad_path):
pc, _ = RadarPointCloud.from_file_multisweep(
self.nusc,
sample_rec=sample,
chan=sd_record_rad[i]['channel'],
ref_chan=self.lidar_name,
nsweeps=self.radar_sweeps,
min_distance=0)
# rot_matrix = Quaternion(cs_record_rad[i]['rotation']).rotation_matrix
# pc.rotate(rot_matrix)
# pc.translate(np.array(cs_record_rad[i]['translation']))
pcl.points = np.hstack((pcl.points, pc.points))
pcl.rotate(
kitti_to_nu_lidar_inv.rotation_matrix) # In KITTI lidar frame.
## Visualize pointclouds
# _, ax = plt.subplots(1, 1, figsize=(9, 9))
# points = view_points(pcl.points[:3, :], np.eye(4), normalize=False)
# dists = np.sqrt(np.sum(pcl.points[:2, :] ** 2, axis=0))
# colors = np.minimum(1, dists / 50)
# ax.scatter(points[0, :], points[1, :], c=colors, s=0.2)
# plt.show()
pcl.points = pcl.points.astype('float32')
with open(dst_rad_path, "w") as rad_file:
pcl.points.T.tofile(rad_file)
# Add to tokens.
out_filenames.append(out_filename)
# Create calibration file.
kitti_transforms = dict()
kitti_transforms['P0'] = np.zeros((3, 4)) # Dummy values.
kitti_transforms['P1'] = np.zeros((3, 4)) # Dummy values.
kitti_transforms['P2'] = p_left_kitti # Left camera transform.
kitti_transforms['P3'] = np.zeros((3, 4)) # Dummy values.
kitti_transforms[
'R0_rect'] = r0_rect.rotation_matrix # Cameras are already rectified.
kitti_transforms['Tr_velo_to_cam'] = np.hstack(
(velo_to_cam_rot, velo_to_cam_trans.reshape(3, 1)))
# kitti_transforms['Tr_radar_to_cam'] = np.hstack((radar_to_cam_rot, radar_to_cam_trans.reshape(3, 1)))
kitti_transforms['Tr_imu_to_velo'] = imu_to_velo_kitti
calib_path = os.path.join(calib_folder, out_filename + '.txt')
with open(calib_path, "w") as calib_file:
for (key, val) in kitti_transforms.items():
val = val.flatten()
val_str = '%.12e' % val[0]
for v in val[1:]:
val_str += ' %.12e' % v
calib_file.write('%s: %s\n' % (key, val_str))
# Write label file.
label_path = os.path.join(label_folder, out_filename + '.txt')
if os.path.exists(label_path):
print('Skipping existing file: %s' % label_path)
continue
with open(label_path, "w") as label_file:
for sample_annotation_token in sample_annotation_tokens:
sample_annotation = self.nusc.get('sample_annotation',
sample_annotation_token)
# Get box in LIDAR frame.
_, box_lidar_nusc, _ = self.nusc.get_sample_data(
lidar_token,
box_vis_level=BoxVisibility.NONE,
selected_anntokens=[sample_annotation_token])
box_lidar_nusc = box_lidar_nusc[0]
# Truncated: Set all objects to 0 which means untruncated.
truncated = 0.0
# Occluded: Set all objects to full visibility as this information is not available in nuScenes.
occluded = 0
# Convert nuScenes category to nuScenes detection challenge category.
detection_name = _C.KITTI_CLASSES.get(
sample_annotation['category_name'])
# Skip categories that are not in the KITTI classes.
if detection_name is None:
continue
# Convert from nuScenes to KITTI box format.
box_cam_kitti = KittiDB.box_nuscenes_to_kitti(
box_lidar_nusc, Quaternion(matrix=velo_to_cam_rot),
velo_to_cam_trans, r0_rect)
# Project 3d box to 2d box in image, ignore box if it does not fall inside.
bbox_2d = KittiDB.project_kitti_box_to_image(box_cam_kitti,
p_left_kitti,
imsize=imsize)
if bbox_2d is None:
# continue
## If box is not inside the image, 2D boxes are set to zero
bbox_2d = (0, 0, 0, 0)
# Set dummy score so we can use this file as result.
box_cam_kitti.score = 0
# Convert box to output string format.
output = KittiDB.box_to_string(name=detection_name,
box=box_cam_kitti,
bbox_2d=bbox_2d,
truncation=truncated,
occlusion=occluded)
# Write to disk.
label_file.write(output + '\n')
id2token.close()